64 lines
1.4 KiB
C++
64 lines
1.4 KiB
C++
#include <cmath>
|
|
#include <sstream>
|
|
|
|
#include <scmp/geom/Vector.h>
|
|
#include <scmp/geom/Quaternion.h>
|
|
#include <scmp/Math.h>
|
|
#include <scmp/filter/Madgwick.h>
|
|
|
|
#include <sctest/Assert.h>
|
|
#include <sctest/Checks.h>
|
|
#include <sctest/SimpleSuite.h>
|
|
|
|
using namespace std;
|
|
using namespace scmp;
|
|
|
|
|
|
bool
|
|
SimpleAngularOrientation()
|
|
{
|
|
filter::Madgwickd mf;
|
|
geom::Vector3d gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
|
geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
|
|
double delta = 0.00917; // assume 109 updates per second, as per the paper.
|
|
double twentyDegrees = scmp::DegreesToRadiansD(20.0);
|
|
|
|
// The paper specifies a minimum of 109 IMU readings to stabilize; for
|
|
// two seconds, that means 218 updates.
|
|
for (int i = 0; i < 218; i++) {
|
|
mf.UpdateAngularOrientation(gyro, delta);
|
|
}
|
|
|
|
SCTEST_CHECK_EQ(mf.Orientation(), frame20Deg);
|
|
|
|
auto euler = mf.Euler();
|
|
SCTEST_CHECK_DEQ_EPS(euler[0], twentyDegrees, 0.01);
|
|
SCTEST_CHECK_DEQ_EPS(euler[1], 0.0, 0.01);
|
|
SCTEST_CHECK_DEQ_EPS(euler[2], 0.0, 0.01);
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
int
|
|
main(int argc, char **argv)
|
|
{
|
|
sctest::SimpleSuite suite;
|
|
|
|
suite.AddTest("SimpleAngularOrientation", SimpleAngularOrientation);
|
|
auto result = suite.Run();
|
|
|
|
if (suite.IsReportReady()) {
|
|
auto report = suite.GetReport();
|
|
std::cout << report.Failing << " / " << report.Total;
|
|
std::cout << " tests failed.\n";
|
|
}
|
|
|
|
if (result) {
|
|
return 0;
|
|
}
|
|
else {
|
|
return 1;
|
|
}
|
|
}
|