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

Further code cleanups and documentation. 

- 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -10,6 +10,7 @@ build
core
core.*
cmake-build-*
compile_commands.json
bufferTest
dictionaryTest

80
include/scmp/filter/Madgwick.h
View File

@ -1,5 +1,5 @@
///
/// \file Madwick.cc
/// \file Madgwick.h
/// \author K. Isom <kyle@imap.cc>
/// \date 2019-08-06
/// \brief Implementation of a Madgwick filter.
@ -22,10 +22,6 @@
/// PERFORMANCE OF THIS SOFTWARE.
///
/// \file madgwick.h
/// \brief Implementation of a Madgwick filter.
///
/// See
#ifndef SCMP_FILTER_MADGWICK_H
#define SCMP_FILTER_MADGWICK_H
@ -56,7 +52,8 @@ template <typename T>
class Madgwick {
public:
/// \brief The Madgwick filter is initialised with an identity quaternion.
Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame() {};
Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame()
{};
/// \brief The Madgwick filter is initialised with a sensor frame.
@ -75,10 +72,12 @@ public:
///
/// \param sf A quaternion representing the current Orientation.
Madgwick(scmp::geom::Quaternion<T> sf) :
deltaT(0.0), previousSensorFrame(), sensorFrame(sf) {};
deltaT(0.0), previousSensorFrame(), sensorFrame(sf)
{};
/// \brief Return the current Orientation as measured by the filter.
/// \brief Return the current orientation as measured by the
/// filter.
///
/// \return The current sensor frame.
scmp::geom::Quaternion<T>
@ -111,27 +110,65 @@ public:
UpdateFrame(const scmp::geom::Quaternion<T> &sf, T delta)
{
this->previousSensorFrame = this->sensorFrame;
this->sensorFrame = sf;
this->deltaT = delta;
this->sensorFrame = sf;
this->deltaT = delta;
}
/// \brief Update the sensor frame to a new frame.
///
/// \warning The filter's default Δt must be set before calling
// this.
///
/// \param sf The new sensor frame replacing the previous one.
void
UpdateFrame(const scmp::geom::Quaternion<T> &sf)
{
this->UpdateFrame(sf, this->deltaT);
}
/// \brief Update the sensor frame with a gyroscope reading.
///
/// This method will assert that the Δt value is not zero
/// within a 100μs tolerance. This assert is compiled out with
/// the compile flag NDEBUG, but may be useful to catch
/// possible errors.
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as w_x, w_y, w_z.
/// \param delta The time step between readings. It must not be zero.
void
UpdateAngularOrientation(const scmp::geom::Vector<T, 3> &gyro, T delta)
{
// Ensure the delta isn't zero within a 100 μs tolerance.
// Ensure the delta isn't zero within a 100 μs
// tolerance. The assert helps to catch bugs in
// testing, but otherwise we should refused to do
// anything.
assert(!scmp::WithinTolerance<T>(delta, 0.0, 0.0001));
scmp::geom::Quaternion<T> q = this->AngularRate(gyro) * delta;
if (scmp::WithinTolerance<T>(delta, 0.0, 0.00001)) {
return;
}
scmp::geom::Quaternion<T> q = this->AngularRate(gyro) * delta;
this->UpdateFrame(this->sensorFrame + q, delta);
}
/// \brief Update the sensor frame with a gyroscope reading.
///
/// If no Δt is provided, the filter's default is used.
///
/// \warning The default Δt must be explicitly set using DeltaT
/// before calling this.
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as w_x, w_y, w_z.
void
UpdateAngularOrientation(const scmp::geom::Vector<T, 3> &gyro)
{
this->UpdateAngularOrientation(gyro, this->deltaT);
}
/// \brief Retrieve a vector of the Euler angles in ZYX Orientation.
///
/// \return A vector of Euler angles as <ψ, θ, ϕ>.
@ -141,6 +178,25 @@ public:
return this->sensorFrame.euler();
}
/// \brief Set the default Δt.
///
/// \note This must be explicitly called before calling any
/// method which uses the filter's internal Δt.
///
/// \param The time delta to use when no time delta is
/// provided.
void
DeltaT(T newDeltaT)
{
this->deltaT = newDeltaT;
}
/// \brief Retrieve the filter's current ΔT.
///
/// \return The current value the filter will default to using
/// if no time delta is provided.
T DeltaT() { return this->deltaT; }
private:
T deltaT;
scmp::geom::Quaternion<T> previousSensorFrame;

2
include/sctest/Assert.h
View File

@ -1,5 +1,5 @@
///
/// \file Test.h
/// \file Assert.h
/// \author K. Isom <kyle@imap.cc>
/// \date 2023-10-09
/// \brief Tooling to assist in building test programs..

10
include/sctest/Report.h
View File

@ -40,10 +40,15 @@ public:
/// \return The number of tests that failed.
size_t Failing() const;
/// \brief Returns the number of tests that have passed
/// successfully.
size_t Passing() const;
/// \brief Total is the number of tests registered.
size_t Total() const;
void Failed();
void Passed();
void AddTest(size_t testCount = 0);
void Reset(size_t testCount = 0);
@ -54,8 +59,9 @@ public:
Report();
private:
size_t failing{};
size_t total{};
size_t failing;
size_t passed;
size_t total;
std::chrono::time_point<std::chrono::steady_clock> start;
std::chrono::time_point<std::chrono::steady_clock> end;

7
include/sctest/SimpleSuite.h
View File

@ -41,6 +41,9 @@ struct UnitTest {
/// This is the test function to be run.
std::function<bool()> test;
/// This is the value the test returns if it passes.
bool expect;
};
/// \brief SimpleSuite is a test-running harness for simple tests.
@ -105,8 +108,8 @@ public:
private:
bool quiet;
std::function<bool(void)> fnSetup, fnTeardown;
std::vector<UnitTest> tests;
std::function<bool(void)> fnSetup, fnTeardown;
std::vector<UnitTest> tests;
// Report functions.
Report report;

2
src/scmp/Coord2D.cc
View File

@ -99,7 +99,7 @@ Point2D::Rotate(std::vector<Polar2D> vertices, double theta)
{
std::vector<Point2D> rotated;
for (auto v : vertices) {
for (auto& v : vertices) {
Point2D p;
v.RotateAround(*this, p, theta);
rotated.push_back(p) ;

65
src/sl/Arena.cc
View File

@ -111,12 +111,12 @@ Arena::Open(const char *path)
this->Destroy();
}
if (stat(path, &st) != 0) {
this->fd = open(path, O_RDWR);
if (this->fd == -1) {
return -1;
}
this->fd = open(path, O_RDWR);
if (this->fd == -1) {
if (stat(path, &st) != 0) {
return -1;
}
@ -152,6 +152,10 @@ Arena::Create(const char *path, size_t fileSize)
bool
Arena::CursorInArena(const uint8_t *cursor)
{
if (cursor == nullptr) {
return false;
}
if (cursor < this->store) {
return false;
}
@ -188,28 +192,26 @@ Arena::Destroy()
}
switch (this->arenaType) {
case ArenaType::Static:
break;
case ArenaType::Alloc:
delete[] this->store;
break;
#if defined(__posix__) || defined(__linux__) || defined(__APPLE__)
case ArenaType::MemoryMapped:
if (munmap(this->store, this->size) == -1) {
abort();
return;
}
if (close(this->fd) == -1) {
abort();
}
this->fd = 0;
break;
#endif
default:
#if defined(NDEBUG)
case ArenaType::Static:
break;
case ArenaType::Alloc:
delete[] this->store;
break;
case ArenaType::MemoryMapped:
if (munmap(this->store, this->size) == -1) {
abort();
return;
}
if (close(this->fd) == -1) {
abort();
}
this->fd = 0;
break;
default:
#if defined(NDEBUG)
return;
#else
abort();
#endif
@ -241,11 +243,9 @@ operator<<(std::ostream &os, Arena &arena)
case ArenaType::Alloc:
os << "allocated";
break;
#if defined(__posix__) || defined(__linux__) || defined(__APPLE__)
case ArenaType::MemoryMapped:
os << "mmap/file";
break;
#endif
case ArenaType::MemoryMapped:
os << "mmap/file";
break;
default:
os << "unknown (this is a bug)";
}
@ -263,15 +263,10 @@ operator<<(std::ostream &os, Arena &arena)
int
Arena::Write(const char *path)
{
FILE *arenaFile = nullptr;
int retc = -1;
#if defined(__posix__) || defined(__linux__) || defined(__APPLE__)
arenaFile = fopen(path, "w");
FILE *arenaFile = fopen(path, "w");
if (arenaFile == nullptr) {
#else
if (fopen_s(&arenaFile, path, "w") != 0) {
#endif
return -1;
}

6
src/sl/Flags.cc
View File

@ -63,7 +63,7 @@ NewFlag(std::string fName, FlagType fType, std::string fDescription)
flag->Type = fType;
flag->WasSet = false;
flag->Name = std::move(fName);
flag->Description = fDescription;
flag->Description = std::move(fDescription);
flag->Value = FlagValue{};
return flag;
@ -114,7 +114,7 @@ Flags::Register(std::string fName, FlagType fType, std::string fDescription)
bool
Flags::Register(std::string fName, bool defaultValue, std::string fDescription)
{
if (!this->Register(fName, FlagType::Boolean, fDescription)) {
if (!this->Register(fName, FlagType::Boolean, std::move(fDescription))) {
return false;
}
@ -164,7 +164,7 @@ Flags::Register(std::string fName, size_t defaultValue, std::string fDescription
bool
Flags::Register(std::string fName, std::string defaultValue, std::string fDescription)
{
if (!this->Register(fName, FlagType::String, fDescription)) {
if (!this->Register(fName, FlagType::String, std::move(fDescription))) {
return false;
}

4
src/sl/StringUtil.cc
View File

@ -38,7 +38,7 @@ namespace S {
std::vector<std::string>
SplitKeyValuePair(std::string line, std::string delimiter)
{
auto pair = SplitN(std::move(line), delimiter, 2);
auto pair = SplitN(std::move(line), std::move(delimiter), 2);
if (pair.size() == 0) {
return {"", ""};
@ -198,7 +198,7 @@ WriteTabIndented(std::ostream &os, std::string line, size_t maxLength,
int tabStop, bool indentFirst)
{
auto lines = WrapText(line, maxLength);
WriteTabIndented(os, lines, tabStop, indentFirst);
WriteTabIndented(os, std::move(lines), tabStop, indentFirst);
}

31
src/sl/TLV.cc
View File

@ -25,12 +25,13 @@
#include <scsl/TLV.h>
using namespace scsl;
/// REC_SIZE calculates the total length of a TLV record, including the
/// two byte header.
#define REC_SIZE(x) ((std::size_t)x.Len + 2)
#define REC_SIZE(x) ((std::size_t)x.Len + 2)
namespace scsl {
@ -100,6 +101,10 @@ SetRecord(Record &rec, uint8_t tag, uint8_t len, const char *val)
void
ReadFromMemory(Record &rec, uint8_t *cursor)
{
assert(cursor != nullptr);
if (cursor == nullptr) {
return;
}
rec.Tag = cursor[0];
rec.Len = cursor[1];
memcpy(rec.Val, cursor + 2, rec.Len);
@ -117,9 +122,10 @@ FindTag(Arena &arena, uint8_t *cursor, Record &rec)
cursor = LocateTag(arena, cursor, rec);
if (rec.Tag != TAG_EMPTY) {
cursor = SkipRecord(rec, cursor);
if (!arena.CursorInArena(cursor)) {
cursor = nullptr;
}
}
if (!arena.CursorInArena(cursor)) {
cursor = nullptr;
}
return cursor;
@ -129,18 +135,19 @@ FindTag(Arena &arena, uint8_t *cursor, Record &rec)
uint8_t *
LocateTag(Arena &arena, uint8_t *cursor, Record &rec)
{
uint8_t tag, len;
if (!arena.CursorInArena(cursor)) {
cursor = nullptr;
}
uint8_t tag = TAG_EMPTY;
uint8_t len;
if (cursor == nullptr) {
cursor = arena.Start();
}
while (((tag = cursor[0]) != rec.Tag) &&
(arena.CursorInArena(cursor))) {
if (!arena.CursorInArena(cursor)) {
cursor = arena.Start();
}
while (arena.CursorInArena(cursor) &&
((tag = cursor[0]) != rec.Tag)) {
assert(arena.CursorInArena(cursor));
len = cursor[1];
if (!spaceAvailable(arena, cursor, len)) {
@ -193,7 +200,7 @@ DeleteRecord(Arena &arena, uint8_t *cursor)
return;
}
uint8_t len = cursor[1] + 2;
uint8_t len = cursor[1] + 2;
uint8_t *stop = arena.Start() + arena.Size();
stop -= len;

4
src/test/Assert.cc
View File

@ -1,8 +1,8 @@
///
/// \file Test.cc
/// \file src/sctest/Assert.cc
/// \author K. Isom <kyle@imap.cc>
/// \date 2023-10-09
/// \brief Tooling to assist in building test programs..
/// \brief Assertion tooling useful in building test programs.
///
/// Copyright 2023 K. Isom <kyle@imap.cc>
///

4
src/test/Exceptions.cc
View File

@ -1,5 +1,5 @@
///
/// \file Exceptions.cc
/// \file src/test/Exceptions.cc
/// \author K. Isom <kyle@imap.cc>
/// \date 2023-10-10
/// \brief Custom exceptions used in writing test programs.
@ -26,7 +26,7 @@
namespace sctest {
AssertionFailed::AssertionFailed(std::string message) : msg(message) {}
AssertionFailed::AssertionFailed(std::string message) : msg(std::move(message)) {}
const char *

24
src/test/Report.cc
View File

@ -45,6 +45,13 @@ Report::Failing() const
}
size_t
Report::Passing() const
{
return this->passed;
}
size_t
Report::Total() const
{
@ -59,6 +66,13 @@ Report::Failed()
}
void
Report::Passed()
{
this->passed++;
}
void
Report::AddTest(size_t testCount)
{
@ -71,6 +85,7 @@ Report::Reset(size_t testCount)
{
auto now = std::chrono::steady_clock::now();
this->total = testCount;
this->passed = 0;
this->failing = 0;
this->Start();
@ -105,13 +120,18 @@ operator<<(std::ostream &os, const Report &report)
{
auto elapsed = report.Elapsed();
os << report.Total() - report.Failing() << "/"
os << report.Passing() << "/"
<< report.Total() << " tests passed in "
<< std::setw(3) << elapsed.count() << "ms";
auto failed = report.Failing();
if (failed > 0) {
os << " (" << failed << " tests failed)";
}
return os;
}
} // end namespace sctest
} // end namespace sctest

16
src/test/SimpleSuite.cc
View File

@ -53,7 +53,7 @@ SimpleSuite::Silence()
void
SimpleSuite::AddTest(std::string name, std::function<bool()> test)
{
const UnitTest test_case = {std::move(name), test};
const UnitTest test_case = {std::move(name), std::move(test), true};
tests.push_back(test_case);
}
@ -61,8 +61,7 @@ SimpleSuite::AddTest(std::string name, std::function<bool()> test)
void
SimpleSuite::AddFailingTest(std::string name, std::function<bool()> test)
{
// auto ntest = [&test]() { return !test(); };
const UnitTest test_case = {std::move(name), [&test]() { return !test(); }};
const UnitTest test_case = {std::move(name), test, false};
tests.push_back(test_case);
}
@ -87,14 +86,19 @@ SimpleSuite::Run()
<< testCase.name << ": ";
}
this->hasPassed = testCase.test();
this->hasPassed = (testCase.test() == testCase.expect);
if (this->hasPassed) {
report.Passed();
} else {
report.Failed();
}
if (quiet) { continue; }
if (this->hasPassed) {
std::cout << "[PASS]";
} else {
std::cout << "[FAIL]";
report.Failed();
}
std::cout << "\n";
}
@ -142,4 +146,4 @@ operator<<(std::ostream &os, SimpleSuite &suite)
}
} // end namespace sctest
} // end namespace sctest

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