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
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();
}