GNSS: a heading from the gyroscope, the course and the sun

The device has no magnetometer (issue #16): north is given by the course over ground while walking, or by aiming at the sun, and the gyroscope follows the turn around the vertical in between. With the sun's position from the time and the place, the bearing to a point, the 16 points and distances as text. Host-tested. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_0162FokPdvY2KsS4NBfwyWPk
2026-10-11 17:01:57 +02:00
parent fd534af971
commit da311a11b6
3 changed files with 389 additions and 0 deletions
@@ -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
@@ -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