mod/mahony/mahony.c
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /** | ||
| 2 | ****************************************************************************** | ||
| 3 | * @file : mahony.c | ||
| 4 | * @brief : Mahony attitude filter interface. | ||
| 5 | ****************************************************************************** | ||
| 6 | * @copyright | ||
| 7 | * | ||
| 8 | * Copyright (c) 2025 Sun Devil Rocketry. | ||
| 9 | * All rights reserved. | ||
| 10 | * | ||
| 11 | * This software is licensed under terms that can be found in the LICENSE file | ||
| 12 | * in the root directory of this software component. | ||
| 13 | * If no LICENSE file comes with this software, it is covered under the | ||
| 14 | * BSD-3-Clause. | ||
| 15 | * | ||
| 16 | * https://opensource.org/license/bsd-3-clause | ||
| 17 | * | ||
| 18 | ****************************************************************************** | ||
| 19 | @verbatim | ||
| 20 | ============================================================================== | ||
| 21 | ##### Integration Guide ##### | ||
| 22 | ============================================================================== | ||
| 23 | [..] | ||
| 24 | (+) Initialize the filter with a pointer to an existing or uninitialized filter, | ||
| 25 | an initial attiutude estimate, and the proportional and integral gains for | ||
| 26 | accelerometer correction. | ||
| 27 | (+) Call updates to the filter with gyroscope and accelerometer vectors and | ||
| 28 | time elapsed. Optionally disable accelerometer fusion during dynamic | ||
| 29 | flight. | ||
| 30 | ****************************************************************************** | ||
| 31 | @endverbatim | ||
| 32 | */ | ||
| 33 | |||
| 34 | /* Standard Includes ---------------------------------------------------------*/ | ||
| 35 | #include <math.h> | ||
| 36 | #include <stddef.h> | ||
| 37 | |||
| 38 | /* Project Includes ----------------------------------------------------------*/ | ||
| 39 | #include "mahony.h" | ||
| 40 | |||
| 41 | /* Private Macros ------------------------------------------------------------*/ | ||
| 42 | |||
| 43 | /* | ||
| 44 | * Accelerometer feedback is only used when the measured magnitude is reasonably | ||
| 45 | * close to one g. These are initial software validation thresholds and should | ||
| 46 | * be tuned later using stationary, vibration, and flight data. | ||
| 47 | */ | ||
| 48 | #define MAHONY_ACCEL_MIN_MAGNITUDE (0.85f * GRAVITY) | ||
| 49 | #define MAHONY_ACCEL_MAX_MAGNITUDE (1.15f * GRAVITY) | ||
| 50 | |||
| 51 | /* | ||
| 52 | * Limits each integral correction component to prevent windup. The value is | ||
| 53 | * expressed as an angular-rate correction in radians per second and should be | ||
| 54 | * tuned using sensor characterization and flight data. | ||
| 55 | */ | ||
| 56 | #define MAHONY_INTEGRAL_LIMIT_RAD_S 0.25f | ||
| 57 | |||
| 58 | /* Public Functions ----------------------------------------------------------*/ | ||
| 59 | |||
| 60 | /** | ||
| 61 | * @brief Initializes a Mahony attitude filter. | ||
| 62 | * | ||
| 63 | * @param filter Filter instance to initialize. | ||
| 64 | * @param initial_attitude Initial body-to-world attitude quaternion. | ||
| 65 | * @param proportional_gain Proportional correction gain. | ||
| 66 | * @param integral_gain Integral correction gain. | ||
| 67 | * | ||
| 68 | * @return MAHONY_OK when initialization succeeds; otherwise a Mahony status | ||
| 69 | * code describing the failure. | ||
| 70 | */ | ||
| 71 | 7 | MAHONY_STATUS mahony_init | |
| 72 | ( | ||
| 73 | MAHONY_FILTER *filter, | ||
| 74 | QUAT initial_attitude, | ||
| 75 | float proportional_gain, | ||
| 76 | float integral_gain | ||
| 77 | ) | ||
| 78 | { | ||
| 79 |
1/2✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
|
7 | if ( filter == NULL ) |
| 80 | { | ||
| 81 | return MAHONY_NULL_POINTER; | ||
| 82 | } | ||
| 83 | |||
| 84 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
7 | if ( !quat_is_finite(initial_attitude) ) |
| 85 | { | ||
| 86 | return MAHONY_INVALID_QUATERNION; | ||
| 87 | } | ||
| 88 | |||
| 89 |
2/4✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
|
7 | if ( !isfinite(proportional_gain) || !isfinite(integral_gain) ) |
| 90 | { | ||
| 91 | return MAHONY_NONFINITE_GAIN; | ||
| 92 | } | ||
| 93 | |||
| 94 |
2/4✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
|
7 | if ( proportional_gain < 0.0f || integral_gain < 0.0f ) |
| 95 | { | ||
| 96 | return MAHONY_NEGATIVE_GAIN; | ||
| 97 | } | ||
| 98 | |||
| 99 | 7 | filter->attitude = quat_normalize(initial_attitude); | |
| 100 | 7 | filter->integral_error.x = 0.0f; | |
| 101 | 7 | filter->integral_error.y = 0.0f; | |
| 102 | 7 | filter->integral_error.z = 0.0f; | |
| 103 | 7 | filter->proportional_gain = proportional_gain; | |
| 104 | 7 | filter->integral_gain = integral_gain; | |
| 105 | |||
| 106 | 7 | return MAHONY_OK; | |
| 107 | } /* mahony_init */ | ||
| 108 | |||
| 109 | |||
| 110 | /** | ||
| 111 | * @brief Propagates attitude using body-frame gyroscope measurements. | ||
| 112 | * | ||
| 113 | * Verify the filter, gyro, and timestep are valid. | ||
| 114 | * Convert the body-frame angular velocity into a pure quaternion. | ||
| 115 | * Use the quaternion differential equation to calculate how quickly the | ||
| 116 | * body-to-world attitude is changing. Multiply that derivative by the | ||
| 117 | * timestep, add it to the current attitude, and normalize the result. | ||
| 118 | * | ||
| 119 | * @param filter Initialized filter instance. | ||
| 120 | * @param gyro_body_rad_s Body-frame angular velocity in radians per second. | ||
| 121 | * @param delta_time_s Elapsed time in seconds. | ||
| 122 | * | ||
| 123 | * @return MAHONY_OK when initialization succeeds; otherwise a Mahony status | ||
| 124 | code describing the failure. | ||
| 125 | */ | ||
| 126 | 1177 | MAHONY_STATUS mahony_update_gyro | |
| 127 | ( | ||
| 128 | MAHONY_FILTER *filter, | ||
| 129 | VECTOR_3F gyro_body_rad_s, | ||
| 130 | float delta_time_s | ||
| 131 | ) | ||
| 132 | { | ||
| 133 | 1177 | QUAT angular_velocity; | |
| 134 | 1177 | QUAT attitude_derivative; | |
| 135 | 1177 | QUAT attitude_delta; | |
| 136 | |||
| 137 |
1/2✓ Branch 0 taken 1177 times.
✗ Branch 1 not taken.
|
1177 | if ( filter == NULL ) |
| 138 | { | ||
| 139 | return MAHONY_NULL_POINTER; | ||
| 140 | } | ||
| 141 | |||
| 142 |
1/2✓ Branch 1 taken 1177 times.
✗ Branch 2 not taken.
|
1177 | if ( !quat_is_finite(filter->attitude) ) |
| 143 | { | ||
| 144 | return MAHONY_INVALID_QUATERNION; | ||
| 145 | } | ||
| 146 | |||
| 147 |
1/2✓ Branch 1 taken 1177 times.
✗ Branch 2 not taken.
|
1177 | if ( !vector_is_finite(gyro_body_rad_s) ) |
| 148 | { | ||
| 149 | return MAHONY_INVALID_GYRO; | ||
| 150 | } | ||
| 151 | |||
| 152 |
2/4✓ Branch 0 taken 1177 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1177 times.
✗ Branch 3 not taken.
|
1177 | if ( !isfinite(delta_time_s) || delta_time_s <= 0.0f ) |
| 153 | { | ||
| 154 | return MAHONY_INVALID_DELTA_TIME; | ||
| 155 | } | ||
| 156 | |||
| 157 | /* | ||
| 158 | * The attitude quaternion is a body-to-world rotation and angular velocity | ||
| 159 | * is expressed in the body frame: | ||
| 160 | */ | ||
| 161 | 1177 | angular_velocity.w = 0.0f; | |
| 162 | 1177 | angular_velocity.x = gyro_body_rad_s.x; | |
| 163 | 1177 | angular_velocity.y = gyro_body_rad_s.y; | |
| 164 | 1177 | angular_velocity.z = gyro_body_rad_s.z; | |
| 165 | |||
| 166 | /* q_dot = 0.5 * q * omega_body */ | ||
| 167 | 1177 | attitude_derivative = quat_mult(filter->attitude, angular_velocity); | |
| 168 | 1177 | attitude_derivative = quat_scale(attitude_derivative, 0.5f); | |
| 169 | |||
| 170 | /* Scale q_dot by delta_t, add to attitude, and normalize*/ | ||
| 171 | 1177 | attitude_delta = quat_scale(attitude_derivative, delta_time_s); | |
| 172 | 1177 | filter->attitude = quat_add(filter->attitude, attitude_delta); | |
| 173 | 1177 | filter->attitude = quat_normalize(filter->attitude); | |
| 174 | |||
| 175 | 1177 | return MAHONY_OK; | |
| 176 | |||
| 177 | } /* mahony_update_gyro */ | ||
| 178 | |||
| 179 | |||
| 180 | /** | ||
| 181 | * @brief Updates attitude using gyroscope propagation and accelerometer | ||
| 182 | * proportional feedback. | ||
| 183 | * | ||
| 184 | * The gyroscope must be expressed in the body frame in radians per second. | ||
| 185 | * The accelerometer must be expressed in the body frame. Its magnitude is | ||
| 186 | * removed internally because the filter uses only its measured direction. | ||
| 187 | * | ||
| 188 | * Accelerometer feedback is applied only when the caller enables it and the | ||
| 189 | * measured acceleration magnitude falls within the configured validity range. | ||
| 190 | * Invalid accelerometer samples are ignored while gyro propagation continues. | ||
| 191 | * | ||
| 192 | * @param filter Initialized filter instance. | ||
| 193 | * @param gyro_body_rad_s Body-frame angular velocity in radians per second. | ||
| 194 | * @param accel_body Body-frame accelerometer measurement. | ||
| 195 | * @param delta_time_s Elapsed time in seconds. | ||
| 196 | * @param use_accel Whether accelerometer feedback should be applied. | ||
| 197 | * | ||
| 198 | * @return MAHONY_OK when the attitude was updated; otherwise a Mahony status | ||
| 199 | code describing the failure. | ||
| 200 | */ | ||
| 201 | 1177 | MAHONY_STATUS mahony_update_imu | |
| 202 | ( | ||
| 203 | MAHONY_FILTER *filter, | ||
| 204 | VECTOR_3F gyro_body_rad_s, | ||
| 205 | VECTOR_3F accel_body, | ||
| 206 | float delta_time_s, | ||
| 207 | bool use_accel | ||
| 208 | ) | ||
| 209 | { | ||
| 210 | 1177 | QUAT gravity_world; | |
| 211 | 1177 | QUAT gravity_body_quat; | |
| 212 | |||
| 213 | 1177 | VECTOR_3F gravity_estimated_body; | |
| 214 | 1177 | VECTOR_3F attitude_error; | |
| 215 | 1177 | VECTOR_3F proportional_correction; | |
| 216 | 1177 | VECTOR_3F gyro_corrected; | |
| 217 | |||
| 218 | 1177 | float accel_magnitude; | |
| 219 | |||
| 220 | 1177 | bool accel_valid; | |
| 221 | 1177 | bool apply_accel; | |
| 222 | |||
| 223 |
1/2✓ Branch 0 taken 1177 times.
✗ Branch 1 not taken.
|
1177 | if ( filter == NULL ) |
| 224 | { | ||
| 225 | return MAHONY_NULL_POINTER; | ||
| 226 | } | ||
| 227 | |||
| 228 |
1/2✓ Branch 1 taken 1177 times.
✗ Branch 2 not taken.
|
1177 | if ( !vector_is_finite(gyro_body_rad_s) ) |
| 229 | { | ||
| 230 | return MAHONY_INVALID_GYRO; | ||
| 231 | } | ||
| 232 | |||
| 233 |
2/4✓ Branch 0 taken 1177 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1177 times.
✗ Branch 3 not taken.
|
1177 | if ( !isfinite(delta_time_s) || delta_time_s <= 0.0f ) |
| 234 | { | ||
| 235 | return MAHONY_INVALID_DELTA_TIME; | ||
| 236 | } | ||
| 237 | |||
| 238 | 1177 | accel_magnitude = vector_magnitude(accel_body); | |
| 239 | |||
| 240 | 1177 | accel_valid = vector_is_finite(accel_body) | |
| 241 |
1/2✓ Branch 0 taken 1177 times.
✗ Branch 1 not taken.
|
1177 | && isfinite(accel_magnitude) |
| 242 |
1/2✓ Branch 0 taken 1177 times.
✗ Branch 1 not taken.
|
1177 | && accel_magnitude >= MAHONY_ACCEL_MIN_MAGNITUDE |
| 243 |
2/4✓ Branch 0 taken 1177 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1177 times.
|
2354 | && accel_magnitude <= MAHONY_ACCEL_MAX_MAGNITUDE; |
| 244 | |||
| 245 | 1177 | apply_accel = use_accel && accel_valid; | |
| 246 | |||
| 247 | 1177 | gyro_corrected = gyro_body_rad_s; | |
| 248 | |||
| 249 | /* | ||
| 250 | * Accelerometer feedback is optional. If the measurement is invalid or has | ||
| 251 | * zero magnitude, continue with gyroscope-only propagation. | ||
| 252 | */ | ||
| 253 |
2/4✓ Branch 0 taken 1177 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1177 times.
✗ Branch 4 not taken.
|
1177 | if ( apply_accel && vector_normalize(&accel_body) ) |
| 254 | { | ||
| 255 | /* | ||
| 256 | * The attitude quaternion represents the body-to-world rotation. | ||
| 257 | * Rotate the fixed world gravity direction into the body frame to | ||
| 258 | * predict where gravity should appear according to the estimate. | ||
| 259 | */ | ||
| 260 | 1177 | gravity_world.w = 0.0f; | |
| 261 | 1177 | gravity_world.x = 0.0f; | |
| 262 | 1177 | gravity_world.y = 0.0f; | |
| 263 | 1177 | gravity_world.z = -1.0f; | |
| 264 | |||
| 265 | 1177 | gravity_body_quat = quat_rotate_world_to_body(filter->attitude, gravity_world); | |
| 266 | |||
| 267 | 1177 | gravity_estimated_body.x = gravity_body_quat.x; | |
| 268 | 1177 | gravity_estimated_body.y = gravity_body_quat.y; | |
| 269 | 1177 | gravity_estimated_body.z = gravity_body_quat.z; | |
| 270 | |||
| 271 | /* | ||
| 272 | * measured x estimated produces a correction that drives the | ||
| 273 | * estimated gravity direction toward the measured direction. | ||
| 274 | */ | ||
| 275 | 1177 | attitude_error = vector_cross(accel_body, gravity_estimated_body); | |
| 276 | |||
| 277 | /* | ||
| 278 | * Accumulate persistent attitude error as a gyro-rate correction. Integral | ||
| 279 | * feedback is updated only while accelerometer feedback is both permitted and | ||
| 280 | * valid, preventing high-dynamic or corrupted measurements from winding up | ||
| 281 | * the correction state. | ||
| 282 | */ | ||
| 283 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1177 times.
|
1177 | if ( filter->integral_gain > 0.0f ) |
| 284 | { | ||
| 285 | ✗ | filter->integral_error.x += filter->integral_gain * attitude_error.x * delta_time_s; | |
| 286 | ✗ | filter->integral_error.y += filter->integral_gain * attitude_error.y * delta_time_s; | |
| 287 | /* The filter has no yaw reference, so avoid accumulating drift on the z axis */ | ||
| 288 | /* filter->integral_error.z += filter->integral_gain * attitude_error.z * delta_time_s; */ | ||
| 289 | |||
| 290 | ✗ | filter->integral_error.x = clamp_float(filter->integral_error.x, -MAHONY_INTEGRAL_LIMIT_RAD_S, MAHONY_INTEGRAL_LIMIT_RAD_S); | |
| 291 | ✗ | filter->integral_error.y = clamp_float(filter->integral_error.y, -MAHONY_INTEGRAL_LIMIT_RAD_S, MAHONY_INTEGRAL_LIMIT_RAD_S); | |
| 292 | /* filter->integral_error.z = clamp_float(filter->integral_error.z, -MAHONY_INTEGRAL_LIMIT_RAD_S, MAHONY_INTEGRAL_LIMIT_RAD_S); */ | ||
| 293 | } | ||
| 294 | |||
| 295 | 1177 | proportional_correction = vector_scale(attitude_error, filter->proportional_gain); | |
| 296 | |||
| 297 | 1177 | gyro_corrected = vector_add(gyro_corrected, proportional_correction); | |
| 298 | 1177 | gyro_corrected = vector_add(gyro_corrected, filter->integral_error); | |
| 299 | } | ||
| 300 | |||
| 301 | 1177 | return mahony_update_gyro(filter, gyro_corrected, delta_time_s); | |
| 302 | |||
| 303 | } /* mahony_update_imu */ | ||
| 304 | |||
| 305 | /******************************************************************************* | ||
| 306 | * END OF FILE | ||
| 307 | ******************************************************************************/ | ||
| 308 |