driver/imu/imu_dflt.c
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /** | ||
| 2 | ****************************************************************************** | ||
| 3 | * @file : imu_dflt.c | ||
| 4 | * @brief : Default Unified IMU System Translation Implementation | ||
| 5 | * @author : Sun Devil Rocketry Firmware Team | ||
| 6 | * | ||
| 7 | * @note Default implementation of the imu.h contract. This file serves as | ||
| 8 | * an reference; projects requiring custom configurations— | ||
| 9 | * such as mid-flight state transitions or alternate ODRs—should exclude | ||
| 10 | * this file from their build and supply a project-specific translation layer. | ||
| 11 | * | ||
| 12 | * Unit Translation: Converts backend-specific native units (such as | ||
| 13 | * the SI units reported by imu_lsm.c) into the unified contract units | ||
| 14 | * of IMU_FlightData (accelerometer in [m/s^2], gyroscope in [dps]). | ||
| 15 | * | ||
| 16 | ****************************************************************************** | ||
| 17 | * @attention | ||
| 18 | * Copyright (c) 2026 Sun Devil Rocketry. All rights reserved. | ||
| 19 | * This software is licensed under terms found in the LICENSE file in the root | ||
| 20 | * directory of this component. If absent, BSD-3-Clause applies: | ||
| 21 | * https://opensource.org/license/bsd-3-clause | ||
| 22 | ****************************************************************************** | ||
| 23 | */ | ||
| 24 | |||
| 25 | #include "imu.h" | ||
| 26 | |||
| 27 | #if defined( A0010 ) | ||
| 28 | |||
| 29 | #include "imu_lsm.h" | ||
| 30 | |||
| 31 | IMU_SYS_STATUS imu_system_init | ||
| 32 | ( | ||
| 33 | void | ||
| 34 | ) | ||
| 35 | { | ||
| 36 | IMU_CONFIG config = | ||
| 37 | { | ||
| 38 | .odr = IMU_ODR_1920HZ, | ||
| 39 | .acc_fs = IMU_ACC_FS_16G, | ||
| 40 | .gyro_fs = IMU_GYRO_FS_2000DPS, | ||
| 41 | .acc_mode = IMU_ACC_MODE_HP, | ||
| 42 | .gyro_mode = IMU_GYRO_MODE_HP, | ||
| 43 | .sflp_enable = true, | ||
| 44 | .fifo_enable = true | ||
| 45 | }; | ||
| 46 | |||
| 47 | if ( imu_init( &config ) != IMU_OK ) | ||
| 48 | { | ||
| 49 | return IMU_SYS_INIT_FAIL; | ||
| 50 | } | ||
| 51 | return IMU_SYS_OK; | ||
| 52 | } | ||
| 53 | |||
| 54 | IMU_SYS_STATUS imu_system_update | ||
| 55 | ( | ||
| 56 | IMU_FlightData* out | ||
| 57 | ) | ||
| 58 | { | ||
| 59 | if ( !imu_has_new_data() ) | ||
| 60 | { | ||
| 61 | return IMU_SYS_BUSY; | ||
| 62 | } | ||
| 63 | |||
| 64 | IMU_RAW raw; | ||
| 65 | IMU_SFLP_DATA sflp; | ||
| 66 | |||
| 67 | if ( imu_get_latest( &raw, &sflp ) != IMU_OK ) | ||
| 68 | { | ||
| 69 | return IMU_SYS_FAIL; | ||
| 70 | } | ||
| 71 | |||
| 72 | IMU_DATA physical; | ||
| 73 | imu_scale_raw( &raw, &physical ); | ||
| 74 | |||
| 75 | /* imu_lsm.c reports accel in m/s^2 & gyro in dps */ | ||
| 76 | out->accel[0] = physical.accel_x; | ||
| 77 | out->accel[1] = physical.accel_y; | ||
| 78 | out->accel[2] = physical.accel_z; | ||
| 79 | |||
| 80 | out->gyro[0] = physical.gyro_x; | ||
| 81 | out->gyro[1] = physical.gyro_y; | ||
| 82 | out->gyro[2] = physical.gyro_z; | ||
| 83 | |||
| 84 | out->quaternion[0] = sflp.quat_w; | ||
| 85 | out->quaternion[1] = sflp.quat_x; | ||
| 86 | out->quaternion[2] = sflp.quat_y; | ||
| 87 | out->quaternion[3] = sflp.quat_z; | ||
| 88 | |||
| 89 | out->quaternion_valid = true; | ||
| 90 | out->is_valid = true; | ||
| 91 | |||
| 92 | return IMU_SYS_OK; | ||
| 93 | } | ||
| 94 | |||
| 95 | /* ------------------------------------------------------------------- | ||
| 96 | Example DMA Completion / Error Callbacks (Reference Only) | ||
| 97 | ------------------------------------------------------------------- | ||
| 98 | The IMU driver's non-blocking async path expects routing from | ||
| 99 | project's global HAL SPI callbacks to the driver's ISR hooks. | ||
| 100 | |||
| 101 | To avoid linker collisions with other peripherals sharing the SPI | ||
| 102 | bus, do not define these globally in the driver. Instead, integrate | ||
| 103 | them into your project's existing callbacks as shown below: | ||
| 104 | |||
| 105 | void HAL_SPI_TxRxCpltCallback( SPI_HandleTypeDef* hspi ) | ||
| 106 | { | ||
| 107 | if ( hspi == &IMU_SPI ) | ||
| 108 | { | ||
| 109 | imu_process_async_cb(); | ||
| 110 | } | ||
| 111 | } | ||
| 112 | |||
| 113 | void HAL_SPI_ErrorCallback( SPI_HandleTypeDef* hspi ) | ||
| 114 | { | ||
| 115 | if ( hspi == &IMU_SPI ) | ||
| 116 | { | ||
| 117 | imu_process_async_error_cb(); | ||
| 118 | } | ||
| 119 | } | ||
| 120 | */ | ||
| 121 | |||
| 122 | #else /* Legacy Target */ | ||
| 123 | |||
| 124 | #include "imu_legacy.h" | ||
| 125 | #include "math_sdr.h" | ||
| 126 | |||
| 127 | #define LEGACY_ACCEL_SCALE ( 16.0f / 32768.0f ) /* raw -> g */ | ||
| 128 | #define LEGACY_GYRO_SCALE ( 2000.0f / 32768.0f ) /* raw -> dps */ | ||
| 129 | |||
| 130 | ✗ | IMU_SYS_STATUS imu_system_init | |
| 131 | ( | ||
| 132 | void | ||
| 133 | ) | ||
| 134 | { | ||
| 135 | ✗ | IMU_CONFIG config = | |
| 136 | { | ||
| 137 | .sensor_enable = IMU_ENABLE_GYRO_ACC_TEMP, | ||
| 138 | .acc_odr = IMU_ODR_100, | ||
| 139 | .gyro_odr = IMU_ODR_100, | ||
| 140 | .acc_filter = IMU_FILTER_NORM_AVG4, | ||
| 141 | .gyro_filter = IMU_FILTER_NORM_AVG4, | ||
| 142 | .acc_filter_mode = IMU_FILTER_FILTER_MODE, | ||
| 143 | .gyro_filter_mode = IMU_FILTER_FILTER_MODE, | ||
| 144 | .acc_range = IMU_ACC_RANGE_16G, | ||
| 145 | .gyro_range = IMU_GYRO_RANGE_2000, | ||
| 146 | .mag_op_mode = MAG_SLEEP_MODE | ||
| 147 | }; | ||
| 148 | |||
| 149 | ✗ | if ( imu_init( &config ) != IMU_OK ) | |
| 150 | { | ||
| 151 | ✗ | return IMU_SYS_INIT_FAIL; | |
| 152 | } | ||
| 153 | return IMU_SYS_OK; | ||
| 154 | } | ||
| 155 | |||
| 156 | ✗ | IMU_SYS_STATUS imu_system_update | |
| 157 | ( | ||
| 158 | IMU_FlightData* out | ||
| 159 | ) | ||
| 160 | { | ||
| 161 | ✗ | IMU_RAW raw; | |
| 162 | ✗ | IMU_STATUS status; | |
| 163 | |||
| 164 | #ifdef A0002_REV2 | ||
| 165 | ✗ | status = imu_get_accel_and_gyro( &raw ); | |
| 166 | #else | ||
| 167 | status = imu_get_accel_xyz( &raw ); | ||
| 168 | if ( status == IMU_OK ) | ||
| 169 | { | ||
| 170 | status = imu_get_gyro_xyz( &raw ); | ||
| 171 | } | ||
| 172 | #endif | ||
| 173 | |||
| 174 | ✗ | if ( status != IMU_OK ) | |
| 175 | { | ||
| 176 | return IMU_SYS_FAIL; | ||
| 177 | } | ||
| 178 | |||
| 179 | /* Legacy driver reports accel in [g] - scale up to the unified | ||
| 180 | contract's SI [m/s^2]. Gyro is already [dps] */ | ||
| 181 | ✗ | out->accel[0] = (float)raw.accel_x * LEGACY_ACCEL_SCALE * GRAVITY; | |
| 182 | ✗ | out->accel[1] = (float)raw.accel_y * LEGACY_ACCEL_SCALE * GRAVITY; | |
| 183 | ✗ | out->accel[2] = (float)raw.accel_z * LEGACY_ACCEL_SCALE * GRAVITY; | |
| 184 | |||
| 185 | ✗ | out->gyro[0] = (float)raw.gyro_x * LEGACY_GYRO_SCALE; | |
| 186 | ✗ | out->gyro[1] = (float)raw.gyro_y * LEGACY_GYRO_SCALE; | |
| 187 | ✗ | out->gyro[2] = (float)raw.gyro_z * LEGACY_GYRO_SCALE; | |
| 188 | |||
| 189 | ✗ | out->quaternion[0] = 0.0f; | |
| 190 | ✗ | out->quaternion[1] = 0.0f; | |
| 191 | ✗ | out->quaternion[2] = 0.0f; | |
| 192 | ✗ | out->quaternion[3] = 0.0f; | |
| 193 | |||
| 194 | ✗ | out->quaternion_valid = false; | |
| 195 | ✗ | out->is_valid = true; | |
| 196 | |||
| 197 | ✗ | return IMU_SYS_OK; | |
| 198 | } | ||
| 199 | |||
| 200 | #endif | ||
| 201 | |||
| 202 | /******************************************************************************* | ||
| 203 | * END OF FILE * | ||
| 204 | *******************************************************************************/ | ||
| 205 |