twisla/roro9stack
Public Access
1
0
Fork 0
Code Issues 34 Pull Requests Actions Packages Releases 27 Activity

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

Browse Source 
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
This commit is contained in:
twisla
2026-10-11 17:01:57 +02:00
co-authored by Claude Opus 5.5
parent fd534af971
commit da311a11b6
3 changed files with 389 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/gnss/src/compass.cpp
+123
View File
@@ -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
+72
View File
@@ -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
+194
View File
@@ -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();
}