roro9stack/lib/gnss/src/compass.cpp

#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