diff --git a/README.md b/README.md index e96e7315..7f715925 100644 --- a/README.md +++ b/README.md @@ -19,14 +19,14 @@ This repository contains the embedded software for the CATS Vega flight computer * Accelerometric liftoff detection * Fully open source * Configuration is done over our application, no need to work with a CLI -* Explore rocket flights recorded with CATS flight computers at [CATS Flights](https://flights.catsystems.io/). +* Explore rocket flights recorded with CATS flight computers at [CATS Flights](https://catsystems.io/flights). ## Quick Links - [CATS User Manual](./Cats%20User%20Manual.pdf) - [CATS Configurator downloads](https://github.com/catsystems/cats-configurator/releases) - [Firmware releases](https://github.com/catsystems/cats-embedded/releases) - [CATS website](https://www.catsystems.io/) -- [CATS Flights](https://flights.catsystems.io/) +- [CATS Flights](https://catsystems.io/flights) - [Discord community](https://discord.gg/r7ErmSNvsy) diff --git a/ground_station/RADIO-UPDATE.md b/ground_station/RADIO-UPDATE.md index 7583347b..ee475fc0 100644 --- a/ground_station/RADIO-UPDATE.md +++ b/ground_station/RADIO-UPDATE.md @@ -35,9 +35,14 @@ new ROM session, but an interruption during final vector activation can require programming result is ambiguous is quarantined until Ground Station restart or repair. This is the unavoidable safety limitation of using the factory ROM instead of a resident custom bootloader. -The bootloader-entry request is `CMD_BOOTLOADER`, a zero payload length, and the usual frame CRC8. The receiver rejects -requests with a nonzero payload length and acknowledges entry before jumping to the ROM bootloader. - -Receiver applications predating the `CMD_BOOTLOADER` implementation need a one-time ST-Link installation of a -ROM-capable production telemetry build. Units provisioned with the earlier protected custom-loader proof of concept need -a separate provisioning procedure. Older Vega firmware remains compatible with the ROM-capable telemetry application. +The bootloader-entry request is `CMD_BOOTLOADER`, a zero payload length, and the usual frame CRC8. Production telemetry +1.2.0 is the first version that supports this update entry protocol. The receiver rejects requests with a nonzero payload +length and acknowledges entry before jumping to the ROM bootloader. + +Telemetry 1.1.3 and earlier cannot enter the updater. An attempted update receives no entry acknowledgement and stops +before ROM synchronization, erase, or write, so the installed application is not modified. Because a missing +acknowledgement is indistinguishable from an acknowledgement lost after entering ROM, the Ground Station quarantines the +attempted link until restart. These receivers need a one-time ST-Link installation of a ROM-capable production telemetry +build before Ground Station updates can be used. Units provisioned with the earlier protected custom-loader proof of +concept need a separate provisioning procedure. Older Vega firmware remains compatible with the ROM-capable telemetry +application. diff --git a/ground_station/gs-wsl-check.sh b/ground_station/gs-wsl-check.sh index 8a571383..8dde266b 100644 --- a/ground_station/gs-wsl-check.sh +++ b/ground_station/gs-wsl-check.sh @@ -233,9 +233,19 @@ test_self_test() { "$project_root/tests/self_test.cpp" "$project_root/src/self_test.cpp" -o "$test_binary" "$test_binary" } +test_attitude() { + local test_binary="$build_dir/attitude-test" + g++ -std=c++20 -Wall -Wextra -Werror \ + -I"$project_root/src" -I"$project_root/lib/VQF/src" \ + "$project_root/tests/attitude_test.cpp" "$project_root/src/attitude.cpp" \ + "$project_root/lib/VQF/src/vqf.cpp" -o "$test_binary" + "$test_binary" +} + timed dependencies prepare_dependencies timed rom-bootloader-test test_rom_bootloader timed self-test test_self_test +timed attitude-test test_attitude timed build build_project timed lint lint_project diff --git a/ground_station/lib/MadgwickAHRS/CATS-VENDORING.md b/ground_station/lib/MadgwickAHRS/CATS-VENDORING.md deleted file mode 100644 index 65deb41c..00000000 --- a/ground_station/lib/MadgwickAHRS/CATS-VENDORING.md +++ /dev/null @@ -1,7 +0,0 @@ -# CATS Madgwick delta - -This library is rebased on MadgwickAHRS 1.2.0, upstream commit -`70fd2c93d7fa88bffdd4ace675db1d7110e80adb`. - -The local compatibility delta retains the Ground Station's -`getQuaternion()` API and its 50 Hz default sample frequency. diff --git a/ground_station/lib/MadgwickAHRS/README.adoc b/ground_station/lib/MadgwickAHRS/README.adoc deleted file mode 100644 index a99e6f6d..00000000 --- a/ground_station/lib/MadgwickAHRS/README.adoc +++ /dev/null @@ -1,30 +0,0 @@ -= Madgwick Library = - -This library wraps the official implementation of MadgwickAHRS algorithm to get orientation of an object based on accelerometer and gyroscope readings - -== License == - -Copyright (c) Arduino LLC. All right reserved. - -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - - -Implementation of Madgwick's IMU and AHRS algorithms. -See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms - -Date Author Notes -29/09/2011 SOH Madgwick Initial release -02/10/2011 SOH Madgwick Optimised for reduced CPU load -19/02/2012 SOH Madgwick Magnetometer measurement is normalised diff --git a/ground_station/lib/MadgwickAHRS/keywords.txt b/ground_station/lib/MadgwickAHRS/keywords.txt deleted file mode 100644 index 1a187737..00000000 --- a/ground_station/lib/MadgwickAHRS/keywords.txt +++ /dev/null @@ -1,24 +0,0 @@ -####################################### -# Syntax Coloring Map For MadgwickAHRS -####################################### - -####################################### -# Datatypes (KEYWORD1) -####################################### - -Madgwick KEYWORD1 - -####################################### -# Methods and Functions (KEYWORD2) -####################################### - -update KEYWORD2 -updateIMU KEYWORD2 -getPitch KEYWORD2 -getYaw KEYWORD2 -getRoll KEYWORD2 - - -####################################### -# Constants (LITERAL1) -####################################### diff --git a/ground_station/lib/MadgwickAHRS/library.properties b/ground_station/lib/MadgwickAHRS/library.properties deleted file mode 100644 index 57e03ddb..00000000 --- a/ground_station/lib/MadgwickAHRS/library.properties +++ /dev/null @@ -1,9 +0,0 @@ -name=Madgwick -version=1.2.0 -author=Arduino -maintainer=Arduino -sentence=Helpers for MadgwickAHRS algorithm -paragraph=This library wraps the official implementation of MadgwickAHRS algorithm to get orientation of an object based on accelerometer and gyroscope readings -category=Data Processing -url=https://github.com/arduino-libraries/MadgwickAHRS -architectures=* diff --git a/ground_station/lib/MadgwickAHRS/src/MadgwickAHRS.cpp b/ground_station/lib/MadgwickAHRS/src/MadgwickAHRS.cpp deleted file mode 100644 index c80cf05d..00000000 --- a/ground_station/lib/MadgwickAHRS/src/MadgwickAHRS.cpp +++ /dev/null @@ -1,250 +0,0 @@ -//============================================================================================= -// MadgwickAHRS.c -//============================================================================================= -// -// Implementation of Madgwick's IMU and AHRS algorithms. -// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ -// -// From the x-io website "Open-source resources available on this website are -// provided under the GNU General Public Licence unless an alternative licence -// is provided in source." -// -// Date Author Notes -// 29/09/2011 SOH Madgwick Initial release -// 02/10/2011 SOH Madgwick Optimised for reduced CPU load -// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised -// -//============================================================================================= - -//------------------------------------------------------------------------------------------- -// Header files - -#include "MadgwickAHRS.h" -#include - -//------------------------------------------------------------------------------------------- -// Definitions - -#define sampleFreqDef 50.0f // Ground Station default sample frequency in Hz -#define betaDef 0.1f // 2 * proportional gain - - -//============================================================================================ -// Functions - -//------------------------------------------------------------------------------------------- -// AHRS algorithm update - -Madgwick::Madgwick() { - beta = betaDef; - q0 = 1.0f; - q1 = 0.0f; - q2 = 0.0f; - q3 = 0.0f; - invSampleFreq = 1.0f / sampleFreqDef; - anglesComputed = 0; -} - -void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float hx, hy; - float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - - // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - updateIMU(gx, gy, gz, ax, ay, az); - return; - } - - // Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; - - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Normalise magnetometer measurement - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - _2q0mx = 2.0f * q0 * mx; - _2q0my = 2.0f * q0 * my; - _2q0mz = 2.0f * q0 * mz; - _2q1mx = 2.0f * q1 * mx; - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; - _2q0q2 = 2.0f * q0 * q2; - _2q2q3 = 2.0f * q2 * q3; - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; - - // Reference direction of Earth's magnetic field - hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; - hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; - _2bx = sqrtf(hx * hx + hy * hy); - _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; - _4bx = 2.0f * _2bx; - _4bz = 2.0f * _2bz; - - // Gradient decent algorithm corrective step - s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - // Apply feedback step - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; - } - - // Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * invSampleFreq; - q1 += qDot2 * invSampleFreq; - q2 += qDot3 * invSampleFreq; - q3 += qDot4 * invSampleFreq; - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - anglesComputed = 0; -} - -//------------------------------------------------------------------------------------------- -// IMU algorithm update - -void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) { - float recipNorm; - float s0, s1, s2, s3; - float qDot1, qDot2, qDot3, qDot4; - float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; - - // Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; - - // Rate of change of quaternion from gyroscope - qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); - qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); - qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); - qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - _2q0 = 2.0f * q0; - _2q1 = 2.0f * q1; - _2q2 = 2.0f * q2; - _2q3 = 2.0f * q3; - _4q0 = 4.0f * q0; - _4q1 = 4.0f * q1; - _4q2 = 4.0f * q2; - _8q1 = 8.0f * q1; - _8q2 = 8.0f * q2; - q0q0 = q0 * q0; - q1q1 = q1 * q1; - q2q2 = q2 * q2; - q3q3 = q3 * q3; - - // Gradient decent algorithm corrective step - s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; - s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; - s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; - s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; - recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - // Apply feedback step - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; - } - - // Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * invSampleFreq; - q1 += qDot2 * invSampleFreq; - q2 += qDot3 * invSampleFreq; - q3 += qDot4 * invSampleFreq; - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - anglesComputed = 0; -} - -//------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root - -float Madgwick::invSqrt(float x) { - float halfx = 0.5f * x; - float y = x; - long i = *(long*)&y; - i = 0x5f3759df - (i>>1); - y = *(float*)&i; - y = y * (1.5f - (halfx * y * y)); - y = y * (1.5f - (halfx * y * y)); - return y; -} - -//------------------------------------------------------------------------------------------- - -void Madgwick::computeAngles() -{ - roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2); - pitch = asinf(-2.0f * (q1*q3 - q0*q2)); - yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); - anglesComputed = 1; -} diff --git a/ground_station/lib/MadgwickAHRS/src/MadgwickAHRS.h b/ground_station/lib/MadgwickAHRS/src/MadgwickAHRS.h deleted file mode 100644 index 9239db16..00000000 --- a/ground_station/lib/MadgwickAHRS/src/MadgwickAHRS.h +++ /dev/null @@ -1,80 +0,0 @@ -//============================================================================================= -// MadgwickAHRS.h -//============================================================================================= -// -// Implementation of Madgwick's IMU and AHRS algorithms. -// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ -// -// From the x-io website "Open-source resources available on this website are -// provided under the GNU General Public Licence unless an alternative licence -// is provided in source." -// -// Date Author Notes -// 29/09/2011 SOH Madgwick Initial release -// 02/10/2011 SOH Madgwick Optimised for reduced CPU load -// -//============================================================================================= -#ifndef MadgwickAHRS_h -#define MadgwickAHRS_h -#include - -//-------------------------------------------------------------------------------------------- -// Variable declaration -class Madgwick{ -private: - static float invSqrt(float x); - float beta; // algorithm gain - float q0; - float q1; - float q2; - float q3; // quaternion of sensor frame relative to auxiliary frame - float invSampleFreq; - float roll; - float pitch; - float yaw; - char anglesComputed; - void computeAngles(); - -//------------------------------------------------------------------------------------------- -// Function declarations -public: - Madgwick(void); - void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } - void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); - void updateIMU(float gx, float gy, float gz, float ax, float ay, float az); - //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);}; - //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);}; - //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);}; - float getRoll() { - if (!anglesComputed) computeAngles(); - return roll * 57.29578f; - } - float getPitch() { - if (!anglesComputed) computeAngles(); - return pitch * 57.29578f; - } - float getYaw() { - if (!anglesComputed) computeAngles(); - return yaw * 57.29578f + 180.0f; - } - float getRollRadians() { - if (!anglesComputed) computeAngles(); - return roll; - } - float getPitchRadians() { - if (!anglesComputed) computeAngles(); - return pitch; - } - float getYawRadians() { - if (!anglesComputed) computeAngles(); - return yaw; - } - void getQuaternion(float* qu0, float* qu1, float* qu2, float* qu3) { - *qu0 = q0; - *qu1 = q1; - *qu2 = q2; - *qu3 = q3; - } -}; -#endif - diff --git a/ground_station/lib/VQF/CATS-VENDORING.md b/ground_station/lib/VQF/CATS-VENDORING.md new file mode 100644 index 00000000..a142e35c --- /dev/null +++ b/ground_station/lib/VQF/CATS-VENDORING.md @@ -0,0 +1,25 @@ +# VQF + +Unmodified `vqf/cpp/vqf.hpp`, `vqf/cpp/vqf.cpp`, and `LICENSES/MIT.txt` +(stored here as `LICENSE`) from [VQF v2.1.2](https://github.com/dlaidig/vqf/tree/v2.1.2), +commit `86ba56bdd3158b9b05f9f9fe5596866ba326438c`. +`library.json` is CATS packaging for PlatformIO. + +The GS uses full online VQF with upstream defaults, including double precision, +rest/motion gyro bias estimation and magnetic disturbance rejection. No upstream +source or algorithm defaults are patched. Units, board mounting and the GS angle +convention are defined in `src/attitude.hpp` and `src/attitude.cpp`. + +Saved factory gyro offsets are subtracted before fusion; VQF estimates residual +bias only. Saving new gyro or magnetometer calibration resets the filter on the +navigation task so learned state from the old calibration is discarded. + +The previous Madgwick vendor folder was removed after this migration; its CATS +delta remains available in repository history. VQF is the only attitude filter +included and linked by the GS. Real-filter integration tests run in +`gs-wsl-check.sh` (also called by `gs-precommit.ps1`). The display simulator injects +GS angles directly and does not simulate VQF. + +Before hardware acceptance, check compass heading through a full rotation, +positive pitch when pointing up, tilt compensation, magnetic disturbance and +recovery, calibration, and navigation task timing/stack headroom at 50 Hz. diff --git a/ground_station/lib/VQF/LICENSE b/ground_station/lib/VQF/LICENSE new file mode 100644 index 00000000..f0fd20ab --- /dev/null +++ b/ground_station/lib/VQF/LICENSE @@ -0,0 +1,20 @@ +MIT License + +Copyright (c) + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is furnished +to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS +FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS +OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF +OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/ground_station/lib/VQF/library.json b/ground_station/lib/VQF/library.json new file mode 100644 index 00000000..341bd1fb --- /dev/null +++ b/ground_station/lib/VQF/library.json @@ -0,0 +1,9 @@ +{ + "name": "VQF", + "version": "2.1.2", + "description": "Versatile Quaternion-based Filter for IMU orientation estimation", + "license": "MIT", + "repository": { "type": "git", "url": "https://github.com/dlaidig/vqf.git" }, + "frameworks": "*", + "platforms": "*" +} diff --git a/ground_station/lib/VQF/src/vqf.cpp b/ground_station/lib/VQF/src/vqf.cpp new file mode 100644 index 00000000..4be221af --- /dev/null +++ b/ground_station/lib/VQF/src/vqf.cpp @@ -0,0 +1,1001 @@ +// SPDX-FileCopyrightText: 2021 Daniel Laidig +// +// SPDX-License-Identifier: MIT + +#include "vqf.hpp" + +#include +#include +#include +#include + +#define EPS std::numeric_limits::epsilon() +#define NaN std::numeric_limits::quiet_NaN() +#define PI 3.14159265358979323846264338327950288 +#define SQRT2 1.41421356237309504880168872420969808 + +inline vqf_real_t square(vqf_real_t x) { return x*x; } + + +VQFParams::VQFParams() + : tauAcc(3.0) + , tauMag(9.0) +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + , motionBiasEstEnabled(true) +#endif + , restBiasEstEnabled(true) + , magDistRejectionEnabled(true) + , biasSigmaInit(0.5) + , biasForgettingTime(100.0) + , biasClip(2.0) +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + , biasSigmaMotion(0.1) + , biasVerticalForgettingFactor(0.0001) +#endif + , biasSigmaRest(0.03) + , restMinT(1.5) + , restFilterTau(0.5) + , restThGyr(2.0) + , restThAcc(0.5) + , magCurrentTau(0.05) + , magRefTau(20.0) + , magNormTh(0.1) + , magDipTh(10.0) + , magNewTime(20.0) + , magNewFirstTime(5.0) + , magNewMinGyr(20.0) + , magMinUndisturbedTime(0.5) + , magMaxRejectionTime(60.0) + , magRejectionFactor(2.0) +{ + +} + +VQF::VQF(vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs) +{ + coeffs.gyrTs = gyrTs; + coeffs.accTs = accTs > 0 ? accTs : gyrTs; + coeffs.magTs = magTs > 0 ? magTs : gyrTs; + + setup(); +} + +VQF::VQF(const VQFParams ¶ms, vqf_real_t gyrTs, vqf_real_t accTs, vqf_real_t magTs) +{ + this->params = params; + + coeffs.gyrTs = gyrTs; + coeffs.accTs = accTs > 0 ? accTs : gyrTs; + coeffs.magTs = magTs > 0 ? magTs : gyrTs; + + setup(); +} + +void VQF::updateGyr(const vqf_real_t gyr[3]) +{ + // rest detection + if (params.restBiasEstEnabled || params.magDistRejectionEnabled) { + filterVec(gyr, 3, params.restFilterTau, coeffs.gyrTs, coeffs.restGyrLpB, coeffs.restGyrLpA, + state.restGyrLpState, state.restLastGyrLp); + + state.restLastSquaredDeviations[0] = square(gyr[0] - state.restLastGyrLp[0]) + + square(gyr[1] - state.restLastGyrLp[1]) + square(gyr[2] - state.restLastGyrLp[2]); + + vqf_real_t biasClip = params.biasClip*vqf_real_t(PI/180.0); + if (state.restLastSquaredDeviations[0] >= square(params.restThGyr*vqf_real_t(PI/180.0)) + || std::fabs(state.restLastGyrLp[0]) > biasClip || std::fabs(state.restLastGyrLp[1]) > biasClip + || std::fabs(state.restLastGyrLp[2]) > biasClip) { + state.restT = 0.0; + state.restDetected = false; + } + } + + // remove estimated gyro bias + vqf_real_t gyrNoBias[3] = {gyr[0]-state.bias[0], gyr[1]-state.bias[1], gyr[2]-state.bias[2]}; + + // gyroscope prediction step + vqf_real_t gyrNorm = norm(gyrNoBias, 3); + vqf_real_t angle = gyrNorm * coeffs.gyrTs; + if (gyrNorm > EPS) { + vqf_real_t c = std::cos(angle/2); + vqf_real_t s = std::sin(angle/2)/gyrNorm; + vqf_real_t gyrStepQuat[4] = {c, s*gyrNoBias[0], s*gyrNoBias[1], s*gyrNoBias[2]}; + quatMultiply(state.gyrQuat, gyrStepQuat, state.gyrQuat); + normalize(state.gyrQuat, 4); + } +} + +void VQF::updateAcc(const vqf_real_t acc[3]) +{ + // ignore [0 0 0] samples + if (acc[0] == vqf_real_t(0.0) && acc[1] == vqf_real_t(0.0) && acc[2] == vqf_real_t(0.0)) { + return; + } + + // rest detection + if (params.restBiasEstEnabled) { + filterVec(acc, 3, params.restFilterTau, coeffs.accTs, coeffs.restAccLpB, coeffs.restAccLpA, + state.restAccLpState, state.restLastAccLp); + + state.restLastSquaredDeviations[1] = square(acc[0] - state.restLastAccLp[0]) + + square(acc[1] - state.restLastAccLp[1]) + square(acc[2] - state.restLastAccLp[2]); + + if (state.restLastSquaredDeviations[1] >= square(params.restThAcc)) { + state.restT = 0.0; + state.restDetected = false; + } else { + state.restT += coeffs.accTs; + if (state.restT >= params.restMinT) { + state.restDetected = true; + } + } + } + + vqf_real_t accEarth[3]; + + // filter acc in inertial frame + quatRotate(state.gyrQuat, acc, accEarth); + filterVec(accEarth, 3, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.accLpState, state.lastAccLp); + + // transform to 6D earth frame and normalize + quatRotate(state.accQuat, state.lastAccLp, accEarth); + normalize(accEarth, 3); + + // inclination correction + vqf_real_t accCorrQuat[4]; + vqf_real_t q_w = std::sqrt((accEarth[2]+1)/2); + if (q_w > vqf_real_t(1e-6)) { + accCorrQuat[0] = q_w; + accCorrQuat[1] = vqf_real_t(0.5)*accEarth[1]/q_w; + accCorrQuat[2] = vqf_real_t(-0.5)*accEarth[0]/q_w; + accCorrQuat[3] = 0; + } else { + // to avoid numeric issues when acc is close to [0 0 -1], i.e. the correction step is close (<= 0.00011°) to 180°: + accCorrQuat[0] = 0; + accCorrQuat[1] = 1; + accCorrQuat[2] = 0; + accCorrQuat[3] = 0; + } + quatMultiply(accCorrQuat, state.accQuat, state.accQuat); + normalize(state.accQuat, 4); + + // calculate correction angular rate to facilitate debugging + state.lastAccCorrAngularRate = std::acos(accEarth[2])/coeffs.accTs; + + // bias estimation +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + if (params.motionBiasEstEnabled || params.restBiasEstEnabled) { + vqf_real_t biasClip = params.biasClip*vqf_real_t(PI/180.0); + + vqf_real_t accGyrQuat[4]; + vqf_real_t R[9]; + vqf_real_t biasLp[2]; + + // get rotation matrix corresponding to accGyrQuat + getQuat6D(accGyrQuat); + R[0] = 1 - 2*square(accGyrQuat[2]) - 2*square(accGyrQuat[3]); // r11 + R[1] = 2*(accGyrQuat[2]*accGyrQuat[1] - accGyrQuat[0]*accGyrQuat[3]); // r12 + R[2] = 2*(accGyrQuat[0]*accGyrQuat[2] + accGyrQuat[3]*accGyrQuat[1]); // r13 + R[3] = 2*(accGyrQuat[0]*accGyrQuat[3] + accGyrQuat[2]*accGyrQuat[1]); // r21 + R[4] = 1 - 2*square(accGyrQuat[1]) - 2*square(accGyrQuat[3]); // r22 + R[5] = 2*(accGyrQuat[2]*accGyrQuat[3] - accGyrQuat[1]*accGyrQuat[0]); // r23 + R[6] = 2*(accGyrQuat[3]*accGyrQuat[1] - accGyrQuat[0]*accGyrQuat[2]); // r31 + R[7] = 2*(accGyrQuat[0]*accGyrQuat[1] + accGyrQuat[3]*accGyrQuat[2]); // r32 + R[8] = 1 - 2*square(accGyrQuat[1]) - 2*square(accGyrQuat[2]); // r33 + + // calculate R*b_hat (only the x and y component, as z is not needed) + biasLp[0] = R[0]*state.bias[0] + R[1]*state.bias[1] + R[2]*state.bias[2]; + biasLp[1] = R[3]*state.bias[0] + R[4]*state.bias[1] + R[5]*state.bias[2]; + + // low-pass filter R and R*b_hat + filterVec(R, 9, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.motionBiasEstRLpState, R); + filterVec(biasLp, 2, params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA, state.motionBiasEstBiasLpState, + biasLp); + + // set measurement error and covariance for the respective Kalman filter update + vqf_real_t w[3]; + vqf_real_t e[3]; + if (state.restDetected && params.restBiasEstEnabled) { + e[0] = state.restLastGyrLp[0] - state.bias[0]; + e[1] = state.restLastGyrLp[1] - state.bias[1]; + e[2] = state.restLastGyrLp[2] - state.bias[2]; + matrix3SetToScaledIdentity(1.0, R); + std::fill(w, w+3, coeffs.biasRestW); + } else if (params.motionBiasEstEnabled) { + e[0] = -accEarth[1]/coeffs.accTs + biasLp[0] - R[0]*state.bias[0] - R[1]*state.bias[1] - R[2]*state.bias[2]; + e[1] = accEarth[0]/coeffs.accTs + biasLp[1] - R[3]*state.bias[0] - R[4]*state.bias[1] - R[5]*state.bias[2]; + e[2] = - R[6]*state.bias[0] - R[7]*state.bias[1] - R[8]*state.bias[2]; + w[0] = coeffs.biasMotionW; + w[1] = coeffs.biasMotionW; + w[2] = coeffs.biasVerticalW; + } else { + std::fill(w, w+3, -1); // disable update + } + + // Kalman filter update + // step 1: P = P + V (also increase covariance if there is no measurement update!) + if (state.biasP[0] < coeffs.biasP0) { + state.biasP[0] += coeffs.biasV; + } + if (state.biasP[4] < coeffs.biasP0) { + state.biasP[4] += coeffs.biasV; + } + if (state.biasP[8] < coeffs.biasP0) { + state.biasP[8] += coeffs.biasV; + } + if (w[0] >= 0) { + // clip disagreement to -2..2 °/s + // (this also effectively limits the harm done by the first inclination correction step) + clip(e, 3, -biasClip, biasClip); + + // step 2: K = P R^T inv(W + R P R^T) + vqf_real_t K[9]; + matrix3MultiplyTpsSecond(state.biasP, R, K); // K = P R^T + matrix3Multiply(R, K, K); // K = R P R^T + K[0] += w[0]; + K[4] += w[1]; + K[8] += w[2]; // K = W + R P R^T + matrix3Inv(K, K); // K = inv(W + R P R^T) + matrix3MultiplyTpsFirst(R, K, K); // K = R^T inv(W + R P R^T) + matrix3Multiply(state.biasP, K, K); // K = P R^T inv(W + R P R^T) + + // step 3: bias = bias + K (y - R bias) = bias + K e + state.bias[0] += K[0]*e[0] + K[1]*e[1] + K[2]*e[2]; + state.bias[1] += K[3]*e[0] + K[4]*e[1] + K[5]*e[2]; + state.bias[2] += K[6]*e[0] + K[7]*e[1] + K[8]*e[2]; + + // step 4: P = P - K R P + matrix3Multiply(K, R, K); // K = K R + matrix3Multiply(K, state.biasP, K); // K = K R P + for(size_t i = 0; i < 9; i++) { + state.biasP[i] -= K[i]; + } + + // clip bias estimate to -2..2 °/s + clip(state.bias, 3, -biasClip, biasClip); + } + } +#else + // simplified implementation of bias estimation for the special case in which only rest bias estimation is enabled + if (params.restBiasEstEnabled) { + vqf_real_t biasClip = params.biasClip*vqf_real_t(PI/180.0); + if (state.biasP < coeffs.biasP0) { + state.biasP += coeffs.biasV; + } + if (state.restDetected) { + vqf_real_t e[3]; + e[0] = state.restLastGyrLp[0] - state.bias[0]; + e[1] = state.restLastGyrLp[1] - state.bias[1]; + e[2] = state.restLastGyrLp[2] - state.bias[2]; + clip(e, 3, -biasClip, biasClip); + + // Kalman filter update, simplified scalar version for rest update + // (this version only uses the first entry of P as P is diagonal and all diagonal elements are the same) + // step 1: P = P + V (done above!) + // step 2: K = P R^T inv(W + R P R^T) + vqf_real_t k = state.biasP/(coeffs.biasRestW + state.biasP); + // step 3: bias = bias + K (y - R bias) = bias + K e + state.bias[0] += k*e[0]; + state.bias[1] += k*e[1]; + state.bias[2] += k*e[2]; + // step 4: P = P - K R P + state.biasP -= k*state.biasP; + clip(state.bias, 3, -biasClip, biasClip); + } + } +#endif +} + +void VQF::updateMag(const vqf_real_t mag[3]) +{ + // ignore [0 0 0] samples + if (mag[0] == vqf_real_t(0.0) && mag[1] == vqf_real_t(0.0) && mag[2] == vqf_real_t(0.0)) { + return; + } + + vqf_real_t magEarth[3]; + + // bring magnetometer measurement into 6D earth frame + vqf_real_t accGyrQuat[4]; + getQuat6D(accGyrQuat); + quatRotate(accGyrQuat, mag, magEarth); + + if (params.magDistRejectionEnabled) { + state.magNormDip[0] = norm(magEarth, 3); + state.magNormDip[1] = -std::asin(magEarth[2]/state.magNormDip[0]); + + if (params.magCurrentTau > 0) { + filterVec(state.magNormDip, 2, params.magCurrentTau, coeffs.magTs, coeffs.magNormDipLpB, + coeffs.magNormDipLpA, state.magNormDipLpState, state.magNormDip); + } + + // magnetic disturbance detection + if (std::fabs(state.magNormDip[0] - state.magRefNorm) < params.magNormTh*state.magRefNorm + && std::fabs(state.magNormDip[1] - state.magRefDip) < params.magDipTh*vqf_real_t(PI/180.0)) { + state.magUndisturbedT += coeffs.magTs; + if (state.magUndisturbedT >= params.magMinUndisturbedTime) { + state.magDistDetected = false; + state.magRefNorm += coeffs.kMagRef*(state.magNormDip[0] - state.magRefNorm); + state.magRefDip += coeffs.kMagRef*(state.magNormDip[1] - state.magRefDip); + } + } else { + state.magUndisturbedT = 0.0; + state.magDistDetected = true; + } + + // new magnetic field acceptance + if (std::fabs(state.magNormDip[0] - state.magCandidateNorm) < params.magNormTh*state.magCandidateNorm + && std::fabs(state.magNormDip[1] - state.magCandidateDip) < params.magDipTh*vqf_real_t(PI/180.0)) { + if (norm(state.restLastGyrLp, 3) >= params.magNewMinGyr*vqf_real_t(PI/180.0)) { + state.magCandidateT += coeffs.magTs; + } + state.magCandidateNorm += coeffs.kMagRef*(state.magNormDip[0] - state.magCandidateNorm); + state.magCandidateDip += coeffs.kMagRef*(state.magNormDip[1] - state.magCandidateDip); + + if (state.magDistDetected && (state.magCandidateT >= params.magNewTime || ( + state.magRefNorm == vqf_real_t(0.0) && state.magCandidateT >= params.magNewFirstTime))) { + state.magRefNorm = state.magCandidateNorm; + state.magRefDip = state.magCandidateDip; + state.magDistDetected = false; + state.magUndisturbedT = params.magMinUndisturbedTime; + } + } else { + state.magCandidateT = 0.0; + state.magCandidateNorm = state.magNormDip[0]; + state.magCandidateDip = state.magNormDip[1]; + } + } + + // calculate disagreement angle based on current magnetometer measurement + state.lastMagDisAngle = std::atan2(magEarth[0], magEarth[1]) - state.delta; + + // make sure the disagreement angle is in the range [-pi, pi] + if (state.lastMagDisAngle > vqf_real_t(PI)) { + state.lastMagDisAngle -= vqf_real_t(2*PI); + } else if (state.lastMagDisAngle < vqf_real_t(-PI)) { + state.lastMagDisAngle += vqf_real_t(2*PI); + } + + vqf_real_t k = coeffs.kMag; + + if (params.magDistRejectionEnabled) { + // magnetic disturbance rejection + if (state.magDistDetected) { + if (state.magRejectT <= params.magMaxRejectionTime) { + state.magRejectT += coeffs.magTs; + k = 0; + } else { + k /= params.magRejectionFactor; + } + } else { + state.magRejectT = (std::max)(state.magRejectT - params.magRejectionFactor*coeffs.magTs, vqf_real_t(0.0)); + } + } + + // ensure fast initial convergence + if (state.kMagInit != vqf_real_t(0.0)) { + // make sure that the gain k is at least 1/N, N=1,2,3,... in the first few samples + if (k < state.kMagInit) { + k = state.kMagInit; + } + + // iterative expression to calculate 1/N + state.kMagInit = state.kMagInit/(state.kMagInit+1); + + // disable if t > tauMag + if (state.kMagInit*params.tauMag < coeffs.magTs) { + state.kMagInit = 0.0; + } + } + + // first-order filter step + state.delta += k*state.lastMagDisAngle; + // calculate correction angular rate to facilitate debugging + state.lastMagCorrAngularRate = k*state.lastMagDisAngle/coeffs.magTs; + + // make sure delta is in the range [-pi, pi] + if (state.delta > vqf_real_t(PI)) { + state.delta -= vqf_real_t(2*PI); + } else if (state.delta < vqf_real_t(-PI)) { + state.delta += vqf_real_t(2*PI); + } +} + +void VQF::update(const vqf_real_t gyr[3], const vqf_real_t acc[3]) +{ + updateGyr(gyr); + updateAcc(acc); +} + +void VQF::update(const vqf_real_t gyr[3], const vqf_real_t acc[3], const vqf_real_t mag[3]) +{ + updateGyr(gyr); + updateAcc(acc); + updateMag(mag); +} + +void VQF::updateBatch(const vqf_real_t gyr[], const vqf_real_t acc[], const vqf_real_t mag[], size_t N, + vqf_real_t out6D[], vqf_real_t out9D[], vqf_real_t outDelta[], vqf_real_t outBias[], + vqf_real_t outBiasSigma[], bool outRest[], bool outMagDist[]) +{ + for (size_t i = 0; i < N; i++) { + if (mag) { + update(gyr+3*i, acc+3*i, mag+3*i); + } else { + update(gyr+3*i, acc+3*i); + } + if (out6D) { + getQuat6D(out6D+4*i); + } + if (out9D) { + getQuat9D(out9D+4*i); + } + if (outDelta) { + outDelta[i] = state.delta; + } + if (outBias) { + std::copy(state.bias, state.bias+3, outBias+3*i); + } + if (outBiasSigma) { + outBiasSigma[i] = getBiasEstimate(0); + } + if (outRest) { + outRest[i] = state.restDetected; + } + if (outMagDist) { + outMagDist[i] = state.magDistDetected; + } + } +} + +void VQF::getQuat3D(vqf_real_t out[4]) const +{ + std::copy(state.gyrQuat, state.gyrQuat+4, out); +} + +void VQF::getQuat6D(vqf_real_t out[4]) const +{ + quatMultiply(state.accQuat, state.gyrQuat, out); +} + +void VQF::getQuat9D(vqf_real_t out[4]) const +{ + quatMultiply(state.accQuat, state.gyrQuat, out); + quatApplyDelta(out, state.delta, out); +} + +vqf_real_t VQF::getDelta() const +{ + return state.delta; +} + +vqf_real_t VQF::getBiasEstimate(vqf_real_t out[3]) const +{ + if (out) { + std::copy(state.bias, state.bias+3, out); + } +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + // use largest absolute row sum as upper bound estimate for largest eigenvalue (Gershgorin circle theorem) + // and clip output to biasSigmaInit + vqf_real_t sum1 = std::fabs(state.biasP[0]) + std::fabs(state.biasP[1]) + std::fabs(state.biasP[2]); + vqf_real_t sum2 = std::fabs(state.biasP[3]) + std::fabs(state.biasP[4]) + std::fabs(state.biasP[5]); + vqf_real_t sum3 = std::fabs(state.biasP[6]) + std::fabs(state.biasP[7]) + std::fabs(state.biasP[8]); + vqf_real_t P = (std::min)((std::max)((std::max)(sum1, sum2), sum3), coeffs.biasP0); +#else + vqf_real_t P = state.biasP; +#endif + // convert standard deviation from 0.01deg to rad + return std::sqrt(P)*vqf_real_t(PI/100.0/180.0); +} + +void VQF::setBiasEstimate(vqf_real_t bias[3], vqf_real_t sigma) +{ + std::copy(bias, bias+3, state.bias); + if (sigma > 0) { + vqf_real_t P = square(sigma*vqf_real_t(180.0*100.0/PI)); +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + matrix3SetToScaledIdentity(P, state.biasP); +#else + state.biasP = P; +#endif + } +} + +bool VQF::getRestDetected() const +{ + return state.restDetected; +} + +bool VQF::getMagDistDetected() const +{ + return state.magDistDetected; +} + +void VQF::getRelativeRestDeviations(vqf_real_t out[2]) const +{ + out[0] = std::sqrt(state.restLastSquaredDeviations[0]) / (params.restThGyr*vqf_real_t(PI/180.0)); + out[1] = std::sqrt(state.restLastSquaredDeviations[1]) / params.restThAcc; +} + +vqf_real_t VQF::getMagRefNorm() const +{ + return state.magRefNorm; +} + +vqf_real_t VQF::getMagRefDip() const +{ + return state.magRefDip; +} + +void VQF::setMagRef(vqf_real_t norm, vqf_real_t dip) +{ + state.magRefNorm = norm; + state.magRefDip = dip; +} + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION +void VQF::setMotionBiasEstEnabled(bool enabled) +{ + if (params.motionBiasEstEnabled == enabled) { + return; + } + params.motionBiasEstEnabled = enabled; + std::fill(state.motionBiasEstRLpState, state.motionBiasEstRLpState + 9*2, NaN); + std::fill(state.motionBiasEstBiasLpState, state.motionBiasEstBiasLpState + 2*2, NaN); +} +#endif + +void VQF::setRestBiasEstEnabled(bool enabled) +{ + if (params.restBiasEstEnabled == enabled) { + return; + } + params.restBiasEstEnabled = enabled; + state.restDetected = false; + std::fill(state.restLastSquaredDeviations, state.restLastSquaredDeviations + 2, 0.0); + state.restT = 0.0; + std::fill(state.restLastGyrLp, state.restLastGyrLp + 3, 0.0); + std::fill(state.restGyrLpState, state.restGyrLpState + 3*2, NaN); + std::fill(state.restLastAccLp, state.restLastAccLp + 3, 0.0); + std::fill(state.restAccLpState, state.restAccLpState + 3*2, NaN); +} + +void VQF::setMagDistRejectionEnabled(bool enabled) +{ + if (params.magDistRejectionEnabled == enabled) { + return; + } + params.magDistRejectionEnabled = enabled; + state.magDistDetected = true; + state.magRefNorm = 0.0; + state.magRefDip = 0.0; + state.magUndisturbedT = 0.0; + state.magRejectT = params.magMaxRejectionTime; + state.magCandidateNorm = -1.0; + state.magCandidateDip = 0.0; + state.magCandidateT = 0.0; + std::fill(state.magNormDipLpState, state.magNormDipLpState + 2*2, NaN); +} + +void VQF::setTauAcc(vqf_real_t tauAcc) +{ + if (params.tauAcc == tauAcc) { + return; + } + params.tauAcc = tauAcc; + double newB[3]; + double newA[3]; + + filterCoeffs(params.tauAcc, coeffs.accTs, newB, newA); + filterAdaptStateForCoeffChange(state.lastAccLp, 3, coeffs.accLpB, coeffs.accLpA, newB, newA, state.accLpState); + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + // For R and biasLP, the last value is not saved in the state. + // Since b0 is small (at reasonable settings), the last output is close to state[0]. + vqf_real_t R[9]; + for (size_t i = 0; i < 9; i++) { + R[i] = state.motionBiasEstRLpState[2*i]; + } + filterAdaptStateForCoeffChange(R, 9, coeffs.accLpB, coeffs.accLpA, newB, newA, state.motionBiasEstRLpState); + vqf_real_t biasLp[2]; + for (size_t i = 0; i < 2; i++) { + biasLp[i] = state.motionBiasEstBiasLpState[2*i]; + } + filterAdaptStateForCoeffChange(biasLp, 2, coeffs.accLpB, coeffs.accLpA, newB, newA, state.motionBiasEstBiasLpState); +#endif + + std::copy(newB, newB+3, coeffs.accLpB); + std::copy(newA, newA+2, coeffs.accLpA); +} + +void VQF::setTauMag(vqf_real_t tauMag) +{ + params.tauMag = tauMag; + coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs); +} + +void VQF::setRestDetectionThresholds(vqf_real_t thGyr, vqf_real_t thAcc) +{ + params.restThGyr = thGyr; + params.restThAcc = thAcc; +} + +const VQFParams& VQF::getParams() const +{ + return params; +} + +const VQFCoefficients& VQF::getCoeffs() const +{ + return coeffs; +} + +const VQFState& VQF::getState() const +{ + return state; +} + +void VQF::setState(const VQFState& state) +{ + this->state = state; +} + +void VQF::resetState() +{ + quatSetToIdentity(state.gyrQuat); + quatSetToIdentity(state.accQuat); + state.delta = 0.0; + + state.restDetected = false; + state.magDistDetected = true; + + std::fill(state.lastAccLp, state.lastAccLp+3, 0); + std::fill(state.accLpState, state.accLpState + 3*2, NaN); + state.lastAccCorrAngularRate = 0.0; + + state.kMagInit = 1.0; + state.lastMagDisAngle = 0.0; + state.lastMagCorrAngularRate = 0.0; + + std::fill(state.bias, state.bias+3, 0); +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + matrix3SetToScaledIdentity(coeffs.biasP0, state.biasP); +#else + state.biasP = coeffs.biasP0; +#endif + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + std::fill(state.motionBiasEstRLpState, state.motionBiasEstRLpState + 9*2, NaN); + std::fill(state.motionBiasEstBiasLpState, state.motionBiasEstBiasLpState + 2*2, NaN); +#endif + + std::fill(state.restLastSquaredDeviations, state.restLastSquaredDeviations + 2, 0.0); + state.restT = 0.0; + std::fill(state.restLastGyrLp, state.restLastGyrLp + 3, 0.0); + std::fill(state.restGyrLpState, state.restGyrLpState + 3*2, NaN); + std::fill(state.restLastAccLp, state.restLastAccLp + 3, 0.0); + std::fill(state.restAccLpState, state.restAccLpState + 3*2, NaN); + + state.magRefNorm = 0.0; + state.magRefDip = 0.0; + state.magUndisturbedT = 0.0; + state.magRejectT = params.magMaxRejectionTime; + state.magCandidateNorm = -1.0; + state.magCandidateDip = 0.0; + state.magCandidateT = 0.0; + std::fill(state.magNormDip, state.magNormDip + 2, 0); + std::fill(state.magNormDipLpState, state.magNormDipLpState + 2*2, NaN); +} + +void VQF::quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]) +{ + vqf_real_t w = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]; + vqf_real_t x = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]; + vqf_real_t y = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]; + vqf_real_t z = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]; + out[0] = w; out[1] = x; out[2] = y; out[3] = z; +} + +void VQF::quatConj(const vqf_real_t q[4], vqf_real_t out[4]) +{ + vqf_real_t w = q[0]; + vqf_real_t x = -q[1]; + vqf_real_t y = -q[2]; + vqf_real_t z = -q[3]; + out[0] = w; out[1] = x; out[2] = y; out[3] = z; +} + + +void VQF::quatSetToIdentity(vqf_real_t out[4]) +{ + out[0] = 1; + out[1] = 0; + out[2] = 0; + out[3] = 0; +} + +void VQF::quatApplyDelta(vqf_real_t q[4], vqf_real_t delta, vqf_real_t out[4]) +{ + // out = quatMultiply([cos(delta/2), 0, 0, sin(delta/2)], q) + vqf_real_t c = std::cos(delta/2); + vqf_real_t s = std::sin(delta/2); + vqf_real_t w = c * q[0] - s * q[3]; + vqf_real_t x = c * q[1] - s * q[2]; + vqf_real_t y = c * q[2] + s * q[1]; + vqf_real_t z = c * q[3] + s * q[0]; + out[0] = w; out[1] = x; out[2] = y; out[3] = z; +} + +void VQF::quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]) +{ + vqf_real_t x = (1 - 2*q[2]*q[2] - 2*q[3]*q[3])*v[0] + 2*v[1]*(q[2]*q[1] - q[0]*q[3]) + 2*v[2]*(q[0]*q[2] + q[3]*q[1]); + vqf_real_t y = 2*v[0]*(q[0]*q[3] + q[2]*q[1]) + v[1]*(1 - 2*q[1]*q[1] - 2*q[3]*q[3]) + 2*v[2]*(q[2]*q[3] - q[1]*q[0]); + vqf_real_t z = 2*v[0]*(q[3]*q[1] - q[0]*q[2]) + 2*v[1]*(q[0]*q[1] + q[3]*q[2]) + v[2]*(1 - 2*q[1]*q[1] - 2*q[2]*q[2]); + out[0] = x; out[1] = y; out[2] = z; +} + +vqf_real_t VQF::norm(const vqf_real_t vec[], size_t N) +{ + vqf_real_t s = 0; + for(size_t i = 0; i < N; i++) { + s += vec[i]*vec[i]; + } + return std::sqrt(s); +} + +void VQF::normalize(vqf_real_t vec[], size_t N) +{ + vqf_real_t n = norm(vec, N); + if (n < EPS) { + return; + } + for(size_t i = 0; i < N; i++) { + vec[i] /= n; + } +} + +void VQF::clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max) +{ + for(size_t i = 0; i < N; i++) { + if (vec[i] < min) { + vec[i] = min; + } else if (vec[i] > max) { + vec[i] = max; + } + } +} + +vqf_real_t VQF::gainFromTau(vqf_real_t tau, vqf_real_t Ts) +{ + assert(Ts > 0); + if (tau < vqf_real_t(0.0)) { + return 0; // k=0 for negative tau (disable update) + } else if (tau == vqf_real_t(0.0)) { + return 1; // k=1 for tau=0 + } else { + return 1 - std::exp(-Ts/tau); // fc = 1/(2*pi*tau) + } +} + +void VQF::filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2]) +{ + assert(tau > 0); + assert(Ts > 0); + + // disable filter and use direct passthrough when tau < Ts/2 to avoid instability + // (this corresponds to fc exceeding 90% of the Nyquist frequency) + if (tau < Ts/2) { + outB[0] = 1; + outB[1] = 0; + outB[2] = 0; + outA[0] = 0; + outA[1] = 0; + return; + } + + // second order Butterworth filter based on https://stackoverflow.com/a/52764064 + double fc = (SQRT2 / (2.0*PI))/double(tau); // time constant of dampened, non-oscillating part of step response + double C = std::tan(PI*fc*double(Ts)); + double D = C*C + std::sqrt(2)*C + 1; + double b0 = C*C/D; + outB[0] = b0; + outB[1] = 2*b0; + outB[2] = b0; + // a0 = 1.0 + outA[0] = 2*(C*C-1)/D; // a1 + outA[1] = (1-std::sqrt(2)*C+C*C)/D; // a2 +} + +void VQF::filterInitialState(vqf_real_t x0, const double b[3], const double a[2], double out[2]) +{ + // initial state for steady state (equivalent to scipy.signal.lfilter_zi, obtained by setting y=x=x0 in the filter + // update equation) + out[0] = double(x0)*(1 - b[0]); + out[1] = double(x0)*(b[2] - a[1]); +} + +void VQF::filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[3], + const double a_old[2], const double b_new[3], + const double a_new[2], double state[]) +{ + if (std::isnan(state[0])) { + return; + } + for (size_t i = 0; i < N; i++) { + state[0+2*i] = state[0+2*i] + (b_old[0] - b_new[0])*double(last_y[i]); + state[1+2*i] = state[1+2*i] + (b_old[1] - b_new[1] - a_old[0] + a_new[0])*double(last_y[i]); + } +} + +vqf_real_t VQF::filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]) +{ + // difference equations based on scipy.signal.lfilter documentation + // assumes that a0 == 1.0 + double y = b[0]*double(x) + state[0]; + state[0] = b[1]*double(x) - a[0]*y + state[1]; + state[1] = b[2]*double(x) - a[1]*y; + return y; +} + +void VQF::filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3], + const double a[2], double state[], vqf_real_t out[]) +{ + assert(N>=2); + + // to avoid depending on a single sample, average the first samples (for duration tau) + // and then use this average to calculate the filter initial state + if (std::isnan(state[0])) { // initialization phase + if (std::isnan(state[1])) { // first sample + state[1] = 0; // state[1] is used to store the sample count + for(size_t i = 0; i < N; i++) { + state[2+i] = 0; // state[2+i] is used to store the sum + } + } + state[1]++; + for (size_t i = 0; i < N; i++) { + state[2+i] += double(x[i]); + out[i] = state[2+i]/state[1]; + } + if (vqf_real_t(state[1])*Ts >= tau) { + for(size_t i = 0; i < N; i++) { + filterInitialState(out[i], b, a, state+2*i); + } + } + return; + } + + for (size_t i = 0; i < N; i++) { + out[i] = filterStep(x[i], b, a, state+2*i); + } +} + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION +void VQF::matrix3SetToScaledIdentity(vqf_real_t scale, vqf_real_t out[9]) +{ + out[0] = scale; + out[1] = 0.0; + out[2] = 0.0; + out[3] = 0.0; + out[4] = scale; + out[5] = 0.0; + out[6] = 0.0; + out[7] = 0.0; + out[8] = scale; +} + +void VQF::matrix3Multiply(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]) +{ + vqf_real_t tmp[9]; + tmp[0] = in1[0]*in2[0] + in1[1]*in2[3] + in1[2]*in2[6]; + tmp[1] = in1[0]*in2[1] + in1[1]*in2[4] + in1[2]*in2[7]; + tmp[2] = in1[0]*in2[2] + in1[1]*in2[5] + in1[2]*in2[8]; + tmp[3] = in1[3]*in2[0] + in1[4]*in2[3] + in1[5]*in2[6]; + tmp[4] = in1[3]*in2[1] + in1[4]*in2[4] + in1[5]*in2[7]; + tmp[5] = in1[3]*in2[2] + in1[4]*in2[5] + in1[5]*in2[8]; + tmp[6] = in1[6]*in2[0] + in1[7]*in2[3] + in1[8]*in2[6]; + tmp[7] = in1[6]*in2[1] + in1[7]*in2[4] + in1[8]*in2[7]; + tmp[8] = in1[6]*in2[2] + in1[7]*in2[5] + in1[8]*in2[8]; + std::copy(tmp, tmp+9, out); +} + +void VQF::matrix3MultiplyTpsFirst(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]) +{ + vqf_real_t tmp[9]; + tmp[0] = in1[0]*in2[0] + in1[3]*in2[3] + in1[6]*in2[6]; + tmp[1] = in1[0]*in2[1] + in1[3]*in2[4] + in1[6]*in2[7]; + tmp[2] = in1[0]*in2[2] + in1[3]*in2[5] + in1[6]*in2[8]; + tmp[3] = in1[1]*in2[0] + in1[4]*in2[3] + in1[7]*in2[6]; + tmp[4] = in1[1]*in2[1] + in1[4]*in2[4] + in1[7]*in2[7]; + tmp[5] = in1[1]*in2[2] + in1[4]*in2[5] + in1[7]*in2[8]; + tmp[6] = in1[2]*in2[0] + in1[5]*in2[3] + in1[8]*in2[6]; + tmp[7] = in1[2]*in2[1] + in1[5]*in2[4] + in1[8]*in2[7]; + tmp[8] = in1[2]*in2[2] + in1[5]*in2[5] + in1[8]*in2[8]; + std::copy(tmp, tmp+9, out); +} + +void VQF::matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]) +{ + vqf_real_t tmp[9]; + tmp[0] = in1[0]*in2[0] + in1[1]*in2[1] + in1[2]*in2[2]; + tmp[1] = in1[0]*in2[3] + in1[1]*in2[4] + in1[2]*in2[5]; + tmp[2] = in1[0]*in2[6] + in1[1]*in2[7] + in1[2]*in2[8]; + tmp[3] = in1[3]*in2[0] + in1[4]*in2[1] + in1[5]*in2[2]; + tmp[4] = in1[3]*in2[3] + in1[4]*in2[4] + in1[5]*in2[5]; + tmp[5] = in1[3]*in2[6] + in1[4]*in2[7] + in1[5]*in2[8]; + tmp[6] = in1[6]*in2[0] + in1[7]*in2[1] + in1[8]*in2[2]; + tmp[7] = in1[6]*in2[3] + in1[7]*in2[4] + in1[8]*in2[5]; + tmp[8] = in1[6]*in2[6] + in1[7]*in2[7] + in1[8]*in2[8]; + std::copy(tmp, tmp+9, out); +} + +bool VQF::matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9]) +{ + // in = [a b c; d e f; g h i] + double A = double(in[4]*in[8] - in[5]*in[7]); // (e*i - f*h) + double D = double(in[2]*in[7] - in[1]*in[8]); // -(b*i - c*h) + double G = double(in[1]*in[5] - in[2]*in[4]); // (b*f - c*e) + double B = double(in[5]*in[6] - in[3]*in[8]); // -(d*i - f*g) + double E = double(in[0]*in[8] - in[2]*in[6]); // (a*i - c*g) + double H = double(in[2]*in[3] - in[0]*in[5]); // -(a*f - c*d) + double C = double(in[3]*in[7] - in[4]*in[6]); // (d*h - e*g) + double F = double(in[1]*in[6] - in[0]*in[7]); // -(a*h - b*g) + double I = double(in[0]*in[4] - in[1]*in[3]); // (a*e - b*d) + + double det = double(in[0])*A + double(in[1])*B + double(in[2])*C; // a*A + b*B + c*C; + + if (det >= double(-EPS) && det <= double(EPS)) { + std::fill(out, out+9, 0); + return false; + } + + // out = [A D G; B E H; C F I]/det + out[0] = A/det; + out[1] = D/det; + out[2] = G/det; + out[3] = B/det; + out[4] = E/det; + out[5] = H/det; + out[6] = C/det; + out[7] = F/det; + out[8] = I/det; + + return true; +} +#endif + +void VQF::setup() +{ + assert(coeffs.gyrTs > 0); + assert(coeffs.accTs > 0); + assert(coeffs.magTs > 0); + + filterCoeffs(params.tauAcc, coeffs.accTs, coeffs.accLpB, coeffs.accLpA); + + coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs); + + coeffs.biasP0 = square(params.biasSigmaInit*vqf_real_t(100.0)); + // the system noise increases the variance from 0 to (0.1 °/s)^2 in biasForgettingTime seconds + coeffs.biasV = square(0.1*100.0)*coeffs.accTs/params.biasForgettingTime; + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + vqf_real_t pMotion = square(params.biasSigmaMotion*vqf_real_t(100.0)); + coeffs.biasMotionW = square(pMotion) / coeffs.biasV + pMotion; + coeffs.biasVerticalW = coeffs.biasMotionW / (std::max)(params.biasVerticalForgettingFactor, vqf_real_t(1e-10)); +#endif + + vqf_real_t pRest = square(params.biasSigmaRest*vqf_real_t(100.0)); + coeffs.biasRestW = square(pRest) / coeffs.biasV + pRest; + + filterCoeffs(params.restFilterTau, coeffs.gyrTs, coeffs.restGyrLpB, coeffs.restGyrLpA); + filterCoeffs(params.restFilterTau, coeffs.accTs, coeffs.restAccLpB, coeffs.restAccLpA); + + coeffs.kMagRef = gainFromTau(params.magRefTau, coeffs.magTs); + if (params.magCurrentTau > 0) { + filterCoeffs(params.magCurrentTau, coeffs.magTs, coeffs.magNormDipLpB, coeffs.magNormDipLpA); + } else { + std::fill(coeffs.magNormDipLpB, coeffs.magNormDipLpB + 3, NaN); + std::fill(coeffs.magNormDipLpA, coeffs.magNormDipLpA + 2, NaN); + } + + resetState(); +} diff --git a/ground_station/lib/VQF/src/vqf.hpp b/ground_station/lib/VQF/src/vqf.hpp new file mode 100644 index 00000000..94866044 --- /dev/null +++ b/ground_station/lib/VQF/src/vqf.hpp @@ -0,0 +1,1067 @@ +// SPDX-FileCopyrightText: 2021 Daniel Laidig +// +// SPDX-License-Identifier: MIT + +#ifndef VQF_HPP +#define VQF_HPP + +#include + +// #define VQF_SINGLE_PRECISION +// #define VQF_NO_MOTION_BIAS_ESTIMATION + +/** + * @brief Typedef for the floating-point data type used for most operations. + * + * By default, all floating-point calculations are performed using `double`. Set the `VQF_SINGLE_PRECISION` define to + * change this type to `float`. Note that the Butterworth filter implementation will always use double precision as + * using floats can cause numeric issues. + */ +#ifndef VQF_SINGLE_PRECISION +typedef double vqf_real_t; +#else +typedef float vqf_real_t; +#endif + +/** + * @brief Struct containing all tuning parameters used by the VQF class. + * + * The parameters influence the behavior of the algorithm and are independent of the sampling rate of the IMU data. The + * constructor sets all parameters to the default values. + * + * The parameters #motionBiasEstEnabled, #restBiasEstEnabled, and #magDistRejectionEnabled can be used to enable/disable + * the main features of the VQF algorithm. The time constants #tauAcc and #tauMag can be tuned to change the trust on + * the accelerometer and magnetometer measurements, respectively. The remaining parameters influence bias estimation + * and magnetometer rejection. + */ +struct VQFParams +{ + /** + * @brief Constructor that initializes the struct with the default parameters. + */ + VQFParams(); + + /** + * @brief Time constant \f$\tau_\mathrm{acc}\f$ for accelerometer low-pass filtering in seconds. + * + * Small values for \f$\tau_\mathrm{acc}\f$ imply trust on the accelerometer measurements and while large values of + * \f$\tau_\mathrm{acc}\f$ imply trust on the gyroscope measurements. + * + * The time constant \f$\tau_\mathrm{acc}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the + * second-order Butterworth low-pass filter as follows: \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau_\mathrm{acc}}\f$. + * + * Default value: 3.0 s + */ + vqf_real_t tauAcc; + /** + * @brief Time constant \f$\tau_\mathrm{mag}\f$ for magnetometer update in seconds. + * + * Small values for \f$\tau_\mathrm{mag}\f$ imply trust on the magnetometer measurements and while large values of + * \f$\tau_\mathrm{mag}\f$ imply trust on the gyroscope measurements. + * + * The time constant \f$\tau_\mathrm{mag}\f$ corresponds to the cutoff frequency \f$f_\mathrm{c}\f$ of the + * first-order low-pass filter for the heading correction as follows: + * \f$f_\mathrm{c} = \frac{1}{2\pi\tau_\mathrm{mag}}\f$. + * + * Default value: 9.0 s + */ + vqf_real_t tauMag; + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Enables gyroscope bias estimation during motion phases. + * + * If set to true (default), gyroscope bias is estimated based on the inclination correction only, i.e. without + * using magnetometer measurements. + */ + bool motionBiasEstEnabled; +#endif + /** + * @brief Enables rest detection and gyroscope bias estimation during rest phases. + * + * If set to true (default), phases in which the IMU is at rest are detected. During rest, the gyroscope bias + * is estimated from the low-pass filtered gyroscope readings. + */ + bool restBiasEstEnabled; + /** + * @brief Enables magnetic disturbance detection and magnetic disturbance rejection. + * + * If set to true (default), the magnetic field is analyzed. For short disturbed phases, the magnetometer-based + * correction is disabled totally. If the magnetic field is always regarded as disturbed or if the duration of + * the disturbances exceeds #magMaxRejectionTime, magnetometer-based updates are performed, but with an increased + * time constant. + */ + bool magDistRejectionEnabled; + + /** + * @brief Standard deviation of the initial bias estimation uncertainty (in degrees per second). + * + * Default value: 0.5 °/s + */ + vqf_real_t biasSigmaInit; + /** + * @brief Time in which the bias estimation uncertainty increases from 0 °/s to 0.1 °/s (in seconds). + * + * This value determines the system noise assumed by the Kalman filter. + * + * Default value: 100.0 s + */ + vqf_real_t biasForgettingTime; + /** + * @brief Maximum expected gyroscope bias (in degrees per second). + * + * This value is used to clip the bias estimate and the measurement error in the bias estimation update step. It is + * further used by the rest detection algorithm in order to not regard measurements with a large but constant + * angular rate as rest. + * + * Default value: 2.0 °/s + */ + vqf_real_t biasClip; +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Standard deviation of the converged bias estimation uncertainty during motion (in degrees per second). + * + * This value determines the trust on motion bias estimation updates. A small value leads to fast convergence. + * + * Default value: 0.1 °/s + */ + vqf_real_t biasSigmaMotion; + /** + * @brief Forgetting factor for unobservable bias in vertical direction during motion. + * + * As magnetometer measurements are deliberately not used during motion bias estimation, gyroscope bias is not + * observable in vertical direction. This value is the relative weight of an artificial zero measurement that + * ensures that the bias estimate in the unobservable direction will eventually decay to zero. + * + * Default value: 0.0001 + */ + vqf_real_t biasVerticalForgettingFactor; +#endif + /** + * @brief Standard deviation of the converged bias estimation uncertainty during rest (in degrees per second). + * + * This value determines the trust on rest bias estimation updates. A small value leads to fast convergence. + * + * Default value: 0.03 ° + */ + vqf_real_t biasSigmaRest; + + /** + * @brief Time threshold for rest detection (in seconds). + * + * Rest is detected when the measurements have been close to the low-pass filtered reference for the given time. + * + * Default value: 1.5 s + */ + vqf_real_t restMinT; + /** + * @brief Time constant for the low-pass filter used in rest detection (in seconds). + * + * This time constant characterizes a second-order Butterworth low-pass filter used to obtain the reference for + * rest detection. + * + * Default value: 0.5 s + */ + vqf_real_t restFilterTau; + /** + * @brief Angular velocity threshold for rest detection (in °/s). + * + * For rest to be detected, the norm of the deviation between measurement and reference must be below the given + * threshold. (Furthermore, the absolute value of each component must be below #biasClip). + * + * Default value: 2.0 °/s + */ + vqf_real_t restThGyr; + /** + * @brief Acceleration threshold for rest detection (in m/s²). + * + * For rest to be detected, the norm of the deviation between measurement and reference must be below the given + * threshold. + * + * Default value: 0.5 m/s² + */ + vqf_real_t restThAcc; + + /** + * @brief Time constant for current norm/dip value in magnetic disturbance detection (in seconds). + * + * This (very fast) low-pass filter is intended to provide additional robustness when the magnetometer measurements + * are noisy or not sampled perfectly in sync with the gyroscope measurements. Set to -1 to disable the low-pass + * filter and directly use the magnetometer measurements. + * + * Default value: 0.05 s + */ + vqf_real_t magCurrentTau; + /** + * @brief Time constant for the adjustment of the magnetic field reference (in seconds). + * + * This adjustment allows the reference estimate to converge to the observed undisturbed field. + * + * Default value: 20.0 s + */ + vqf_real_t magRefTau; + /** + * @brief Relative threshold for the magnetic field strength for magnetic disturbance detection. + * + * This value is relative to the reference norm. + * + * Default value: 0.1 (10%) + */ + vqf_real_t magNormTh; + /** + * @brief Threshold for the magnetic field dip angle for magnetic disturbance detection (in degrees). + * + * Default vaule: 10 ° + */ + vqf_real_t magDipTh; + /** + * @brief Duration after which to accept a different homogeneous magnetic field (in seconds). + * + * A different magnetic field reference is accepted as the new field when the measurements are within the thresholds + * #magNormTh and #magDipTh for the given time. Additionally, only phases with sufficient movement, specified by + * #magNewMinGyr, count. + * + * Default value: 20.0 + */ + vqf_real_t magNewTime; + /** + * @brief Duration after which to accept a homogeneous magnetic field for the first time (in seconds). + * + * This value is used instead of #magNewTime when there is no current estimate in order to allow for the initial + * magnetic field reference to be obtained faster. + * + * Default value: 5.0 + */ + vqf_real_t magNewFirstTime; + /** + * @brief Minimum angular velocity needed in order to count time for new magnetic field acceptance (in °/s). + * + * Durations for which the angular velocity norm is below this threshold do not count towards reaching #magNewTime. + * + * Default value: 20.0 °/s + */ + vqf_real_t magNewMinGyr; + /** + * @brief Minimum duration within thresholds after which to regard the field as undisturbed again (in seconds). + * + * Default value: 0.5 s + */ + vqf_real_t magMinUndisturbedTime; + /** + * @brief Maximum duration of full magnetic disturbance rejection (in seconds). + * + * For magnetic disturbances up to this duration, heading correction is fully disabled and heading changes are + * tracked by gyroscope only. After this duration (or for many small disturbed phases without sufficient time in the + * undisturbed field in between), the heading correction is performed with an increased time constant (see + * #magRejectionFactor). + * + * Default value: 60.0 s + */ + vqf_real_t magMaxRejectionTime; + /** + * @brief Factor by which to slow the heading correction during long disturbed phases. + * + * After #magMaxRejectionTime of full magnetic disturbance rejection, heading correction is performed with an + * increased time constant. This parameter (approximately) specifies the factor of the increase. + * + * Furthermore, after spending #magMaxRejectionTime/#magRejectionFactor seconds in an undisturbed magnetic field, + * the time is reset and full magnetic disturbance rejection will be performed for up to #magMaxRejectionTime again. + * + * Default value: 2.0 + */ + vqf_real_t magRejectionFactor; +}; + +/** + * @brief Struct containing the filter state of the VQF class. + * + * The relevant parts of the state can be accessed via functions of the VQF class, e.g. VQF::getQuat6D(), + * VQF::getQuat9D(), VQF::getGyrBiasEstimate(), VQF::setGyrBiasEstimate(), VQF::getRestDetected() and + * VQF::getMagDistDetected(). To reset the state to the initial values, use VQF::resetState(). + * + * Direct access to the full state is typically not needed but can be useful in some cases, e.g. for debugging. For this + * purpose, the state can be accessed by VQF::getState() and set by VQF::setState(). + */ +struct VQFState { + /** + * @brief Angular velocity strapdown integration quaternion \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. + */ + vqf_real_t gyrQuat[4]; + /** + * @brief Inclination correction quaternion \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$. + */ + vqf_real_t accQuat[4]; + /** + * @brief Heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$. + * + * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix}^T\f$. + */ + vqf_real_t delta; + /** + * @brief True if it has been detected that the IMU is currently at rest. + * + * Used to switch between rest and motion gyroscope bias estimation. + */ + bool restDetected; + /** + * @brief True if magnetic disturbances have been detected. + */ + bool magDistDetected; + + /** + * @brief Last low-pass filtered acceleration in the \f$\mathcal{I}_i\f$ frame. + */ + vqf_real_t lastAccLp[3]; + /** + * @brief Internal low-pass filter state for #lastAccLp. + */ + double accLpState[3*2]; + /** + * @brief Last inclination correction angular rate. + * + * Change to inclination correction quaternion \f$^{\mathcal{I}_i}_{\mathcal{E}_i}\mathbf{q}\f$ performed in the + * last accelerometer update, expressed as an angular rate (in rad/s). + */ + vqf_real_t lastAccCorrAngularRate; + + /** + * @brief Gain used for heading correction to ensure fast initial convergence. + * + * This value is used as the gain for heading correction in the beginning if it is larger than the normal filter + * gain. It is initialized to 1 and then updated to 0.5, 0.33, 0.25, ... After VQFParams::tauMag seconds, it is + * set to zero. + */ + vqf_real_t kMagInit; + /** + * @brief Last heading disagreement angle. + * + * Disagreement between the heading \f$\hat\delta\f$ estimated from the last magnetometer sample and the state + * \f$\delta\f$ (in rad). + */ + vqf_real_t lastMagDisAngle; + /** + * @brief Last heading correction angular rate. + * + * Change to heading \f$\delta\f$ performed in the last magnetometer update, + * expressed as an angular rate (in rad/s). + */ + vqf_real_t lastMagCorrAngularRate; + + /** + * @brief Current gyroscope bias estimate (in rad/s). + */ + vqf_real_t bias[3]; +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Covariance matrix of the gyroscope bias estimate. + * + * The 3x3 matrix is stored in row-major order. Note that for numeric reasons the internal unit used is 0.01 °/s, + * i.e. to get the standard deviation in degrees per second use \f$\sigma = \frac{\sqrt{p_{ii}}}{100}\f$. + */ + vqf_real_t biasP[9]; +#else + // If only rest gyr bias estimation is enabled, P and K of the KF are always diagonal + // and matrix inversion is not needed. If motion bias estimation is disabled at compile + // time, storing the full P matrix is not necessary. + vqf_real_t biasP; +#endif + +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Internal state of the Butterworth low-pass filter for the rotation matrix coefficients used in motion + * bias estimation. + */ + double motionBiasEstRLpState[9*2]; + /** + * @brief Internal low-pass filter state for the rotated bias estimate used in motion bias estimation. + */ + double motionBiasEstBiasLpState[2*2]; +#endif + /** + * @brief Last (squared) deviations from the reference of the last sample used in rest detection. + * + * Looking at those values can be useful to understand how rest detection is working and which thresholds are + * suitable. The array contains the last values for gyroscope and accelerometer in the respective + * units. Note that the values are squared. + * + * The method VQF::getRelativeRestDeviations() provides an easier way to obtain and interpret those values. + */ + vqf_real_t restLastSquaredDeviations[2]; + /** + * @brief The current duration for which all sensor readings are within the rest detection thresholds. + * + * Rest is detected if this value is larger or equal to VQFParams::restMinT. + */ + vqf_real_t restT; + /** + * @brief Last low-pass filtered gyroscope measurement used as the reference for rest detection. + * + * Note that this value is also used for gyroscope bias estimation when rest is detected. + */ + vqf_real_t restLastGyrLp[3]; + /** + * @brief Internal low-pass filter state for #restLastGyrLp. + */ + double restGyrLpState[3*2]; + /** + * @brief Last low-pass filtered accelerometer measurement used as the reference for rest detection. + */ + vqf_real_t restLastAccLp[3]; + /** + * @brief Internal low-pass filter state for #restLastAccLp. + */ + double restAccLpState[3*2]; + + /** + * @brief Norm of the currently accepted magnetic field reference. + * + * A value of -1 indicates that no homogeneous field is found yet. + */ + vqf_real_t magRefNorm; + /** + * @brief Dip angle of the currently accepted magnetic field reference. + */ + vqf_real_t magRefDip; + /** + * @brief The current duration for which the current norm and dip are close to the reference. + * + * The magnetic field is regarded as undisturbed when this value reaches VQFParams::magMinUndisturbedTime. + */ + vqf_real_t magUndisturbedT; + /** + * @brief The current duration for which the magnetic field was rejected. + * + * If the magnetic field is disturbed and this value is smaller than VQFParams::magMaxRejectionTime, heading + * correction updates are fully disabled. + */ + vqf_real_t magRejectT; + /** + * @brief Norm of the alternative magnetic field reference currently being evaluated. + */ + vqf_real_t magCandidateNorm; + /** + * @brief Dip angle of the alternative magnetic field reference currently being evaluated. + */ + vqf_real_t magCandidateDip; + /** + * @brief The current duration for which the norm and dip are close to the candidate. + * + * If this value exceeds VQFParams::magNewTime (or VQFParams::magNewFirstTime if #magRefNorm < 0), the current + * candidate is accepted as the new reference. + */ + vqf_real_t magCandidateT; + /** + * @brief Norm and dip angle of the current magnetometer measurements. + * + * Slightly low-pass filtered, see VQFParams::magCurrentTau. + */ + vqf_real_t magNormDip[2]; + /** + * @brief Internal low-pass filter state for the current norm and dip angle. + */ + double magNormDipLpState[2*2]; +}; + +/** + * @brief Struct containing coefficients used by the VQF class. + * + * Coefficients are values that depend on the parameters and the sampling times, but do not change during update steps. + * They are calculated in VQF::setup(). + */ +struct VQFCoefficients +{ + /** + * @brief Sampling time of the gyroscope measurements (in seconds). + */ + vqf_real_t gyrTs; + /** + * @brief Sampling time of the accelerometer measurements (in seconds). + */ + vqf_real_t accTs; + /** + * @brief Sampling time of the magnetometer measurements (in seconds). + */ + vqf_real_t magTs; + + /** + * @brief Numerator coefficients of the acceleration low-pass filter. + * + * The array contains \f$\begin{bmatrix}b_0 & b_1 & b_2\end{bmatrix}\f$. + */ + double accLpB[3]; + /** + * @brief Denominator coefficients of the acceleration low-pass filter. + * + * The array contains \f$\begin{bmatrix}a_1 & a_2\end{bmatrix}\f$ and \f$a_0=1\f$. + */ + double accLpA[2]; + + /** + * @brief Gain of the first-order filter used for heading correction. + */ + vqf_real_t kMag; + + /** + * @brief Variance of the initial gyroscope bias estimate. + */ + vqf_real_t biasP0; + /** + * @brief System noise variance used in gyroscope bias estimation. + */ + vqf_real_t biasV; +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Measurement noise variance for the motion gyroscope bias estimation update. + */ + vqf_real_t biasMotionW; + /** + * @brief Measurement noise variance for the motion gyroscope bias estimation update in vertical direction. + */ + vqf_real_t biasVerticalW; +#endif + /** + * @brief Measurement noise variance for the rest gyroscope bias estimation update. + */ + vqf_real_t biasRestW; + + /** + * @brief Numerator coefficients of the gyroscope measurement low-pass filter for rest detection. + */ + double restGyrLpB[3]; + /** + * @brief Denominator coefficients of the gyroscope measurement low-pass filter for rest detection. + */ + double restGyrLpA[2]; + /** + * @brief Numerator coefficients of the accelerometer measurement low-pass filter for rest detection. + */ + double restAccLpB[3]; + /** + * @brief Denominator coefficients of the accelerometer measurement low-pass filter for rest detection. + */ + double restAccLpA[2]; + + /** + * @brief Gain of the first-order filter used for to update the magnetic field reference and candidate. + */ + vqf_real_t kMagRef; + /** + * @brief Numerator coefficients of the low-pass filter for the current magnetic norm and dip. + */ + double magNormDipLpB[3]; + /** + * @brief Denominator coefficients of the low-pass filter for the current magnetic norm and dip. + */ + double magNormDipLpA[2]; +}; + +/** + * @brief A Versatile Quaternion-based Filter for IMU Orientation Estimation. + * + * \rst + * This class implements the orientation estimation filter described in the following publication: + * + * + * D. Laidig and T. Seel. "VQF: Highly Accurate IMU Orientation Estimation with Bias Estimation and Magnetic + * Disturbance Rejection." Information Fusion 2023, 91, 187--204. + * `doi:10.1016/j.inffus.2022.10.014 `_. + * [Accepted manuscript available at `arXiv:2203.17024 `_.] + * + * The filter can perform simultaneous 6D (magnetometer-free) and 9D (gyr+acc+mag) sensor fusion and can also be used + * without magnetometer data. It performs rest detection, gyroscope bias estimation during rest and motion, and magnetic + * disturbance detection and rejection. Different sampling rates for gyroscopes, accelerometers, and magnetometers are + * supported as well. While in most cases, the defaults will be reasonable, the algorithm can be influenced via a + * number of tuning parameters. + * + * To use this C++ implementation, + * + * 1. create a instance of the class and provide the sampling time and, optionally, parameters + * 2. for every sample, call one of the update functions to feed the algorithm with IMU data + * 3. access the estimation results with :meth:`getQuat6D() `, :meth:`getQuat9D() ` and + * the other getter methods. + * + * If the full data is available in (row-major) data buffers, you can use :meth:`updateBatch() `. + * + * This class is the main C++ implementation of the algorithm. Depending on use case and programming language of choice, + * the following alternatives might be useful: + * + * +------------------------+--------------------------+--------------------------+---------------------------+ + * | | Full Version | Basic Version | Offline Version | + * | | | | | + * +========================+==========================+==========================+===========================+ + * | **C++** | **VQF (this class)** | :cpp:class:`BasicVQF` | :cpp:func:`offlineVQF` | + * +------------------------+--------------------------+--------------------------+---------------------------+ + * | **Python/C++ (fast)** | :py:class:`vqf.VQF` | :py:class:`vqf.BasicVQF` | :py:meth:`vqf.offlineVQF` | + * +------------------------+--------------------------+--------------------------+---------------------------+ + * | **Pure Python (slow)** | :py:class:`vqf.PyVQF` | -- | -- | + * +------------------------+--------------------------+--------------------------+---------------------------+ + * | **Pure Matlab (slow)** | :mat:class:`VQF.m ` | -- | -- | + * +------------------------+--------------------------+--------------------------+---------------------------+ + * \endrst + */ + +class VQF +{ +public: + /** + * Initializes the object with default parameters. + * + * In the most common case (using the default parameters and all data being sampled with the same frequency, + * create the class like this: + * \rst + * .. code-block:: c++ + * + * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * \endrst + * + * @param gyrTs sampling time of the gyroscope measurements in seconds + * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1) + * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1) + * + */ + VQF(vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0); + /** + * @brief Initializes the object with custom parameters. + * + * Example code to create an object with magnetic disturbance rejection disabled: + * \rst + * .. code-block:: c++ + * + * VQFParams params; + * params.magDistRejectionEnabled = false; + * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * \endrst + * + * @param params VQFParams struct containing the desired parameters + * @param gyrTs sampling time of the gyroscope measurements in seconds + * @param accTs sampling time of the accelerometer measurements in seconds (the value of `gyrTs` is used if set to -1) + * @param magTs sampling time of the magnetometer measurements in seconds (the value of `gyrTs` is used if set to -1) + */ + VQF(const VQFParams& params, vqf_real_t gyrTs, vqf_real_t accTs=-1.0, vqf_real_t magTs=-1.0); + + /** + * @brief Performs gyroscope update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have + * different sampling rates. Otherwise, simply use #update(). + * + * @param gyr gyroscope measurement in rad/s + */ + void updateGyr(const vqf_real_t gyr[3]); + /** + * @brief Performs accelerometer update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have + * different sampling rates. Otherwise, simply use #update(). + * + * Should be called after #updateGyr and before #updateMag. + * + * @param acc accelerometer measurement in m/s² + */ + void updateAcc(const vqf_real_t acc[3]); + /** + * @brief Performs magnetometer update step. + * + * It is only necessary to call this function directly if gyroscope, accelerometers and magnetometers have + * different sampling rates. Otherwise, simply use #update(). + * + * Should be called after #updateAcc. + * + * @param mag magnetometer measurement in arbitrary units + */ + void updateMag(const vqf_real_t mag[3]); + /** + * @brief Performs filter update step for one sample (magnetometer-free). + * @param gyr gyroscope measurement in rad/s + * @param acc accelerometer measurement in m/s² + */ + void update(const vqf_real_t gyr[3], const vqf_real_t acc[3]); + /** + * @brief Performs filter update step for one sample (with magnetometer measurement). + * @param gyr gyroscope measurement in rad/s + * @param acc accelerometer measurement in m/s² + * @param mag magnetometer measurement in arbitrary units + */ + void update(const vqf_real_t gyr[3], const vqf_real_t acc[3], const vqf_real_t mag[3]); + + /** + * @brief Performs batch update for multiple samples at once. + * + * In order to use this function, the input data must be available in an array in row-major order. A null pointer + * can be passed for mag in order to skip the magnetometer update. All data must have the same sampling rate. + * + * The output pointer arguments must be null pointers or point to sufficiently large data buffers. + * + * Example usage: + * \rst + * .. code-block:: c++ + * + * int N = 1000; + * double gyr[N*3]; // fill with gyroscope measurements in rad/s + * double acc[N*3]; // fill will accelerometer measurements in m/s² + * double mag[N*3]; // fill with magnetometer measurements (arbitrary units) + * double quat9D[N*4]; // output buffer + * + * VQF vqf(0.01); // 0.01 s sampling time, i.e. 100 Hz + * vqf.updateBatch(gyr, acc, mag, N, nullptr, quat9D, nullptr, nullptr, nullptr, nullptr, nullptr); + * \endrst + * + * @param gyr gyroscope measurement in rad/s (N*3 elements, must not be null) + * @param acc accelerometer measurement in m/s² (N*3 elements, must not be null) + * @param mag magnetometer measurement in arbitrary units (N*3 elements, can be a null pointer) + * @param N number of samples + * @param out6D output buffer for the 6D quaternion (N*4 elements, can be a null pointer) + * @param out9D output buffer for the 9D quaternion (N*4 elements, can be a null pointer) + * @param outDelta output buffer for heading difference angle (N elements, can be a null pointer) + * @param outBias output buffer for the gyroscope bias estimate (N*3 elements, can be a null pointer) + * @param outBiasSigma output buffer for the bias estimation uncertainty (N elements, can be a null pointer) + * @param outRest output buffer for the rest detection state (N elements, can be a null pointer) + * @param outMagDist output buffer for the magnetic disturbance state (N elements, can be a null pointer) + */ + void updateBatch(const vqf_real_t gyr[], const vqf_real_t acc[], const vqf_real_t mag[], size_t N, + vqf_real_t out6D[], vqf_real_t out9D[], vqf_real_t outDelta[], vqf_real_t outBias[], + vqf_real_t outBiasSigma[], bool outRest[], bool outMagDist[]); + + /** + * @brief Returns the angular velocity strapdown integration quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{I}_i}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat3D(vqf_real_t out[4]) const; + /** + * @brief Returns the 6D (magnetometer-free) orientation quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{E}_i}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat6D(vqf_real_t out[4]) const; + /** + * @brief Returns the 9D (with magnetometers) orientation quaternion + * \f$^{\mathcal{S}_i}_{\mathcal{E}}\mathbf{q}\f$. + * @param out output array for the quaternion + */ + void getQuat9D(vqf_real_t out[4]) const; + /** + * @brief Returns the heading difference \f$\delta\f$ between \f$\mathcal{E}_i\f$ and \f$\mathcal{E}\f$. + * + * \f$^{\mathcal{E}_i}_{\mathcal{E}}\mathbf{q} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix}^T\f$. + * + * @return delta angle in rad (VQFState::delta) + */ + vqf_real_t getDelta() const; + + /** + * @brief Returns the current gyroscope bias estimate and the uncertainty. + * + * The returned standard deviation sigma represents the estimation uncertainty in the worst direction and is based + * on an upper bound of the largest eigenvalue of the covariance matrix. + * + * @param out output array for the gyroscope bias estimate (rad/s) + * @return standard deviation sigma of the estimation uncertainty (rad/s) + */ + vqf_real_t getBiasEstimate(vqf_real_t out[3]) const; + /** + * @brief Sets the current gyroscope bias estimate and the uncertainty. + * + * If a value for the uncertainty sigma is given, the covariance matrix is set to a corresponding scaled identity + * matrix. + * + * @param bias gyroscope bias estimate (rad/s) + * @param sigma standard deviation of the estimation uncertainty (rad/s) - set to -1 (default) in order to not + * change the estimation covariance matrix + */ + void setBiasEstimate(vqf_real_t bias[3], vqf_real_t sigma=-1.0); + /** + * @brief Returns true if rest was detected. + */ + bool getRestDetected() const; + /** + * @brief Returns true if a disturbed magnetic field was detected. + */ + bool getMagDistDetected() const; + /** + * @brief Returns the relative deviations used in rest detection. + * + * Looking at those values can be useful to understand how rest detection is working and which thresholds are + * suitable. The output array is filled with the last values for gyroscope and accelerometer, + * relative to the threshold. In order for rest to be detected, both values must stay below 1. + * + * @param out output array of size 2 for the relative rest deviations + */ + void getRelativeRestDeviations(vqf_real_t out[2]) const; + /** + * @brief Returns the norm of the currently accepted magnetic field reference. + */ + vqf_real_t getMagRefNorm() const; + /** + * @brief Returns the dip angle of the currently accepted magnetic field reference. + */ + vqf_real_t getMagRefDip() const; + /** + * @brief Overwrites the current magnetic field reference. + * @param norm norm of the magnetic field reference + * @param dip dip angle of the magnetic field reference + */ + void setMagRef(vqf_real_t norm, vqf_real_t dip); + + /** + * @brief Sets the time constant for accelerometer low-pass filtering. + * + * For more details, see VQFParams.tauAcc. + * + * @param tauAcc time constant \f$\tau_\mathrm{acc}\f$ in seconds + */ + void setTauAcc(vqf_real_t tauAcc); + /** + * @brief Sets the time constant for the magnetometer update. + * + * For more details, see VQFParams.tauMag. + * + * @param tauMag time constant \f$\tau_\mathrm{mag}\f$ in seconds + */ + void setTauMag(vqf_real_t tauMag); +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Enables/disabled gyroscope bias estimation during motion. + */ + void setMotionBiasEstEnabled(bool enabled); +#endif + /** + * @brief Enables/disables rest detection and bias estimation during rest. + */ + void setRestBiasEstEnabled(bool enabled); + /** + * @brief Enables/disables magnetic disturbance detection and rejection. + */ + void setMagDistRejectionEnabled(bool enabled); + /** + * @brief Sets the current thresholds for rest detection. + * + * For details about the parameters, see VQFParams.restThGyr and VQFParams.restThAcc. + */ + void setRestDetectionThresholds(vqf_real_t thGyr, vqf_real_t thAcc); + + /** + * @brief Returns the current parameters. + */ + const VQFParams& getParams() const; + /** + * @brief Returns the coefficients used by the algorithm. + */ + const VQFCoefficients& getCoeffs() const; + /** + * @brief Returns the current state. + */ + const VQFState& getState() const; + /** + * @brief Overwrites the current state. + * + * This method allows to set a completely arbitrary filter state and is intended for debugging purposes. In + * combination with #getState, individual elements of the state can be modified. + * + * @param state A VQFState struct containing the new state + */ + void setState(const VQFState& state); + /** + * @brief Resets the state to the default values at initialization. + * + * Resetting the state is equivalent to creating a new instance of this class. + */ + void resetState(); + + /** + * @brief Performs quaternion multiplication (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}_1 \otimes \mathbf{q}_2\f$). + */ + static void quatMultiply(const vqf_real_t q1[4], const vqf_real_t q2[4], vqf_real_t out[4]); + /** + * @brief Calculates the quaternion conjugate (\f$\mathbf{q}_\mathrm{out} = \mathbf{q}^*\f$). + */ + static void quatConj(const vqf_real_t q[4], vqf_real_t out[4]); + /** + * @brief Sets the output quaternion to the identity quaternion (\f$\mathbf{q}_\mathrm{out} = + * \begin{bmatrix}1 & 0 & 0 & 0\end{bmatrix}\f$). + */ + static void quatSetToIdentity(vqf_real_t out[4]); + /** + * @brief Applies a heading rotation by the angle delta (in rad) to a quaternion. + * + * \f$\mathbf{q}_\mathrm{out} = \begin{bmatrix}\cos\frac{\delta}{2} & 0 & 0 & + * \sin\frac{\delta}{2}\end{bmatrix} \otimes \mathbf{q}\f$ + */ + static void quatApplyDelta(vqf_real_t q[4], vqf_real_t delta, vqf_real_t out[4]); + /** + * @brief Rotates a vector with a given quaternion. + * + * \f$\begin{bmatrix}0 & \mathbf{v}_\mathrm{out}\end{bmatrix} = + * \mathbf{q} \otimes \begin{bmatrix}0 & \mathbf{v}\end{bmatrix} \otimes \mathbf{q}^*\f$ + */ + static void quatRotate(const vqf_real_t q[4], const vqf_real_t v[3], vqf_real_t out[3]); + /** + * @brief Calculates the Euclidean norm of a vector. + * @param vec pointer to an array of N elements + * @param N number of elements + */ + static vqf_real_t norm(const vqf_real_t vec[], size_t N); + /** + * @brief Normalizes a vector in-place. + * @param vec pointer to an array of N elements that will be normalized + * @param N number of elements + */ + static void normalize(vqf_real_t vec[], size_t N); + /** + * @brief Clips a vector in-place. + * @param vec pointer to an array of N elements that will be clipped + * @param N number of elements + * @param min smallest allowed value + * @param max largest allowed value + */ + static void clip(vqf_real_t vec[], size_t N, vqf_real_t min, vqf_real_t max); + /** + * @brief Calculates the gain for a first-order low-pass filter from the 1/e time constant. + * + * \f$k = 1 - \exp\left(-\frac{T_\mathrm{s}}{\tau}\right)\f$ + * + * The cutoff frequency of the resulting filter is \f$f_\mathrm{c} = \frac{1}{2\pi\tau}\f$. + * + * @param tau time constant \f$\tau\f$ in seconds - use -1 to disable update (\f$k=0\f$) or 0 to obtain + * unfiltered values (\f$k=1\f$) + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds + * @return filter gain *k* + */ + static vqf_real_t gainFromTau(vqf_real_t tau, vqf_real_t Ts); + /** + * @brief Calculates coefficients for a second-order Butterworth low-pass filter. + * + * The filter is parametrized via the time constant of the dampened, non-oscillating part of step response and the + * resulting cutoff frequency is \f$f_\mathrm{c} = \frac{\sqrt{2}}{2\pi\tau}\f$. + * + * When \f$\tau < \frac{T_\mathrm{s}}{2}\f$ (which corresponds to \f$f_\mathrm{c}\f$ exceeding 90 % of the + * Nyquist frequency), a direct passthrough fallback is used to prevent instability. + * + * @param tau time constant \f$\tau\f$ in seconds + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds + * @param outB output array for numerator coefficients + * @param outA output array for denominator coefficients (without \f$a_0=1\f$) + */ + static void filterCoeffs(vqf_real_t tau, vqf_real_t Ts, double outB[3], double outA[2]); + /** + * @brief Calculates the initial filter state for a given steady-state value. + * @param x0 steady state value + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param out output array for filter state + */ + static void filterInitialState(vqf_real_t x0, const double b[3], const double a[2], double out[2]); + /** + * @brief Adjusts the filter state when changing coefficients. + * + * This function assumes that the filter is currently in a steady state, i.e. the last input values and the last + * output values are all equal. Based on this, the filter state is adjusted to new filter coefficients so that the + * output does not jump. + * + * @param last_y last filter output values (array of size N) + * @param N number of values in vector-valued signal + * @param b_old previous numerator coefficients + * @param a_old previous denominator coefficients (without \f$a_0=1\f$) + * @param b_new new numerator coefficients + * @param a_new new denominator coefficients (without \f$a_0=1\f$) + * @param state filter state (array of size N*2, will be modified) + */ + static void filterAdaptStateForCoeffChange(vqf_real_t last_y[], size_t N, const double b_old[3], + const double a_old[2], const double b_new[3], + const double a_new[2], double state[]); + /** + * @brief Performs a filter step for a scalar value. + * @param x input value + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param state filter state array (will be modified) + * @return filtered value + */ + static vqf_real_t filterStep(vqf_real_t x, const double b[3], const double a[2], double state[2]); + /** + * @brief Performs filter step for vector-valued signal with averaging-based initialization. + * + * During the first \f$\tau\f$ seconds, the filter output is the mean of the previous samples. At \f$t=\tau\f$, the + * initial conditions for the low-pass filter are calculated based on the current mean value and from then on, + * regular filtering with the rational transfer function described by the coefficients b and a is performed. + * + * @param x input values (array of size N) + * @param N number of values in vector-valued signal + * @param tau filter time constant \f$\tau\f$ in seconds (used for initialization) + * @param Ts sampling time \f$T_\mathrm{s}\f$ in seconds (used for initialization) + * @param b numerator coefficients + * @param a denominator coefficients (without \f$a_0=1\f$) + * @param state filter state (array of size N*2, will be modified) + * @param out output array for filtered values (size N) + */ + static void filterVec(const vqf_real_t x[], size_t N, vqf_real_t tau, vqf_real_t Ts, const double b[3], + const double a[2], double state[], vqf_real_t out[]); +#ifndef VQF_NO_MOTION_BIAS_ESTIMATION + /** + * @brief Sets a 3x3 matrix to a scaled version of the identity matrix. + * @param scale value of diagonal elements + * @param out output array of size 9 (3x3 matrix stored in row-major order) + */ + static void matrix3SetToScaledIdentity(vqf_real_t scale, vqf_real_t out[9]); + /** + * @brief Performs 3x3 matrix multiplication (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) + */ + static void matrix3Multiply(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); + /** + * @brief Performs 3x3 matrix multiplication after transposing the first matrix + * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1^T\mathbf{M}_2\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) + */ + static void matrix3MultiplyTpsFirst(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); + /** + * @brief Performs 3x3 matrix multiplication after transposing the second matrix + * (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}_1\mathbf{M}_2^T\f$). + * @param in1 input 3x3 matrix \f$\mathbf{M}_1\f$ (stored in row-major order) + * @param in2 input 3x3 matrix \f$\mathbf{M}_2\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) + */ + static void matrix3MultiplyTpsSecond(const vqf_real_t in1[9], const vqf_real_t in2[9], vqf_real_t out[9]); + /** + * @brief Calculates the inverse of a 3x3 matrix (\f$\mathbf{M}_\mathrm{out} = \mathbf{M}^{-1}\f$). + * @param in input 3x3 matrix \f$\mathbf{M}\f$ (stored in row-major order) + * @param out output 3x3 matrix \f$\mathbf{M}_\mathrm{out}\f$ (stored in row-major order) + */ + static bool matrix3Inv(const vqf_real_t in[9], vqf_real_t out[9]); +#endif + +protected: + /** + * @brief Calculates coefficients based on parameters and sampling rates. + */ + void setup(); + + /** + * @brief Contains the current parameters. + * + * See #getParams. To set parameters, pass them to the constructor. Part of the parameters can be changed with + * #setTauAcc, #setTauMag, #setMotionBiasEstEnabled, #setRestBiasEstEnabled, #setMagDistRejectionEnabled, and + * #setRestDetectionThresholds. + */ + VQFParams params; + /** + * @brief Contains the current state. + * + * See #getState, #getState and #resetState. + */ + VQFState state; + /** + * @brief Contains the current coefficients (calculated in #setup). + * + * See #getCoeffs. + */ + VQFCoefficients coeffs; +}; + +#endif // VQF_HPP diff --git a/ground_station/platformio.ini b/ground_station/platformio.ini index 5a025280..7c9aeac7 100644 --- a/ground_station/platformio.ini +++ b/ground_station/platformio.ini @@ -28,7 +28,7 @@ lib_deps = build_flags = -fexceptions ; Required by the Nayuki C++ QR encoder - -DFIRMWARE_VERSION='"1.3.0"' ; Enter Firmware Version here + -DFIRMWARE_VERSION='"1.3.1"' ; Enter Firmware Version here -DUSB_MANUFACTURER='"CATS"' ; USB Manufacturer string -DUSB_PRODUCT='"CATS Ground Station"' ; USB Product String -D USB_SERIAL="0" ; Enter Device Serial Number here diff --git a/ground_station/simulator/gs_sim.py b/ground_station/simulator/gs_sim.py index 139fab6f..662d434e 100644 --- a/ground_station/simulator/gs_sim.py +++ b/ground_station/simulator/gs_sim.py @@ -243,7 +243,7 @@ def defaults() -> dict[str, Any]: "elevationRad": 0.0, "ax": 0.0, "ay": 0.0, "az": 1.0, "gx": 0.0, "gy": 0.0, "gz": 0.0, "mx": 0.0, "my": 0.0, "mz": 0.0, "calibrationPercentage": 0.0, "calibrationState": 0, "updated": False}, - "deviceStatus": {"batteryVoltage": 0.0, "usb": False, "freeStoragePercent": 100, "gnss": False, + "deviceStatus": {"batteryVoltage": 4.2, "usb": False, "freeStoragePercent": 100, "gnss": False, "clockValid": False, "hour": 0, "minute": 0, "logging": False, "recorderWriteFailure": False, "deleteFailure": False, "finalizeFailure": False, "usbStorageState": "firmware"}, diff --git a/ground_station/simulator/hmi_controller.hpp b/ground_station/simulator/hmi_controller.hpp index 172296a1..4c3cb3ed 100644 --- a/ground_station/simulator/hmi_controller.hpp +++ b/ground_station/simulator/hmi_controller.hpp @@ -74,7 +74,7 @@ struct NavigationSnapshot { }; struct DeviceStatusSnapshot { - float batteryVoltage = 0.0F; + float batteryVoltage = 4.2F; bool usb = false; uint32_t freeStoragePercent = 100; bool gnss = false; diff --git a/ground_station/simulator/web/app.js b/ground_station/simulator/web/app.js index a8037d98..66ceb606 100644 --- a/ground_station/simulator/web/app.js +++ b/ground_station/simulator/web/app.js @@ -384,7 +384,10 @@ document.querySelector('#pause').addEventListener('click', event => { canvas.focus({ preventScroll: true }); }); async function resetDemo(play = false) { + demoPlayback = null; if (wasm) controllerCall('gs_restart'); + // Finish startup before log loading yields to the browser's render loop. + if (wasm && lightMode && play) controllerCall('gs_advance', [fixtureById(fixtureManifest, 'light-demo').lightAdvanceMs]); fixtureStatus = null; try { await loadDefaultDemo(play); diff --git a/ground_station/simulator/web/browser-golden.json b/ground_station/simulator/web/browser-golden.json index cef00389..38a3be88 100644 --- a/ground_station/simulator/web/browser-golden.json +++ b/ground_station/simulator/web/browser-golden.json @@ -1,20 +1,20 @@ { - "menu": "2102f0f91cd0c03176ee70f679063cdc9fb95593e64c401b88149c4316c5c117", + "menu": "732e19b61a301fdf58e6f639f4c5bdd21b07a945495e49d3849e98c752e4b35e", "startupRocket": "c8db9d72adcd669e7f60cb33e213e1f05c9f4eecead98bf8d2510d254163ae64", - "lightDemo": "4f619bbbf53bda15a3e7d7a7f3e152808b01f6f65aa24f8624f4a12f8e5228af", - "recovery": "eda5a41b5f22562e8628f879dbe04f9e3d7e929d54157840398fe49b2dca0a56", - "compassOrientation": "6d5a84bc0813d49a491c11b1dbd3924046aa5ac43bcf8767dc4738e15eb1954a", - "phoneHeading0": "41742a8683e13fcebfa75265c01050a76aacaa7dcd4bc0e148ac991c22128d2e", - "phoneHeading45": "f4bb2bcc468dc9af42b6d6e278b1d3a663e59216908030dc96d06d9eff68cb53", - "phoneHeading90": "2bff3a8f661784e963e42f45e65057acbcac36f5a4eb8514edb6ab8dacebe697", - "phoneHeading135": "2eedbd095f7aa8153030e42f8e35ad66a12339dd646079d2d2e25d95a34a20f2", - "phoneHeading180": "1cc123e8ef83d9586b0d754c06934d0956552f880c46660bb5ca86252208716d", - "phoneHeading225": "7ae8ea2756a5eebea4d6b0b4944ded1748377844a198ceb3bf41fa9d7ea427f9", - "phoneHeading270": "f8496afd46f01c1ae8a7adb2b2f41d8041ef70af98acace3f33b90f63ae85f5e", - "phoneHeading315": "98f68820f68cf7049458c78f7a68dd5e46bbd6c4ace40efc98a67061eca69e4b", + "lightDemo": "53647eb4bf7d8ffd75329855b488abdd994a2d4d447d7c033a8ea22b3ac82f6f", + "recovery": "8112b7e92a6097567a8cc2124d1579d24efc2fc1161de07a1055f60abc00b196", + "compassOrientation": "71f5260781e480b3ad5efd6cc304bbb32eac4ea7dd06bf425d11af458c7523a9", + "phoneHeading0": "af810b5b3d8f61f35be452a7d6e128ea9147fcf71f0c846a99f32eff649bc734", + "phoneHeading45": "27e9ca56642433772004559ae8e617274c233b0d4643853e72b120952f0fedcc", + "phoneHeading90": "548922b41202554641357f42041aac01d75b6ed7793dd95f339285cef9526191", + "phoneHeading135": "a0356f5189c0e141da6dfca94fbd1e30db78189750cd0dc36fb885b6551c4a07", + "phoneHeading180": "bd46a099145b18a08f1ae9b1c29218f1f1d7907568bc945067fa08e4f2679aea", + "phoneHeading225": "d6028104cd4783380acfe5219b1d8d8ddf69e1419e16bd5f3537226a0f6d66de", + "phoneHeading270": "54e15a0c450e58330c5f8b652772f2ddf55c3af6046051049cce274b89256e11", + "phoneHeading315": "818b61f055e87eb5ebbd68a0bd01c8c96c953417af739a0d028f2ed8f58c2f18", "usbStorage": "cd4a76e556b38614ce3194c15fb40de92c012ab78f128d9624d4c3043047c089", - "recorderFault": "32219f426fded5eafe2a0a5199c593e94d9e858f5bb973190bcd71ce76e5ca2c", - "liveAheadRight": "1cfb144c6e9a8c51eac0a67ef329e83f1f2bc56b7c79e9a117f3793d3962bf65", - "liveLeftAhead": "0b4cce4e33c9e93fe44c5e36e8ac0a178e1f9b16ab7704775116aba5480ace08", - "liveBehindLeft": "7c1cba8d8e141bcacb76a3bc538ed3f973d7464cc29ecff42ca3c23ddc780198" + "recorderFault": "29c8f06ba5aa1cdb16e2e27044f6ccda914f19481d7dd05a3f4a7c847af052b3", + "liveAheadRight": "ac5416478ebcc9101cdf7397bf80455c82df6375f0490c3eb25ff666b0e79ed7", + "liveLeftAhead": "2648f881d0de5263cfc6b3733dd20a50aabc3706e20b589afe2517873b9c830c", + "liveBehindLeft": "926a95c0bb564756aa4c665d9e67ba4963d9c6f311780cb4ad0d929fbdfae3d6" } diff --git a/ground_station/simulator/web/browser-tests.js b/ground_station/simulator/web/browser-tests.js index b5c6a6a3..603e2d66 100644 --- a/ground_station/simulator/web/browser-tests.js +++ b/ground_station/simulator/web/browser-tests.js @@ -158,12 +158,32 @@ async function run() { tap(wasm, 'down'); tap(wasm, 'right'); tap(wasm, 'ok'); + captureSelfTest('Sensors - next-page arrow and angular-rate units'); tap(wasm, 'right'); const state = snapshot(wasm); assert(state.activeScreen === 'sensors', `expected sensors, got ${state.activeScreen}`); assert(state.sensorView === 'orientation', `expected orientation, got ${state.sensorView}`); actualHashes.compassOrientation = await framebufferHash(wasm); captureSelfTest('Compass status bar'); + for (const [degrees, ballY] of [[90, 80], [0, 132], [-90, 184], [120, 80], [-120, 184], [0, 132]]) { + wasm.ccall('gs_set_navigation_json', null, ['string'], [JSON.stringify({pitchRad: degrees * Math.PI / 180})]); + wasm.ccall('gs_advance', null, ['number'], [200]); + const bytes = framebuffer(wasm); + for (const y of [80, 132, 184]) { + const pixel = (y + 2) * 400 + 381; + const black = (bytes[pixel >> 3] & (0x80 >> (pixel & 7))) === 0; + assert(black === (y === ballY), `pitch ${degrees}: ball position or stale pixels at ${y}`); + } + if (Math.abs(degrees) <= 90) captureSelfTest(`Compass pitch ${degrees} degrees`); + } + wasm.ccall('gs_set_navigation_json', null, ['string'], [JSON.stringify({ + northRad: Math.PI / 1800, pitchRad: -89.9 * Math.PI / 180, rollRad: -Math.PI + })]); + wasm.ccall('gs_advance', null, ['number'], [200]); + captureSelfTest('Compass - wide signed readings'); + tap(wasm, 'left'); + assert(snapshot(wasm).sensorView === 'readings', 'Left returns to raw sensors'); + captureSelfTest('Sensors - returned from compass'); }); await test('phone compass headings through the browser controls', async () => { @@ -527,6 +547,20 @@ async function run() { tap(wasm, 'ok'); assert(snapshot(wasm).activeScreen === 'firmware_update' && snapshot(wasm).firmwareUpdateSelection === 0, 'Update Firmware opens the target chooser'); captureSelfTest('Update Firmware - Ground Station'); + tap(wasm, 'down'); + assert(snapshot(wasm).firmwareUpdateSelection === 1, 'Radio Receivers is the second update target'); + captureSelfTest('Update Firmware - Radio Receivers'); + tap(wasm, 'ok'); + assert(snapshot(wasm).activeScreen === 'radio_update' && snapshot(wasm).radioUpdateState === 'browse', 'Radio Receivers opens the firmware list'); + captureSelfTest('Radio Update - file list'); + tap(wasm, 'ok'); + assert(snapshot(wasm).radioUpdateState === 'confirm', 'selecting radio firmware opens confirmation'); + captureSelfTest('Radio Update - telemetry 1.2.0 requirement'); + tap(wasm, 'back'); + assert(snapshot(wasm).radioUpdateState === 'browse', 'Back returns to the radio firmware list'); + tap(wasm, 'back'); + assert(snapshot(wasm).activeScreen === 'firmware_update' && snapshot(wasm).firmwareUpdateSelection === 1, 'Back returns to the radio update target'); + tap(wasm, 'up'); tap(wasm, 'ok'); assert(snapshot(wasm).activeScreen === 'bootloader' && snapshot(wasm).actions.some(action => action.type === 'bootloader_requested'), 'Ground Station invokes the existing bootloader'); }); diff --git a/ground_station/src/attitude.cpp b/ground_station/src/attitude.cpp new file mode 100644 index 00000000..b8a22111 --- /dev/null +++ b/ground_station/src/attitude.cpp @@ -0,0 +1,44 @@ +/// Copyright (C) 2026 Control and Telemetry Systems GmbH +/// +/// SPDX-License-Identifier: GPL-3.0-or-later + +#include "attitude.hpp" + +#include +#include +#include + +Attitude AttitudeFilter::update(const std::array &gyro, const std::array &acceleration, + const std::array &magnetic) { + constexpr double pi = std::numbers::pi; + constexpr double radiansPerDegree = pi / 180.0; + constexpr double gravity = 9.80665; + // Preserve the board mounting used by the previous estimator. Both mappings + // are proper rotations (right-handed); VQF requires rad/s and m/s^2. + const vqf_real_t gyr[3]{static_cast(gyro[1]) * radiansPerDegree, + static_cast(gyro[0]) * radiansPerDegree, + -static_cast(gyro[2]) * radiansPerDegree}; + const vqf_real_t acc[3]{static_cast(acceleration[1]) * gravity, + static_cast(acceleration[0]) * gravity, + -static_cast(acceleration[2]) * gravity}; + const vqf_real_t mag[3]{-magnetic[1], -magnetic[0], -magnetic[2]}; + const auto finite = [](const auto &values) { + return std::all_of(values.begin(), values.end(), [](float value) { return std::isfinite(value); }); + }; + if (finite(gyro)) filter.updateGyr(gyr); + if (finite(acceleration)) filter.updateAcc(acc); + if (finite(magnetic)) filter.updateMag(mag); + + vqf_real_t q[4]; + filter.getQuat9D(q); // Scalar-first quaternion, sensor to ENU earth frame. + const auto [w, x, y, z] = q; + const double yaw = std::atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)); + return { + // ENU north is +Y. Subtract its quarter-turn to retain NWU yaw. + .north = static_cast(std::remainder(yaw - pi / 2, 2 * pi)), + // Pointing up was negative with the old Euler pitch getter. Define the + // GS sign here once, for every consumer, rather than in screen drawing. + .pitch = static_cast(-std::asin(std::clamp(2 * (w * y - x * z), -1.0, 1.0))), + .roll = static_cast(std::atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))), + }; +} diff --git a/ground_station/src/attitude.hpp b/ground_station/src/attitude.hpp new file mode 100644 index 00000000..7044ea3c --- /dev/null +++ b/ground_station/src/attitude.hpp @@ -0,0 +1,32 @@ +/// Copyright (C) 2026 Control and Telemetry Systems GmbH +/// +/// SPDX-License-Identifier: GPL-3.0-or-later + +#pragma once + +#include +#include + +// GS angles in radians. North retains the navigation/recovery convention: +// NWU yaw, zero along magnetic north, positive counterclockwise. Pitch is +// positive when pointing up, negative when pointing down; roll retains the +// existing right-hand rotation about the aligned IMU X axis. +struct Attitude { + float north = 0; + float pitch = 0; + float roll = 0; +}; + +// Owned by the navigation task. Input vectors use native sensor axes: +// factory-corrected gyro in degrees/s, acceleration in g, calibrated magnetic +// field in arbitrary units. Calibration must precede the board-axis mapping. +class AttitudeFilter { + public: + static constexpr unsigned sampleRateHz = 50; + Attitude update(const std::array &gyro, const std::array &acceleration, + const std::array &magnetic); + void reset() { filter.resetState(); } + + private: + VQF filter{1.0 / sampleRateHz}; +}; diff --git a/ground_station/src/hmi/window.cpp b/ground_station/src/hmi/window.cpp index 5d4eb5fe..dce1fac4 100644 --- a/ground_station/src/hmi/window.cpp +++ b/ground_station/src/hmi/window.cpp @@ -1330,9 +1330,9 @@ void Window::initRadioUpdateConfirm(const char *filename, uint32_t size, uint32_ snprintf(identity, sizeof(identity), "%lu bytes CRC32 %08lX", static_cast(size), static_cast(crc)); drawCentreString(identity, 200, 91); - drawCentreString("Only use production telemetry firmware", 200, 117); - drawCentreString("intended for this Ground Station.", 200, 137); - drawCentreString("Wrong firmware may require ST-Link recovery.", 200, 157); + drawCentreString("Telemetry 1.2.0 or newer is required.", 200, 117); + drawCentreString("Older versions require ST-Link first.", 200, 137); + drawCentreString("Only use firmware for this Ground Station.", 200, 157); display.setFont(&FreeSansBold9pt7b); drawCentreString("Do not disconnect power during this update.", 200, 184); display.setFont(&FreeSans9pt7b); @@ -1441,6 +1441,7 @@ void Window::initSensors() { display.fillRect(200, 19, 200, 30, BLACK); drawCentreString("GNSS", 300, 42); + display.fillTriangle(386, 33, 378, 25, 378, 41, WHITE); display.setTextColor(BLACK); display.setFont(&FreeSansBold9pt7b); @@ -1451,7 +1452,10 @@ void Window::initSensors() { display.print("[G]"); display.setCursor(120, 65); - display.print("[deg/s]"); + display.print("["); + display.drawCircle(static_cast(display.getCursorX() + 4), 54, 3, BLACK); + display.setCursor(static_cast(display.getCursorX() + 10), 65); + display.print("/s]"); display.setCursor(87, 170); display.print("[-]"); @@ -1460,23 +1464,15 @@ void Window::initSensors() { display.print("Press A to calibrate"); display.setCursor(220, 225); display.print("the compass"); - display.setCursor(12, 225); - display.print("Right: compass"); surface.present(); } void Window::initSensorOrientation() { clearMainScreen(); - display.setTextSize(1); - display.setTextColor(WHITE); - display.setFont(&FreeSansBold12pt7b); - display.fillRect(0, 19, 400, 30, BLACK); - drawCentreString("Compass / 3D Orientation", 200, 42); + drawPageHeader("Compass / 3D Orientation", true, false); display.setTextColor(BLACK); display.setFont(&FreeSans9pt7b); - display.setCursor(8, 233); - display.print("Left: sensors"); display.setCursor(285, 233); display.print("A: calibrate"); @@ -1544,18 +1540,20 @@ void Window::updateSensorOrientation(Navigation *navigation) { display.setFont(&FreeSansBold12pt7b); display.setCursor(215, 105); display.print(headingDegrees, 1); - display.print(" deg "); + display.drawCircle(static_cast(display.getCursorX() + 4), 90, 3, BLACK); + display.setCursor(static_cast(display.getCursorX() + 13), 105); display.print(directions[direction]); + const float pitchDegrees = navigation->getPitch() * 180.0F / PI_F; display.setFont(&FreeSans9pt7b); display.setCursor(215, 137); display.print("Pitch: "); - display.print(navigation->getPitch() * 180.0F / PI_F, 1); - display.print(" deg"); + display.print(pitchDegrees, 1); + display.drawCircle(static_cast(display.getCursorX() + 4), 125, 3, BLACK); display.setCursor(215, 164); display.print("Roll: "); display.print(navigation->getRoll() * 180.0F / PI_F, 1); - display.print(" deg"); + display.drawCircle(static_cast(display.getCursorX() + 4), 152, 3, BLACK); const float magneticMagnitude = sqrtf(navigation->getMX() * navigation->getMX() + navigation->getMY() * navigation->getMY() + navigation->getMZ() * navigation->getMZ()) / @@ -1564,6 +1562,19 @@ void Window::updateSensorOrientation(Navigation *navigation) { display.print("|M|: "); display.print(magneticMagnitude, 2); + // Positive pitch moves the ball up; keep it inside the scale at +/-90 degrees. + constexpr int16_t pitchX = 378; + constexpr int16_t pitchTravel = 52; + const float boundedPitch = fmaxf(-90.0F, fminf(90.0F, pitchDegrees)); + const auto pitchY = static_cast(centerY - lroundf(boundedPitch * pitchTravel / 90.0F)); + drawCentreString("UP", pitchX, 65); + drawCentreString("DN", pitchX, 208); + display.drawRoundRect(pitchX - 12, centerY - pitchTravel - 8, 25, 2 * pitchTravel + 17, 6, BLACK); + display.drawFastVLine(pitchX, centerY - pitchTravel, 2 * pitchTravel + 1, BLACK); + display.drawFastHLine(pitchX - 11, centerY, 23, BLACK); + display.fillCircle(pitchX, pitchY, 7, WHITE); + display.fillCircle(pitchX, pitchY, 5, BLACK); + surface.present(); } diff --git a/ground_station/src/navigation.cpp b/ground_station/src/navigation.cpp index baf00cf8..4cd98a64 100644 --- a/ground_station/src/navigation.cpp +++ b/ground_station/src/navigation.cpp @@ -9,7 +9,7 @@ #include "console.hpp" #include "utils.hpp" -constexpr uint8_t NAVIGATION_TASK_FREQUENCY = 50; +constexpr uint8_t NAVIGATION_TASK_FREQUENCY = AttitudeFilter::sampleRateHz; namespace { // The compass library intentionally remains unchanged. Factory diagnostics use @@ -37,6 +37,13 @@ SelfTestSensorObservation Navigation::selfTestSensors() const { return copy; } +Attitude Navigation::getAttitude() const { + portENTER_CRITICAL(&sensorMux); + const auto copy = attitude; + portEXIT_CRITICAL(&sensorMux); + return copy; +} + bool Navigation::saveGyroCalibration(const std::array &bias) { if (!std::all_of(bias.begin(), bias.end(), [](float value) { return std::isfinite(value) && std::fabs(value) <= SelfTestProfile::kMaximumGyroBias; @@ -53,6 +60,7 @@ bool Navigation::saveGyroCalibration(const std::array &bias) { if (!saved) return false; portENTER_CRITICAL(&sensorMux); gyroBias = bias; + resetAttitude = true; portEXIT_CRITICAL(&sensorMux); return true; } @@ -81,7 +89,7 @@ bool Navigation::begin() { preferences.end(); } - filter.begin(NAVIGATION_TASK_FREQUENCY); + filter.reset(); calibration = CALIB_CONCLUDED; @@ -260,7 +268,10 @@ void Navigation::navigationTask(void *pvParameter) { const std::array rawGyro{ref->gx, ref->gy, ref->gz}; portENTER_CRITICAL(&ref->sensorMux); const auto bias = ref->gyroBias; + const bool resetAttitude = ref->resetAttitude; + ref->resetAttitude = false; portEXIT_CRITICAL(&ref->sensorMux); + if (resetAttitude) ref->filter.reset(); ref->gx -= bias[0]; ref->gy -= bias[1]; ref->gz -= bias[2]; @@ -278,11 +289,15 @@ void Navigation::navigationTask(void *pvParameter) { portEXIT_CRITICAL(&ref->sensorMux); } - // Align magnetic axes with the IMU before fusion. The quarter-turn belongs - // after calibration, which remains in the magnetometer's native axes. - ref->filter.update(ref->gy, ref->gx, -ref->gz, ref->ay, ref->ax, -ref->az, -ref->m[1], -ref->m[0], -ref->m[2]); - - ref->filter.getQuaternion(&ref->q0, &ref->q1, &ref->q2, &ref->q3); + // Do not integrate stale samples after a failed IMU read. Publish only the + // angles; the filter's mutable state stays on this task. + if (accelerationRead && gyroRead) { + const auto attitude = ref->filter.update({ref->gx, ref->gy, ref->gz}, {ref->ax, ref->ay, ref->az}, + {ref->m[0], ref->m[1], ref->m[2]}); + portENTER_CRITICAL(&ref->sensorMux); + ref->attitude = attitude; + portEXIT_CRITICAL(&ref->sensorMux); + } if (ref->calibration == CALIB_ONGOING) { ref->calibrate(ref->raw_m); @@ -293,6 +308,7 @@ void Navigation::navigationTask(void *pvParameter) { ref->setCalibrationState(CALIB_CONCLUDED); ref->mag_calib = ref->mag_calib_temp; ref->set_saved_calib(ref->mag_calib); + ref->filter.reset(); ref->resetCalib(); } diff --git a/ground_station/src/navigation.hpp b/ground_station/src/navigation.hpp index 5ebfed97..44fecbc3 100644 --- a/ground_station/src/navigation.hpp +++ b/ground_station/src/navigation.hpp @@ -4,13 +4,13 @@ #pragma once +#include "attitude.hpp" #include "console.hpp" #include "self_test.hpp" #include "utils.hpp" // clang-format off #include -#include #include #include // clang-format on @@ -68,17 +68,17 @@ class Navigation { inline float getNorth() { updated = false; - return filter.getYawRadians(); + return getAttitude().north; } inline float getPitch() { updated = false; - return filter.getPitchRadians(); + return getAttitude().pitch; } inline float getRoll() { updated = false; - return filter.getRollRadians(); + return getAttitude().roll; } inline float getGX() const { return gx; } @@ -118,7 +118,7 @@ class Navigation { return elevation; } - inline float computeBearing() { return (azimuth + filter.getYawRadians() - PI_F / 2) / (2 * PI_F / 360); } + inline float computeBearing() { return (azimuth + getAttitude().north - PI_F / 2) / (2 * PI_F / 360); } struct mag_calibration_t { float offset[3]; @@ -152,6 +152,8 @@ class Navigation { mutable portMUX_TYPE sensorMux = portMUX_INITIALIZER_UNLOCKED; SelfTestSensorObservation sensorObservation{}; std::array gyroBias{}; // Degrees/s, protected by sensorMux. + bool resetAttitude = false; // Protected by sensorMux, consumed by navigation task. + Attitude attitude{}; // Published by navigation task under sensorMux. bool updated = false; calibration_state_e calibration = INV_CALIB; @@ -161,7 +163,7 @@ class Navigation { std::unique_ptr compass; LSM6DS3Class imu; - Madgwick filter; + AttitudeFilter filter; float gx, gy, gz, ax, ay, az; float m[3]; @@ -172,8 +174,6 @@ class Navigation { bool sphere_checked[n_points]; float calibration_progress = 0; - float q0, q1, q2, q3; - float dist = 0; float azimuth = 0; float elevation = 0; @@ -193,6 +193,7 @@ class Navigation { .min_vals_scal = {100, 100, 100}}; static void navigationTask(void *pvParameter); + Attitude getAttitude() const; void calculateDistanceDirection(); void initFibonacciSphere(); void calibrate(const float *val); diff --git a/ground_station/src/telemetry/telemetry.cpp b/ground_station/src/telemetry/telemetry.cpp index ba919ae6..663b77db 100644 --- a/ground_station/src/telemetry/telemetry.cpp +++ b/ground_station/src/telemetry/telemetry.cpp @@ -10,8 +10,12 @@ #include constexpr uint8_t TASK_TELE_FREQ = 100; +// Telemetry 1.1.3 does not start its host UART receiver until after the +// 4.4-second GNSS startup sequence. Early traffic leaves USART2 overrun. +constexpr uint32_t TELEMETRY_STARTUP_GUARD_MS = 5000; void Telemetry::begin() { + startupStarted = millis(); uartMutex = xSemaphoreCreateMutex(); if (uartMutex == nullptr) { return; @@ -181,15 +185,18 @@ void Telemetry::update(void* pvParameter) { ref->newSetting = true; } - if (ref->newSetting) { + const bool startupGuardElapsed = ref->startupGuardElapsed(); + + if (ref->newSetting && startupGuardElapsed) { ref->newSetting = false; ref->initLink(); ref->controlApplied = ref->selfTestControl.generation; } - // The telemetry MCU waits 4 seconds for GNSS at boot; allow 8 seconds for its reply. + // The telemetry MCU waits for GNSS at boot. Start requesting only after + // the legacy startup guard, while retaining the existing 8-second deadline. // Explicit requests remain available to self-test. - if (!ref->versionReadDone.load()) { + if (!ref->versionReadDone.load() && startupGuardElapsed) { const uint32_t now = millis(); if (ref->diagnostics().versionReplies != 0 || now - versionReadStarted >= 8000U) { ref->versionReadDone = true; @@ -199,7 +206,7 @@ void Telemetry::update(void* pvParameter) { } } - if (ref->versionRequested.exchange(false)) { + if (startupGuardElapsed && ref->versionRequested.exchange(false)) { const uint8_t header[] = {CMD_VERSION_INFO, 0}; uint8_t request[] = {CMD_VERSION_INFO, 0, crc8(header, sizeof(header))}; ref->serial.write(request, sizeof(request)); @@ -233,7 +240,7 @@ void Telemetry::update(void* pvParameter) { } bool Telemetry::lockNormalWriter() { - if (uartMutex == nullptr || xSemaphoreTake(uartMutex, pdMS_TO_TICKS(1500)) != pdTRUE) { + if (!startupGuardElapsed() || uartMutex == nullptr || xSemaphoreTake(uartMutex, pdMS_TO_TICKS(1500)) != pdTRUE) { return false; } if (updateRequested || quarantined) { @@ -269,10 +276,12 @@ bool Telemetry::safeForUpdateLocked() const { const auto packet = data.snapshot(); const bool recent = (xTaskGetTickCount() - data.getLastUpdateTime()) <= pdMS_TO_TICKS(2000); const bool airborne = packet.state > 2 && packet.state < 7; - return initialized && !quarantined && !testingActive && !requestExitTesting && !triggerAction && - !(recent && (airborne || packet.testing_mode)); + return initialized && startupGuardElapsed() && !quarantined && !testingActive && !requestExitTesting && + !triggerAction && !(recent && (airborne || packet.testing_mode)); } +bool Telemetry::startupGuardElapsed() const { return millis() - startupStarted >= TELEMETRY_STARTUP_GUARD_MS; } + bool Telemetry::safeForUpdate() { if (uartMutex == nullptr || xSemaphoreTake(uartMutex, pdMS_TO_TICKS(1500)) != pdTRUE) { return false; diff --git a/ground_station/src/telemetry/telemetry.hpp b/ground_station/src/telemetry/telemetry.hpp index eb270f87..adf008aa 100644 --- a/ground_station/src/telemetry/telemetry.hpp +++ b/ground_station/src/telemetry/telemetry.hpp @@ -77,6 +77,7 @@ class Telemetry { bool testingActive{false}; bool safeForUpdateLocked() const; bool lockNormalWriter(); + bool startupGuardElapsed() const; void initLink(); @@ -90,6 +91,7 @@ class Telemetry { volatile bool initialized = false; volatile bool linkInitialized = false; + uint32_t startupStarted = 0; Parser parser; int rxPin; diff --git a/ground_station/src/update/rom_bootloader.cpp b/ground_station/src/update/rom_bootloader.cpp index b1b6f615..fe6c2f18 100644 --- a/ground_station/src/update/rom_bootloader.cpp +++ b/ground_station/src/update/rom_bootloader.cpp @@ -178,7 +178,7 @@ bool RomBootloader::enter(LinkResult& result) { size_t length = 0; if (!port.write(request, sizeof(request)) || !frame(kBootCommand, response, 2, length, 1000) || length != 2 || response[0] != 1 || response[1] != kAck) { - return fail("No entry ACK; ST-Link bootstrap/recovery"); + return fail("No entry ACK; if <1.2.0 use ST-Link"); } port.wait(50); port.configure(true); diff --git a/ground_station/tests/attitude_test.cpp b/ground_station/tests/attitude_test.cpp new file mode 100644 index 00000000..4aff25ab --- /dev/null +++ b/ground_station/tests/attitude_test.cpp @@ -0,0 +1,144 @@ +// Copyright (C) 2026 Control and Telemetry Systems GmbH +// SPDX-License-Identifier: GPL-3.0-or-later + +#include "attitude.hpp" + +#include +#include +#include +#include +#include + +namespace { +constexpr double rad = std::numbers::pi / 180; +using Vector = std::array; + +void nearAngle(float actual, double expectedDegrees, const char *label, double tolerance = 0.1) { + const double error = std::remainder(actual / rad - expectedDegrees, 360.0); + if (!std::isfinite(actual) || std::abs(error) > tolerance) { + std::cerr << label << ": expected " << expectedDegrees << ", got " << actual / rad << " degrees\n"; + std::exit(1); + } +} + +// Independent rotation-matrix fixture. Heading is NWU yaw; elevation is the +// user-facing positive-up angle. Return sensor-native gravity and magnetic +// measurements for the existing board mounting, without calling adapter math. +struct Pose { + Vector acc; + Vector mag; + Pose(double heading, double elevation, double roll) { + const double cy = std::cos(heading * rad), sy = std::sin(heading * rad); + const double cp = std::cos(-elevation * rad), sp = std::sin(-elevation * rad); + const double cr = std::cos(roll * rad), sr = std::sin(roll * rad); + const double r00 = cy * cp, r01 = cy * sp * sr - sy * cr, r02 = cy * sp * cr + sy * sr; + const double r20 = -sp, r21 = cp * sr, r22 = cp * cr; + acc = {static_cast(r21), static_cast(r20), static_cast(-r22)}; + // Magnetic north has a horizontal component of 40 and vertical of -20. + mag = {static_cast(-40 * r01 + 20 * r21), static_cast(-40 * r00 + 20 * r20), + static_cast(-40 * r02 + 20 * r22)}; + } +}; + +Attitude settle(AttitudeFilter &filter, const Pose &pose, const Vector &gyro = {}, unsigned count = 500) { + Attitude result; + for (unsigned i = 0; i < count; ++i) result = filter.update(gyro, pose.acc, pose.mag); + return result; +} + +void staticPoses() { + for (double heading : {0., 45., 90., 135., 180., -135., -90., -45.}) { + for (double elevation : {-60., -30., 0., 30., 60.}) { + for (double roll : {-35., 0., 35.}) { + AttitudeFilter filter; + const auto result = settle(filter, Pose(heading, elevation, roll)); + nearAngle(result.north, heading, "tilt-compensated heading"); + nearAngle(result.pitch, elevation, "positive-up pitch"); + nearAngle(result.roll, roll, "roll"); + } + } + } + for (double elevation : {-90., 90.}) { + AttitudeFilter filter; + const auto result = settle(filter, Pose(0, elevation, 0)); + nearAngle(result.pitch, elevation, "vertical pitch"); + if (!std::isfinite(result.north) || !std::isfinite(result.roll)) std::exit(1); + } +} + +void gyroUnitsAndAxes() { + const Pose level(0, 0, 0); + // Raw native gyro rotations: Z inverted -> yaw; X inverted -> elevation; + // native Y -> roll. No accelerometer/magnetometer correction during motion. + for (unsigned axis = 0; axis < 3; ++axis) { + AttitudeFilter filter; + settle(filter, level); + Vector gyro{}; + gyro[axis] = axis == 1 ? 30 : -30; + Attitude result; + for (unsigned i = 0; i < AttitudeFilter::sampleRateHz; ++i) result = filter.update(gyro, {}, {}); + nearAngle(result.pitch, axis == 0 ? 30 : 0, "gyro pitch"); + nearAngle(result.roll, axis == 1 ? 30 : 0, "gyro roll"); + nearAngle(result.north, axis == 2 ? 30 : 0, "gyro heading"); + } +} + +void restBiasAndReset() { + AttitudeFilter filter; + const Pose pose(40, 20, -15); + // Existing stored factory offsets have already been removed. VQF must learn + // this remaining 0.5 deg/s offset, including the otherwise unobservable yaw. + const Vector bias{0, 0, -0.5F}; + const auto before = settle(filter, pose, bias, 3000); + Attitude after; + for (unsigned i = 0; i < 500; ++i) after = filter.update(bias, pose.acc, {}); + nearAngle(after.north, before.north / rad, "rest bias with missing magnetometer", 0.2); + nearAngle(after.pitch, 20, "rest pitch", 0.2); + filter.reset(); // New calibration must discard the old learned offset. + const auto reset = settle(filter, Pose(-90, -30, 20)); + nearAngle(reset.north, -90, "reset heading"); + nearAngle(reset.pitch, -30, "reset pitch"); + nearAngle(reset.roll, 20, "reset roll"); +} + +void magneticDisturbance() { + AttitudeFilter filter; + settle(filter, Pose(0, 0, 0)); + // VQF requires movement to establish a trusted magnetic reference. Rotate + // at 45 deg/s for two turns, then stop at north before applying interference. + for (unsigned i = 1; i <= 800; ++i) { + const Pose pose(i * 45.0 / AttitudeFilter::sampleRateHz, 0, 0); + filter.update({0, 0, -45}, pose.acc, pose.mag); + } + const auto before = settle(filter, Pose(0, 0, 0)); + Pose disturbed(90, 0, 0); + for (auto &value : disturbed.mag) value *= 3; + Attitude result; + for (unsigned i = 0; i < 250; ++i) result = filter.update({}, disturbed.acc, disturbed.mag); + nearAngle(result.north, before.north / rad, "short magnetic disturbance rejection", 0.5); + nearAngle(result.pitch, 0, "magnetic disturbance pitch"); + nearAngle(settle(filter, Pose(0, 0, 0)).north, 0, "magnetic recovery", 0.5); +} + +void invalidInputs() { + AttitudeFilter filter; + const Pose pose(45, 30, 20); + settle(filter, pose); + const float nan = std::numeric_limits::quiet_NaN(); + const float inf = std::numeric_limits::infinity(); + for (unsigned i = 0; i < 100; ++i) filter.update({nan, 0, 0}, {0, inf, 0}, {0, 0, nan}); + const auto result = settle(filter, pose); + nearAngle(result.north, 45, "invalid input recovery heading"); + nearAngle(result.pitch, 30, "invalid input recovery pitch"); +} +} // namespace + +int main() { + staticPoses(); + gyroUnitsAndAxes(); + restBiasAndReset(); + magneticDisturbance(); + invalidInputs(); + std::cout << "VQF attitude tests passed: 120 poses, vertical limits, gyro units/axes, bias, reset, magnetic disturbance, invalid input.\n" + << "AttitudeFilter storage: " << sizeof(AttitudeFilter) << " bytes\n"; +} diff --git a/ground_station/tests/rom_bootloader_test.cpp b/ground_station/tests/rom_bootloader_test.cpp index 9988047a..9fb2cb25 100644 --- a/ground_station/tests/rom_bootloader_test.cpp +++ b/ground_station/tests/rom_bootloader_test.cpp @@ -34,7 +34,7 @@ class FakePort final : public Port { public: bool write(const uint8_t* data, size_t size) override { if (!rom) { - if (size == 3 && data[0] == 0x80 && data[1] == 0 && data[2] == crc8(data, 2)) { + if (size == 3 && data[0] == 0x80 && data[1] == 0 && data[2] == crc8(data, 2) && bootEntrySupported) { const uint8_t frame[] = {0x80, 2, 1, 0x79, 0x42}; queue(frame, sizeof(frame) - 1); const uint8_t content[] = {0x80, 2, 1, 0x79}; @@ -161,6 +161,7 @@ class FakePort final : public Port { bool rom{false}; bool dropWriteAck{true}; bool protectedFlash{false}; + bool bootEntrySupported{true}; }; int main() { @@ -196,6 +197,14 @@ int main() { assert(!protectedUpdater.run(image, info, result)); assert(!result.destructive && protectedRom.writes.empty()); + FakePort legacyApplication; + legacyApplication.bootEntrySupported = false; + RomBootloader legacyUpdater(legacyApplication); + assert(!legacyUpdater.run(image, info, result)); + assert(std::string(legacyUpdater.error()) == "No entry ACK; if <1.2.0 use ST-Link"); + assert(result.attempted && result.entryRequested && !result.destructive && !result.success); + assert(legacyApplication.writes.empty()); + image.bytes[0] = 1; assert(!inspect(image, info)); return 0;