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

GNSS App: a Compass, with Places and a level #98

Merged
twisla merged 4 commits from compass into main 2026-10-11 15:17:18 +00:00
Conversation 0 Commits 4 Files Changed 18
+389
3 changed files with 389 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit da311a11b6 - Show all commits
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();
}