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

Slow working on bringing the old code up to standard.

Browse Source 
- Documentation updates - most of the old files use non-Doxygen or
  no/minimal header comments.
- Rework SimpleSuite to be more useful.
- Coverity-surfaced fixes.
This commit is contained in:
Kyle Isom
2023-10-19 20:32:46 -07:00
parent a9991f241a
commit b1bbaebdac
19 changed files with 825 additions and 441 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -34,8 +34,9 @@
#include <scmp/geom/Quaternion.h>
/// scmp contains the wntrmute robotics code.
/// scmp contains the chimmering clarity math and physics code.
namespace scmp {
/// filter contains filtering algorithms.
namespace filter {
@@ -54,30 +55,30 @@ namespace filter {
template <typename T>
class Madgwick {
public:
/// The Madgwick filter is initialised with an identity quaternion.
/// \brief The Madgwick filter is initialised with an identity quaternion.
Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame() {};
/// The Madgwick filter is initialised with a sensor frame.
/// \brief The Madgwick filter is initialised with a sensor frame.
///
/// \param sf A sensor frame; if zero, the sensor frame will be
/// initialised as an identity quaternion.
Madgwick(scmp::geom::Vector<T, 3> sf) : deltaT(0.0), previousSensorFrame()
{
if (!sf.isZero()) {
sensorFrame = scmp::geom::quaternion(sf, 0.0);
sensorFrame = scmp::geom::quaternion<T>(sf, 0.0);
}
}
/// Initialise the filter with a sensor frame quaternion.
/// \brief Initialise the filter with a sensor frame quaternion.
///
/// \param sf A quaternion representing the current Orientation.
Madgwick(scmp::geom::Quaternion<T> sf) :
deltaT(0.0), previousSensorFrame(), sensorFrame(sf) {};
/// 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>
@@ -87,6 +88,9 @@ public:
}
/// \brief Return the filter's rate of angular change from a
/// sensor frame.
///
/// Return the rate of change of the Orientation of the earth frame
/// with respect to the sensor frame.
///
@@ -99,7 +103,7 @@ public:
return (this->sensorFrame * 0.5) * scmp::geom::Quaternion<T>(gyro, 0.0);
}
/// Update the sensor frame to a new frame.
/// \brief Update the sensor frame to a new frame.
///
/// \param sf The new sensor frame replacing the previous one.
/// \param delta The time delta since the last update.
@@ -112,7 +116,7 @@ public:
}
/// Update the sensor frame with a gyroscope reading.
/// \brief Update the sensor frame with a gyroscope reading.
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as w_x, w_y, w_z.
@@ -121,14 +125,14 @@ public:
UpdateAngularOrientation(const scmp::geom::Vector<T, 3> &gyro, T delta)
{
// Ensure the delta isn't zero within a 100 μs tolerance.
assert(!scmp::WithinTolerance(delta, 0.0, 0.0001));
assert(!scmp::WithinTolerance<T>(delta, 0.0, 0.0001));
scmp::geom::Quaternion<T> q = this->AngularRate(gyro) * delta;
this->UpdateFrame(this->sensorFrame + q, delta);
}
/// Retrieve a vector of the Euler angles in ZYX Orientation.
/// \brief Retrieve a vector of the Euler angles in ZYX Orientation.
///
/// \return A vector of Euler angles as <ψ, θ, ϕ>.
scmp::geom::Vector<T, 3>
@@ -144,11 +148,11 @@ private:
};
/// Madgwickd is a shorthand alias for a Madgwick<double>.
typedef Madgwick<double> Madgwickd;
/// \brief Madgwickd is a shorthand alias for a Madgwick<double>.
using Madgwickd = Madgwick<double>;
/// Madgwickf is a shorthand alias for a Madgwick<float>.
typedef Madgwick<float> Madgwickf;
/// \brief Madgwickf is a shorthand alias for a Madgwick<float>.
using Madgwickf = Madgwick<float>;
} // namespace filter