sc/scsl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Further code cleanups and documentation.

Browse Source 
- Coverity defects.
- Documentation.
This commit is contained in:
Kyle Isom
2023-10-20 01:31:06 -07:00
parent 2a23d2e204
commit 4eb4008130
16 changed files with 254 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
test/madgwick.cc
View File

@@ -29,6 +29,60 @@ SimpleAngularOrientationFloat()
mflt.UpdateAngularOrientation(gyro, delta);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
SCTEST_CHECK_FEQ_EPS(euler[0], twentyDegrees, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[1], 0.0, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[2], 0.0, 0.01);
return true;
}
bool
SimpleAngularOrientationFloatDefaultDT()
{
filter::Madgwickf mflt;
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float delta = 0.00917; // assume 109 updates per second, as per the paper.
const float twentyDegrees = scmp::DegreesToRadiansF(20.0);
mflt.DeltaT(delta);
// 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++) {
mflt.UpdateAngularOrientation(gyro);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
SCTEST_CHECK_FEQ_EPS(euler[0], twentyDegrees, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[1], 0.0, 0.01);
SCTEST_CHECK_FEQ_EPS(euler[2], 0.0, 0.01);
return true;
}
bool
VerifyUpdateWithZeroDeltaTFails()
{
filter::Madgwickf mflt;
const geom::Vector3f gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
const geom::Quaternionf frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
const float twentyDegrees = scmp::DegreesToRadiansF(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++) {
mflt.UpdateAngularOrientation(gyro);
}
SCTEST_CHECK_EQ(mflt.Orientation(), frame20Deg);
auto euler = mflt.Euler();
@@ -182,22 +236,25 @@ main(int argc, char **argv)
sctest::SimpleSuite suite;
suite.AddTest("SimpleAngularOrientationDouble",
suite.AddTest("SimpleAngularOrientationFloat",
SimpleAngularOrientationFloat);
suite.AddTest("SimpleAngularOrientationFloatDefaultDT",
SimpleAngularOrientationFloatDefaultDT);
suite.AddFailingTest("VerifyUpdateWithZeroDeltaTFails",
VerifyUpdateWithZeroDeltaTFails);
suite.AddTest("SimpleAngularOrientationDouble",
SimpleAngularOrientationDouble);
suite.AddTest("SimpleAngularOrientationDouble (iniital vector3f)",
suite.AddTest("SimpleAngularOrientationFloat (inital vector3f)",
SimpleAngularOrientation2InitialVector3f);
suite.AddTest("SimpleAngularOrientationDouble (iniital vector3d)",
suite.AddTest("SimpleAngularOrientationDouble (inital vector3d)",
SimpleAngularOrientation2InitialVector3d);
suite.AddTest("SimpleAngularOrientationDouble (iniital quaternionf)",
suite.AddTest("SimpleAngularOrientationFloat (inital quaternionf)",
SimpleAngularOrientation2InitialQuaternionf);
suite.AddTest("SimpleAngularOrientationDouble (iniital quaterniond)",
suite.AddTest("SimpleAngularOrientationDouble (inital quaterniond)",
SimpleAngularOrientation2InitialQuaterniond);
auto result = suite.Run();
std::cout << suite.GetReport() << "\n";
return result ? 0 : 1;
}

28
test/tlv.cc
View File

@@ -28,38 +28,44 @@ tlvTestSuite(Arena &backend)
rec4.Tag = 1;
std::cout << "\twriting new rec1" << "\n";
assert(TLV::WriteToMemory(backend, cursor, rec1) != nullptr);
cursor = TLV::WriteToMemory(backend, cursor, rec1);
sctest::Assert(cursor != nullptr,
"cursor should not be NULL after writing rec1");
std::cout << "\twriting new rec2" << "\n";
assert((cursor = TLV::WriteToMemory(backend, cursor, rec2)) != nullptr);
cursor = TLV::WriteToMemory(backend, cursor, rec2);
sctest::Assert(cursor != nullptr,
"cursor should not be NULL after writing rec2");
std::cout << "\twriting new rec3" << "\n";
assert(TLV::WriteToMemory(backend, cursor, rec3) != nullptr);
cursor = TLV::WriteToMemory(backend, cursor, rec3);
sctest::Assert(cursor != nullptr);
cursor = nullptr;
// the cursor should point at the next record,
// and rec4 should contain the same data as rec1.
std::cout << "\tFindTag 1" << "\n";
cursor = TLV::FindTag(backend, cursor, rec4);
assert(cursor != nullptr);
assert(cursor != backend.Start());
assert(cmpRecord(rec1, rec4));
sctest::Assert(cursor != nullptr, "cursor should not be null");
sctest::Assert(cursor != backend.Start());
sctest::Assert(cmpRecord(rec1, rec4));
std::cout << "\tFindTag 2" << "\n";
cursor = TLV::FindTag(backend, cursor, rec4);
assert(cursor != nullptr);
assert(cmpRecord(rec3, rec4));
sctest::Assert(cursor != nullptr,
"cursor should not be null after reading last record");
sctest::Assert(cmpRecord(rec3, rec4), "rec3 != rec4");
std::cout << "\tSetRecord 1\n";
TLV::SetRecord(rec4, 3, TEST_STRLEN3, TEST_STR3);
assert(TLV::WriteToMemory(backend, nullptr, rec4));
sctest::Assert(TLV::WriteToMemory(backend, nullptr, rec4));
std::cout << "FindTag 3\n";
rec4.Tag = 2;
cursor = TLV::FindTag(backend, nullptr, rec4);
assert(cursor != nullptr);
sctest::Assert(cursor != nullptr);
std::cout << "DeleteRecord\n";
TLV::DeleteRecord(backend, cursor);
assert(cursor[0] == 3);
sctest::Assert(cursor[0] == 3);
}
bool