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

Continuing refactor work.

Browse Source
This commit is contained in:
Kyle Isom
2023-10-19 00:37:56 -07:00
parent 8d02d078e7
commit 36fe049485
28 changed files with 1658 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/scmp/Math.h
View File

@@ -71,7 +71,7 @@ WithinTolerance(T a, T b, T epsilon)
}
} // namespace math
} // namespace scmp
#endif //SCCCL_MATH_H

158
include/scmp/filter/Madgwick.h Normal file
View File

@@ -0,0 +1,158 @@
///
/// \file Madwick.cc
/// \author K. Isom <kyle@imap.cc>
/// \date 2019-08-06
/// \brief Implementation of a Madgwick filter.
///
/// See https://courses.cs.washington.edu/courses/cse466/14au/labs/l4/madgwick_internal_report.pdf.
///
/// Copyright 2019 K. Isom <kyle@imap.cc>
///
/// Permission to use, copy, modify, and/or distribute this software for
/// any purpose with or without fee is hereby granted, provided that
/// the above copyright notice and this permission notice appear in all /// copies.
///
/// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
/// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
/// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
/// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
/// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///
/// \file madgwick.h
/// \brief Implementation of a Madgwick filter.
///
/// See
#ifndef SCMP_FILTER_MADGWICK_H
#define SCMP_FILTER_MADGWICK_H
#include <scmp/geom/Vector.h>
#include <scmp/geom/Quaternion.h>
/// scmp contains the wntrmute robotics code.
namespace scmp {
/// filter contains filtering algorithms.
namespace filter {
/// @brief Madgwick implements an efficient Orientation filter for IMUs.
///
/// Madgwick is a novel Orientation filter applicable to IMUs
/// consisting of tri-axis gyroscopes and accelerometers, and MARG
/// sensor arrays that also include tri-axis magnetometers. The MARG
/// implementation incorporates magnetic distortionand gyroscope bias
/// drift compensation.
///
/// It is described in the paper [An efficient Orientation filter for inertial and inertial/magnetic sensor arrays](http://x-io.co.uk/res/doc/madgwick_internal_report.pdf).
///
/// \tparam T A floating point type.
template <typename T>
class Madgwick {
public:
/// The Madgwick filter is initialised with an identity quaternion.
Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame() {};
/// 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);
}
}
/// 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.
///
/// \return The current sensor frame.
scmp::geom::Quaternion<T>
Orientation() const
{
return this->sensorFrame;
}
/// Return the rate of change of the Orientation of the earth frame
/// with respect to the sensor frame.
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as w_x, w_y, w_z.
/// \return A quaternion representing the rate of angular change.
scmp::geom::Quaternion<T>
AngularRate(const scmp::geom::Vector<T, 3> &gyro) const
{
return (this->sensorFrame * 0.5) * scmp::geom::Quaternion<T>(gyro, 0.0);
}
/// 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.
void
UpdateFrame(const scmp::geom::Quaternion<T> &sf, T delta)
{
this->previousSensorFrame = this->sensorFrame;
this->sensorFrame = sf;
this->deltaT = delta;
}
/// Update the sensor frame with a gyroscope reading.
///
/// \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.
assert(!scmp::WithinTolerance(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.
///
/// \return A vector of Euler angles as <ψ, θ, ϕ>.
scmp::geom::Vector<T, 3>
Euler()
{
return this->sensorFrame.euler();
}
private:
T deltaT;
scmp::geom::Quaternion<T> previousSensorFrame;
scmp::geom::Quaternion<T> sensorFrame;
};
/// Madgwickd is a shorthand alias for a Madgwick<double>.
typedef Madgwick<double> Madgwickd;
/// Madgwickf is a shorthand alias for a Madgwick<float>.
typedef Madgwick<float> Madgwickf;
} // namespace filter
} // namespace scmp
#endif // SCMP_FILTER_MADGWICK_H

12
include/scmp/geom/Orientation.h
View File

@@ -1,5 +1,5 @@
/**
* orientation.h concerns itself with computing the orientation of some
* orientation.h concerns itself with computing the Orientation of some
* vector with respect to a reference plane that is assumed to be the
* of the Earth.
*/
@@ -61,28 +61,28 @@ static const Vector3f Basis3f[] = {
/// Heading2f returns a compass heading for a Vector2f.
/// @param vec A vector orientation.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
float Heading2f(Vector2f vec);
/// Heading2d returns a compass heading for a Vector2d.
/// @param vec A vector orientation.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
double Heading2d(Vector2d vec);
/// Heading3f returns a compass heading for a Vector2f.
/// @param vec A vector orientation.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
float Heading3f(Vector3f vec);
/// Heading3d returns a compass heading for a Vector2f.
/// @param vec A vector orientation.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
double Heading3d(Vector3d vec);
} // namespace geom
} // namespace math
} // namespace scmp
#endif // __WRMATH_ORIENTATION_H

6
include/scmp/geom/Quaternion.h
View File

@@ -19,7 +19,7 @@ namespace scmp {
namespace geom {
/// @brief Quaternions provide a representation of orientation and rotations
/// @brief Quaternions provide a representation of Orientation and rotations
/// in three dimensions.
///
/// Quaternions encode rotations in three-dimensional space. While technically
@@ -441,7 +441,7 @@ quaternion(Vector<T, 3> axis, T angle)
/// return a quaternion.
///
/// @param euler A vector Euler angle in ZYX sequence.
/// @return A Quaternion representation of the orientation represented
/// @return A Quaternion representation of the Orientation represented
/// by the Euler angles.
/// @relatesalso Quaternion
Quaternionf quaternionf_from_euler(Vector3f euler);
@@ -451,7 +451,7 @@ Quaternionf quaternionf_from_euler(Vector3f euler);
/// return a quaternion.
///
/// @param euler A vector Euler angle in ZYX sequence.
/// @return A Quaternion representation of the orientation represented
/// @return A Quaternion representation of the Orientation represented
/// by the Euler angles.
/// @relatesalso Quaternion
Quaterniond quaterniond_from_euler(Vector3d euler);

4
include/scmp/geom/Vector.h
View File

@@ -6,7 +6,7 @@
// Namespace: math::vectors.
//
// vectors.h defines the Vector2D class and associated functions in the
// namespace math::vectors.
// namespace scmp::vectors.
//
// Copyright 2017 Kyle Isom <kyle@imap.cc>
//
@@ -415,7 +415,7 @@ typedef Vector<double, 4> Vector4d;
} // namespace geom
} // namespace math
} // namespace scmp

2
include/scsl/Arena.h
View File

@@ -37,7 +37,7 @@
#include <iostream>
#include <sys/stat.h>
#include "Exceptions.h"
#include "sctest/Exceptions.h"
#if defined(__WIN64__) || defined(__WIN32__) || defined(WIN32)

16
include/sctest/Assert.h
View File

@@ -27,23 +27,23 @@
#include <string>
namespace scsl {
namespace sctest {
/// TestAssert is a variant on the assert macro. This variant is intended to be
/// Assert is a variant on the assert macro. This variant is intended to be
/// a drop-in replacement for the cassert macro: even in release mode, the tests
/// should still run.
///
/// If NDEBUG is set, TestAssert will throw an exception if condition is false.
/// If NDEBUG is set, Assert will throw an exception if condition is false.
/// Otherwise, it calls assert after printing the message.
///
/// \param condition If true, TestAssert throws an exception.
void TestAssert(bool condition);
/// \param condition If true, Assert throws an exception.
void Assert(bool condition);
/// TestAssert is a variant on the assert macro.
/// Assert is a variant on the assert macro.
///
/// If NDEBUG is set, TestAssert will throw an exception if condition is false.
/// If NDEBUG is set, Assert will throw an exception if condition is false.
/// Otherwise, it calls assert after printing the message.
///
/// In addition to NDEBUG, SCSL_NOEXCEPT will suppress assertions.
@@ -52,7 +52,7 @@ void TestAssert(bool condition);
///
/// \param condition The condition to assert.
/// \param message The message that should be displayed if condition is false.
void TestAssert(bool condition, std::string message);
void Assert(bool condition, std::string message);
} // namespace scsl

53
include/sctest/Checks.h Executable file
View File

@@ -0,0 +1,53 @@
///
/// \file Checks.h
/// \author K. Isom <kyle@imap.cc>
/// \date 2017-06-05
/// \brief Provides a number of utility macros for testing.
///
/// Copyright 2017 K. Isom <kyle@imap.cc>
///
/// Permission to use, copy, modify, and/or distribute this software for
/// any purpose with or without fee is hereby granted, provided that
/// the above copyright notice and this permission notice appear in all /// copies.
///
/// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
/// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
/// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
/// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
/// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///
#ifndef SCTEST_CHECKS_H
#define SCTEST_CHECKS_H
#include <scmp/Math.h>
namespace sctest {
// The following checks are designed as shortcuts that return false on
// if some condition isn't met.
#define SCTEST_CHECK(x) if (!(x)) { return false; }
#define SCTEST_CHECK_FALSE(x) if ((x)) { return false; }
#define SCTEST_CHECK_EQ(x, y) if ((x) != (y)) { return false; }
#define SCTEST_CHECK_NE(x, y) if ((x) == (y)) { return false; }
#define SCTEST_CHECK_ZERO(x) if ((x) != 0) { return false; }
#define SCTEST_CHECK_GTZ(x) if ((x) > 0) { return false; }
#define SCTEST_CHECK_GEZ(x) if ((x) >= 0) { return false; }
#define SCTEST_CHECK_LEZ(x) if ((x) <= 0) { return false; }
#define SCTEST_CHECK_LTZ(x) if ((x) < 0) { return false; }
#define SCTEST_CHECK_FEQ(x, y) { float eps; scmp::DefaultEpsilon(eps); if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
#define SCTEST_CHECK_DEQ(x, y) { double eps; scmp::DefaultEpsilon(eps); if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
#define SCTEST_CHECK_FEQ_EPS(x, y, eps) { if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
#define SCTEST_CHECK_DEQ_EPS(x, y, eps) { if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
} // namespace sctest
#endif

0
include/sctest/debug.h → include/sctest/Debug.h
View File

7
include/scsl/Exceptions.h → include/sctest/Exceptions.h
View File

@@ -19,6 +19,7 @@
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///
#ifndef SCSL_EXCEPTIONS_H
#define SCSL_EXCEPTIONS_H
@@ -27,7 +28,7 @@
#include <string>
namespace scsl {
namespace sctest {
/// NotImplemented is an exception reserved for unsupported platforms.
@@ -63,7 +64,7 @@ private:
};
} // namespace scsl
} // namespace sctest
#endif //SCSL_EXCEPTIONS_H
#endif // SCSL_EXCEPTIONS_H

4
include/sctest/Report.h
View File

@@ -21,8 +21,8 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef __SCTEST_REPORT_H
#define __SCTEST_REPORT_H
#ifndef SCTEST_REPORT_H
#define SCTEST_REPORT_H
#include <chrono>

4
include/sctest/SimpleSuite.h
View File

@@ -20,8 +20,8 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef __SCTEST_SIMPLESUITE_H
#define __SCTEST_SIMPLESUITE_H
#ifndef SCTEST_SIMPLESUITE_H
#define SCTEST_SIMPLESUITE_H
// SimpleSuite.h
// This header file defines the interface for a simple suite of tests.

50
include/sctest/checks.h
View File

@@ -1,50 +0,0 @@
//
// Project: scccl
// File: include/test/checks.h
// Author: Kyle Isom
// Date: 2017-06-05
// Namespace: test.
//
// checks.h defines a number of macros (which are global in scope) for
// use in test functions that return bools.
//
// Copyright 2017 Kyle Isom <kyle@imap.cc>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef __SCTEST_CHECKS_H
#define __SCTEST_CHECKS_H
#include <scmp/Math.h>
namespace sctest {
// The following checks are designed as shortcuts that just return false on certain
// conditions.
#define SCTEST_CHECK(x) if (!(x)) { return false; }
#define SCTEST_CHECK_FALSE(x) if ((x)) { return false; }
#define SCTEST_CHECK_EQ(x, y) if ((x) != (y)) { return false; }
#define SCTEST_CHECK_NE(x, y) if ((x) == (y)) { return false; }
#define SCTEST_CHECK_ZERO(x) if ((x) != 0) { return false; }
#define SCTEST_CHECK_GTZ(x) if ((x) > 0) { return false; }
#define SCTEST_CHECK_GEZ(x) if ((x) >= 0) { return false; }
#define SCTEST_CHECK_LEZ(x) if ((x) <= 0) { return false; }
#define SCTEST_CHECK_LTZ(x) if ((x) < 0) { return false; }
#define SCTEST_CHECK_FEQ(x, y) { float eps; scmp::DefaultEpsilon(eps); if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
#define SCTEST_CHECK_DEQ(x, y) { double eps; scmp::DefaultEpsilon(eps); if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
} // namespace test
#endif