lib/gnss/src/compass.cpp

@@ -0,0 +1,123 @@
+#include "compass.h"
+
+#include <cmath>
+#include <cstdio>
+
+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<int>(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<float>(azimuth / kRad)), static_cast<float>(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

lib/gnss/src/compass.h

@@ -0,0 +1,72 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+
+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

test/test_compass/test_compass.cpp

@@ -0,0 +1,194 @@
+#include <unity.h>
+
+#include <cmath>
+#include <string>
+
+#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<int>(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();
+}