diff --git a/lib/gnss/src/compass.cpp b/lib/gnss/src/compass.cpp new file mode 100644 index 0000000..91370da --- /dev/null +++ b/lib/gnss/src/compass.cpp @@ -0,0 +1,123 @@ +#include "compass.h" + +#include +#include + +namespace roro::gnss { + +namespace { +constexpr double kRad = M_PI / 180.0; +// The sensor's axes are right-handed, so a positive turn around "up" is anticlockwise seen from +// above, and the heading (clockwise) goes down. Measured on the device: issue #16. +constexpr float kTurnSign = -1.0f; +} // namespace + +float wrap360(float degrees) { + float d = std::fmod(degrees, 360.0f); + return d < 0 ? d + 360.0f : d; +} + +float angleDiff(float to, float from) { + float d = wrap360(to - from); + return d >= 180.0f ? d - 360.0f : d; +} + +double bearingDegrees(double lat1, double lon1, double lat2, double lon2) { + double p1 = lat1 * kRad, p2 = lat2 * kRad, dl = (lon2 - lon1) * kRad; + double y = std::sin(dl) * std::cos(p2); + double x = std::cos(p1) * std::sin(p2) - std::sin(p1) * std::cos(p2) * std::cos(dl); + double b = std::atan2(y, x) / kRad; + return b < 0 ? b + 360.0 : b; +} + +const char* cardinal(float degrees) { + static const char* const kPoints[16] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", + "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"}; + return kPoints[static_cast(wrap360(degrees) / 22.5f + 0.5f) % 16]; +} + +std::string formatDistance(double meters) { + char buf[24]; + if (meters < 999.5) std::snprintf(buf, sizeof buf, "%.0f m", meters); + else if (meters < 9950.0) std::snprintf(buf, sizeof buf, "%.1f km", meters / 1000.0); + else std::snprintf(buf, sizeof buf, "%.0f km", meters / 1000.0); + return buf; +} + +SunPosition sunPosition(int64_t utcSeconds, double latitude, double longitude) { + double d = utcSeconds / 86400.0 - 10957.5; // days since 2000-01-01 12:00 UTC + double g = (357.529 + 0.98560028 * d) * kRad; // mean anomaly + double q = 280.459 + 0.98564736 * d; // mean longitude + double l = (q + 1.915 * std::sin(g) + 0.020 * std::sin(2 * g)) * kRad; // ecliptic longitude + double e = (23.439 - 0.00000036 * d) * kRad; // the ecliptic's tilt + double rightAscension = std::atan2(std::cos(e) * std::sin(l), std::cos(l)); + double declination = std::asin(std::sin(e) * std::sin(l)); + double siderealDeg = std::fmod(18.697374558 + 24.06570982441908 * d, 24.0) * 15.0; // at Greenwich + double hourAngle = (siderealDeg + longitude) * kRad - rightAscension; + double lat = latitude * kRad; + double elevation = std::asin(std::sin(lat) * std::sin(declination) + + std::cos(lat) * std::cos(declination) * std::cos(hourAngle)); + double azimuth = std::atan2(-std::sin(hourAngle), + std::tan(declination) * std::cos(lat) - std::sin(lat) * std::cos(hourAngle)); + return {wrap360(static_cast(azimuth / kRad)), static_cast(elevation / kRad)}; +} + +void HeadingFilter::motion(const float gyro[3], const float accel[3], float dt) { + if (dt <= 0) return; + // "Up" is where the accelerometer points at rest, smoothed so a shake doesn't move it. + float norm = std::sqrt(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]); + if (norm > 0.5f && norm < 1.5f) { + float k = any_ ? std::fmin(1.0f, dt / kUpSeconds) : 1.0f; + float length = 0; + for (int i = 0; i < 3; i++) { + up_[i] += k * (accel[i] / norm - up_[i]); + length += up_[i] * up_[i]; + } + length = std::sqrt(length); + if (length > 0.01f) + for (float& u : up_) u /= length; + any_ = true; + } + + float rate[3], turning = 0; + for (int i = 0; i < 3; i++) { + rate[i] = gyro[i] - bias_[i]; + turning += rate[i] * rate[i]; + } + if (std::sqrt(turning) < kStillDps) { + stillFor_ += dt; + if (stillFor_ >= kStillSeconds) { // lying still: what the gyroscope still reads is its offset + float k = std::fmin(1.0f, dt / kBiasSeconds); + for (int i = 0; i < 3; i++) { + bias_[i] += k * rate[i]; + rate[i] = gyro[i] - bias_[i]; + } + } + } else stillFor_ = 0; + + if (!known()) return; + float aroundUp = rate[0] * up_[0] + rate[1] * up_[1] + rate[2] * up_[2]; + heading_ = wrap360(heading_ + kTurnSign * aroundUp * dt); + held_ += dt; +} + +void HeadingFilter::course(float courseDeg, float speedKmh) { + if (speedKmh < kMinSpeedKmh) return; + heading_ = known() ? wrap360(heading_ + kCourseGain * angleDiff(courseDeg, heading_)) : wrap360(courseDeg); + source_ = Source::Course; + held_ = 0; +} + +void HeadingFilter::sun(float azimuthDeg) { + heading_ = wrap360(azimuthDeg); + source_ = Source::Sun; + held_ = 0; +} + +void HeadingFilter::forget() { + source_ = Source::None; + held_ = 0; + stillFor_ = 0; +} + +} // namespace roro::gnss diff --git a/lib/gnss/src/compass.h b/lib/gnss/src/compass.h new file mode 100644 index 0000000..ba02c08 --- /dev/null +++ b/lib/gnss/src/compass.h @@ -0,0 +1,72 @@ +#pragma once + +#include +#include + +namespace roro::gnss { + +// Angles are degrees from true north, clockwise, 0 up to 360. +float wrap360(float degrees); +// How far to turn from `from` to `to`: -180 up to 180, positive clockwise. +float angleDiff(float to, float from); +// The initial great-circle bearing from the first point to the second. +double bearingDegrees(double lat1, double lon1, double lat2, double lon2); +// One of the 16 points: "N", "NNE", "NE"... +const char* cardinal(float degrees); +// "340 m", "1.2 km", "12 km". +std::string formatDistance(double meters); + +// Where the sun is in the sky, from the time and the place (the low-precision formulas of the +// Astronomical Almanac: within a hundredth of a degree until 2050, far finer than a hand can aim). +struct SunPosition { + float azimuthDeg; + float elevationDeg; // above the horizon; refraction is left out +}; +SunPosition sunPosition(int64_t utcSeconds, double latitude, double longitude); + +// The Compass's heading (issue #16): where the top of the device points. There is no +// magnetometer, so north comes from outside (the course over ground while walking, or the sun +// when the user aims at it) and the gyroscope follows the turns in between. Host-tested. +class HeadingFilter { + public: + enum class Source : uint8_t { None, Course, Sun }; + // Below a walking pace the receiver's course is noise. + static constexpr float kMinSpeedKmh = 3.0f; + // Each course reading pulls the heading this much of the way: a few seconds to settle, + // without jumping at every reading. + static constexpr float kCourseGain = 0.3f; + // The gyroscope's own offset is learnt while the device lies still: turning slower than + // this for kStillSeconds counts as still. + static constexpr float kStillDps = 2.0f; + static constexpr float kStillSeconds = 1.0f; + static constexpr float kBiasSeconds = 4.0f; + // How fast the measured "up" follows the accelerometer. + static constexpr float kUpSeconds = 0.3f; + + // One reading of the IMU: degrees a second and g, on the sensor's axes, `dt` seconds after + // the one before. Only the turn around the vertical counts, however the device is held. + void motion(const float gyro[3], const float accel[3], float dt); + // The receiver's course over ground, once a second: used when moving fast enough. + void course(float courseDeg, float speedKmh); + // The top of the device is aimed at the sun, which is at this azimuth. + void sun(float azimuthDeg); + void forget(); // the device was not watched for a while: north has to come again + + bool known() const { return source_ != Source::None; } + float heading() const { return heading_; } + Source source() const { return source_; } + // Seconds the gyroscope has carried the heading alone since north was last given. + float heldSeconds() const { return held_; } + // "Up" along the sensor's x and y axes, -1 to 1: both 0 when the device lies flat. + float tiltX() const { return up_[0]; } + float tiltY() const { return up_[1]; } + bool still() const { return stillFor_ >= kStillSeconds; } + + private: + float heading_ = 0, held_ = 0, stillFor_ = 0; + float up_[3] = {0, 0, 1}, bias_[3] = {0, 0, 0}; + bool any_ = false; + Source source_ = Source::None; +}; + +} // namespace roro::gnss diff --git a/test/test_compass/test_compass.cpp b/test/test_compass/test_compass.cpp new file mode 100644 index 0000000..eab38de --- /dev/null +++ b/test/test_compass/test_compass.cpp @@ -0,0 +1,194 @@ +#include + +#include +#include + +#include "compass.h" + +using namespace roro::gnss; + +void setUp() {} +void tearDown() {} + +namespace { +const float kFlat[3] = {0, 0, 1}; +const float kNoTurn[3] = {0, 0, 0}; + +// `seconds` of readings at 100 Hz. +void run(HeadingFilter& f, const float gyro[3], const float accel[3], float seconds) { + for (int i = 0; i < static_cast(seconds * 100 + 0.5f); i++) f.motion(gyro, accel, 0.01f); +} +} // namespace + +void test_angles_wrap() { + TEST_ASSERT_FLOAT_WITHIN(0.001, 350.0, wrap360(-10.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001, 10.0, wrap360(370.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001, 20.0, angleDiff(10.0f, 350.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001, -20.0, angleDiff(350.0f, 10.0f)); + TEST_ASSERT_FLOAT_WITHIN(0.001, -180.0, angleDiff(180.0f, 0.0f)); +} + +void test_bearing_to_a_point() { + TEST_ASSERT_FLOAT_WITHIN(0.01, 0.0, bearingDegrees(50.0, 4.0, 51.0, 4.0)); + TEST_ASSERT_FLOAT_WITHIN(0.01, 180.0, bearingDegrees(50.0, 4.0, 49.0, 4.0)); + TEST_ASSERT_FLOAT_WITHIN(0.5, 90.0, bearingDegrees(50.0, 4.0, 50.0, 4.1)); + TEST_ASSERT_FLOAT_WITHIN(0.5, 270.0, bearingDegrees(50.0, 4.0, 50.0, 3.9)); + // Brussels to Paris: south-south-west. + TEST_ASSERT_FLOAT_WITHIN(1.0, 214.0, bearingDegrees(50.8503, 4.3517, 48.8566, 2.3522)); +} + +void test_cardinal_points() { + TEST_ASSERT_EQUAL_STRING("N", cardinal(0)); + TEST_ASSERT_EQUAL_STRING("N", cardinal(359)); + TEST_ASSERT_EQUAL_STRING("NNE", cardinal(22.5f)); + TEST_ASSERT_EQUAL_STRING("E", cardinal(90)); + TEST_ASSERT_EQUAL_STRING("WSW", cardinal(247)); + TEST_ASSERT_EQUAL_STRING("NNW", cardinal(340)); + TEST_ASSERT_EQUAL_STRING("N", cardinal(-5)); +} + +void test_distances_read_well() { + TEST_ASSERT_EQUAL_STRING("0 m", formatDistance(0.2).c_str()); + TEST_ASSERT_EQUAL_STRING("340 m", formatDistance(340.4).c_str()); + TEST_ASSERT_EQUAL_STRING("1.0 km", formatDistance(999.6).c_str()); + TEST_ASSERT_EQUAL_STRING("1.2 km", formatDistance(1234).c_str()); + TEST_ASSERT_EQUAL_STRING("12 km", formatDistance(12345).c_str()); +} + +void test_the_sun_at_an_equinox() { + // 2024-03-20 03:06 UTC, the March equinox: the sun is over the equator. Twelve hours of + // turning later, at 12:07:30 UTC (the equation of time is -7.5 min), it is over Greenwich. + int64_t noon = 1710936450; + SunPosition s = sunPosition(noon, 0.0, 0.0); + TEST_ASSERT_FLOAT_WITHIN(0.3, 90.0, s.elevationDeg); + s = sunPosition(noon, 50.0, 0.0); // from 50° north: due south, 40° up + TEST_ASSERT_FLOAT_WITHIN(0.5, 180.0, s.azimuthDeg); + TEST_ASSERT_FLOAT_WITHIN(0.3, 40.0, s.elevationDeg); + s = sunPosition(noon, -50.0, 0.0); // from 50° south: due north + TEST_ASSERT_TRUE(s.azimuthDeg < 1.0f || s.azimuthDeg > 359.0f); + // Six hours earlier it rises, due east at an equinox, anywhere. + s = sunPosition(noon - 6 * 3600, 50.0, 0.0); + TEST_ASSERT_FLOAT_WITHIN(1.0, 90.0, s.azimuthDeg); + TEST_ASSERT_FLOAT_WITHIN(1.0, 0.0, s.elevationDeg); + s = sunPosition(noon + 6 * 3600, 50.0, 0.0); // and sets due west + TEST_ASSERT_FLOAT_WITHIN(1.0, 270.0, s.azimuthDeg); +} + +void test_the_sun_at_a_solstice() { + // 2024-06-20 20:51 UTC: 23.44° north of the equator. At noon in Brussels that day + // (11:44 UTC by the sun) it stands 90 - 50.85 + 23.44 = 62.6° up, in the south. + SunPosition s = sunPosition(1718883840, 50.85, 4.35); + TEST_ASSERT_FLOAT_WITHIN(1.0, 180.0, s.azimuthDeg); + TEST_ASSERT_FLOAT_WITHIN(0.3, 62.6, s.elevationDeg); + // At midnight it is under the horizon, in the north. + s = sunPosition(1718883840 + 12 * 3600, 50.85, 4.35); + TEST_ASSERT_TRUE(s.elevationDeg < 0); + TEST_ASSERT_TRUE(s.azimuthDeg < 3.0f || s.azimuthDeg > 357.0f); +} + +void test_no_heading_until_north_is_given() { + HeadingFilter f; + const float turn[3] = {0, 0, 30}; + run(f, turn, kFlat, 1); + TEST_ASSERT_FALSE(f.known()); + f.course(120, 1.0f); // too slow: the course means nothing + TEST_ASSERT_FALSE(f.known()); + f.course(120, 4.0f); + TEST_ASSERT_TRUE(f.known()); + TEST_ASSERT_FLOAT_WITHIN(0.01, 120.0, f.heading()); + TEST_ASSERT_TRUE(f.source() == HeadingFilter::Source::Course); +} + +void test_the_gyroscope_follows_a_turn() { + HeadingFilter f; + f.course(100, 5.0f); + // A positive turn around "up" is anticlockwise from above: the heading goes down. + const float left[3] = {0, 0, 45}; + run(f, left, kFlat, 2); + TEST_ASSERT_FLOAT_WITHIN(0.5, 10.0, f.heading()); + const float right[3] = {0, 0, -45}; + run(f, right, kFlat, 4); + TEST_ASSERT_FLOAT_WITHIN(0.5, 190.0, f.heading()); + TEST_ASSERT_FLOAT_WITHIN(0.05, 6.0, f.heldSeconds()); +} + +void test_only_the_turn_around_the_vertical_counts() { + HeadingFilter f; + f.course(0, 5.0f); + // Held upright, the screen facing the user: "up" is along the sensor's y. + const float upright[3] = {0, 1, 0}; + run(f, kNoTurn, upright, 1); + const float roll[3] = {0, 0, 90}; // around the screen's own axis: no change of heading + run(f, roll, upright, 1); + TEST_ASSERT_TRUE(std::fabs(angleDiff(f.heading(), 0)) < 0.5f); + const float yaw[3] = {0, -90, 0}; // around the vertical + run(f, yaw, upright, 1); + TEST_ASSERT_FLOAT_WITHIN(1.0, 90.0, f.heading()); +} + +void test_the_course_pulls_the_heading_gently() { + HeadingFilter f; + f.course(350, 5.0f); + f.course(10, 5.0f); // through north, the short way + TEST_ASSERT_FLOAT_WITHIN(0.01, 356.0, f.heading()); + for (int i = 0; i < 20; i++) f.course(10, 5.0f); + TEST_ASSERT_FLOAT_WITHIN(0.1, 10.0, f.heading()); + TEST_ASSERT_FLOAT_WITHIN(0.001, 0.0, f.heldSeconds()); +} + +void test_the_sun_sets_north() { + HeadingFilter f; + f.sun(203); + TEST_ASSERT_TRUE(f.known()); + TEST_ASSERT_TRUE(f.source() == HeadingFilter::Source::Sun); + TEST_ASSERT_FLOAT_WITHIN(0.01, 203.0, f.heading()); + f.forget(); + TEST_ASSERT_FALSE(f.known()); +} + +void test_the_gyroscope_offset_is_learnt_at_rest() { + HeadingFilter f; + f.sun(90); + const float offset[3] = {0.3f, -0.2f, 0.8f}; // what a BMI270 reads lying still + run(f, offset, kFlat, 30); + TEST_ASSERT_TRUE(f.still()); + float before = f.heading(); + run(f, offset, kFlat, 60); // a minute later it has not drifted + TEST_ASSERT_FLOAT_WITHIN(0.2, before, f.heading()); + // A real turn is still measured, with the offset taken off. + const float turn[3] = {0.3f, -0.2f, 0.8f - 90.0f}; + run(f, turn, kFlat, 1); + TEST_ASSERT_FLOAT_WITHIN(0.5, before + 90.0f, f.heading()); + TEST_ASSERT_FALSE(f.still()); +} + +void test_tilt_follows_the_accelerometer() { + HeadingFilter f; + run(f, kNoTurn, kFlat, 1); + TEST_ASSERT_FLOAT_WITHIN(0.01, 0.0, f.tiltX()); + TEST_ASSERT_FLOAT_WITHIN(0.01, 0.0, f.tiltY()); + const float tilted[3] = {0.5f, 0, 0.866f}; // 30° over + run(f, kNoTurn, tilted, 2); + TEST_ASSERT_FLOAT_WITHIN(0.01, 0.5, f.tiltX()); + const float shaken[3] = {3, 3, 3}; // not gravity: ignored + run(f, kNoTurn, shaken, 1); + TEST_ASSERT_FLOAT_WITHIN(0.01, 0.5, f.tiltX()); +} + +int main() { + UNITY_BEGIN(); + RUN_TEST(test_angles_wrap); + RUN_TEST(test_bearing_to_a_point); + RUN_TEST(test_cardinal_points); + RUN_TEST(test_distances_read_well); + RUN_TEST(test_the_sun_at_an_equinox); + RUN_TEST(test_the_sun_at_a_solstice); + RUN_TEST(test_no_heading_until_north_is_given); + RUN_TEST(test_the_gyroscope_follows_a_turn); + RUN_TEST(test_only_the_turn_around_the_vertical_counts); + RUN_TEST(test_the_course_pulls_the_heading_gently); + RUN_TEST(test_the_sun_sets_north); + RUN_TEST(test_the_gyroscope_offset_is_learnt_at_rest); + RUN_TEST(test_tilt_follows_the_accelerometer); + return UNITY_END(); +}