math_sdr/math_sdr.c
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /** | ||
| 2 | ****************************************************************************** | ||
| 3 | * @file : math_sdr.c | ||
| 4 | * @brief : Contains math and utility functions for SDR code. | ||
| 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 | */ | ||
| 20 | |||
| 21 | /* Standard Includes ---------------------------------------------------------*/ | ||
| 22 | #include <math.h> | ||
| 23 | |||
| 24 | /* Project Includes ----------------------------------------------------------*/ | ||
| 25 | #include "main.h" | ||
| 26 | #include "math_sdr.h" | ||
| 27 | |||
| 28 | /* API Functions -------------------------------------------------------------*/ | ||
| 29 | |||
| 30 | /** | ||
| 31 | * @brief Computes a CRC-32 checksum over a block of data. | ||
| 32 | * | ||
| 33 | * @param[in] data Pointer to the input data buffer. | ||
| 34 | * @param[in] len Number of bytes in the buffer. | ||
| 35 | * | ||
| 36 | * @return The 32-bit CRC checksum of the input data. | ||
| 37 | */ | ||
| 38 | 2 | uint32_t crc32 | |
| 39 | ( | ||
| 40 | const uint8_t *data, | ||
| 41 | size_t len | ||
| 42 | ) | ||
| 43 | { | ||
| 44 | 2 | uint32_t crc = 0xFFFFFFFF; | |
| 45 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 2 times.
|
11 | while (len--) |
| 46 | { | ||
| 47 | 9 | crc ^= *data++; | |
| 48 |
2/2✓ Branch 0 taken 72 times.
✓ Branch 1 taken 9 times.
|
81 | for (int i = 0; i < 8; ++i) |
| 49 | 72 | crc = (crc >> 1) ^ (0x82F63B78 & -(crc & 1)); | |
| 50 | } | ||
| 51 | 2 | return ~crc; | |
| 52 | |||
| 53 | } /* crc32 */ | ||
| 54 | |||
| 55 | /** | ||
| 56 | * @brief Clamps a floating-point value between minimum and maximum bounds. | ||
| 57 | * | ||
| 58 | * @param value Value to clamp. | ||
| 59 | * @param minimum Minimum value. | ||
| 60 | * @param maximum Maximum value. | ||
| 61 | */ | ||
| 62 | 31 | float clamp_float | |
| 63 | ( | ||
| 64 | float value, | ||
| 65 | float minimum, | ||
| 66 | float maximum | ||
| 67 | ) | ||
| 68 | { | ||
| 69 |
2/2✓ Branch 0 taken 29 times.
✓ Branch 1 taken 2 times.
|
31 | if ( value < minimum ) |
| 70 | { | ||
| 71 | return minimum; | ||
| 72 | } | ||
| 73 | |||
| 74 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 28 times.
|
29 | if ( value > maximum ) |
| 75 | { | ||
| 76 | 1 | return maximum; | |
| 77 | } | ||
| 78 | |||
| 79 | return value; | ||
| 80 | |||
| 81 | } /* clamp_float */ | ||
| 82 | |||
| 83 | /* Quaternions ---------------------------------------------------------------*/ | ||
| 84 | |||
| 85 | /** | ||
| 86 | * @brief Converts ZYX Euler angles to a quaternion. | ||
| 87 | * | ||
| 88 | * @note Angles must be provided in radians. | ||
| 89 | * @note The rotation order is ZYX (standard aerospace convention). | ||
| 90 | * @todo The rotation sequence has not been validated yet. | ||
| 91 | * | ||
| 92 | * @param yaw Rotation about the Z axis in radians. | ||
| 93 | * @param pitch Rotation about the Y axis in radians. | ||
| 94 | * @param roll Rotation about the X axis in radians. | ||
| 95 | * | ||
| 96 | * @return The quaternion representing the rotation. | ||
| 97 | */ | ||
| 98 | 18 | QUAT eul_to_quat | |
| 99 | ( | ||
| 100 | float yaw, | ||
| 101 | float pitch, | ||
| 102 | float roll | ||
| 103 | ) | ||
| 104 | { | ||
| 105 | 18 | float cos_yaw = cosf(yaw / 2.0f); | |
| 106 | 18 | float cos_pitch = cosf(pitch / 2.0f); | |
| 107 | 18 | float cos_roll = cosf(roll / 2.0f); | |
| 108 | |||
| 109 | 18 | float sin_yaw = sinf(yaw / 2.0f); | |
| 110 | 18 | float sin_pitch = sinf(pitch / 2.0f); | |
| 111 | 18 | float sin_roll = sinf(roll / 2.0f); | |
| 112 | |||
| 113 | 18 | QUAT q; | |
| 114 | 18 | q.w = cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw; | |
| 115 | 18 | q.x = sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw; | |
| 116 | 18 | q.y = cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw; | |
| 117 | 18 | q.z = cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw; | |
| 118 | |||
| 119 | 18 | return q; | |
| 120 | |||
| 121 | } /* eul_to_quat */ | ||
| 122 | |||
| 123 | |||
| 124 | /** | ||
| 125 | * @brief Hamilton product of two quaternions. | ||
| 126 | * | ||
| 127 | * @note Quaternion multiplication is NOT commutative. | ||
| 128 | * | ||
| 129 | * @param a The left-hand quaternion. | ||
| 130 | * @param b The right-hand quaternion. | ||
| 131 | * | ||
| 132 | * @return The quaternion product a * b. | ||
| 133 | */ | ||
| 134 | 38097 | QUAT quat_mult | |
| 135 | ( | ||
| 136 | QUAT a, | ||
| 137 | QUAT b | ||
| 138 | ) | ||
| 139 | { | ||
| 140 | 38097 | QUAT result; | |
| 141 | |||
| 142 | 38097 | result.w = (a.w * b.w) - (a.x * b.x) - (a.y * b.y) - (a.z * b.z); | |
| 143 | 38097 | result.x = (a.w * b.x) + (a.x * b.w) + (a.y * b.z) - (a.z * b.y); | |
| 144 | 38097 | result.y = (a.w * b.y) - (a.x * b.z) + (a.y * b.w) + (a.z * b.x); | |
| 145 | 38097 | result.z = (a.w * b.z) + (a.x * b.y) - (a.y * b.x) + (a.z * b.w); | |
| 146 | |||
| 147 | 38097 | return result; | |
| 148 | |||
| 149 | } /* quat_mult */ | ||
| 150 | |||
| 151 | |||
| 152 | /** | ||
| 153 | * @brief Provides the dot product of two quaternions. | ||
| 154 | * | ||
| 155 | * @note Quaternion dot products are commutative. | ||
| 156 | * | ||
| 157 | * @param a The left-hand quaternion. | ||
| 158 | * @param b The right-hand quaternion. | ||
| 159 | * | ||
| 160 | * @return The quaternion scalar product a . b | ||
| 161 | */ | ||
| 162 | 1 | float quat_dot | |
| 163 | ( | ||
| 164 | QUAT a, | ||
| 165 | QUAT b | ||
| 166 | ) | ||
| 167 | { | ||
| 168 | 1 | return( a.w * b.w + | |
| 169 | 1 | a.x * b.x + | |
| 170 | 1 | a.y * b.y + | |
| 171 | 1 | a.z * b.z ); | |
| 172 | |||
| 173 | } /* quat_mult */ | ||
| 174 | |||
| 175 | |||
| 176 | /** | ||
| 177 | * @brief Adds two quaternions component-wise. | ||
| 178 | * | ||
| 179 | * @param a The first quaternion. | ||
| 180 | * @param b The second quaternion. | ||
| 181 | * | ||
| 182 | * @return The quaternion sum a + b. | ||
| 183 | */ | ||
| 184 | 20031 | QUAT quat_add | |
| 185 | ( | ||
| 186 | QUAT a, | ||
| 187 | QUAT b | ||
| 188 | ) | ||
| 189 | { | ||
| 190 | 20031 | QUAT result; | |
| 191 | |||
| 192 | 20031 | result.w = a.w + b.w; | |
| 193 | 20031 | result.x = a.x + b.x; | |
| 194 | 20031 | result.y = a.y + b.y; | |
| 195 | 20031 | result.z = a.z + b.z; | |
| 196 | |||
| 197 | 20031 | return result; | |
| 198 | |||
| 199 | } /* quat_add */ | ||
| 200 | |||
| 201 | |||
| 202 | /** | ||
| 203 | * @brief Scales a quaternion by a scalar value. | ||
| 204 | * | ||
| 205 | * @param q The quaternion to scale. | ||
| 206 | * @param s The scalar factor. | ||
| 207 | * | ||
| 208 | * @return The quaternion @p q scaled by @p s (q * s). | ||
| 209 | */ | ||
| 210 | 40061 | QUAT quat_scale | |
| 211 | ( | ||
| 212 | QUAT q, | ||
| 213 | float s | ||
| 214 | ) | ||
| 215 | { | ||
| 216 | 40061 | QUAT result; | |
| 217 | |||
| 218 | 40061 | result.w = q.w * s; | |
| 219 | 40061 | result.x = q.x * s; | |
| 220 | 40061 | result.y = q.y * s; | |
| 221 | 40061 | result.z = q.z * s; | |
| 222 | |||
| 223 | 40061 | return result; | |
| 224 | |||
| 225 | } /* quat_scale */ | ||
| 226 | |||
| 227 | |||
| 228 | /** | ||
| 229 | * @brief Normalizes a quaternion to unit length. | ||
| 230 | * | ||
| 231 | * @param q The quaternion to normalize. | ||
| 232 | * | ||
| 233 | * @return The normalized unit quaternion. | ||
| 234 | * | ||
| 235 | * Divides each component by the quaternion's norm. If the norm is | ||
| 236 | * zero (e.g. a zero-initialized quaternion), returns the identity | ||
| 237 | * quaternion (1, 0, 0, 0) to avoid a divide-by-zero. Values near | ||
| 238 | * but not equal to zero are divided normally and may exhibit | ||
| 239 | * floating-point roundoff. | ||
| 240 | */ | ||
| 241 | 20086 | QUAT quat_normalize | |
| 242 | ( | ||
| 243 | QUAT q | ||
| 244 | ) | ||
| 245 | { | ||
| 246 | 20086 | QUAT result; | |
| 247 | |||
| 248 | 20086 | float norm = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); | |
| 249 | |||
| 250 | /* Fallback for zero quaternion: initialize to identity quaternion */ | ||
| 251 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 20084 times.
|
20086 | if ( norm == 0.0f ) |
| 252 | { | ||
| 253 | 2 | result.w = 1.0f; | |
| 254 | 2 | result.x = 0.0f; | |
| 255 | 2 | result.y = 0.0f; | |
| 256 | 2 | result.z = 0.0f; | |
| 257 | } | ||
| 258 | else | ||
| 259 | { | ||
| 260 | 20084 | result.w = q.w / norm; | |
| 261 | 20084 | result.x = q.x / norm; | |
| 262 | 20084 | result.y = q.y / norm; | |
| 263 | 20084 | result.z = q.z / norm; | |
| 264 | } | ||
| 265 | |||
| 266 | 20086 | return result; | |
| 267 | |||
| 268 | } /* quat_normalize */ | ||
| 269 | |||
| 270 | |||
| 271 | /** | ||
| 272 | * @brief Computes the conjugate of a quaternion. | ||
| 273 | * | ||
| 274 | * @param q The input quaternion. | ||
| 275 | * | ||
| 276 | * @return The conjugate quaternion (w, -x, -y, -z). | ||
| 277 | */ | ||
| 278 | 9034 | QUAT quat_conj | |
| 279 | ( | ||
| 280 | QUAT q | ||
| 281 | ) | ||
| 282 | { | ||
| 283 | 9034 | QUAT result = { q.w, -q.x, -q.y, -q.z }; | |
| 284 | 9034 | return result; | |
| 285 | |||
| 286 | } /* quat_conj */ | ||
| 287 | |||
| 288 | /** | ||
| 289 | * @brief Rotates a vector from the world frame into the body frame. | ||
| 290 | * | ||
| 291 | * The attitude quaternion represents the body-to-world rotation: | ||
| 292 | * | ||
| 293 | * vector_body = conjugate(q) * vector_world * q | ||
| 294 | * | ||
| 295 | * @param attitude Body-to-world attitude quaternion. | ||
| 296 | * @param vector_world Pure quaternion containing the world-frame vector. | ||
| 297 | * | ||
| 298 | * @return Pure quaternion containing the body-frame vector. | ||
| 299 | */ | ||
| 300 | 9018 | QUAT quat_rotate_world_to_body | |
| 301 | ( | ||
| 302 | QUAT attitude, | ||
| 303 | QUAT vector_world | ||
| 304 | ) | ||
| 305 | { | ||
| 306 | 9018 | QUAT attitude_conj = quat_conj(attitude); | |
| 307 | |||
| 308 | 9018 | return quat_mult | |
| 309 | ( | ||
| 310 | quat_mult(attitude_conj, vector_world), | ||
| 311 | attitude | ||
| 312 | ); | ||
| 313 | |||
| 314 | } /* quat_rotate_world_to_body */ | ||
| 315 | |||
| 316 | |||
| 317 | /** | ||
| 318 | * @brief Rotates a vector from the body frame into the world frame. | ||
| 319 | * | ||
| 320 | * The attitude quaternion represents the body-to-world rotation: | ||
| 321 | * | ||
| 322 | * vector_world = q * vector_body * conjugate(q) | ||
| 323 | * | ||
| 324 | * @param attitude Body-to-world attitude quaternion. | ||
| 325 | * @param vector_body Pure quaternion containing the body-frame vector. | ||
| 326 | * | ||
| 327 | * @return Pure quaternion containing the world-frame vector. | ||
| 328 | */ | ||
| 329 | 15 | QUAT quat_rotate_body_to_world | |
| 330 | ( | ||
| 331 | QUAT attitude, | ||
| 332 | QUAT vector_body | ||
| 333 | ) | ||
| 334 | { | ||
| 335 | 15 | QUAT attitude_conj = quat_conj(attitude); | |
| 336 | |||
| 337 | 15 | return quat_mult | |
| 338 | ( | ||
| 339 | quat_mult(attitude, vector_body), | ||
| 340 | attitude_conj | ||
| 341 | ); | ||
| 342 | |||
| 343 | } /* quat_rotate_body_to_world */ | ||
| 344 | |||
| 345 | /** | ||
| 346 | * @brief Determines whether every quaternion component is finite. | ||
| 347 | */ | ||
| 348 | 20070 | bool quat_is_finite | |
| 349 | ( | ||
| 350 | QUAT quaternion | ||
| 351 | ) | ||
| 352 | { | ||
| 353 | 20070 | return | |
| 354 | ( | ||
| 355 | 20070 | isfinite(quaternion.w) | |
| 356 |
2/2✓ Branch 0 taken 20068 times.
✓ Branch 1 taken 1 time.
|
20069 | && isfinite(quaternion.x) |
| 357 |
1/2✓ Branch 0 taken 20068 times.
✗ Branch 1 not taken.
|
20068 | && isfinite(quaternion.y) |
| 358 |
4/4✓ Branch 0 taken 20069 times.
✓ Branch 1 taken 1 time.
✓ Branch 2 taken 1 time.
✓ Branch 3 taken 20067 times.
|
40138 | && isfinite(quaternion.z) |
| 359 | ); | ||
| 360 | |||
| 361 | } /* quat_is_finite */ | ||
| 362 | |||
| 363 | /* Vectors -------------------------------------------------------------------*/ | ||
| 364 | |||
| 365 | /** | ||
| 366 | * @brief Determines whether every vector component is finite. | ||
| 367 | */ | ||
| 368 | 59086 | bool vector_is_finite | |
| 369 | ( | ||
| 370 | VECTOR_3F vector | ||
| 371 | ) | ||
| 372 | { | ||
| 373 | 59086 | return | |
| 374 | ( | ||
| 375 | 59086 | isfinite(vector.x) | |
| 376 |
2/2✓ Branch 0 taken 59084 times.
✓ Branch 1 taken 1 time.
|
59085 | && isfinite(vector.y) |
| 377 |
3/4✓ Branch 0 taken 59085 times.
✓ Branch 1 taken 1 time.
✗ Branch 2 not taken.
✓ Branch 3 taken 59084 times.
|
118170 | && isfinite(vector.z) |
| 378 | ); | ||
| 379 | |||
| 380 | } /* vector_is_finite */ | ||
| 381 | |||
| 382 | /** | ||
| 383 | * @brief Calculates the magnitude of a three-dimensional vector. | ||
| 384 | */ | ||
| 385 | 24035 | float vector_magnitude | |
| 386 | ( | ||
| 387 | VECTOR_3F vector | ||
| 388 | ) | ||
| 389 | { | ||
| 390 | 24035 | return sqrtf | |
| 391 | ( | ||
| 392 | 24035 | vector.x * vector.x + | |
| 393 | 24035 | vector.y * vector.y + | |
| 394 | 24035 | vector.z * vector.z | |
| 395 | ); | ||
| 396 | |||
| 397 | } /* vector_magnitude */ | ||
| 398 | |||
| 399 | /** | ||
| 400 | * @brief Normalizes a three-dimensional vector in place. | ||
| 401 | */ | ||
| 402 | 9016 | bool vector_normalize | |
| 403 | ( | ||
| 404 | VECTOR_3F *vector | ||
| 405 | ) | ||
| 406 | { | ||
| 407 | 9016 | float magnitude; | |
| 408 | |||
| 409 |
1/2✓ Branch 0 taken 9016 times.
✗ Branch 1 not taken.
|
9016 | if ( vector == NULL ) |
| 410 | { | ||
| 411 | return false; | ||
| 412 | } | ||
| 413 | |||
| 414 |
1/2✓ Branch 0 taken 9016 times.
✗ Branch 1 not taken.
|
9016 | if ( !vector_is_finite(*vector) ) |
| 415 | { | ||
| 416 | return false; | ||
| 417 | } | ||
| 418 | |||
| 419 | 9016 | magnitude = vector_magnitude(*vector); | |
| 420 | |||
| 421 |
3/4✓ Branch 0 taken 9015 times.
✓ Branch 1 taken 1 time.
✓ Branch 2 taken 9015 times.
✗ Branch 3 not taken.
|
9016 | if ( magnitude <= 0.0f || !isfinite(magnitude) ) |
| 422 | { | ||
| 423 | return false; | ||
| 424 | } | ||
| 425 | |||
| 426 | 9015 | vector->x /= magnitude; | |
| 427 | 9015 | vector->y /= magnitude; | |
| 428 | 9015 | vector->z /= magnitude; | |
| 429 | |||
| 430 | 9015 | return true; | |
| 431 | |||
| 432 | } /* vector_normalize */ | ||
| 433 | |||
| 434 | |||
| 435 | /** | ||
| 436 | * @brief Calculates the cross product of two three-dimensional vectors. | ||
| 437 | */ | ||
| 438 | 9015 | VECTOR_3F vector_cross | |
| 439 | ( | ||
| 440 | VECTOR_3F a, | ||
| 441 | VECTOR_3F b | ||
| 442 | ) | ||
| 443 | { | ||
| 444 | 9015 | VECTOR_3F result; | |
| 445 | |||
| 446 | 9015 | result.x = a.y * b.z - a.z * b.y; | |
| 447 | 9015 | result.y = a.z * b.x - a.x * b.z; | |
| 448 | 9015 | result.z = a.x * b.y - a.y * b.x; | |
| 449 | |||
| 450 | 9015 | return result; | |
| 451 | |||
| 452 | } /* vector_cross */ | ||
| 453 | |||
| 454 | |||
| 455 | /** | ||
| 456 | * @brief Adds two three-dimensional vectors. | ||
| 457 | */ | ||
| 458 | 18029 | VECTOR_3F vector_add | |
| 459 | ( | ||
| 460 | VECTOR_3F a, | ||
| 461 | VECTOR_3F b | ||
| 462 | ) | ||
| 463 | { | ||
| 464 | 18029 | VECTOR_3F result; | |
| 465 | |||
| 466 | 18029 | result.x = a.x + b.x; | |
| 467 | 18029 | result.y = a.y + b.y; | |
| 468 | 18029 | result.z = a.z + b.z; | |
| 469 | |||
| 470 | 18029 | return result; | |
| 471 | |||
| 472 | } /* vector_add */ | ||
| 473 | |||
| 474 | |||
| 475 | /** | ||
| 476 | * @brief Scales a three-dimensional vector by a scalar. | ||
| 477 | */ | ||
| 478 | 9015 | VECTOR_3F vector_scale | |
| 479 | ( | ||
| 480 | VECTOR_3F vector, | ||
| 481 | float scalar | ||
| 482 | ) | ||
| 483 | { | ||
| 484 | 9015 | VECTOR_3F result; | |
| 485 | |||
| 486 | 9015 | result.x = vector.x * scalar; | |
| 487 | 9015 | result.y = vector.y * scalar; | |
| 488 | 9015 | result.z = vector.z * scalar; | |
| 489 | |||
| 490 | 9015 | return result; | |
| 491 | |||
| 492 | } /* vector_scale */ | ||
| 493 | |||
| 494 | /******************************************************************************* | ||
| 495 | * END OF FILE * | ||
| 496 | *******************************************************************************/ | ||
| 497 |