From 145137aa532a462899e91ee2bdff9a9cdd918136 Mon Sep 17 00:00:00 2001 From: ETSsound <147210601+ETSsound@users.noreply.github.com> Date: Thu, 7 May 2026 08:21:17 -0700 Subject: [PATCH 01/29] Decrease size of telemetry data structure --- commands/commands.c | 27 ++++++++------------------- commands/commands.h | 25 ++++++++----------------- error_sdr/error_sdr.c | 23 +++++++++++++++-------- error_sdr/error_sdr.h | 26 +++++++++++++++++++++----- telemetry/telemetry.c | 2 +- telemetry/telemetry.h | 8 ++++---- 6 files changed, 57 insertions(+), 54 deletions(-) diff --git a/commands/commands.c b/commands/commands.c index ed8762d..eebe5d2 100644 --- a/commands/commands.c +++ b/commands/commands.c @@ -165,29 +165,18 @@ void dashboard_construct_dump DASHBOARD_DUMP_TYPE* dump_buffer_ptr /* must be DASHBOARD_DUMP_SIZE */ ) { - -/* IMU (6 axes) */ -memcpy( dump_buffer_ptr, - &(sensor_data.imu_data.imu_converted), - sizeof( float ) * 6 ); - -/* Roll/Pitch + Rates */ -dump_buffer_ptr->pitch_angle = sensor_data.imu_data.state_estimate.pitch_angle; -dump_buffer_ptr->roll_angle = sensor_data.imu_data.state_estimate.roll_angle; -dump_buffer_ptr->yaw_angle = sensor_data.imu_data.state_estimate.yaw_angle; -dump_buffer_ptr->pitch_rate = sensor_data.imu_data.state_estimate.pitch_rate; -dump_buffer_ptr->roll_rate = sensor_data.imu_data.state_estimate.roll_rate; -dump_buffer_ptr->yaw_rate = sensor_data.imu_data.state_estimate.yaw_rate; +/* Quats (TODO) */ /* Baro */ -dump_buffer_ptr->baro_pressure = sensor_data.baro_pressure; -dump_buffer_ptr->baro_temp = sensor_data.baro_temp; -dump_buffer_ptr->baro_alt = sensor_data.baro_alt; -dump_buffer_ptr->baro_velo = sensor_data.baro_velo; +dump_buffer_ptr->alt = sensor_data.baro_alt; /* GPS */ -dump_buffer_ptr->gps_dec_longitude = sensor_data.gps_dec_longitude; -dump_buffer_ptr->gps_dec_latitude = sensor_data.gps_dec_latitude; +dump_buffer_ptr->longitude = sensor_data.gps_dec_longitude; +dump_buffer_ptr->latitude = sensor_data.gps_dec_latitude; + +/* Controls */ +dump_buffer_ptr->acc_x = sensor_data.imu_data.imu_converted.accel_x; +dump_buffer_ptr->roll_rate = sensor_data.imu_data.state_estimate.roll_rate; } #endif diff --git a/commands/commands.h b/commands/commands.h index 95e82b6..2a0f783 100644 --- a/commands/commands.h +++ b/commands/commands.h @@ -99,26 +99,17 @@ extern "C" { typedef struct __attribute__((packed)) _DASHBOARD_DUMP_TYPE { + float quat_w; + float quat_x; + float quat_y; + float quat_z; + float alt; + float latitude; + float longitude; float acc_x; - float acc_y; - float acc_z; - float gyro_x; - float gyro_y; - float gyro_z; - float roll_angle; - float pitch_angle; - float yaw_angle; float roll_rate; - float pitch_rate; - float yaw_rate; - float baro_pressure; - float baro_temp; - float baro_alt; - float baro_velo; - float gps_dec_longitude; - float gps_dec_latitude; } DASHBOARD_DUMP_TYPE; - _Static_assert( sizeof(DASHBOARD_DUMP_TYPE) == 72, "DASHBOARD_DUMP_TYPE size invalid."); + _Static_assert( sizeof(DASHBOARD_DUMP_TYPE) == 36, "DASHBOARD_DUMP_TYPE size invalid."); /*------------------------------------------------------------------------------ Function Prototypes diff --git a/error_sdr/error_sdr.c b/error_sdr/error_sdr.c index 064640f..2ca696b 100644 --- a/error_sdr/error_sdr.c +++ b/error_sdr/error_sdr.c @@ -141,19 +141,23 @@ default_error_handler.error_callback( error_code ); /******************************************************************************* * * * PROCEDURE: * -* error_log_warning * +* __sdr_log_warning * * * * DESCRIPTION: * * Place a warning message in the buffer. * * * +* NOTE: * +* Should be called through the error_log_warning macro for bounds * +* checking. * +* * *******************************************************************************/ -void error_log_warning +void __sdr_log_warning ( - char* message + const char* message ) { last_warning.systick = HAL_GetTick(); -memcpy( last_warning.message, message, 72 ); +memcpy( last_warning.message, message, 36 ); is_pending_warning = true; } /* error_log_warning */ @@ -162,19 +166,22 @@ is_pending_warning = true; /******************************************************************************* * * * PROCEDURE: * -* error_log_info * +* __sdr_log_info * * * * DESCRIPTION: * * Place a warning message in the buffer. * * * +* NOTE: * +* Should be called through the error_log_info macro for bounds checking. * +* * *******************************************************************************/ -void error_log_info +void __sdr_log_info ( - char* message + const char* message ) { last_info.systick = HAL_GetTick(); -memcpy( last_info.message, message, 72 ); +memcpy( last_info.message, message, 36 ); is_pending_info = true; } /* error_log_info */ diff --git a/error_sdr/error_sdr.h b/error_sdr/error_sdr.h index b0a907e..0da80c8 100644 --- a/error_sdr/error_sdr.h +++ b/error_sdr/error_sdr.h @@ -37,7 +37,7 @@ extern "C" { /*------------------------------------------------------------------------------ Constants ------------------------------------------------------------------------------*/ -#define TEXT_MESSAGE_LENGTH 72 +#define TEXT_MESSAGE_LENGTH 36 #ifndef F1_TESTBED #define Error_Handler( a ) error_fail_fast( a ) /* backwards compatible */ @@ -142,6 +142,22 @@ typedef struct TEXT_MESSAGE *******************************************************************************/ #define assert_fail_fast( condition, error ) if ( !condition ) error_fail_fast( error ) +/* type check, bounds check, then execute */ +/* if either of these macros fail to expand, then its possible the user passed a pointer and not a literal. */ +#define error_log_warning( string ) \ + do { \ + _Static_assert( sizeof( "" string ) <= TEXT_MESSAGE_LENGTH, "Warning Message is oversized." ); \ + __sdr_log_warning( string ); \ + } while(0) + +/* type check, bounds check, then execute */ +/* if either of these macros fail to expand, then its possible the user passed a pointer and not a literal. */ +#define error_log_info( string ) \ + do { \ + _Static_assert( sizeof( "" string ) <= TEXT_MESSAGE_LENGTH, "Info Message is oversized." ); \ + __sdr_log_info( string ); \ + } while(0) + /*------------------------------------------------------------------------------ Function Prototypes @@ -154,14 +170,14 @@ void error_fail_fast ); /* warnings and info for telemetry */ -void error_log_info +void __sdr_log_info ( - char* message + const char* message ); -void error_log_warning +void __sdr_log_warning ( - char* message + const char* message ); bool error_get_warning diff --git a/telemetry/telemetry.c b/telemetry/telemetry.c index 2c905ea..5c4ea74 100644 --- a/telemetry/telemetry.c +++ b/telemetry/telemetry.c @@ -321,7 +321,6 @@ void telemetry_build_payload Construct Header ------------------------------------------------------------------------------*/ memset(msg_buf, 0, LORA_MESSAGE_SIZE); -get_uid( &(msg_buf->header.uid) ); msg_buf->header.mid = message_type; msg_buf->header.timestamp = HAL_GetTick(); @@ -390,6 +389,7 @@ static void telemetry_build_msg_vehicle_id /* hardware & firmware identifiers */ msg_buf->payload.vehicle_id.hw_opcode = PING_RESPONSE_CODE; msg_buf->payload.vehicle_id.fw_opcode = FIRMWARE_APPA; +get_uid( &(msg_buf->payload.vehicle_id.uid) ); /* version string */ msg_buf->payload.vehicle_id.version |= ( VERSION_HARDWARE << 24 ); diff --git a/telemetry/telemetry.h b/telemetry/telemetry.h index 4eba468..bf7bcbd 100644 --- a/telemetry/telemetry.h +++ b/telemetry/telemetry.h @@ -49,8 +49,8 @@ extern "C" { /*------------------------------------------------------------------------------ Constants ------------------------------------------------------------------------------*/ -#define LORA_INTERNAL_HEADER_SIZE 20U -#define LORA_PAYLOAD_SIZE 76U +#define LORA_INTERNAL_HEADER_SIZE 8U +#define LORA_PAYLOAD_SIZE 40U #define LORA_MESSAGE_SIZE (LORA_INTERNAL_HEADER_SIZE + LORA_PAYLOAD_SIZE) /*------------------------------------------------------------------------------ @@ -78,7 +78,6 @@ typedef enum _LORA_MESSAGE_TYPES typedef struct __attribute__((packed)) _LORA_INTERNAL_HEADER_TYPE { - ST_UID_TYPE uid; LORA_MESSAGE_TYPES mid; uint32_t timestamp; } LORA_INTERNAL_HEADER_TYPE; @@ -86,11 +85,12 @@ typedef struct __attribute__((packed)) _LORA_INTERNAL_HEADER_TYPE typedef struct __attribute((packed)) _LORA_MSG_VEHICLE_ID_TYPE { + ST_UID_TYPE uid; uint8_t hw_opcode; uint8_t fw_opcode; VERSION_INFO_TYPE version; char flight_id[16]; - uint8_t explicit_padding[54]; + uint8_t explicit_padding[6]; } LORA_MSG_VEHICLE_ID_TYPE; _Static_assert( sizeof(LORA_MSG_VEHICLE_ID_TYPE) == LORA_PAYLOAD_SIZE, "LORA_MSG_VEHICLE_ID_TYPE size invalid."); From 7b18443ec6d9e04a57ad0994305a7cb57489c9c2 Mon Sep 17 00:00:00 2001 From: ETSsound <147210601+ETSsound@users.noreply.github.com> Date: Sun, 10 May 2026 16:09:00 -0700 Subject: [PATCH 02/29] documentation for telem structure updates --- telemetry/telemetry.h | 42 ++++++++++++++++++++++++------------------ 1 file changed, 24 insertions(+), 18 deletions(-) diff --git a/telemetry/telemetry.h b/telemetry/telemetry.h index bf7bcbd..ed3b0c4 100644 --- a/telemetry/telemetry.h +++ b/telemetry/telemetry.h @@ -5,6 +5,12 @@ * * DESCRIPTION: * Definitions for the telemetry data structures. +* +* NOTE: +* Every data structure given here is incredibly regression sensitive. All +* changes to structs must also change their factory/constructor procedures +* and ensure compatibility through the flight computer software stack +* (SDEC/API/CLI & Dashboard). * * COPYRIGHT: * Copyright (c) 2025 Sun Devil Rocketry. @@ -70,54 +76,54 @@ typedef enum _LORA_MESSAGE_TYPES { LORA_MSG_VEHICLE_ID = 0x00000001, LORA_MSG_DASHBOARD_DATA = 0x00000002, - LORA_MSG_WARNING_MESSAGE = 0x00000003, - LORA_MSG_INFO_MESSAGE = 0x00000004, + LORA_MSG_WARNING_MESSAGE = 0x00000003, /* ETS TODO */ + LORA_MSG_INFO_MESSAGE = 0x00000004, /* ETS TODO */ __LORA_MSG_FORCE_32BIT = 0xFFFFFFFF /* used to force this type size to 32 bits */ } LORA_MESSAGE_TYPES; _Static_assert( sizeof(LORA_MESSAGE_TYPES) == 4, "LORA_MESSAGE_TYPES size invalid."); typedef struct __attribute__((packed)) _LORA_INTERNAL_HEADER_TYPE { - LORA_MESSAGE_TYPES mid; - uint32_t timestamp; + LORA_MESSAGE_TYPES mid; /* message identifier -- currently 32 bits but will reserve command/control bits later*/ + uint32_t timestamp; /* systick in ms */ } LORA_INTERNAL_HEADER_TYPE; _Static_assert( sizeof(LORA_INTERNAL_HEADER_TYPE) == LORA_INTERNAL_HEADER_SIZE, "LORA_INTERNAL_HEADER size invalid."); typedef struct __attribute((packed)) _LORA_MSG_VEHICLE_ID_TYPE { - ST_UID_TYPE uid; - uint8_t hw_opcode; - uint8_t fw_opcode; - VERSION_INFO_TYPE version; - char flight_id[16]; - uint8_t explicit_padding[6]; + ST_UID_TYPE uid; /* unique identifier per stm32 MCU */ + uint8_t hw_opcode; /* hardware identifier as defined by connect command */ + uint8_t fw_opcode; /* firmware identifier as defined by connect command */ + VERSION_INFO_TYPE version; /* version string defined above */ + char flight_id[16]; /* a 16 character c-string for the current flight (future: configurable) */ + uint8_t explicit_padding[6]; /* pad the end of this struct so the union behaves as expected */ } LORA_MSG_VEHICLE_ID_TYPE; _Static_assert( sizeof(LORA_MSG_VEHICLE_ID_TYPE) == LORA_PAYLOAD_SIZE, "LORA_MSG_VEHICLE_ID_TYPE size invalid."); typedef struct __attribute__((packed)) _LORA_MSG_DASHBOARD_DUMP_TYPE { - FLIGHT_COMP_STATE_TYPE fsm_state; - DASHBOARD_DUMP_TYPE data; - uint8_t explicit_padding[3]; + FLIGHT_COMP_STATE_TYPE fsm_state; /* current state of the flight computer */ + DASHBOARD_DUMP_TYPE data; /* the data used by the dashboard for location/orientation */ + uint8_t explicit_padding[3]; /* pad the end of this struct so the union behaves as expected */ } LORA_MSG_DASHBOARD_DUMP_TYPE; _Static_assert( sizeof(LORA_MSG_DASHBOARD_DUMP_TYPE) == LORA_PAYLOAD_SIZE, "LORA_MSG_DASHBOARD_DUMP_TYPE size invalid."); /* maps to warning and info messages */ typedef struct __attribute__((packed)) _LORA_MSG_TEXT_MESSAGE_TYPE { - TEXT_MESSAGE msg; + TEXT_MESSAGE msg; /* encodes the systick where it was generated + a short message */ } LORA_MSG_TEXT_MESSAGE_TYPE; _Static_assert( sizeof(LORA_MSG_TEXT_MESSAGE_TYPE) == LORA_PAYLOAD_SIZE, "LORA_MSG_TEXT_MESSAGE_TYPE size invalid."); /* struct is packed to inhibit padding */ typedef struct __attribute__((packed)) _LORA_MESSAGE { - LORA_INTERNAL_HEADER_TYPE header; + LORA_INTERNAL_HEADER_TYPE header; /* data common to every message */ union _payload { - LORA_MSG_VEHICLE_ID_TYPE vehicle_id; - LORA_MSG_DASHBOARD_DUMP_TYPE dashboard_dump; - LORA_MSG_TEXT_MESSAGE_TYPE text_message; + LORA_MSG_VEHICLE_ID_TYPE vehicle_id; /* provide information about the transmitting device */ + LORA_MSG_DASHBOARD_DUMP_TYPE dashboard_dump; /* gives vehicle state info (location, orientation) */ + LORA_MSG_TEXT_MESSAGE_TYPE text_message; /* TODO: a short message generated by the firmware */ } payload; } LORA_MESSAGE; _Static_assert( sizeof(LORA_MESSAGE) == LORA_MESSAGE_SIZE, "LORA_PAYLOAD size invalid."); From 45316a6ed6c41da337853d60e4ba479d440d824c Mon Sep 17 00:00:00 2001 From: NArmistead Date: Mon, 8 Jun 2026 22:41:38 -0700 Subject: [PATCH 03/29] Add quat math and prototype body state --- math_sdr/math_sdr.c | 124 +++++++++++++++++++++++++++++ math_sdr/math_sdr.h | 42 ++++++++++ sensor/sensor.c | 185 ++++++++++++++++++++++++++------------------ 3 files changed, 274 insertions(+), 77 deletions(-) diff --git a/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index 2e1f724..738ab0a 100644 --- a/math_sdr/math_sdr.c +++ b/math_sdr/math_sdr.c @@ -60,6 +60,130 @@ return ~crc; } /* crc32 */ + +/* Standard ZYX conversion - maybe not the right order? */ +/* TAKES DEGREES */ +QUAT eul2quat(float yaw, float pitch, float roll) +{ +yaw = deg_to_rad(yaw); +pitch = deg_to_rad(pitch); +roll = deg_to_rad(roll); + +float cos_yaw = cosf(yaw / 2.0f); +float cos_pitch = cosf(pitch / 2.0f); +float cos_roll = cosf(roll / 2.0f); + +float sin_yaw = sinf(yaw / 2.0f); +float sin_pitch = sinf(pitch / 2.0f); +float sin_roll = sinf(roll / 2.0f); + +QUAT q; +q.w = cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw; +q.x = sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw; +q.y = cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw; +q.z = cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw; + +return q; + +} + +/* Note: quaternion multiplication is NOT commutative */ +QUAT quat_mult + ( + QUAT a, + QUAT b + ) +{ +QUAT result; + +result.w = (a.w * b.w) - (a.x * b.x) - (a.y * b.y) - (a.z * b.z); +result.x = (a.w * b.x) + (a.x * b.w) + (a.y * b.z) - (a.z * b.y); +result.y = (a.w * b.y) - (a.x * b.z) + (a.y * b.w) + (a.z * b.x); +result.z = (a.w * b.z) + (a.x * b.y) - (a.y * b.x) + (a.z * b.w); + +return result; + +} + + +QUAT quat_add + ( + QUAT a, + QUAT b + ) +{ +QUAT result; + +result.w = a.w + b.w; +result.x = a.x + b.x; +result.y = a.y + b.y; +result.z = a.z + b.z; + +return result; + +} + + +QUAT quat_scale + ( + QUAT q, + float s + ) +{ +QUAT result; + +result.w = q.w * s; +result.x = q.x * s; +result.y = q.y * s; +result.z = q.z * s; + +return result; + +} + + +QUAT quat_normalize + ( + QUAT q + ) +{ +QUAT result; + +float norm = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); + +/* Fallback for zero quaternion: initialize to unit quaternion + This check is really only for potentially zero initialized quats to avoid divide by zero. + Otherwise, values really close to zero could just have bad floating point roundoff */ +if ( norm == 0.0f ) + { + result.w = 1.0f; + result.x = 0.0f; + result.y = 0.0f; + result.z = 0.0f; + } +else + { + result.w = q.w / norm; + result.x = q.x / norm; + result.y = q.y / norm; + result.z = q.z / norm; + } + +return result; + +} + + +QUAT quat_conj + ( + QUAT q + ) +{ +QUAT result = { q.w, -q.x, -q.y, -q.z }; +return result; + +} + /******************************************************************************* * END OF FILE * *******************************************************************************/ \ No newline at end of file diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index a716944..4cd9b4e 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -51,6 +51,13 @@ typedef struct __attribute__((packed)) _ST_UID_TYPE _Static_assert( sizeof(ST_UID_TYPE) == 12, "ST_UID_TYPE packing incorrect." ); +/* Quaternion */ +typedef struct _QUAT + { + float w, x, y, z; + } QUAT; + + /*------------------------------------------------------------------------------ Macros and Inlines ------------------------------------------------------------------------------*/ @@ -141,6 +148,41 @@ uint32_t crc32 const uint8_t *data, size_t len ); + +QUAT eul2quat + ( + float yaw, + float pitch, + float roll + ); + +QUAT quat_mult + ( + QUAT a, + QUAT b + ); + +QUAT quat_add + ( + QUAT a, + QUAT b + ); + +QUAT quat_scale + ( + QUAT q, + float s + ); + +QUAT quat_normalize + ( + QUAT q + ); + +QUAT quat_conj + ( + QUAT q + ); #ifdef __cplusplus } diff --git a/sensor/sensor.c b/sensor/sensor.c index bf6bdf0..41cc4f0 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -198,83 +198,83 @@ void sensor_init #if defined( FLIGHT_COMPUTER ) /* Sensor offsets */ sensor_size_offsets_table[ 0 ].offset = 0; /* SENSOR_ACCX */ - sensor_size_offsets_table[ 1 ].offset = 2; /* SENSOR_ACCY */ - sensor_size_offsets_table[ 2 ].offset = 4; /* SENSOR_ACCZ */ - sensor_size_offsets_table[ 3 ].offset = 6; /* SENSOR_GYROX */ - sensor_size_offsets_table[ 4 ].offset = 8; /* SENSOR_GYROY */ - sensor_size_offsets_table[ 5 ].offset = 10; /* SENSOR_GYROZ */ - sensor_size_offsets_table[ 6 ].offset = 12; /* SENSOR_MAGX */ - sensor_size_offsets_table[ 7 ].offset = 14; /* SENSOR_MAGY */ - sensor_size_offsets_table[ 8 ].offset = 16; /* SENSOR_MAGZ */ - sensor_size_offsets_table[ 9 ].offset = 18; /* SENSOR_IMUT */ - sensor_size_offsets_table[ 10 ].offset = 20; /* SENSOR_ACCX_CONV */ - sensor_size_offsets_table[ 11 ].offset = 24; /* SENSOR_ACCY_CONV */ - sensor_size_offsets_table[ 12 ].offset = 28; /* SENSOR_ACCZ_CONV */ - sensor_size_offsets_table[ 13 ].offset = 32; /* SENSOR_GYROX_CONV */ - sensor_size_offsets_table[ 14 ].offset = 36; /* SENSOR_GYROY_CONV */ - sensor_size_offsets_table[ 15 ].offset = 40; /* SENSOR_GYROZ_CONV */ - sensor_size_offsets_table[ 16 ].offset = 44; /* SENSOR_ROLL_DEG */ - sensor_size_offsets_table[ 17 ].offset = 48; /* SENSOR_PITCH_DEG */ - sensor_size_offsets_table[ 18 ].offset = 52; /* SENSOR_ROLL_RATE */ - sensor_size_offsets_table[ 19 ].offset = 56; /* SENSOR_PITCH_RATE */ - sensor_size_offsets_table[ 20 ].offset = 60; /* SENSOR_VELOCITY */ - sensor_size_offsets_table[ 21 ].offset = 64; /* SENSOR_VELO_X */ - sensor_size_offsets_table[ 22 ].offset = 68; /* SENSOR_VELO_Y */ - sensor_size_offsets_table[ 23 ].offset = 72; /* SENSOR_VELO_Z */ - sensor_size_offsets_table[ 24 ].offset = 76; /* SENSOR_POSITION */ - sensor_size_offsets_table[ 25 ].offset = 80; /* SENSOR_BARO_PRES */ - sensor_size_offsets_table[ 26 ].offset = 84; /* SENSOR_BARO_TEMP */ - sensor_size_offsets_table[ 27 ].offset = 88; /* SENSOR_BARO_ALT */ - sensor_size_offsets_table[ 28 ].offset = 92; /* SENSOR_BARO_VELO */ - sensor_size_offsets_table[ 29 ].offset = 96; /* SENSOR_GPS_ALT */ - sensor_size_offsets_table[ 30 ].offset = 100; /* SENSOR_GPS_SPEED */ - sensor_size_offsets_table[ 31 ].offset = 104; /* SENSOR_GPS_TIME */ - sensor_size_offsets_table[ 32 ].offset = 108; /* SENSOR_GPS_LONG */ - sensor_size_offsets_table[ 33 ].offset = 112; /* SENSOR_GPS_LAT */ - sensor_size_offsets_table[ 34 ].offset = 116; /* SENSOR_GPS_NS */ - sensor_size_offsets_table[ 35 ].offset = 117; /* SENSOR_GPS_EW */ - sensor_size_offsets_table[ 36 ].offset = 118; /* SENSOR_GPS_GLL */ - sensor_size_offsets_table[ 37 ].offset = 119; /* SENSOR_GPS_RMC */ - - /* Sensor Sizes */ - sensor_size_offsets_table[ 0 ].size = 2; /* SENSOR_ACCX */ - sensor_size_offsets_table[ 1 ].size = 2; /* SENSOR_ACCY */ - sensor_size_offsets_table[ 2 ].size = 2; /* SENSOR_ACCZ */ - sensor_size_offsets_table[ 3 ].size = 2; /* SENSOR_GYROX */ - sensor_size_offsets_table[ 4 ].size = 2; /* SENSOR_GYROY */ - sensor_size_offsets_table[ 5 ].size = 2; /* SENSOR_GYROZ */ - sensor_size_offsets_table[ 6 ].size = 2; /* SENSOR_MAGX */ - sensor_size_offsets_table[ 7 ].size = 2; /* SENSOR_MAGY */ - sensor_size_offsets_table[ 8 ].size = 2; /* SENSOR_MAGZ */ - sensor_size_offsets_table[ 9 ].size = 2; /* SENSOR_IMUT */ - sensor_size_offsets_table[ 10 ].size = 4; /* SENSOR_ACCX_CONV */ - sensor_size_offsets_table[ 11 ].size = 4; /* SENSOR_ACCY_CONV */ - sensor_size_offsets_table[ 12 ].size = 4; /* SENSOR_ACCZ_CONV */ - sensor_size_offsets_table[ 13 ].size = 4; /* SENSOR_GYROX_CONV */ - sensor_size_offsets_table[ 14 ].size = 4; /* SENSOR_GYROY_CONV */ - sensor_size_offsets_table[ 15 ].size = 4; /* SENSOR_GYROZ_CONV */ - sensor_size_offsets_table[ 16 ].size = 4; /* SENSOR_ROLL_DEG */ - sensor_size_offsets_table[ 17 ].size = 4; /* SENSOR_PITCH_DEG */ - sensor_size_offsets_table[ 18 ].size = 4; /* SENSOR_ROLL_RATE */ - sensor_size_offsets_table[ 19 ].size = 4; /* SENSOR_PITCH_RATE */ - sensor_size_offsets_table[ 20 ].size = 4; /* VELOCITY */ - sensor_size_offsets_table[ 21 ].size = 4; /* VELO_X */ - sensor_size_offsets_table[ 22 ].size = 4; /* VELO_Y */ - sensor_size_offsets_table[ 23 ].size = 4; /* VELO_Z */ - sensor_size_offsets_table[ 24 ].size = 4; /* POSITION */ - sensor_size_offsets_table[ 25 ].size = 4; /* SENSOR_PRES */ - sensor_size_offsets_table[ 26 ].size = 4; /* SENSOR_TEMP */ - sensor_size_offsets_table[ 27 ].size = 4; /* SENSOR_BARO_ALT */ - sensor_size_offsets_table[ 28 ].size = 4; /* SENSOR_BARO_VELO */ - sensor_size_offsets_table[ 29 ].size = 4; /* SENSOR_GPS_ALT */ - sensor_size_offsets_table[ 30 ].size = 4; /* SENSOR_GPS_SPEED */ - sensor_size_offsets_table[ 31 ].size = 4; /* SENSOR_GPS_TIME */ - sensor_size_offsets_table[ 32 ].size = 4; /* SENSOR_GPS_LONG */ - sensor_size_offsets_table[ 33 ].size = 4; /* SENSOR_GPS_LAT */ - sensor_size_offsets_table[ 34 ].size = 1; /* SENSOR_GPS_NS */ - sensor_size_offsets_table[ 35 ].size = 1; /* SENSOR_GPS_EW */ - sensor_size_offsets_table[ 36 ].size = 1; /* SENSOR_GPS_GLL */ - sensor_size_offsets_table[ 37 ].size = 1; /* SENSOR_GPS_RMC */ + sensor_size_offsets_table[ 1 ].offset = 2; /* SENSOR_ACCY */ + sensor_size_offsets_table[ 2 ].offset = 4; /* SENSOR_ACCZ */ + sensor_size_offsets_table[ 3 ].offset = 6; /* SENSOR_GYROX */ + sensor_size_offsets_table[ 4 ].offset = 8; /* SENSOR_GYROY */ + sensor_size_offsets_table[ 5 ].offset = 10; /* SENSOR_GYROZ */ + sensor_size_offsets_table[ 6 ].offset = 12; /* SENSOR_MAGX */ + sensor_size_offsets_table[ 7 ].offset = 14; /* SENSOR_MAGY */ + sensor_size_offsets_table[ 8 ].offset = 16; /* SENSOR_MAGZ */ + sensor_size_offsets_table[ 9 ].offset = 18; /* SENSOR_IMUT */ + sensor_size_offsets_table[ 10 ].offset = 20; /* SENSOR_ACCX_CONV */ + sensor_size_offsets_table[ 11 ].offset = 24; /* SENSOR_ACCY_CONV */ + sensor_size_offsets_table[ 12 ].offset = 28; /* SENSOR_ACCZ_CONV */ + sensor_size_offsets_table[ 13 ].offset = 32; /* SENSOR_GYROX_CONV */ + sensor_size_offsets_table[ 14 ].offset = 36; /* SENSOR_GYROY_CONV */ + sensor_size_offsets_table[ 15 ].offset = 40; /* SENSOR_GYROZ_CONV */ + sensor_size_offsets_table[ 16 ].offset = 44; /* SENSOR_ROLL_DEG */ + sensor_size_offsets_table[ 17 ].offset = 48; /* SENSOR_PITCH_DEG */ + sensor_size_offsets_table[ 18 ].offset = 52; /* SENSOR_ROLL_RATE */ + sensor_size_offsets_table[ 19 ].offset = 56; /* SENSOR_PITCH_RATE */ + sensor_size_offsets_table[ 20 ].offset = 60; /* SENSOR_VELOCITY */ + sensor_size_offsets_table[ 21 ].offset = 64; /* SENSOR_VELO_X */ + sensor_size_offsets_table[ 22 ].offset = 68; /* SENSOR_VELO_Y */ + sensor_size_offsets_table[ 23 ].offset = 72; /* SENSOR_VELO_Z */ + sensor_size_offsets_table[ 24 ].offset = 76; /* SENSOR_POSITION */ + sensor_size_offsets_table[ 25 ].offset = 80; /* SENSOR_BARO_PRES */ + sensor_size_offsets_table[ 26 ].offset = 84; /* SENSOR_BARO_TEMP */ + sensor_size_offsets_table[ 27 ].offset = 88; /* SENSOR_BARO_ALT */ + sensor_size_offsets_table[ 28 ].offset = 92; /* SENSOR_BARO_VELO */ + sensor_size_offsets_table[ 29 ].offset = 96; /* SENSOR_GPS_ALT */ + sensor_size_offsets_table[ 30 ].offset = 100; /* SENSOR_GPS_SPEED */ + sensor_size_offsets_table[ 31 ].offset = 104; /* SENSOR_GPS_TIME */ + sensor_size_offsets_table[ 32 ].offset = 108; /* SENSOR_GPS_LONG */ + sensor_size_offsets_table[ 33 ].offset = 112; /* SENSOR_GPS_LAT */ + sensor_size_offsets_table[ 34 ].offset = 116; /* SENSOR_GPS_NS */ + sensor_size_offsets_table[ 35 ].offset = 117; /* SENSOR_GPS_EW */ + sensor_size_offsets_table[ 36 ].offset = 118; /* SENSOR_GPS_GLL */ + sensor_size_offsets_table[ 37 ].offset = 119; /* SENSOR_GPS_RMC */ + + /* Sensor Sizes */ + sensor_size_offsets_table[ 0 ].size = 2; /* SENSOR_ACCX */ + sensor_size_offsets_table[ 1 ].size = 2; /* SENSOR_ACCY */ + sensor_size_offsets_table[ 2 ].size = 2; /* SENSOR_ACCZ */ + sensor_size_offsets_table[ 3 ].size = 2; /* SENSOR_GYROX */ + sensor_size_offsets_table[ 4 ].size = 2; /* SENSOR_GYROY */ + sensor_size_offsets_table[ 5 ].size = 2; /* SENSOR_GYROZ */ + sensor_size_offsets_table[ 6 ].size = 2; /* SENSOR_MAGX */ + sensor_size_offsets_table[ 7 ].size = 2; /* SENSOR_MAGY */ + sensor_size_offsets_table[ 8 ].size = 2; /* SENSOR_MAGZ */ + sensor_size_offsets_table[ 9 ].size = 2; /* SENSOR_IMUT */ + sensor_size_offsets_table[ 10 ].size = 4; /* SENSOR_ACCX_CONV */ + sensor_size_offsets_table[ 11 ].size = 4; /* SENSOR_ACCY_CONV */ + sensor_size_offsets_table[ 12 ].size = 4; /* SENSOR_ACCZ_CONV */ + sensor_size_offsets_table[ 13 ].size = 4; /* SENSOR_GYROX_CONV */ + sensor_size_offsets_table[ 14 ].size = 4; /* SENSOR_GYROY_CONV */ + sensor_size_offsets_table[ 15 ].size = 4; /* SENSOR_GYROZ_CONV */ + sensor_size_offsets_table[ 16 ].size = 4; /* SENSOR_ROLL_DEG */ + sensor_size_offsets_table[ 17 ].size = 4; /* SENSOR_PITCH_DEG */ + sensor_size_offsets_table[ 18 ].size = 4; /* SENSOR_ROLL_RATE */ + sensor_size_offsets_table[ 19 ].size = 4; /* SENSOR_PITCH_RATE */ + sensor_size_offsets_table[ 20 ].size = 4; /* VELOCITY */ + sensor_size_offsets_table[ 21 ].size = 4; /* VELO_X */ + sensor_size_offsets_table[ 22 ].size = 4; /* VELO_Y */ + sensor_size_offsets_table[ 23 ].size = 4; /* VELO_Z */ + sensor_size_offsets_table[ 24 ].size = 4; /* POSITION */ + sensor_size_offsets_table[ 25 ].size = 4; /* SENSOR_PRES */ + sensor_size_offsets_table[ 26 ].size = 4; /* SENSOR_TEMP */ + sensor_size_offsets_table[ 27 ].size = 4; /* SENSOR_BARO_ALT */ + sensor_size_offsets_table[ 28 ].size = 4; /* SENSOR_BARO_VELO */ + sensor_size_offsets_table[ 29 ].size = 4; /* SENSOR_GPS_ALT */ + sensor_size_offsets_table[ 30 ].size = 4; /* SENSOR_GPS_SPEED */ + sensor_size_offsets_table[ 31 ].size = 4; /* SENSOR_GPS_TIME */ + sensor_size_offsets_table[ 32 ].size = 4; /* SENSOR_GPS_LONG */ + sensor_size_offsets_table[ 33 ].size = 4; /* SENSOR_GPS_LAT */ + sensor_size_offsets_table[ 34 ].size = 1; /* SENSOR_GPS_NS */ + sensor_size_offsets_table[ 35 ].size = 1; /* SENSOR_GPS_EW */ + sensor_size_offsets_table[ 36 ].size = 1; /* SENSOR_GPS_GLL */ + sensor_size_offsets_table[ 37 ].size = 1; /* SENSOR_GPS_RMC */ @@ -1532,6 +1532,37 @@ float gz = imu_data->imu_converted.gyro_z; float acc_roll = -rad_to_deg(atan2f(ay, ax)); float acc_pitch = rad_to_deg(atan2f(-az, sqrtf(ax * ax + ay * ay))); +/* --------- WIP Quaternion body state --------- */ + +static QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; /* NA temp: consider using q15 to reduce size since this will always end up as a unit quat */ + +/* Convert gyro to pure quaternion */ +QUAT q_gyro; /* Radians for the conversion! */ +q_gyro.w = 0.0f; +q_gyro.y = deg_to_rad(gy); +q_gyro.x = deg_to_rad(gx); +q_gyro.z = deg_to_rad(gz); + +/* NA temp: consider RK4 methods for integrating rate (if possible and reasonable), or don't if sensor fusion mitigates inaccuracies */ + +/* q_rate = 0.5 * attitude * q_gyro */ +QUAT q_rate = quat_mult(attitude, q_gyro); +q_rate = quat_scale(q_rate, 0.5f); + +/* attitude += dt * q_rate */ +QUAT rate_dt = quat_scale(q_rate, dt); +attitude = quat_add(attitude, rate_dt); + +/* Complementary filter fusion */ +/* attitude = COMP_ALPHA * attitude + (1 - COMP_ALPHA) * q_acc */ +QUAT q_acc = eul2quat(0.0f, acc_pitch, acc_roll); /* NA temp: Not sure if using gravity vector like this works under acceleration */ +QUAT comp_gyro = quat_scale(attitude, COMP_ALPHA); +QUAT comp_acc = quat_scale(q_acc, 1.0f - COMP_ALPHA); +attitude = quat_add(comp_gyro, comp_acc); + +/* Scale back to unit quaternion to avoid drift */ +attitude = quat_normalize(attitude); + /* Integrate gyro data */ static float roll = 0.0f; static float pitch = 0.0f; From ea31f2be22b6030f132615c3b32d9827c221e23e Mon Sep 17 00:00:00 2001 From: NArmistead Date: Wed, 10 Jun 2026 13:29:09 -0700 Subject: [PATCH 04/29] Remap axes --- sensor/sensor.c | 37 ++++++++++++++++++++++++++++--------- sensor/sensor.h | 17 +++++++++++++++++ 2 files changed, 45 insertions(+), 9 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 41cc4f0..6f41099 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -79,10 +79,9 @@ extern volatile uint32_t tdelta, previous_time; uint64_t baro_velo_tick = 0; uint64_t imu_velo_tick = 0; -#ifdef FLIGHT_COMPUTER extern GPS_DATA gps_data; extern IMU_OFFSET imu_offset; -#endif +MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_Z_UP; /* Assume up by default */ /*------------------------------------------------------------------------------ @@ -1472,10 +1471,10 @@ void sensor_conv_imu IMU_RAW* imu_raw ) { -/* Convert raw accel values */ -imu_data->imu_converted.accel_x = sensor_acc_conv(imu_raw->accel_x); +/* Convert raw accel values and remap axes so Z is vertical in the flight configuration */ +imu_data->imu_converted.accel_x = sensor_acc_conv(imu_raw->accel_z); imu_data->imu_converted.accel_y = sensor_acc_conv(imu_raw->accel_y); -imu_data->imu_converted.accel_z = sensor_acc_conv(imu_raw->accel_z); +imu_data->imu_converted.accel_z = mount_orientation * sensor_acc_conv(imu_raw->accel_x); /* Flip so gravity is always down*/ /* Do not use offset compensation for accel to preserve gravity */ /* @@ -1484,10 +1483,10 @@ imu_data->imu_converted.accel_y -= imu_offset.accel_y; imu_data->imu_converted.accel_z -= imu_offset.accel_z; */ -/* Convert raw gyroscope values to deg/s */ -imu_data->imu_converted.gyro_x = sensor_gyro_conv(imu_raw->gyro_x); +/* Convert raw gyroscope values to deg/s and remap axes */ +imu_data->imu_converted.gyro_x = sensor_gyro_conv(imu_raw->gyro_z); imu_data->imu_converted.gyro_y = sensor_gyro_conv(imu_raw->gyro_y); -imu_data->imu_converted.gyro_z = sensor_gyro_conv(imu_raw->gyro_z); +imu_data->imu_converted.gyro_z = mount_orientation * sensor_gyro_conv(imu_raw->gyro_x); /* Remove gyro bias */ imu_data->imu_converted.gyro_x -= imu_offset.gyro_x; @@ -1498,6 +1497,24 @@ sensor_conv_mag(imu_data, imu_raw); } +MOUNT_ORIENTATION get_mount_orientation + ( + void + ) +{ +return mount_orientation; +} + + +void set_mount_orientation + ( + MOUNT_ORIENTATION orientation + ) +{ +mount_orientation = orientation; +} + + /******************************************************************************* * * * PROCEDURE: * @@ -1534,7 +1551,7 @@ float acc_pitch = rad_to_deg(atan2f(-az, sqrtf(ax * ax + ay * ay))); /* --------- WIP Quaternion body state --------- */ -static QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; /* NA temp: consider using q15 to reduce size since this will always end up as a unit quat */ +static QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; /* TODO: initialize at calibration with accelerometer only */ /* Convert gyro to pure quaternion */ QUAT q_gyro; /* Radians for the conversion! */ @@ -1563,6 +1580,8 @@ attitude = quat_add(comp_gyro, comp_acc); /* Scale back to unit quaternion to avoid drift */ attitude = quat_normalize(attitude); +/* --------- End Quaternion body state --------- */ + /* Integrate gyro data */ static float roll = 0.0f; static float pitch = 0.0f; diff --git a/sensor/sensor.h b/sensor/sensor.h index a32792e..514993f 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -198,6 +198,13 @@ typedef enum #endif } SENSOR_IDS; +/* Mount configuration of FC */ +typedef enum + { + MOUNT_ORIENTATION_Z_DOWN = -1, + MOUNT_ORIENTATION_Z_UP = 1 + } MOUNT_ORIENTATION; + /* Sensor Data */ typedef struct SENSOR_DATA { @@ -300,6 +307,16 @@ void sensor_reset_velo(void); void sensor_body_state(IMU_DATA* imu_data); void sensor_imu_velo(IMU_DATA* imu_data); void sensor_conv_imu(IMU_DATA* imu_data, IMU_RAW* imu_raw); +MOUNT_ORIENTATION get_mount_orientation + ( + void + ); + +void set_mount_orientation + ( + MOUNT_ORIENTATION orientation + ); + float sensor_acc_conv(int16_t readout); float sensor_gyro_conv(int16_t readout); void sensor_baro_velo(SENSOR_DATA* sensor_data_ptr); From c491e40cfaced920a5b69ea324bef295ab5d9785 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Thu, 11 Jun 2026 19:54:46 -0700 Subject: [PATCH 05/29] Consolidate sensor init functionality --- math_sdr/math_sdr.c | 8 ++--- math_sdr/math_sdr.h | 2 +- sensor/sensor.c | 82 +++++++++++++++++++++++++++++++++++---------- sensor/sensor.h | 8 +++-- 4 files changed, 73 insertions(+), 27 deletions(-) diff --git a/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index 738ab0a..285f87c 100644 --- a/math_sdr/math_sdr.c +++ b/math_sdr/math_sdr.c @@ -62,13 +62,9 @@ return ~crc; /* Standard ZYX conversion - maybe not the right order? */ -/* TAKES DEGREES */ -QUAT eul2quat(float yaw, float pitch, float roll) +/* TAKES RADIANS */ +QUAT eul_to_quat(float yaw, float pitch, float roll) { -yaw = deg_to_rad(yaw); -pitch = deg_to_rad(pitch); -roll = deg_to_rad(roll); - float cos_yaw = cosf(yaw / 2.0f); float cos_pitch = cosf(pitch / 2.0f); float cos_roll = cosf(roll / 2.0f); diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index 4cd9b4e..ea674e2 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -149,7 +149,7 @@ uint32_t crc32 size_t len ); -QUAT eul2quat +QUAT eul_to_quat ( float yaw, float pitch, diff --git a/sensor/sensor.c b/sensor/sensor.c index 6f41099..ca42a9e 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -70,6 +70,8 @@ /*------------------------------------------------------------------------------ Global Variables ------------------------------------------------------------------------------*/ +extern GPS_DATA gps_data; +extern IMU_OFFSET imu_offset; /* Hash table of sensor readout sizes and offsets */ static SENSOR_DATA_SIZE_OFFSETS sensor_size_offsets_table[ NUM_SENSORS ]; @@ -79,8 +81,14 @@ extern volatile uint32_t tdelta, previous_time; uint64_t baro_velo_tick = 0; uint64_t imu_velo_tick = 0; -extern GPS_DATA gps_data; -extern IMU_OFFSET imu_offset; +/* IMU */ +float velo_x_prev, velo_y_prev, velo_z_prev = 0.0; + +/* Baro */ +float velo_prev, alt_prev = 0.0; + +/* State estimation */ +QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_Z_UP; /* Assume up by default */ @@ -107,6 +115,13 @@ void static extract_sensor_bytes uint8_t* num_sensor_bytes ); +static QUAT quat_acc_attitude + ( + float ax, + float ay, + float az + ); + /* reads from all sensors using the MCUs ADCs */ #ifdef L0002_REV5 SENSOR_STATUS sensor_adc_burst_read @@ -182,13 +197,13 @@ static void sensor_conv_mag /******************************************************************************* * * * PROCEDURE: * -* sensor_init * +* sensor_init_offsets * * * * DESCRIPTION: * -* Initialize the sensor module * +* Initialize sensor module offsets * * * *******************************************************************************/ -void sensor_init +void sensor_init_offsets ( void ) @@ -319,7 +334,7 @@ void sensor_init sensor_size_offsets_table[ 1 ].size = 4; /* SENSOR_ENCF */ #endif -} /* sensor_init */ +} /* sensor_init_offsets */ /******************************************************************************* @@ -1439,27 +1454,39 @@ return SENSOR_OK; /******************************************************************************* * * * PROCEDURE: * -* sensor_initialize_tick * +* sensor_init * * * * DESCRIPTION: * -* Set the initial values for baro and imu tick at calibration * +* Initialize sensor ticks, velo, and attitude at calibration * * * *******************************************************************************/ -void sensor_initialize_tick +void sensor_init ( - void + PRESET_DATA* preset_data ) { baro_velo_tick = get_us_tick(); imu_velo_tick = baro_velo_tick; -} /* sensor_initialize_tick */ +velo_prev = 0.0; +velo_x_prev = 0.00; +velo_y_prev = 0.00; +velo_z_prev = 0.00; + +float ax = preset_data->imu_offset.accel_x; +float ay = preset_data->imu_offset.accel_y; +float az = preset_data->imu_offset.accel_z; + +attitude = quat_acc_attitude(ax, ay, az); + + +} /* sensor_init */ /******************************************************************************* * * * PROCEDURE: * -* sensor_conv_imu * +* sensor_conv_imu * * * * DESCRIPTION: * * Conversion of IMU raw chip readouts into 9-axis Accelerometer and Gyro.* @@ -1551,8 +1578,6 @@ float acc_pitch = rad_to_deg(atan2f(-az, sqrtf(ax * ax + ay * ay))); /* --------- WIP Quaternion body state --------- */ -static QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; /* TODO: initialize at calibration with accelerometer only */ - /* Convert gyro to pure quaternion */ QUAT q_gyro; /* Radians for the conversion! */ q_gyro.w = 0.0f; @@ -1572,7 +1597,7 @@ attitude = quat_add(attitude, rate_dt); /* Complementary filter fusion */ /* attitude = COMP_ALPHA * attitude + (1 - COMP_ALPHA) * q_acc */ -QUAT q_acc = eul2quat(0.0f, acc_pitch, acc_roll); /* NA temp: Not sure if using gravity vector like this works under acceleration */ +QUAT q_acc = quat_acc_attitude(ax, ay, az); QUAT comp_gyro = quat_scale(attitude, COMP_ALPHA); QUAT comp_acc = quat_scale(q_acc, 1.0f - COMP_ALPHA); attitude = quat_add(comp_gyro, comp_acc); @@ -1619,6 +1644,31 @@ imu_data->state_estimate.yaw_rate = yaw_rate; } +/******************************************************************************* +* * +* PROCEDURE: * +* quat_acc_attitude * +* * +* DESCRIPTION: * +* Computes quaternion attitude from static accelerometer data * +* * +*******************************************************************************/ +static QUAT quat_acc_attitude + ( + float ax, + float ay, + float az + ) +{ +/* Compute pitch/roll from accelerometer */ +float acc_roll = -atan2f(ay, ax); +float acc_pitch = atan2f(-az, sqrtf(ax * ax + ay * ay)); + +return eul_to_quat(0.0f, acc_pitch, acc_roll); + +} + + /******************************************************************************* * * * PROCEDURE: * @@ -1669,7 +1719,6 @@ return readout / gyro_sens; * Calculate the velocity depending on accel * * * *******************************************************************************/ -float velo_x_prev, velo_y_prev, velo_z_prev = 0.0; void sensor_imu_velo(IMU_DATA* imu_data){ float velo_x, velo_y, velo_z, velocity; @@ -1718,7 +1767,6 @@ void sensor_imu_velo(IMU_DATA* imu_data){ * Calculate the velocity from pressure readings * * * *******************************************************************************/ -float velo_prev, alt_prev = 0.0; void sensor_baro_velo(SENSOR_DATA* sen_data) { float velocity; diff --git a/sensor/sensor.h b/sensor/sensor.h index 514993f..5f5d03d 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -48,6 +48,8 @@ Includes #include "pressure.h" #endif +typedef struct _PRESET_DATA PRESET_DATA; /* From main.h */ + /*------------------------------------------------------------------------------ Macros @@ -201,7 +203,7 @@ typedef enum /* Mount configuration of FC */ typedef enum { - MOUNT_ORIENTATION_Z_DOWN = -1, + MOUNT_ORIENTATION_Z_DOWN = -1, MOUNT_ORIENTATION_Z_UP = 1 } MOUNT_ORIENTATION; @@ -271,7 +273,7 @@ typedef struct SENSOR_DATA_SIZE_OFFSETS ------------------------------------------------------------------------------*/ /* Initialize the sensor module */ -void sensor_init +void sensor_init_offsets ( void ); @@ -302,7 +304,7 @@ SENSOR_STATUS sensor_dump ); #ifdef FLIGHT_COMPUTER -void sensor_initialize_tick(void); +void sensor_init(PRESET_DATA* preset_data); void sensor_reset_velo(void); void sensor_body_state(IMU_DATA* imu_data); void sensor_imu_velo(IMU_DATA* imu_data); From 86930a5dccf03a641e365bf82e8d20fc5f930d82 Mon Sep 17 00:00:00 2001 From: ETSsound <147210601+ETSsound@users.noreply.github.com> Date: Sun, 21 Jun 2026 18:04:46 -0700 Subject: [PATCH 06/29] Refactor LoRa: Move async FSM to lora_async.c --- commands/commands.h | 1 + telemetry/telemetry.c | 250 ++++-------------------------------------- telemetry/telemetry.h | 89 +++++---------- 3 files changed, 52 insertions(+), 288 deletions(-) diff --git a/commands/commands.h b/commands/commands.h index 2a0f783..6e680ba 100644 --- a/commands/commands.h +++ b/commands/commands.h @@ -30,6 +30,7 @@ extern "C" { /* platform specific includes */ #if defined( A0002_REV2 ) || defined( A0005_REV1 ) #include "imu.h" +#include "usb.h" #endif /*------------------------------------------------------------------------------ diff --git a/telemetry/telemetry.c b/telemetry/telemetry.c index 5c4ea74..0c2ad2f 100644 --- a/telemetry/telemetry.c +++ b/telemetry/telemetry.c @@ -42,13 +42,7 @@ extern PRESET_DATA preset_data; /* Struct with preset data */ extern SENSOR_DATA sensor_data; /* Struct with all sensor */ -static TELEMETRY_FSM_STATE telemetry_state - = TELEMETRY_STATE_BLOCKING; -static uint8_t register_contents[2] = {0x00, 0x00}; -static LORA_STATUS lora_status = LORA_OK; static uint32_t message_idx = 0; -static LORA_MESSAGE payload; -static uint8_t burst_write_buf[LORA_MESSAGE_SIZE + 1]; /*------------------------------------------------------------------------------ Statics @@ -56,20 +50,20 @@ static uint8_t burst_write_buf[LORA_MESSAGE_SIZE + 1]; static void telemetry_build_msg_vehicle_id ( - LORA_MESSAGE* msg_buf + TELEMETRY_MESSAGE* msg_buf ); static void telemetry_build_msg_dashboard_dump ( - LORA_MESSAGE* msg_buf + TELEMETRY_MESSAGE* msg_buf ); static void telemetry_build_text_message ( - LORA_MESSAGE* msg_buf, - LORA_MESSAGE_TYPES message_type + TELEMETRY_MESSAGE* msg_buf, + TELEMETRY_MESSAGE_TYPES message_type ); @@ -77,194 +71,6 @@ static void telemetry_build_text_message Public APIs ------------------------------------------------------------------------------*/ -/********************************************************************************* -* * -* FUNCTION: * -* telemetry_update * -* * -* DESCRIPTION: * -* Update the telemetry system. Holds state; consider this the "main" * -* function for the telemetry system. * -* * -*********************************************************************************/ -void telemetry_update - ( - TELEMETRY_EVENT update_cause /* i: which kind of event triggered this update */ - ) -{ -/* Local Variables */ - -/* precondition: lora is not broken */ -if( ( lora_status & ( LORA_FAIL | LORA_TRANSMIT_FAIL | LORA_TIMEOUT_FAIL ) ) - || ( update_cause == TELEMETRY_EVENT_CANCEL ) ) - { - telemetry_state = TELEMETRY_STATE_BLOCKING; /* cancel telemetry FSM */ - return; - } - -// ETS TEMP: Test -// telemetry_get_next_message(); -// lora_transmit( &payload, sizeof( LORA_MESSAGE ) ); -// return; - -/* update the current telemetry state */ -switch( telemetry_state ) - { - case TELEMETRY_STATE_BLOCKING: /* FSM start */ - { - /* Assumptions: LoRa is initialized with valid configs, telem should only initialize - with a synchronous event from main loop. */ - if( update_cause != TELEMETRY_EVENT_SYNCHRONOUS_UPDATE ) - { - /* do nothing */ - return; - } - telemetry_state = TELEMETRY_STATE_STATUS_CHECK; - lora_status = lora_read_register_IT(LORA_REG_OPERATION_MODE, register_contents); - return; - } - - case TELEMETRY_STATE_STATUS_CHECK: /* check if in standby mode */ - { - if( update_cause != TELEMETRY_EVENT_REG_READ_CPLT ) - { - /* do nothing */ - return; - } - - /* check return. if not standby, cancel telem fsm and go back to blocking */ - if ((register_contents[1] & 0b111) != LORA_STANDBY_MODE) - { - telemetry_state = TELEMETRY_STATE_BLOCKING; /* wait for next synchronous check */ - /* Preserve LoRa mode (bit 7) and other upper bits; only set mode. */ - lora_status = lora_write_register_IT( - LORA_REG_OPERATION_MODE, - (uint8_t)( ( register_contents[1] & (uint8_t) ~0x7 ) | (uint8_t) LORA_STANDBY_MODE ) - ); - return; - } - else /* success: go to next state*/ - { - telemetry_state = TELEMETRY_STATE_GETTING_BUF; - lora_status = lora_read_register_IT(LORA_REG_FIFO_TX_BASE_ADDR, register_contents); - } - return; - } - - case TELEMETRY_STATE_GETTING_BUF: /* get fifo base ptr */ - { - if( update_cause != TELEMETRY_EVENT_REG_READ_CPLT ) - { - /* do nothing */ - return; - } - - telemetry_state = TELEMETRY_STATE_SETTING_TX_BASE; - lora_status = lora_write_register_IT(LORA_REG_FIFO_SPI_POINTER, register_contents[1]); - return; - } - - case TELEMETRY_STATE_SETTING_TX_BASE: /* set tx base ptr */ - { - if( update_cause != TELEMETRY_EVENT_WRITE_CPLT ) - { - /* do nothing */ - return; - } - - telemetry_state = TELEMETRY_STATE_WRITING_MSG_LEN; - lora_status = lora_write_register_IT(LORA_REG_SIGNAL_TO_NOISE, LORA_MESSAGE_SIZE); - return; - } - - case TELEMETRY_STATE_WRITING_MSG_LEN: /* write the length of the lora message */ - { - if( update_cause != TELEMETRY_EVENT_WRITE_CPLT ) - { - /* do nothing */ - return; - } - - telemetry_state = TELEMETRY_STATE_WRITING_MSG; - telemetry_get_next_message(); - burst_write_buf[0] = (LORA_REG_FIFO_RW | 0x80); /* set up reg write */ - memcpy(&(burst_write_buf[1]), &payload, LORA_MESSAGE_SIZE); - lora_status = lora_write_IT(burst_write_buf, LORA_MESSAGE_SIZE + 1); - return; - } - - case TELEMETRY_STATE_WRITING_MSG: /* writing the message itself */ - { - if( update_cause != TELEMETRY_EVENT_WRITE_CPLT ) - { - /* do nothing */ - return; - } - - /* check status register */ - telemetry_state = TELEMETRY_STATE_PRE_TX_STATUS_CHECK; - lora_status = lora_read_register_IT(LORA_REG_OPERATION_MODE, register_contents); - return; - } - - case TELEMETRY_STATE_PRE_TX_STATUS_CHECK: /* writing the message itself */ - { - if( update_cause != TELEMETRY_EVENT_REG_READ_CPLT ) - { - /* do nothing */ - return; - } - - /* switch to TX mode */ - telemetry_state = TELEMETRY_STATE_STARTING_TRANSMISSION; - uint8_t new_opmode_register = (register_contents[1] & ~(0x7)); - new_opmode_register = (new_opmode_register | LORA_TRANSMIT_MODE); - lora_status = lora_write_register_IT( LORA_REG_OPERATION_MODE, new_opmode_register ); - return; - } - - case TELEMETRY_STATE_STARTING_TRANSMISSION: - { - if( update_cause != TELEMETRY_EVENT_WRITE_CPLT ) - { - /* do nothing */ - return; - } - - /* opmode change complete, we are now transmitting */ - telemetry_state = TELEMETRY_STATE_TRANSMITTING; - register_contents[1] = 0xFF; /* set this to FF so we can detect when the contents have changed */ - lora_status = lora_read_register_IT(LORA_REG_OPERATION_MODE, register_contents); - return; - } - - case TELEMETRY_STATE_TRANSMITTING: - { - if( ( update_cause != TELEMETRY_EVENT_EXTI_RAISED ) - && ( update_cause != TELEMETRY_EVENT_REG_READ_CPLT ) ) - { - /* do nothing */ - return; - } - - if( (register_contents[1] & 0b111) == LORA_STANDBY_MODE ) - { - /* transmission is complete! start the buffer retrieval operation and jump higher on the FSM */ - telemetry_state = TELEMETRY_STATE_GETTING_BUF; - lora_status = lora_read_register_IT(LORA_REG_FIFO_TX_BASE_ADDR, register_contents); - } - else - { - lora_status = lora_read_register_IT(LORA_REG_OPERATION_MODE, register_contents); - } - return; - } - - } - - -} /* telemetry_update */ - /********************************************************************************* * * @@ -281,22 +87,22 @@ switch( telemetry_state ) *********************************************************************************/ void telemetry_get_next_message ( - void + TELEMETRY_MESSAGE* payload /* o: constructed telemetry message */ ) { -LORA_MESSAGE_TYPES msg_type; +TELEMETRY_MESSAGE_TYPES msg_type; /* Determine which payload to send */ if( ( message_idx % 2 == 0 ) && ( get_fc_state() == FC_STATE_LAUNCH_DETECT ) ) { - msg_type = LORA_MSG_VEHICLE_ID; + msg_type = TELEMETRY_MSG_VEHICLE_ID; } else { - msg_type = LORA_MSG_DASHBOARD_DATA; + msg_type = TELEMETRY_MSG_DASHBOARD_DATA; } -telemetry_build_payload(&payload, msg_type); +telemetry_build_payload(payload, msg_type); message_idx++; } /* telemetry_get_next_message */ @@ -313,14 +119,14 @@ message_idx++; *********************************************************************************/ void telemetry_build_payload ( - LORA_MESSAGE* msg_buf, /* o: buffer passed by caller */ - LORA_MESSAGE_TYPES message_type /* i: what kind of message */ + TELEMETRY_MESSAGE* msg_buf, /* o: buffer passed by caller */ + TELEMETRY_MESSAGE_TYPES message_type /* i: what kind of message */ ) { /*------------------------------------------------------------------------------ Construct Header ------------------------------------------------------------------------------*/ -memset(msg_buf, 0, LORA_MESSAGE_SIZE); +memset(msg_buf, 0, TELEMETRY_MESSAGE_SIZE); msg_buf->header.mid = message_type; msg_buf->header.timestamp = HAL_GetTick(); @@ -329,18 +135,18 @@ msg_buf->header.timestamp = HAL_GetTick(); ------------------------------------------------------------------------------*/ switch( message_type ) { - case LORA_MSG_VEHICLE_ID: + case TELEMETRY_MSG_VEHICLE_ID: { telemetry_build_msg_vehicle_id(msg_buf); break; } - case LORA_MSG_DASHBOARD_DATA: + case TELEMETRY_MSG_DASHBOARD_DATA: { telemetry_build_msg_dashboard_dump(msg_buf); break; } - case LORA_MSG_WARNING_MESSAGE: /* intentional fallthrough */ - case LORA_MSG_INFO_MESSAGE: + case TELEMETRY_MSG_WARNING_MESSAGE: /* intentional fallthrough */ + case TELEMETRY_MSG_INFO_MESSAGE: { telemetry_build_text_message(msg_buf, message_type); break; @@ -383,7 +189,7 @@ switch( message_type ) *********************************************************************************/ static void telemetry_build_msg_vehicle_id ( - LORA_MESSAGE* msg_buf + TELEMETRY_MESSAGE* msg_buf ) { /* hardware & firmware identifiers */ @@ -414,7 +220,7 @@ strncpy( msg_buf->payload.vehicle_id.flight_id, "AVIONICS_TEST", 16 ); *********************************************************************************/ static void telemetry_build_msg_dashboard_dump ( - LORA_MESSAGE* msg_buf + TELEMETRY_MESSAGE* msg_buf ) { msg_buf->payload.dashboard_dump.fsm_state = get_fc_state(); @@ -435,20 +241,20 @@ dashboard_construct_dump( &(msg_buf->payload.dashboard_dump.data) ); *********************************************************************************/ static void telemetry_build_text_message ( - LORA_MESSAGE* msg_buf, - LORA_MESSAGE_TYPES message_type + TELEMETRY_MESSAGE* msg_buf, + TELEMETRY_MESSAGE_TYPES message_type ) { TEXT_MESSAGE text_message; /* extra copy is required due to packed struct */ switch( message_type ) { - case LORA_MSG_WARNING_MESSAGE: + case TELEMETRY_MSG_WARNING_MESSAGE: { /* return discarded; presence of warning checked earlier */ error_get_warning( &text_message ); break; } - case LORA_MSG_INFO_MESSAGE: + case TELEMETRY_MSG_INFO_MESSAGE: { /* return discarded; presence of warning checked earlier */ error_get_info( &text_message ); @@ -477,15 +283,3 @@ switch( message_type ) memcpy( &(msg_buf->payload.text_message.msg), &text_message, sizeof( TEXT_MESSAGE ) ); } /* telemetry_build_text_message */ - - -#ifdef DEBUG -TELEMETRY_FSM_STATE telemetry_get_fsm_state - ( - void - ) -{ -return telemetry_state; - -} /* telemetry_get_fsm_state */ -#endif \ No newline at end of file diff --git a/telemetry/telemetry.h b/telemetry/telemetry.h index ed3b0c4..c648e15 100644 --- a/telemetry/telemetry.h +++ b/telemetry/telemetry.h @@ -57,7 +57,7 @@ extern "C" { ------------------------------------------------------------------------------*/ #define LORA_INTERNAL_HEADER_SIZE 8U #define LORA_PAYLOAD_SIZE 40U -#define LORA_MESSAGE_SIZE (LORA_INTERNAL_HEADER_SIZE + LORA_PAYLOAD_SIZE) +#define TELEMETRY_MESSAGE_SIZE (LORA_INTERNAL_HEADER_SIZE + LORA_PAYLOAD_SIZE) /*------------------------------------------------------------------------------ Typedefs @@ -72,24 +72,24 @@ typedef uint8_t FLIGHT_COMP_STATE_TYPE; typedef uint32_t VERSION_INFO_TYPE; /* hw version : fw version : fw patch : fw prerelease */ /* msb lsb */ -typedef enum _LORA_MESSAGE_TYPES +typedef enum _TELEMETRY_MESSAGE_TYPES { - LORA_MSG_VEHICLE_ID = 0x00000001, - LORA_MSG_DASHBOARD_DATA = 0x00000002, - LORA_MSG_WARNING_MESSAGE = 0x00000003, /* ETS TODO */ - LORA_MSG_INFO_MESSAGE = 0x00000004, /* ETS TODO */ - __LORA_MSG_FORCE_32BIT = 0xFFFFFFFF /* used to force this type size to 32 bits */ - } LORA_MESSAGE_TYPES; - _Static_assert( sizeof(LORA_MESSAGE_TYPES) == 4, "LORA_MESSAGE_TYPES size invalid."); + TELEMETRY_MSG_VEHICLE_ID = 0x00000001, + TELEMETRY_MSG_DASHBOARD_DATA = 0x00000002, + TELEMETRY_MSG_WARNING_MESSAGE = 0x00000003, /* ETS TODO */ + TELEMETRY_MSG_INFO_MESSAGE = 0x00000004, /* ETS TODO */ + __TELEMETRY_MSG_FORCE_32BIT = 0xFFFFFFFF /* used to force this type size to 32 bits */ + } TELEMETRY_MESSAGE_TYPES; + _Static_assert( sizeof(TELEMETRY_MESSAGE_TYPES) == 4, "TELEMETRY_MESSAGE_TYPES size invalid."); typedef struct __attribute__((packed)) _LORA_INTERNAL_HEADER_TYPE { - LORA_MESSAGE_TYPES mid; /* message identifier -- currently 32 bits but will reserve command/control bits later*/ + TELEMETRY_MESSAGE_TYPES mid; /* message identifier -- currently 32 bits but will reserve command/control bits later*/ uint32_t timestamp; /* systick in ms */ } LORA_INTERNAL_HEADER_TYPE; _Static_assert( sizeof(LORA_INTERNAL_HEADER_TYPE) == LORA_INTERNAL_HEADER_SIZE, "LORA_INTERNAL_HEADER size invalid."); -typedef struct __attribute((packed)) _LORA_MSG_VEHICLE_ID_TYPE +typedef struct __attribute((packed)) _TELEMETRY_MSG_VEHICLE_ID_TYPE { ST_UID_TYPE uid; /* unique identifier per stm32 MCU */ uint8_t hw_opcode; /* hardware identifier as defined by connect command */ @@ -97,56 +97,36 @@ typedef struct __attribute((packed)) _LORA_MSG_VEHICLE_ID_TYPE VERSION_INFO_TYPE version; /* version string defined above */ char flight_id[16]; /* a 16 character c-string for the current flight (future: configurable) */ uint8_t explicit_padding[6]; /* pad the end of this struct so the union behaves as expected */ - } LORA_MSG_VEHICLE_ID_TYPE; - _Static_assert( sizeof(LORA_MSG_VEHICLE_ID_TYPE) == LORA_PAYLOAD_SIZE, "LORA_MSG_VEHICLE_ID_TYPE size invalid."); + } TELEMETRY_MSG_VEHICLE_ID_TYPE; + _Static_assert( sizeof(TELEMETRY_MSG_VEHICLE_ID_TYPE) == LORA_PAYLOAD_SIZE, "TELEMETRY_MSG_VEHICLE_ID_TYPE size invalid."); -typedef struct __attribute__((packed)) _LORA_MSG_DASHBOARD_DUMP_TYPE +typedef struct __attribute__((packed)) _TELEMETRY_MSG_DASHBOARD_DUMP_TYPE { FLIGHT_COMP_STATE_TYPE fsm_state; /* current state of the flight computer */ DASHBOARD_DUMP_TYPE data; /* the data used by the dashboard for location/orientation */ uint8_t explicit_padding[3]; /* pad the end of this struct so the union behaves as expected */ - } LORA_MSG_DASHBOARD_DUMP_TYPE; - _Static_assert( sizeof(LORA_MSG_DASHBOARD_DUMP_TYPE) == LORA_PAYLOAD_SIZE, "LORA_MSG_DASHBOARD_DUMP_TYPE size invalid."); + } TELEMETRY_MSG_DASHBOARD_DUMP_TYPE; + _Static_assert( sizeof(TELEMETRY_MSG_DASHBOARD_DUMP_TYPE) == LORA_PAYLOAD_SIZE, "TELEMETRY_MSG_DASHBOARD_DUMP_TYPE size invalid."); /* maps to warning and info messages */ -typedef struct __attribute__((packed)) _LORA_MSG_TEXT_MESSAGE_TYPE +typedef struct __attribute__((packed)) _TELEMETRY_MSG_TEXT_MESSAGE_TYPE { TEXT_MESSAGE msg; /* encodes the systick where it was generated + a short message */ - } LORA_MSG_TEXT_MESSAGE_TYPE; - _Static_assert( sizeof(LORA_MSG_TEXT_MESSAGE_TYPE) == LORA_PAYLOAD_SIZE, "LORA_MSG_TEXT_MESSAGE_TYPE size invalid."); + } TELEMETRY_MSG_TEXT_MESSAGE_TYPE; + _Static_assert( sizeof(TELEMETRY_MSG_TEXT_MESSAGE_TYPE) == LORA_PAYLOAD_SIZE, "TELEMETRY_MSG_TEXT_MESSAGE_TYPE size invalid."); /* struct is packed to inhibit padding */ -typedef struct __attribute__((packed)) _LORA_MESSAGE +typedef struct __attribute__((packed)) _TELEMETRY_MESSAGE { LORA_INTERNAL_HEADER_TYPE header; /* data common to every message */ union _payload { - LORA_MSG_VEHICLE_ID_TYPE vehicle_id; /* provide information about the transmitting device */ - LORA_MSG_DASHBOARD_DUMP_TYPE dashboard_dump; /* gives vehicle state info (location, orientation) */ - LORA_MSG_TEXT_MESSAGE_TYPE text_message; /* TODO: a short message generated by the firmware */ + TELEMETRY_MSG_VEHICLE_ID_TYPE vehicle_id; /* provide information about the transmitting device */ + TELEMETRY_MSG_DASHBOARD_DUMP_TYPE dashboard_dump; /* gives vehicle state info (location, orientation) */ + TELEMETRY_MSG_TEXT_MESSAGE_TYPE text_message; /* TODO: a short message generated by the firmware */ } payload; - } LORA_MESSAGE; - _Static_assert( sizeof(LORA_MESSAGE) == LORA_MESSAGE_SIZE, "LORA_PAYLOAD size invalid."); - -typedef enum TELEMETRY_FSM_STATE { - TELEMETRY_STATE_BLOCKING = 0, - TELEMETRY_STATE_STATUS_CHECK, - TELEMETRY_STATE_GETTING_BUF, - TELEMETRY_STATE_SETTING_TX_BASE, - TELEMETRY_STATE_WRITING_MSG_LEN, - TELEMETRY_STATE_WRITING_MSG, - TELEMETRY_STATE_PRE_TX_STATUS_CHECK, - TELEMETRY_STATE_STARTING_TRANSMISSION, - TELEMETRY_STATE_TRANSMITTING -} TELEMETRY_FSM_STATE; - -typedef enum TELEMETRY_EVENT { - TELEMETRY_EVENT_CANCEL = 0, - TELEMETRY_EVENT_SYNCHRONOUS_UPDATE, - TELEMETRY_EVENT_REG_READ_CPLT, - TELEMETRY_EVENT_WRITE_CPLT, - TELEMETRY_EVENT_EXTI_RAISED -} TELEMETRY_EVENT; + } TELEMETRY_MESSAGE; + _Static_assert( sizeof(TELEMETRY_MESSAGE) == TELEMETRY_MESSAGE_SIZE, "LORA_PAYLOAD size invalid."); /*------------------------------------------------------------------------------ @@ -154,27 +134,16 @@ typedef enum TELEMETRY_EVENT { ------------------------------------------------------------------------------*/ /* telemetry.c */ -void telemetry_update - ( - TELEMETRY_EVENT update_cause - ); void telemetry_get_next_message ( - void - ); -void telemetry_build_payload - ( - LORA_MESSAGE* msg_buf, /* o: buffer passed by caller */ - LORA_MESSAGE_TYPES message_type /* i: what kind of message */ + TELEMETRY_MESSAGE* payload ); -/* Debug only function for retrieval of telemetry FSM state */ -#ifdef DEBUG -TELEMETRY_FSM_STATE telemetry_get_fsm_state +void telemetry_build_payload ( - void + TELEMETRY_MESSAGE* msg_buf, /* o: buffer passed by caller */ + TELEMETRY_MESSAGE_TYPES message_type /* i: what kind of message */ ); -#endif #ifdef __cplusplus } From 76e84adbbd6db4a153f8fc0503b5bff133106fd3 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Sun, 21 Jun 2026 21:57:06 -0700 Subject: [PATCH 07/29] Clean old code, split IMU_DATA --- sensor/sensor.c | 170 ++++++++++++++++++++---------------------------- sensor/sensor.h | 22 +++++-- 2 files changed, 86 insertions(+), 106 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index ae0c224..7602969 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -80,7 +80,7 @@ static SENSOR_STATUS sensor_get_it_ready static void sensor_conv_mag ( - IMU_DATA* imu_data, + IMU_CONVERTED* imu_converted, IMU_RAW* imu_raw ); @@ -256,13 +256,13 @@ baro_status = get_baro_it( &(sensor_data_ptr->baro_pressure), &(sensor_data_ptr- /*Compute State Estimations*/ /* Calculated and retrieve converted IMU data */ -sensor_conv_imu( &(sensor_data_ptr->imu_data), &imu_raw ); +sensor_conv_imu( &(sensor_data_ptr->imu_converted), &imu_raw ); /* Calculated to get body state */ -sensor_body_state( &(sensor_data_ptr->imu_data) ); +sensor_body_state( &(sensor_data_ptr->imu_converted) ); /* Calculated velocity and position */ -sensor_imu_velo( &(sensor_data_ptr->imu_data) ); +sensor_imu_velo( &(sensor_data_ptr->imu_converted) ); /* Calculated velocity from barometer */ sensor_baro_velo( sensor_data_ptr ); @@ -341,33 +341,33 @@ attitude = quat_acc_attitude(ax, ay, az); /* NA temp: we do sensor dump before c *******************************************************************************/ void sensor_conv_imu ( - IMU_DATA* imu_data, + IMU_CONVERTED* imu_converted, IMU_RAW* imu_raw ) { /* Convert raw accel values and remap axes so Z is vertical in the flight configuration */ -imu_data->imu_converted.accel_x = sensor_acc_conv(imu_raw->accel_z); -imu_data->imu_converted.accel_y = sensor_acc_conv(imu_raw->accel_y); -imu_data->imu_converted.accel_z = mount_orientation * sensor_acc_conv(imu_raw->accel_x); /* Flip so gravity is always down*/ +imu_converted->accel_x = sensor_acc_conv(imu_raw->accel_z); +imu_converted->accel_y = sensor_acc_conv(imu_raw->accel_y); +imu_converted->accel_z = mount_orientation * sensor_acc_conv(imu_raw->accel_x); /* Flip so gravity is always down*/ /* Do not use offset compensation for accel to preserve gravity */ /* -imu_data->imu_converted.accel_x -= imu_offset.accel_x; -imu_data->imu_converted.accel_y -= imu_offset.accel_y; -imu_data->imu_converted.accel_z -= imu_offset.accel_z; +imu_converted.accel_x -= imu_offset.accel_x; +imu_converted.accel_y -= imu_offset.accel_y; +imu_converted.accel_z -= imu_offset.accel_z; */ /* Convert raw gyroscope values to deg/s and remap axes */ -imu_data->imu_converted.gyro_x = sensor_gyro_conv(imu_raw->gyro_z); -imu_data->imu_converted.gyro_y = sensor_gyro_conv(imu_raw->gyro_y); -imu_data->imu_converted.gyro_z = mount_orientation * sensor_gyro_conv(imu_raw->gyro_x); +imu_converted->gyro_x = sensor_gyro_conv(imu_raw->gyro_z); +imu_converted->gyro_y = sensor_gyro_conv(imu_raw->gyro_y); +imu_converted->gyro_z = mount_orientation * sensor_gyro_conv(imu_raw->gyro_x); /* Remove gyro bias */ -imu_data->imu_converted.gyro_x -= imu_offset.gyro_x; -imu_data->imu_converted.gyro_y -= imu_offset.gyro_y; -imu_data->imu_converted.gyro_z -= imu_offset.gyro_z; +imu_converted->gyro_x -= imu_offset.gyro_x; +imu_converted->gyro_y -= imu_offset.gyro_y; +imu_converted->gyro_z -= imu_offset.gyro_z; -sensor_conv_mag(imu_data, imu_raw); +sensor_conv_mag(imu_converted, imu_raw); } @@ -401,39 +401,36 @@ mount_orientation = orientation; static uint32_t last_tick = 0; void sensor_body_state ( - IMU_DATA* imu_data + const IMU_CONVERTED* imu_converted, + STATE_ESTIMATION* state_estimate ) { /* Determine delta T */ uint32_t now_tick = HAL_GetTick(); float dt = (now_tick - last_tick) / 1000.0f; -if (dt <= 0.0f || dt > 1.0f) dt = 0.01f; +if ( dt <= 0.0f || dt > 1.0f ) + { + dt = 0.01f; + } last_tick = now_tick; /* Copy IMU data for readability */ -float ax = imu_data->imu_converted.accel_x; -float ay = imu_data->imu_converted.accel_y; -float az = imu_data->imu_converted.accel_z; - -float gx = imu_data->imu_converted.gyro_x; -float gy = imu_data->imu_converted.gyro_y; -float gz = imu_data->imu_converted.gyro_z; - -/* Compute pitch/roll from accelerometer */ -float acc_roll = -rad_to_deg(atan2f(ay, ax)); -float acc_pitch = rad_to_deg(atan2f(-az, sqrtf(ax * ax + ay * ay))); +float ax = imu_converted->accel_x; +float ay = imu_converted->accel_y; +float az = imu_converted->accel_z; -/* --------- WIP Quaternion body state --------- */ +/* Raw gyro data in deg/s */ +float gx = imu_converted->gyro_x; +float gy = imu_converted->gyro_y; +float gz = imu_converted->gyro_z; /* Convert gyro to pure quaternion */ -QUAT q_gyro; /* Radians for the conversion! */ +QUAT q_gyro; /* Must be in radians */ q_gyro.w = 0.0f; q_gyro.y = deg_to_rad(gy); q_gyro.x = deg_to_rad(gx); q_gyro.z = deg_to_rad(gz); -/* NA temp: consider RK4 methods for integrating rate (if possible and reasonable), or don't if sensor fusion mitigates inaccuracies */ - /* q_rate = 0.5 * attitude * q_gyro */ QUAT q_rate = quat_mult(attitude, q_gyro); q_rate = quat_scale(q_rate, 0.5f); @@ -452,41 +449,9 @@ attitude = quat_add(comp_gyro, comp_acc); /* Scale back to unit quaternion to avoid drift */ attitude = quat_normalize(attitude); -/* --------- End Quaternion body state --------- */ - -/* Integrate gyro data */ -static float roll = 0.0f; -static float pitch = 0.0f; -static float yaw = 0.0f; - -roll += gx * dt; -pitch += gy * dt; -yaw += gz * dt; - -/* Wrap yaw to -180..180 degrees */ -if (yaw > 180.0f) yaw -= 360.0f; -if (yaw < -180.0f) yaw += 360.0f; - -/* Complementary filter fusion */ -roll = COMP_ALPHA * roll + (1.0f - COMP_ALPHA) * acc_roll; -pitch = COMP_ALPHA * pitch + (1.0f - COMP_ALPHA) * acc_pitch; -// yaw uses gyro data only - -/* Compute angular rates (deg/s) */ -float roll_r = deg_to_rad(roll); -float pitch_r = deg_to_rad(pitch); - -float roll_rate = gx + sinf(roll_r) * tanf(pitch_r) * gy + cosf(roll_r) * tanf(pitch_r) * gz; -float pitch_rate = cosf(roll_r) * gy - sinf(roll_r) * gz; -float yaw_rate = (sinf(roll_r) / cosf(pitch_r)) * gy + (cosf(roll_r) / cosf(pitch_r)) * gz; - -/* Store results (angles & rates in degrees / deg/s) */ -imu_data->state_estimate.roll_angle = roll; -imu_data->state_estimate.pitch_angle = pitch; -imu_data->state_estimate.yaw_angle = yaw; -imu_data->state_estimate.roll_rate = roll_rate; -imu_data->state_estimate.pitch_rate = pitch_rate; -imu_data->state_estimate.yaw_rate = yaw_rate; +/* Store results */ +state_estimate->attitude = attitude; +state_estimate->roll_rate = gz; /* Rate in deg/s */ } @@ -566,42 +531,45 @@ return readout / gyro_sens; * Calculate the velocity depending on accel * * * *******************************************************************************/ -void sensor_imu_velo(IMU_DATA* imu_data){ - float velo_x, velo_y, velo_z, velocity; +void sensor_imu_velo + ( + const IMU_CONVERTED* imu_converted, + STATE_ESTIMATION* state_estimate + ) +{ +float velo_x, velo_y, velo_z, velocity; - float accel_x = imu_data->imu_converted.accel_x; - float accel_y = imu_data->imu_converted.accel_y; - float accel_z = imu_data->imu_converted.accel_z; +float accel_x = imu_converted->accel_x; +float accel_y = imu_converted->accel_y; +float accel_z = imu_converted->accel_z; - float ts_delta; - - uint64_t current_tick = get_us_tick(); - uint64_t imu_tdelta = current_tick - imu_velo_tick; - ts_delta = imu_tdelta / MICROSEC_PER_SEC; +float ts_delta; - // Calculate 3 velocity vectors using motion equations - velo_x = velo_x_prev + accel_x*ts_delta; - velo_y = velo_y_prev + accel_y*ts_delta; - velo_z = velo_z_prev + accel_z*ts_delta; +uint64_t current_tick = get_us_tick(); +uint64_t imu_tdelta = current_tick - imu_velo_tick; +ts_delta = imu_tdelta / MICROSEC_PER_SEC; - // Calculate the velocity scalar - velocity = sqrtf(powf(velo_x, 2.0) + powf(velo_y, 2.0) + powf(velo_z, 2.0)); +// Calculate 3 velocity vectors using motion equations +velo_x = velo_x_prev + accel_x*ts_delta; +velo_y = velo_y_prev + accel_y*ts_delta; +velo_z = velo_z_prev + accel_z*ts_delta; - /* Update state estimations*/ - imu_data->state_estimate.velo_x = velo_x; - imu_data->state_estimate.velo_y = velo_y; - imu_data->state_estimate.velo_z = velo_z; +// Calculate the velocity scalar +velocity = sqrtf(powf(velo_x, 2.0) + powf(velo_y, 2.0) + powf(velo_z, 2.0)); - imu_data->state_estimate.velocity = velocity; +/* Update state estimations*/ +state_estimate->velo_x = velo_x; +state_estimate->velo_y = velo_y; +state_estimate->velo_z = velo_z; - // Save current velocity for next computation - velo_x_prev = velo_x; - velo_y_prev = velo_y; - velo_z_prev = velo_z; +state_estimate->velocity = velocity; - imu_data->state_estimate.position = 0; //TODO: Implement position +// Save current velocity for next computation +velo_x_prev = velo_x; +velo_y_prev = velo_y; +velo_z_prev = velo_z; - imu_velo_tick = current_tick; +imu_velo_tick = current_tick; } @@ -834,7 +802,7 @@ return SENSOR_IT_TIMEOUT; *******************************************************************************/ static void sensor_conv_mag ( - IMU_DATA* imu_data, + IMU_CONVERTED* imu_converted, IMU_RAW* imu_raw ) { @@ -885,9 +853,9 @@ mag_z = process_comp_z2 / 4.0f / 10.0f; // µT /*------------------------------------------------------------------------------ Store converted field data ------------------------------------------------------------------------------*/ -imu_data->imu_converted.mag_x = mag_x; -imu_data->imu_converted.mag_y = mag_y; -imu_data->imu_converted.mag_z = mag_z; +imu_converted->mag_x = mag_x; +imu_converted->mag_y = mag_y; +imu_converted->mag_z = mag_z; } /* sensor_conv_mag */ #endif diff --git a/sensor/sensor.h b/sensor/sensor.h index ddb8c57..740ef77 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -59,7 +59,6 @@ typedef struct _PRESET_DATA PRESET_DATA; /* From main.h */ /* General */ #define NUM_SENSORS ( 38 ) -// #define IMU_DATA_SIZE ( 20 ) #define SENSOR_DATA_SIZE ( 128 ) /*------------------------------------------------------------------------------ @@ -93,10 +92,21 @@ typedef enum MOUNT_ORIENTATION_Z_UP = 1 } MOUNT_ORIENTATION; +/* State estimation from processed sensors */ +typedef struct _STATE_ESTIMATION { + QUAT attitude; + float roll_rate; + float velocity; + float velo_x; + float velo_y; + float velo_z; +} STATE_ESTIMATION; + /* Sensor Data */ typedef struct SENSOR_DATA { - IMU_DATA imu_data; + IMU_CONVERTED imu_converted; + STATE_ESTIMATION state_estimate; float baro_pressure; float baro_temp; float baro_alt; @@ -159,19 +169,21 @@ void sensor_reset_velo /* Perform sensor fusion on imu converted data to get body rate */ void sensor_body_state ( - IMU_DATA* imu_data + const IMU_CONVERTED* imu_converted, + STATE_ESTIMATION* state_estimate ); /* Calculate the velocity depending on accel */ void sensor_imu_velo ( - IMU_DATA* imu_data + const IMU_CONVERTED* imu_converted, + STATE_ESTIMATION* state_estimate ); /* Conversion of IMU raw chip readouts into 9-axis Accelerometer and Gyro. */ void sensor_conv_imu ( - IMU_DATA* imu_data, + IMU_CONVERTED* imu_converted, IMU_RAW* imu_raw ); From 2d22655bfe411d66adfd821c6657ae855586c7bf Mon Sep 17 00:00:00 2001 From: NArmistead Date: Sun, 21 Jun 2026 22:01:04 -0700 Subject: [PATCH 08/29] Fix some function arguments --- sensor/sensor.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 7602969..8b17858 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -259,10 +259,10 @@ baro_status = get_baro_it( &(sensor_data_ptr->baro_pressure), &(sensor_data_ptr- sensor_conv_imu( &(sensor_data_ptr->imu_converted), &imu_raw ); /* Calculated to get body state */ -sensor_body_state( &(sensor_data_ptr->imu_converted) ); +sensor_body_state( &(sensor_data_ptr->imu_converted), &(sensor_data_ptr->state_estimate) ); /* Calculated velocity and position */ -sensor_imu_velo( &(sensor_data_ptr->imu_converted) ); +sensor_imu_velo( &(sensor_data_ptr->imu_converted), &(sensor_data_ptr->state_estimate) ); /* Calculated velocity from barometer */ sensor_baro_velo( sensor_data_ptr ); From 4eccd837913bda759b287446e855d27371df6a3b Mon Sep 17 00:00:00 2001 From: NArmistead Date: Sun, 21 Jun 2026 22:16:11 -0700 Subject: [PATCH 09/29] Add quats to dashboard dump --- commands/commands.c | 7 ++++--- commands/commands.h | 5 +---- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/commands/commands.c b/commands/commands.c index eebe5d2..f239da1 100644 --- a/commands/commands.c +++ b/commands/commands.c @@ -165,7 +165,8 @@ void dashboard_construct_dump DASHBOARD_DUMP_TYPE* dump_buffer_ptr /* must be DASHBOARD_DUMP_SIZE */ ) { -/* Quats (TODO) */ +/* Quats */ +dump_buffer_ptr->attitude = sensor_data.state_estimate.attitude; /* Baro */ dump_buffer_ptr->alt = sensor_data.baro_alt; @@ -175,8 +176,8 @@ dump_buffer_ptr->longitude = sensor_data.gps_dec_longitude; dump_buffer_ptr->latitude = sensor_data.gps_dec_latitude; /* Controls */ -dump_buffer_ptr->acc_x = sensor_data.imu_data.imu_converted.accel_x; -dump_buffer_ptr->roll_rate = sensor_data.imu_data.state_estimate.roll_rate; +dump_buffer_ptr->acc_x = sensor_data.imu_converted.accel_x; +dump_buffer_ptr->roll_rate = sensor_data.state_estimate.roll_rate; } #endif diff --git a/commands/commands.h b/commands/commands.h index 6e680ba..ad36eed 100644 --- a/commands/commands.h +++ b/commands/commands.h @@ -100,10 +100,7 @@ extern "C" { typedef struct __attribute__((packed)) _DASHBOARD_DUMP_TYPE { - float quat_w; - float quat_x; - float quat_y; - float quat_z; + QUAT attitude; float alt; float latitude; float longitude; From 203e341fb5721b33b8d8503afbe386d79dfa8428 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Sun, 21 Jun 2026 22:46:16 -0700 Subject: [PATCH 10/29] Remove baro velo --- sensor/sensor.c | 38 +++++++------------------------------- sensor/sensor.h | 3 +-- 2 files changed, 8 insertions(+), 33 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 8b17858..1937eea 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -54,16 +54,11 @@ extern GPS_DATA gps_data; extern IMU_OFFSET imu_offset; /* Timing (sensors) */ -extern volatile uint32_t tdelta, previous_time; -uint64_t baro_velo_tick = 0; uint64_t imu_velo_tick = 0; /* IMU */ float velo_x_prev, velo_y_prev, velo_z_prev = 0.0; -/* Baro */ -float velo_prev, alt_prev = 0.0; - /* State estimation */ QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_Z_UP; /* Assume up by default */ @@ -264,8 +259,8 @@ sensor_body_state( &(sensor_data_ptr->imu_converted), &(sensor_data_ptr->state_e /* Calculated velocity and position */ sensor_imu_velo( &(sensor_data_ptr->imu_converted), &(sensor_data_ptr->state_estimate) ); -/* Calculated velocity from barometer */ -sensor_baro_velo( sensor_data_ptr ); +/* Calculated altitude from barometer */ +sensor_baro_alt( sensor_data_ptr ); /* CRITICAL SECTION END */ @@ -312,13 +307,9 @@ void sensor_init PRESET_DATA* preset_data ) { -baro_velo_tick = get_us_tick(); -imu_velo_tick = baro_velo_tick; +imu_velo_tick = get_us_tick(); -velo_prev = 0.0; -velo_x_prev = 0.00; -velo_y_prev = 0.00; -velo_z_prev = 0.00; +sensor_reset_velo(); float ax = preset_data->imu_offset.accel_x; float ay = preset_data->imu_offset.accel_y; @@ -576,23 +567,18 @@ imu_velo_tick = current_tick; /******************************************************************************* * * * PROCEDURE: * -* sensor_baro_velo * +* sensor_baro_alt * * * * DESCRIPTION: * -* Calculate the velocity from pressure readings * +* Calculate the altitude from pressure readings * * * *******************************************************************************/ -void sensor_baro_velo(SENSOR_DATA* sen_data) +void sensor_baro_alt(SENSOR_DATA* sen_data) { - float velocity; - float pressure = sen_data->baro_pressure; float temp = sen_data->baro_temp; // conv pressure to pascal for equation // pressure *= 6894.76; - uint64_t current_tick = get_us_tick(); - uint64_t baro_tdelta = current_tick - baro_velo_tick; - float ts_delta = baro_tdelta / MICROSEC_PER_SEC; // calc altitude float PRESSURE_SEA_LEVEL = 101325; @@ -601,16 +587,7 @@ void sensor_baro_velo(SENSOR_DATA* sen_data) float alt = (pow(PRESSURE_SEA_LEVEL / pressure, EXP) - 1) * (temp + 273.15) / TEMP_LAPSE_RATE; - - // Calculate the velocity scalar - velocity = (alt-alt_prev)/ts_delta; - alt_prev = alt; - velo_prev = velocity; - sen_data->baro_alt = alt; - sen_data->baro_velo = velocity; - - baro_velo_tick = current_tick; } @@ -629,7 +606,6 @@ void sensor_reset_velo void ) { -velo_prev = 0; velo_x_prev = 0; velo_y_prev = 0; velo_z_prev = 0; diff --git a/sensor/sensor.h b/sensor/sensor.h index 740ef77..2784978 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -110,7 +110,6 @@ typedef struct SENSOR_DATA float baro_pressure; float baro_temp; float baro_alt; - float baro_velo; float gps_altitude_ft; float gps_speed_kmh; float gps_utc_time; @@ -200,7 +199,7 @@ float sensor_gyro_conv ); /* Calculate the velocity from pressure readings */ -void sensor_baro_velo +void sensor_baro_alt ( SENSOR_DATA* sensor_data_ptr ); From f03bfd125ce0b314084ceba00a3c82400fb541eb Mon Sep 17 00:00:00 2001 From: ETSsound <147210601+ETSsound@users.noreply.github.com> Date: Mon, 22 Jun 2026 13:51:25 -0700 Subject: [PATCH 11/29] Fix dashboard dump acceleration Z value --- commands/commands.c | 2 +- commands/commands.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/commands/commands.c b/commands/commands.c index f239da1..3cf44bb 100644 --- a/commands/commands.c +++ b/commands/commands.c @@ -176,7 +176,7 @@ dump_buffer_ptr->longitude = sensor_data.gps_dec_longitude; dump_buffer_ptr->latitude = sensor_data.gps_dec_latitude; /* Controls */ -dump_buffer_ptr->acc_x = sensor_data.imu_converted.accel_x; +dump_buffer_ptr->acc_z = sensor_data.imu_converted.accel_z; dump_buffer_ptr->roll_rate = sensor_data.state_estimate.roll_rate; } diff --git a/commands/commands.h b/commands/commands.h index ad36eed..46758ad 100644 --- a/commands/commands.h +++ b/commands/commands.h @@ -104,7 +104,7 @@ typedef struct __attribute__((packed)) _DASHBOARD_DUMP_TYPE float alt; float latitude; float longitude; - float acc_x; + float acc_z; float roll_rate; } DASHBOARD_DUMP_TYPE; _Static_assert( sizeof(DASHBOARD_DUMP_TYPE) == 36, "DASHBOARD_DUMP_TYPE size invalid."); From fc5ca8ee85599b948a0bb70983e1e985439446b3 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Mon, 22 Jun 2026 15:18:40 -0700 Subject: [PATCH 12/29] Change default orientation --- sensor/sensor.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 1937eea..c6c2b88 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -61,7 +61,7 @@ float velo_x_prev, velo_y_prev, velo_z_prev = 0.0; /* State estimation */ QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; -MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_Z_UP; /* Assume up by default */ +MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_Z_DOWN; /* Default assumption: antennta pointing up */ /*------------------------------------------------------------------------------ From 60299614910d5505a0e80c6e1b361ad16d6fd9c5 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Mon, 22 Jun 2026 23:42:58 -0700 Subject: [PATCH 13/29] complete comments in math_sdr and convert to doxygen style --- math_sdr/math_sdr.c | 165 +++++++++++++++++++++++++++++--------------- math_sdr/math_sdr.h | 127 ++++++++++++++++++---------------- 2 files changed, 180 insertions(+), 112 deletions(-) diff --git a/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index 285f87c..4705742 100644 --- a/math_sdr/math_sdr.c +++ b/math_sdr/math_sdr.c @@ -1,56 +1,54 @@ -/******************************************************************************* -* -* FILE: -* math_sdr.c -* -* DESCRIPTION: -* Contains math functions for SDR code. -* -* COPYRIGHT: -* Copyright (c) 2025 Sun Devil Rocketry. -* All rights reserved. -* -* This software is licensed under terms that can be found in the LICENSE -* file in the root directory of this software component. -* If no LICENSE file comes with this software, it is covered under the -* BSD-3-Clause. -* -* https://opensource.org/license/bsd-3-clause -* -*******************************************************************************/ +/** + ****************************************************************************** + * @file : math_sdr.c + * @brief : Contains math and utility functions for SDR code. + ****************************************************************************** + * @copyright + * + * Copyright (c) 2025 Sun Devil Rocketry. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is covered under the + * BSD-3-Clause. + * + * https://opensource.org/license/bsd-3-clause + * + ****************************************************************************** + */ /*------------------------------------------------------------------------------ - Standard Includes + Standard Includes ------------------------------------------------------------------------------*/ /*------------------------------------------------------------------------------ - Project Includes + Project Includes ------------------------------------------------------------------------------*/ #include "main.h" #include "math_sdr.h" /*------------------------------------------------------------------------------ - API Functions + API Functions ------------------------------------------------------------------------------*/ -/******************************************************************************* -* * -* PROCEDURE: * -* crc32 * -* * -* DESCRIPTION: * -* Returns a 32bit checksum from the given data. * -* * -*******************************************************************************/ +/** + * @brief Computes a CRC-32 checksum over a block of data. + * + * @param[in] data Pointer to the input data buffer. + * @param[in] len Number of bytes in the buffer. + * + * @return The 32-bit CRC checksum of the input data. + */ uint32_t crc32 ( - const uint8_t *data, + const uint8_t *data, size_t len - ) + ) { uint32_t crc = 0xFFFFFFFF; -while (len--) +while (len--) { crc ^= *data++; for (int i = 0; i < 8; ++i) @@ -61,9 +59,25 @@ return ~crc; } /* crc32 */ -/* Standard ZYX conversion - maybe not the right order? */ -/* TAKES RADIANS */ -QUAT eul_to_quat(float yaw, float pitch, float roll) +/** + * @brief Converts ZYX Euler angles to a quaternion. + * + * @note Angles must be provided in radians. + * @note The rotation order is ZYX (standard aerospace convention). + * @todo The rotation sequence has not been validated yet. + * + * @param yaw Rotation about the Z axis in radians. + * @param pitch Rotation about the Y axis in radians. + * @param roll Rotation about the X axis in radians. + * + * @return The quaternion representing the rotation. + */ +QUAT eul_to_quat + ( + float yaw, + float pitch, + float roll + ) { float cos_yaw = cosf(yaw / 2.0f); float cos_pitch = cosf(pitch / 2.0f); @@ -81,9 +95,18 @@ q.z = cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw; return q; -} - -/* Note: quaternion multiplication is NOT commutative */ +} /* eul_to_quat */ + +/** + * @brief Multiplies two quaternions. + * + * @note Quaternion multiplication is NOT commutative. + * + * @param a The left-hand quaternion. + * @param b The right-hand quaternion. + * + * @return The quaternion product a * b. + */ QUAT quat_mult ( QUAT a, @@ -99,9 +122,17 @@ result.z = (a.w * b.z) + (a.x * b.y) - (a.y * b.x) + (a.z * b.w); return result; -} +} /* quat_mult */ +/** + * @brief Adds two quaternions component-wise. + * + * @param a The first quaternion. + * @param b The second quaternion. + * + * @return The quaternion sum a + b. + */ QUAT quat_add ( QUAT a, @@ -117,9 +148,17 @@ result.z = a.z + b.z; return result; -} +} /* quat_add */ +/** + * @brief Scales a quaternion by a scalar value. + * + * @param q The quaternion to scale. + * @param s The scalar factor. + * + * @return The quaternion @p q scaled by @p s (q * s). + */ QUAT quat_scale ( QUAT q, @@ -135,9 +174,22 @@ result.z = q.z * s; return result; -} - - +} /* quat_scale */ + + +/** + * @brief Normalizes a quaternion to unit length. + * + * @param q The quaternion to normalize. + * + * @return The normalized unit quaternion. + * + * Divides each component by the quaternion's norm. If the norm is + * zero (e.g. a zero-initialized quaternion), returns the identity + * quaternion (1, 0, 0, 0) to avoid a divide-by-zero. Values near + * but not equal to zero are divided normally and may exhibit + * floating-point roundoff. + */ QUAT quat_normalize ( QUAT q @@ -147,11 +199,9 @@ QUAT result; float norm = sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); -/* Fallback for zero quaternion: initialize to unit quaternion - This check is really only for potentially zero initialized quats to avoid divide by zero. - Otherwise, values really close to zero could just have bad floating point roundoff */ -if ( norm == 0.0f ) - { +/* Fallback for zero quaternion: initialize to identity quaternion */ +if ( norm == 0.0f ) + { result.w = 1.0f; result.x = 0.0f; result.y = 0.0f; @@ -167,9 +217,16 @@ else return result; -} +} /* quat_normalize */ +/** + * @brief Computes the conjugate of a quaternion. + * + * @param q The input quaternion. + * + * @return The conjugate quaternion (w, -x, -y, -z). + */ QUAT quat_conj ( QUAT q @@ -178,8 +235,8 @@ QUAT quat_conj QUAT result = { q.w, -q.x, -q.y, -q.z }; return result; -} +} /* quat_conj */ /******************************************************************************* -* END OF FILE * +* END OF FILE * *******************************************************************************/ \ No newline at end of file diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index ea674e2..5b99baa 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -1,23 +1,32 @@ -/******************************************************************************* -* -* FILE: -* math_sdr.h -* -* DESCRIPTION: -* Contains utility functions for SDR code. -* -* COPYRIGHT: -* Copyright (c) 2025 Sun Devil Rocketry. -* All rights reserved. -* -* This software is licensed under terms that can be found in the LICENSE -* file in the root directory of this software component. -* If no LICENSE file comes with this software, it is covered under the -* BSD-3-Clause. -* -* https://opensource.org/license/bsd-3-clause -* -*******************************************************************************/ +/** + ****************************************************************************** + * @file : math_sdr.h + * @brief : Contains math and utility functions for SDR code. + ****************************************************************************** + * @copyright + * + * Copyright (c) 2025 Sun Devil Rocketry. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is covered under the + * BSD-3-Clause. + * + * https://opensource.org/license/bsd-3-clause + * + ****************************************************************************** + @verbatim + ============================================================================== + ##### Math module features ##### + ============================================================================== + [..] + (+) Macros for common values, conversions, and utilities + (+) CRC-32 checksum of data + (+) Quaternion arithmetic + ****************************************************************************** + @endverbatim + */ /* Define to prevent recursive inclusion -------------------------------------*/ @@ -66,56 +75,52 @@ typedef struct _QUAT #define COMP_ALPHA 0.98f /* Used in sensor fusion */ #define GRAVITY 9.8f -/******************************************************************************* -* * -* MACRO: * -* util_set_bit * -* * -* DESCRIPTION: * -* Sets a certain bit and returns the new value * -* * -*******************************************************************************/ + +/** + * @brief Sets a certain bit and returns the new value. + * + * @param orig The original value. + * @param idx Index of the bit to set. + * + * @return @p orig with bit @p idx set. + */ #define util_set_bit( orig, idx ) ( orig | ( 1 << idx ) ) -/******************************************************************************* -* * -* MACRO: * -* rad_to_deg * -* * -* DESCRIPTION: * -* Convert a value in radians to degrees. * -* * -*******************************************************************************/ +/** + * @brief Convert a value in radians to degrees. + * + * @param x Value in radians. + * + * @return Equivalent value in degrees. + */ #define rad_to_deg(x) ((x) * 57.29577951f) -/******************************************************************************* -* * -* MACRO: * -* deg_to_rad * -* * -* DESCRIPTION: * -* Convert a value in degrees to radians. * -* * -*******************************************************************************/ +/** + * @brief Convert a value in degrees to radians. + * + * @param x Value in degrees. + * + * @return Equivalent value in radians. + */ #define deg_to_rad(x) ((x) * 0.01745329252f) -/******************************************************************************* -* * -* MACRO: * -* array_size * -* * -* DESCRIPTION: * -* Returns the number of elements in an array where each element is a * -* fixed size. An error or warning from this macro indicates that it * -* can't be used in that context. * -* * -*******************************************************************************/ +/** + * @brief Returns the number of elements in an array where each element is a fixed size. + * + * @note An error or warning from this macro indicates that it can't be used + * in that context. + * + * @param array The array with desired element count. + * + * @return Number of elements in @p array. + */ #define array_size( array ) ( sizeof( array ) / sizeof( array[0] ) ) +// NA TODO: preserving the old banner in case this is moved /******************************************************************************* * * * INLINE: * @@ -125,6 +130,11 @@ typedef struct _QUAT * Gets the 12 byte UID value for the H7 chip. * * * *******************************************************************************/ +/** + * @brief Gets the 12 byte UID value for the H7 chip. + * + * @param[out] uid_buffer Pointer to the buffer to receive the UID. + */ static inline void get_uid ( ST_UID_TYPE* uid_buffer @@ -143,6 +153,7 @@ memcpy( uid_buffer, uid, sizeof( ST_UID_TYPE ) ); /*------------------------------------------------------------------------------ Function Prototypes ------------------------------------------------------------------------------*/ + uint32_t crc32 ( const uint8_t *data, From 55a0ed2f94df25d5f6198c1bbd9b53bce50f1114 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Mon, 22 Jun 2026 23:44:21 -0700 Subject: [PATCH 14/29] Move COMP_ALPHA macro to sensor --- math_sdr/math_sdr.h | 1 - sensor/sensor.h | 2 ++ 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index 5b99baa..f3f3027 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -72,7 +72,6 @@ typedef struct _QUAT ------------------------------------------------------------------------------*/ /* Constants */ -#define COMP_ALPHA 0.98f /* Used in sensor fusion */ #define GRAVITY 9.8f diff --git a/sensor/sensor.h b/sensor/sensor.h index 2784978..61df320 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -60,6 +60,8 @@ typedef struct _PRESET_DATA PRESET_DATA; /* From main.h */ /* General */ #define NUM_SENSORS ( 38 ) #define SENSOR_DATA_SIZE ( 128 ) +#define COMP_ALPHA ( 0.98f ) /* Used in sensor fusion */ + /*------------------------------------------------------------------------------ Typdefs From cd5f0a751830fa887d7fc73691f5fa8d1bb1add7 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Tue, 23 Jun 2026 16:55:41 -0700 Subject: [PATCH 15/29] Move ST_UID_TYPE and get_uid to telemetry header --- math_sdr/math_sdr.h | 44 +------------------------------------------ telemetry/telemetry.h | 38 ++++++++++++++++++++++++++++++++++++- 2 files changed, 38 insertions(+), 44 deletions(-) diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index f3f3027..9fffd2c 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -49,17 +49,6 @@ extern "C" { Typedefs ------------------------------------------------------------------------------*/ -/* UID serial number (packed struct inhibits padding) */ -typedef struct __attribute__((packed)) _ST_UID_TYPE - { - uint32_t wafer_coords; - char lot_num_1[3]; - uint8_t wafer_num; - char lot_num_2[4]; - } ST_UID_TYPE; -_Static_assert( sizeof(ST_UID_TYPE) == 12, "ST_UID_TYPE packing incorrect." ); - - /* Quaternion */ typedef struct _QUAT { @@ -68,7 +57,7 @@ typedef struct _QUAT /*------------------------------------------------------------------------------ - Macros and Inlines + Macros ------------------------------------------------------------------------------*/ /* Constants */ @@ -118,37 +107,6 @@ typedef struct _QUAT */ #define array_size( array ) ( sizeof( array ) / sizeof( array[0] ) ) - -// NA TODO: preserving the old banner in case this is moved -/******************************************************************************* -* * -* INLINE: * -* get_uid * -* * -* DESCRIPTION: * -* Gets the 12 byte UID value for the H7 chip. * -* * -*******************************************************************************/ -/** - * @brief Gets the 12 byte UID value for the H7 chip. - * - * @param[out] uid_buffer Pointer to the buffer to receive the UID. - */ -static inline void get_uid - ( - ST_UID_TYPE* uid_buffer - ) -{ -uint32_t uid[3]; -uid[0] = HAL_GetUIDw0(); -uid[1] = HAL_GetUIDw1(); -uid[2] = HAL_GetUIDw2(); - -memcpy( uid_buffer, uid, sizeof( ST_UID_TYPE ) ); - -} /* get_uid */ - - /*------------------------------------------------------------------------------ Function Prototypes ------------------------------------------------------------------------------*/ diff --git a/telemetry/telemetry.h b/telemetry/telemetry.h index c648e15..c2cff70 100644 --- a/telemetry/telemetry.h +++ b/telemetry/telemetry.h @@ -42,7 +42,6 @@ extern "C" { /*------------------------------------------------------------------------------ Project Includes ------------------------------------------------------------------------------*/ -#include "math_sdr.h" #include "error_sdr.h" #include "commands.h" #include "main.h" @@ -82,6 +81,16 @@ typedef enum _TELEMETRY_MESSAGE_TYPES } TELEMETRY_MESSAGE_TYPES; _Static_assert( sizeof(TELEMETRY_MESSAGE_TYPES) == 4, "TELEMETRY_MESSAGE_TYPES size invalid."); +/* UID serial number (packed struct inhibits padding) */ +typedef struct __attribute__((packed)) _ST_UID_TYPE + { + uint32_t wafer_coords; + char lot_num_1[3]; + uint8_t wafer_num; + char lot_num_2[4]; + } ST_UID_TYPE; +_Static_assert( sizeof(ST_UID_TYPE) == 12, "ST_UID_TYPE packing incorrect." ); + typedef struct __attribute__((packed)) _LORA_INTERNAL_HEADER_TYPE { TELEMETRY_MESSAGE_TYPES mid; /* message identifier -- currently 32 bits but will reserve command/control bits later*/ @@ -128,6 +137,33 @@ typedef struct __attribute__((packed)) _TELEMETRY_MESSAGE } TELEMETRY_MESSAGE; _Static_assert( sizeof(TELEMETRY_MESSAGE) == TELEMETRY_MESSAGE_SIZE, "LORA_PAYLOAD size invalid."); +/*------------------------------------------------------------------------------ + Inlines +------------------------------------------------------------------------------*/ + +/******************************************************************************* +* * +* INLINE: * +* get_uid * +* * +* DESCRIPTION: * +* Gets the 12 byte UID value for the H7 chip. * +* * +*******************************************************************************/ +static inline void get_uid + ( + ST_UID_TYPE* uid_buffer + ) +{ +uint32_t uid[3]; +uid[0] = HAL_GetUIDw0(); +uid[1] = HAL_GetUIDw1(); +uid[2] = HAL_GetUIDw2(); + +memcpy( uid_buffer, uid, sizeof( ST_UID_TYPE ) ); + +} /* get_uid */ + /*------------------------------------------------------------------------------ Function prototypes From 91a019869ee5641ac6b6307281d7454613cc7a27 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Tue, 23 Jun 2026 21:34:47 -0700 Subject: [PATCH 16/29] Rename mount orientaton enum --- sensor/sensor.c | 5 +++-- sensor/sensor.h | 4 ++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index c6c2b88..eaf76f4 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -61,7 +61,7 @@ float velo_x_prev, velo_y_prev, velo_z_prev = 0.0; /* State estimation */ QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; -MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_Z_DOWN; /* Default assumption: antennta pointing up */ +MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_INVERTED; /* Default assumption: antennta pointing up */ /*------------------------------------------------------------------------------ @@ -296,7 +296,7 @@ else /******************************************************************************* * * * PROCEDURE: * -* sensor_initialize_tick * +* sensor_init * * * * DESCRIPTION: * * Initialize sensor ticks, velo, and attitude at calibration * @@ -432,6 +432,7 @@ attitude = quat_add(attitude, rate_dt); /* Complementary filter fusion */ /* attitude = COMP_ALPHA * attitude + (1 - COMP_ALPHA) * q_acc */ +// TODO: only use gravity during LD QUAT q_acc = quat_acc_attitude(ax, ay, az); QUAT comp_gyro = quat_scale(attitude, COMP_ALPHA); QUAT comp_acc = quat_scale(q_acc, 1.0f - COMP_ALPHA); diff --git a/sensor/sensor.h b/sensor/sensor.h index 61df320..c5a02bc 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -90,8 +90,8 @@ typedef enum /* Mount configuration of FC */ typedef enum { - MOUNT_ORIENTATION_Z_DOWN = -1, - MOUNT_ORIENTATION_Z_UP = 1 + MOUNT_ORIENTATION_IMU_INVERTED = -1, + MOUNT_ORIENTATION_IMU_NORMAL = 1 } MOUNT_ORIENTATION; /* State estimation from processed sensors */ From 1276b7b36121ce401562968ef1db1703df062cc8 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Sat, 27 Jun 2026 18:44:07 -0700 Subject: [PATCH 17/29] Move comp filter to its own function, remap mag axes --- sensor/sensor.c | 55 +++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 46 insertions(+), 9 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index eaf76f4..46332bf 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -86,6 +86,12 @@ static QUAT quat_acc_attitude float az ); +static void gravity_comp_filter + ( + QUAT* gyro_attitude, + QUAT g_orientation + ); + /*------------------------------------------------------------------------------ API Functions @@ -426,17 +432,17 @@ q_gyro.z = deg_to_rad(gz); QUAT q_rate = quat_mult(attitude, q_gyro); q_rate = quat_scale(q_rate, 0.5f); +/* Dead reckoing orientation by integrating gyro */ /* attitude += dt * q_rate */ QUAT rate_dt = quat_scale(q_rate, dt); attitude = quat_add(attitude, rate_dt); -/* Complementary filter fusion */ -/* attitude = COMP_ALPHA * attitude + (1 - COMP_ALPHA) * q_acc */ -// TODO: only use gravity during LD -QUAT q_acc = quat_acc_attitude(ax, ay, az); -QUAT comp_gyro = quat_scale(attitude, COMP_ALPHA); -QUAT comp_acc = quat_scale(q_acc, 1.0f - COMP_ALPHA); -attitude = quat_add(comp_gyro, comp_acc); +/* Sensor fuson with gravity if not in flight */ +if ( get_fc_state() <= FC_STATE_LAUNCH_DETECT ) + { + QUAT q_acc = quat_acc_attitude(ax, ay, az); + gravity_comp_filter(&attitude, q_acc); + } /* Scale back to unit quaternion to avoid drift */ attitude = quat_normalize(attitude); @@ -473,6 +479,36 @@ return eul_to_quat(0.0f, acc_pitch, acc_roll); } +/******************************************************************************* +* * +* PROCEDURE: * +* gravity_comp_filter * +* * +* DESCRIPTION: * +* Fuses gyroscope rotation data with gravity vector to compensate for * +* drift according to the formula * +* attitude = alpha * gyro_attitude + (1 - alpha) * g_orientation * +* * +* NOTE: * +* This type of sensor fusion is only valid when the vehicle is mostly * +* static (e.g prelaunch). Do not use this during flight when large * +* accerations come from sources other than gravity. * +* * +*******************************************************************************/ +static void gravity_comp_filter + ( + QUAT* gyro_attitude, + QUAT g_orientation + ) +{ +QUAT comp_gyro = quat_scale(*gyro_attitude, COMP_ALPHA); +QUAT comp_acc = quat_scale(g_orientation, 1.0f - COMP_ALPHA); + +*gyro_attitude = quat_add(comp_gyro, comp_acc); + +} + + /******************************************************************************* * * * PROCEDURE: * @@ -830,9 +866,10 @@ mag_z = process_comp_z2 / 4.0f / 10.0f; // µT /*------------------------------------------------------------------------------ Store converted field data ------------------------------------------------------------------------------*/ -imu_converted->mag_x = mag_x; +// NA TODO (DONT LET ME FORGET): This probably needs to be remapped +imu_converted->mag_x = mag_z; imu_converted->mag_y = mag_y; -imu_converted->mag_z = mag_z; +imu_converted->mag_z = mount_orientation * mag_x; } /* sensor_conv_mag */ #endif From c0b06af19d4c18d68290c9334bdf401a25ec705a Mon Sep 17 00:00:00 2001 From: NArmistead Date: Sat, 27 Jun 2026 18:46:22 -0700 Subject: [PATCH 18/29] critical(TM) comment update on comp filter --- sensor/sensor.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 46332bf..047b6a0 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -485,13 +485,13 @@ return eul_to_quat(0.0f, acc_pitch, acc_roll); * gravity_comp_filter * * * * DESCRIPTION: * -* Fuses gyroscope rotation data with gravity vector to compensate for * -* drift according to the formula * +* Fuses integrated gyroscope rotation data with gravity vector to * +* compensate for drift according to the formula * * attitude = alpha * gyro_attitude + (1 - alpha) * g_orientation * * * * NOTE: * * This type of sensor fusion is only valid when the vehicle is mostly * -* static (e.g prelaunch). Do not use this during flight when large * +* static (e.g. prelaunch). Do not use this during flight when large * * accerations come from sources other than gravity. * * * *******************************************************************************/ @@ -866,7 +866,6 @@ mag_z = process_comp_z2 / 4.0f / 10.0f; // µT /*------------------------------------------------------------------------------ Store converted field data ------------------------------------------------------------------------------*/ -// NA TODO (DONT LET ME FORGET): This probably needs to be remapped imu_converted->mag_x = mag_z; imu_converted->mag_y = mag_y; imu_converted->mag_z = mount_orientation * mag_x; From 1edd7ae5e9e1d161f9a4e35b0712aabbff19cd91 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Sun, 28 Jun 2026 23:40:45 -0700 Subject: [PATCH 19/29] Set axis remapping to always use right-handed coordinates --- sensor/sensor.c | 44 +++++++++++++++++++++++++++++++++++++------- sensor/sensor.h | 9 +++++++++ 2 files changed, 46 insertions(+), 7 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 047b6a0..6bdc48a 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -321,8 +321,7 @@ float ax = preset_data->imu_offset.accel_x; float ay = preset_data->imu_offset.accel_y; float az = preset_data->imu_offset.accel_z; -attitude = quat_acc_attitude(ax, ay, az); /* NA temp: we do sensor dump before calib too, so this might be unnecessary */ - +attitude = quat_acc_attitude(ax, ay, az); } /* sensor_init */ @@ -342,10 +341,12 @@ void sensor_conv_imu IMU_RAW* imu_raw ) { -/* Convert raw accel values and remap axes so Z is vertical in the flight configuration */ +/* Convert raw accel values */ imu_converted->accel_x = sensor_acc_conv(imu_raw->accel_z); imu_converted->accel_y = sensor_acc_conv(imu_raw->accel_y); -imu_converted->accel_z = mount_orientation * sensor_acc_conv(imu_raw->accel_x); /* Flip so gravity is always down*/ +imu_converted->accel_z = sensor_acc_conv(imu_raw->accel_x); + +sensor_axis_remap( &(imu_converted->accel_x), &(imu_converted->accel_y), &(imu_converted->accel_z) ); /* Do not use offset compensation for accel to preserve gravity */ /* @@ -357,7 +358,9 @@ imu_converted.accel_z -= imu_offset.accel_z; /* Convert raw gyroscope values to deg/s and remap axes */ imu_converted->gyro_x = sensor_gyro_conv(imu_raw->gyro_z); imu_converted->gyro_y = sensor_gyro_conv(imu_raw->gyro_y); -imu_converted->gyro_z = mount_orientation * sensor_gyro_conv(imu_raw->gyro_x); +imu_converted->gyro_z = sensor_gyro_conv(imu_raw->gyro_x); + +sensor_axis_remap( &(imu_converted->gyro_x), &(imu_converted->gyro_y), &(imu_converted->gyro_z) ); /* Remove gyro bias */ imu_converted->gyro_x -= imu_offset.gyro_x; @@ -509,6 +512,31 @@ QUAT comp_acc = quat_scale(g_orientation, 1.0f - COMP_ALPHA); } +/******************************************************************************* +* * +* PROCEDURE: * +* sensor_axis_remap * +* * +* DESCRIPTION: * +* Remaps sensor xyz readings so +Z is vertical in the flight * +* configuration or flips X to maintain right-handed coordinates * +* * +*******************************************************************************/ +void sensor_axis_remap + ( + float* x, + float* y, + float* z + ) +{ +float temp = *x; +*x = -mount_orientation * (*z); +(void)y; /* Unchanged */ +*z = mount_orientation * temp; + +} + + /******************************************************************************* * * * PROCEDURE: * @@ -866,9 +894,11 @@ mag_z = process_comp_z2 / 4.0f / 10.0f; // µT /*------------------------------------------------------------------------------ Store converted field data ------------------------------------------------------------------------------*/ -imu_converted->mag_x = mag_z; +sensor_axis_remap(&mag_x, &mag_y, &mag_z); + +imu_converted->mag_x = mag_x; imu_converted->mag_y = mag_y; -imu_converted->mag_z = mount_orientation * mag_x; +imu_converted->mag_z = mag_z; } /* sensor_conv_mag */ #endif diff --git a/sensor/sensor.h b/sensor/sensor.h index c5a02bc..2e1c65a 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -174,6 +174,15 @@ void sensor_body_state STATE_ESTIMATION* state_estimate ); +/* Remaps sensor xyz readings so +Z is vertical in the flight configuration + or flips X to maintain right-handed coordinates */ +void sensor_axis_remap + ( + float* x, + float* y, + float* z + ); + /* Calculate the velocity depending on accel */ void sensor_imu_velo ( From 61e4ecdebebf9355bf63da927443bea186c554a9 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Mon, 29 Jun 2026 17:21:00 -0700 Subject: [PATCH 20/29] Undo mag axis remap --- sensor/sensor.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 6bdc48a..1180dc3 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -894,8 +894,6 @@ mag_z = process_comp_z2 / 4.0f / 10.0f; // µT /*------------------------------------------------------------------------------ Store converted field data ------------------------------------------------------------------------------*/ -sensor_axis_remap(&mag_x, &mag_y, &mag_z); - imu_converted->mag_x = mag_x; imu_converted->mag_y = mag_y; imu_converted->mag_z = mag_z; From e4db23eb872d2ebf67aec28b5e9bcd9eeadb86eb Mon Sep 17 00:00:00 2001 From: etsells Date: Sun, 5 Jul 2026 19:13:47 -0700 Subject: [PATCH 21/29] Add handling for discarded qualifiers in the allowed emulator warnings --- debug_sdr/debug_sdr.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/debug_sdr/debug_sdr.h b/debug_sdr/debug_sdr.h index 3e98f97..b567925 100644 --- a/debug_sdr/debug_sdr.h +++ b/debug_sdr/debug_sdr.h @@ -166,7 +166,8 @@ void debug_callback_handler #if defined( EMULATOR ) #define debug_ignore_emulator_warnings_start() \ _Pragma("GCC diagnostic push") \ - _Pragma("GCC diagnostic ignored \"-Wformat\"") + _Pragma("GCC diagnostic ignored \"-Wformat\"") \ + _Pragma("GCC diagnostic ignored \"-Wdiscarded-qualifiers\"") #else #define debug_ignore_emulator_warnings_start() /* do nothing */ #endif From fda21c44cffc908267233b7c5c37e365d2278b09 Mon Sep 17 00:00:00 2001 From: etsells Date: Sun, 5 Jul 2026 20:01:36 -0700 Subject: [PATCH 22/29] Telemetry: Remove text messages since their limited size inhibits their usefulness --- telemetry/telemetry.c | 70 ------------------------------------------- telemetry/telemetry.h | 10 ------- 2 files changed, 80 deletions(-) diff --git a/telemetry/telemetry.c b/telemetry/telemetry.c index 0c2ad2f..7ee033a 100644 --- a/telemetry/telemetry.c +++ b/telemetry/telemetry.c @@ -59,14 +59,6 @@ static void telemetry_build_msg_dashboard_dump TELEMETRY_MESSAGE* msg_buf ); - -static void telemetry_build_text_message - ( - TELEMETRY_MESSAGE* msg_buf, - TELEMETRY_MESSAGE_TYPES message_type - ); - - /*------------------------------------------------------------------------------ Public APIs ------------------------------------------------------------------------------*/ @@ -145,12 +137,6 @@ switch( message_type ) telemetry_build_msg_dashboard_dump(msg_buf); break; } - case TELEMETRY_MSG_WARNING_MESSAGE: /* intentional fallthrough */ - case TELEMETRY_MSG_INFO_MESSAGE: - { - telemetry_build_text_message(msg_buf, message_type); - break; - } default: { error_fail_fast( ERROR_RECORD_FLIGHT_EVENTS_ERROR ); @@ -227,59 +213,3 @@ msg_buf->payload.dashboard_dump.fsm_state = get_fc_state(); dashboard_construct_dump( &(msg_buf->payload.dashboard_dump.data) ); } /* telemetry_build_msg_dashboard_dump */ - - -/********************************************************************************* -* * -* FUNCTION: * -* telemetry_build_text_message * -* * -* DESCRIPTION: * -* Build the payload for warning and info messages. Assume header is filled * -* by caller. * -* * -*********************************************************************************/ -static void telemetry_build_text_message - ( - TELEMETRY_MESSAGE* msg_buf, - TELEMETRY_MESSAGE_TYPES message_type - ) -{ -TEXT_MESSAGE text_message; /* extra copy is required due to packed struct */ -switch( message_type ) - { - case TELEMETRY_MSG_WARNING_MESSAGE: - { - /* return discarded; presence of warning checked earlier */ - error_get_warning( &text_message ); - break; - } - case TELEMETRY_MSG_INFO_MESSAGE: - { - /* return discarded; presence of warning checked earlier */ - error_get_info( &text_message ); - break; - } - /** - * GCOVR_EXCL_START - * - * Protective default case to prevent programmer error. Called by one function that will fall into one - * of the above two cases. - * - * ETS TEMP: Whoever writes this test should evaluate whether this covex works correctly. The default - * branch should be excluded from coverage. - */ - default: - { - /* shouldn't get here */ - error_fail_fast( ERROR_UNSUPPORTED_OP_ERROR ); - break; - } - /** - * GCOVR_EXCL_STOP - */ - } - -memcpy( &(msg_buf->payload.text_message.msg), &text_message, sizeof( TEXT_MESSAGE ) ); - -} /* telemetry_build_text_message */ diff --git a/telemetry/telemetry.h b/telemetry/telemetry.h index c2cff70..38b1959 100644 --- a/telemetry/telemetry.h +++ b/telemetry/telemetry.h @@ -75,8 +75,6 @@ typedef enum _TELEMETRY_MESSAGE_TYPES { TELEMETRY_MSG_VEHICLE_ID = 0x00000001, TELEMETRY_MSG_DASHBOARD_DATA = 0x00000002, - TELEMETRY_MSG_WARNING_MESSAGE = 0x00000003, /* ETS TODO */ - TELEMETRY_MSG_INFO_MESSAGE = 0x00000004, /* ETS TODO */ __TELEMETRY_MSG_FORCE_32BIT = 0xFFFFFFFF /* used to force this type size to 32 bits */ } TELEMETRY_MESSAGE_TYPES; _Static_assert( sizeof(TELEMETRY_MESSAGE_TYPES) == 4, "TELEMETRY_MESSAGE_TYPES size invalid."); @@ -117,13 +115,6 @@ typedef struct __attribute__((packed)) _TELEMETRY_MSG_DASHBOARD_DUMP_TYPE } TELEMETRY_MSG_DASHBOARD_DUMP_TYPE; _Static_assert( sizeof(TELEMETRY_MSG_DASHBOARD_DUMP_TYPE) == LORA_PAYLOAD_SIZE, "TELEMETRY_MSG_DASHBOARD_DUMP_TYPE size invalid."); -/* maps to warning and info messages */ -typedef struct __attribute__((packed)) _TELEMETRY_MSG_TEXT_MESSAGE_TYPE - { - TEXT_MESSAGE msg; /* encodes the systick where it was generated + a short message */ - } TELEMETRY_MSG_TEXT_MESSAGE_TYPE; - _Static_assert( sizeof(TELEMETRY_MSG_TEXT_MESSAGE_TYPE) == LORA_PAYLOAD_SIZE, "TELEMETRY_MSG_TEXT_MESSAGE_TYPE size invalid."); - /* struct is packed to inhibit padding */ typedef struct __attribute__((packed)) _TELEMETRY_MESSAGE { @@ -132,7 +123,6 @@ typedef struct __attribute__((packed)) _TELEMETRY_MESSAGE { TELEMETRY_MSG_VEHICLE_ID_TYPE vehicle_id; /* provide information about the transmitting device */ TELEMETRY_MSG_DASHBOARD_DUMP_TYPE dashboard_dump; /* gives vehicle state info (location, orientation) */ - TELEMETRY_MSG_TEXT_MESSAGE_TYPE text_message; /* TODO: a short message generated by the firmware */ } payload; } TELEMETRY_MESSAGE; _Static_assert( sizeof(TELEMETRY_MESSAGE) == TELEMETRY_MESSAGE_SIZE, "LORA_PAYLOAD size invalid."); From c857f8af0225ed4fc62cfcd54c855bdbe140e948 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Wed, 8 Jul 2026 19:56:35 -0700 Subject: [PATCH 23/29] Comp filter updates --- sensor/sensor.c | 68 ++++++++++++++++++++++++++++++++++--------------- 1 file changed, 47 insertions(+), 21 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 1180dc3..ed653c3 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -61,7 +61,12 @@ float velo_x_prev, velo_y_prev, velo_z_prev = 0.0; /* State estimation */ QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; -MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_INVERTED; /* Default assumption: antennta pointing up */ + + +/*------------------------------------------------------------------------------ + Static Variables +------------------------------------------------------------------------------*/ +static MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_INVERTED; /* Default assumption: antennta pointing up */ /*------------------------------------------------------------------------------ @@ -79,11 +84,12 @@ static void sensor_conv_mag IMU_RAW* imu_raw ); -static QUAT quat_acc_attitude +static QUAT quat_grav_attitude ( float ax, float ay, - float az + float az, + QUAT attitude ); static void gravity_comp_filter @@ -92,6 +98,11 @@ static void gravity_comp_filter QUAT g_orientation ); +static float quat_to_yaw + ( + QUAT q + ); + /*------------------------------------------------------------------------------ API Functions @@ -321,7 +332,7 @@ float ax = preset_data->imu_offset.accel_x; float ay = preset_data->imu_offset.accel_y; float az = preset_data->imu_offset.accel_z; -attitude = quat_acc_attitude(ax, ay, az); +attitude = quat_grav_attitude(ax, ay, az, attitude); } /* sensor_init */ @@ -392,7 +403,7 @@ mount_orientation = orientation; /******************************************************************************* * * * PROCEDURE: * -* sensor_body_state * +* sensor_body_state * * * * DESCRIPTION: * * Perform sensor fusion on imu converted data to get body rate * @@ -427,8 +438,8 @@ float gz = imu_converted->gyro_z; /* Convert gyro to pure quaternion */ QUAT q_gyro; /* Must be in radians */ q_gyro.w = 0.0f; -q_gyro.y = deg_to_rad(gy); q_gyro.x = deg_to_rad(gx); +q_gyro.y = deg_to_rad(gy); q_gyro.z = deg_to_rad(gz); /* q_rate = 0.5 * attitude * q_gyro */ @@ -443,7 +454,7 @@ attitude = quat_add(attitude, rate_dt); /* Sensor fuson with gravity if not in flight */ if ( get_fc_state() <= FC_STATE_LAUNCH_DETECT ) { - QUAT q_acc = quat_acc_attitude(ax, ay, az); + QUAT q_acc = quat_grav_attitude(ax, ay, az, attitude); gravity_comp_filter(&attitude, q_acc); } @@ -452,7 +463,7 @@ attitude = quat_normalize(attitude); /* Store results */ state_estimate->attitude = attitude; -state_estimate->roll_rate = gz; /* Rate in deg/s */ +state_estimate->roll_rate = gx; /* Rate in deg/s */ } @@ -460,25 +471,42 @@ state_estimate->roll_rate = gz; /* Rate in deg/s */ /******************************************************************************* * * * PROCEDURE: * -* quat_acc_attitude * +* quat_grav_attitude * * * * DESCRIPTION: * * Computes quaternion attitude from static accelerometer data * +* Experiences gimbal lock at pitch = +/- 90 degrees * * * *******************************************************************************/ -static QUAT quat_acc_attitude +static QUAT quat_grav_attitude ( float ax, float ay, - float az + float az, + QUAT attitude ) { /* Compute pitch/roll from accelerometer */ -float acc_roll = -atan2f(ay, ax); -float acc_pitch = atan2f(-az, sqrtf(ax * ax + ay * ay)); +float grav_pitch = atan2f(ax, sqrtf(ay * ay + az * az)); +float grav_roll = atan2f(ay, az); -return eul_to_quat(0.0f, acc_pitch, acc_roll); +float yaw = quat_to_yaw(attitude); +return eul_to_quat(yaw, grav_pitch, grav_roll); + +} + +/* Comment deferred until mod#132 + Formula in https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles */ +static float quat_to_yaw + ( + QUAT q + ) +{ +float y = 2.0f * (q.w * q.z + q.x * q.y); +float x = 1.0f - 2.0f * (q.y * q.y + q.z * q.z); + +return atan2f(y, x); } @@ -518,8 +546,8 @@ QUAT comp_acc = quat_scale(g_orientation, 1.0f - COMP_ALPHA); * sensor_axis_remap * * * * DESCRIPTION: * -* Remaps sensor xyz readings so +Z is vertical in the flight * -* configuration or flips X to maintain right-handed coordinates * +* Remaps sensor xyz readings so +X always points towards the nose in * +* the flight configuration * * * *******************************************************************************/ void sensor_axis_remap @@ -529,11 +557,9 @@ void sensor_axis_remap float* z ) { -float temp = *x; -*x = -mount_orientation * (*z); -(void)y; /* Unchanged */ -*z = mount_orientation * temp; - +*x *= mount_orientation; +*y *= mount_orientation; /* flip y too to maintain right-handedness */ +(void)z; /* Points down when FC is horizontal */ } From dcaee6f5aade6bcc4367b22d7f0afed5480cbc59 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Wed, 8 Jul 2026 20:01:31 -0700 Subject: [PATCH 24/29] put statics where they go --- sensor/sensor.c | 144 ++++++++++++++++++++++++------------------------ 1 file changed, 72 insertions(+), 72 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index ed653c3..3ae8852 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -468,78 +468,6 @@ state_estimate->roll_rate = gx; /* Rate in deg/s */ } -/******************************************************************************* -* * -* PROCEDURE: * -* quat_grav_attitude * -* * -* DESCRIPTION: * -* Computes quaternion attitude from static accelerometer data * -* Experiences gimbal lock at pitch = +/- 90 degrees * -* * -*******************************************************************************/ -static QUAT quat_grav_attitude - ( - float ax, - float ay, - float az, - QUAT attitude - ) -{ -/* Compute pitch/roll from accelerometer */ -float grav_pitch = atan2f(ax, sqrtf(ay * ay + az * az)); -float grav_roll = atan2f(ay, az); - -float yaw = quat_to_yaw(attitude); - -return eul_to_quat(yaw, grav_pitch, grav_roll); - -} - -/* Comment deferred until mod#132 - Formula in https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles */ -static float quat_to_yaw - ( - QUAT q - ) -{ -float y = 2.0f * (q.w * q.z + q.x * q.y); -float x = 1.0f - 2.0f * (q.y * q.y + q.z * q.z); - -return atan2f(y, x); -} - - -/******************************************************************************* -* * -* PROCEDURE: * -* gravity_comp_filter * -* * -* DESCRIPTION: * -* Fuses integrated gyroscope rotation data with gravity vector to * -* compensate for drift according to the formula * -* attitude = alpha * gyro_attitude + (1 - alpha) * g_orientation * -* * -* NOTE: * -* This type of sensor fusion is only valid when the vehicle is mostly * -* static (e.g. prelaunch). Do not use this during flight when large * -* accerations come from sources other than gravity. * -* * -*******************************************************************************/ -static void gravity_comp_filter - ( - QUAT* gyro_attitude, - QUAT g_orientation - ) -{ -QUAT comp_gyro = quat_scale(*gyro_attitude, COMP_ALPHA); -QUAT comp_acc = quat_scale(g_orientation, 1.0f - COMP_ALPHA); - -*gyro_attitude = quat_add(comp_gyro, comp_acc); - -} - - /******************************************************************************* * * * PROCEDURE: * @@ -781,6 +709,78 @@ HAL_NVIC_EnableIRQ( GPS_UART_IRQn ); ------------------------------------------------------------------------------*/ +/******************************************************************************* +* * +* PROCEDURE: * +* gravity_comp_filter * +* * +* DESCRIPTION: * +* Fuses integrated gyroscope rotation data with gravity vector to * +* compensate for drift according to the formula * +* attitude = alpha * gyro_attitude + (1 - alpha) * g_orientation * +* * +* NOTE: * +* This type of sensor fusion is only valid when the vehicle is mostly * +* static (e.g. prelaunch). Do not use this during flight when large * +* accerations come from sources other than gravity. * +* * +*******************************************************************************/ +static void gravity_comp_filter + ( + QUAT* gyro_attitude, + QUAT g_orientation + ) +{ +QUAT comp_gyro = quat_scale(*gyro_attitude, COMP_ALPHA); +QUAT comp_acc = quat_scale(g_orientation, 1.0f - COMP_ALPHA); + +*gyro_attitude = quat_add(comp_gyro, comp_acc); + +} + + +/******************************************************************************* +* * +* PROCEDURE: * +* quat_grav_attitude * +* * +* DESCRIPTION: * +* Computes quaternion attitude from static accelerometer data * +* Experiences gimbal lock at pitch = +/- 90 degrees * +* * +*******************************************************************************/ +static QUAT quat_grav_attitude + ( + float ax, + float ay, + float az, + QUAT attitude + ) +{ +/* Compute pitch/roll from accelerometer */ +float grav_pitch = atan2f(ax, sqrtf(ay * ay + az * az)); +float grav_roll = atan2f(ay, az); + +float yaw = quat_to_yaw(attitude); + +return eul_to_quat(yaw, grav_pitch, grav_roll); + +} + +/* Comment deferred until mod#132 + Formula in https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles */ +static float quat_to_yaw + ( + QUAT q + ) +{ +float y = 2.0f * (q.w * q.z + q.x * q.y); +float x = 1.0f - 2.0f * (q.y * q.y + q.z * q.z); + +return atan2f(y, x); +} + + #ifdef A0002_REV2 /******************************************************************************* * * From 654bbb346c14c6e8f8c37e69680d462e5f2b0b01 Mon Sep 17 00:00:00 2001 From: etsells Date: Sat, 11 Jul 2026 12:20:56 -0700 Subject: [PATCH 25/29] Fix gyro -> quat conv & disable acc filter temporarily --- math_sdr/math_sdr.c | 25 +++++++++++++++++++++++++ math_sdr/math_sdr.h | 6 ++++++ sensor/sensor.c | 12 ++++++------ 3 files changed, 37 insertions(+), 6 deletions(-) diff --git a/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index 4705742..272a993 100644 --- a/math_sdr/math_sdr.c +++ b/math_sdr/math_sdr.c @@ -97,6 +97,7 @@ return q; } /* eul_to_quat */ + /** * @brief Multiplies two quaternions. * @@ -125,6 +126,30 @@ return result; } /* quat_mult */ +/** + * @brief Provides the dot product of two quaternions. + * + * @note Quaternion dot products are commutative. + * + * @param a The left-hand quaternion. + * @param b The right-hand quaternion. + * + * @return The quaternion scalar product a . b + */ +float quat_dot + ( + QUAT a, + QUAT b + ) +{ +return( a.w * b.w + + a.x * b.x + + a.y * b.y + + a.z * b.z ); + +} /* quat_mult */ + + /** * @brief Adds two quaternions component-wise. * diff --git a/math_sdr/math_sdr.h b/math_sdr/math_sdr.h index 9fffd2c..4f7c80d 100644 --- a/math_sdr/math_sdr.h +++ b/math_sdr/math_sdr.h @@ -130,6 +130,12 @@ QUAT quat_mult QUAT b ); +float quat_dot + ( + QUAT a, + QUAT b + ); + QUAT quat_add ( QUAT a, diff --git a/sensor/sensor.c b/sensor/sensor.c index 3ae8852..ef7198b 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -353,9 +353,9 @@ void sensor_conv_imu ) { /* Convert raw accel values */ -imu_converted->accel_x = sensor_acc_conv(imu_raw->accel_z); +imu_converted->accel_x = sensor_acc_conv(imu_raw->accel_x); imu_converted->accel_y = sensor_acc_conv(imu_raw->accel_y); -imu_converted->accel_z = sensor_acc_conv(imu_raw->accel_x); +imu_converted->accel_z = sensor_acc_conv(imu_raw->accel_z); sensor_axis_remap( &(imu_converted->accel_x), &(imu_converted->accel_y), &(imu_converted->accel_z) ); @@ -367,9 +367,9 @@ imu_converted.accel_z -= imu_offset.accel_z; */ /* Convert raw gyroscope values to deg/s and remap axes */ -imu_converted->gyro_x = sensor_gyro_conv(imu_raw->gyro_z); +imu_converted->gyro_x = sensor_gyro_conv(imu_raw->gyro_x); imu_converted->gyro_y = sensor_gyro_conv(imu_raw->gyro_y); -imu_converted->gyro_z = sensor_gyro_conv(imu_raw->gyro_x); +imu_converted->gyro_z = sensor_gyro_conv(imu_raw->gyro_z); sensor_axis_remap( &(imu_converted->gyro_x), &(imu_converted->gyro_y), &(imu_converted->gyro_z) ); @@ -454,8 +454,8 @@ attitude = quat_add(attitude, rate_dt); /* Sensor fuson with gravity if not in flight */ if ( get_fc_state() <= FC_STATE_LAUNCH_DETECT ) { - QUAT q_acc = quat_grav_attitude(ax, ay, az, attitude); - gravity_comp_filter(&attitude, q_acc); + // QUAT q_acc = quat_grav_attitude(ax, ay, az, attitude); + // gravity_comp_filter(&attitude, q_acc); } /* Scale back to unit quaternion to avoid drift */ From 91d13bb685965cf9166f698fa9e551d37622bb76 Mon Sep 17 00:00:00 2001 From: etsells Date: Sat, 11 Jul 2026 12:27:14 -0700 Subject: [PATCH 26/29] Comment out a postponed function --- sensor/sensor.c | 42 ++++++++++++++++++++++-------------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index ef7198b..c3e2fb3 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -92,11 +92,12 @@ static QUAT quat_grav_attitude QUAT attitude ); -static void gravity_comp_filter - ( - QUAT* gyro_attitude, - QUAT g_orientation - ); +// ETS: Postponed +// static void gravity_comp_filter +// ( +// QUAT* gyro_attitude, +// QUAT g_orientation +// ); static float quat_to_yaw ( @@ -426,9 +427,9 @@ if ( dt <= 0.0f || dt > 1.0f ) last_tick = now_tick; /* Copy IMU data for readability */ -float ax = imu_converted->accel_x; -float ay = imu_converted->accel_y; -float az = imu_converted->accel_z; +// float ax = imu_converted->accel_x; +// float ay = imu_converted->accel_y; +// float az = imu_converted->accel_z; /* Raw gyro data in deg/s */ float gx = imu_converted->gyro_x; @@ -725,18 +726,19 @@ HAL_NVIC_EnableIRQ( GPS_UART_IRQn ); * accerations come from sources other than gravity. * * * *******************************************************************************/ -static void gravity_comp_filter - ( - QUAT* gyro_attitude, - QUAT g_orientation - ) -{ -QUAT comp_gyro = quat_scale(*gyro_attitude, COMP_ALPHA); -QUAT comp_acc = quat_scale(g_orientation, 1.0f - COMP_ALPHA); - -*gyro_attitude = quat_add(comp_gyro, comp_acc); - -} +// ETS: Postponed +// static void gravity_comp_filter +// ( +// QUAT* gyro_attitude, +// QUAT g_orientation +// ) +// { +// QUAT comp_gyro = quat_scale(*gyro_attitude, COMP_ALPHA); +// QUAT comp_acc = quat_scale(g_orientation, 1.0f - COMP_ALPHA); + +// *gyro_attitude = quat_add(comp_gyro, comp_acc); + +// } /******************************************************************************* From 2680f5b5184fdcb1078754a8526e1f0b1b70b68c Mon Sep 17 00:00:00 2001 From: etsells Date: Mon, 13 Jul 2026 18:17:28 -0700 Subject: [PATCH 27/29] Try to prevent a potential hardware issue by updating dump size macro --- commands/commands.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/commands/commands.h b/commands/commands.h index 46758ad..fadbfbf 100644 --- a/commands/commands.h +++ b/commands/commands.h @@ -96,7 +96,7 @@ extern "C" { #define FIRMWARE_RECEIVER ( 0x11 ) /* Reciever Firmware */ /* Other macros */ -#define DASHBOARD_DUMP_SIZE ( 72 ) +#define DASHBOARD_DUMP_SIZE ( 36 ) typedef struct __attribute__((packed)) _DASHBOARD_DUMP_TYPE { @@ -107,7 +107,7 @@ typedef struct __attribute__((packed)) _DASHBOARD_DUMP_TYPE float acc_z; float roll_rate; } DASHBOARD_DUMP_TYPE; - _Static_assert( sizeof(DASHBOARD_DUMP_TYPE) == 36, "DASHBOARD_DUMP_TYPE size invalid."); + _Static_assert( sizeof(DASHBOARD_DUMP_TYPE) == DASHBOARD_DUMP_SIZE, "DASHBOARD_DUMP_TYPE size invalid."); /*------------------------------------------------------------------------------ Function Prototypes From 719e2eb84f76a5ed71774be1f875c618c605ec81 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Mon, 20 Jul 2026 01:46:48 -0700 Subject: [PATCH 28/29] Corrections to mount orientation --- sensor/sensor.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sensor/sensor.c b/sensor/sensor.c index c3e2fb3..8e7fc09 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -52,6 +52,7 @@ ------------------------------------------------------------------------------*/ extern GPS_DATA gps_data; extern IMU_OFFSET imu_offset; +extern PRESET_DATA preset_data; /* Timing (sensors) */ uint64_t imu_velo_tick = 0; @@ -398,6 +399,7 @@ void set_mount_orientation ) { mount_orientation = orientation; +preset_data.last_orientation = orientation; } From 2c2e9c8ce8f84ce7fc340b30ff8f5463447a47c8 Mon Sep 17 00:00:00 2001 From: NArmistead Date: Tue, 21 Jul 2026 09:59:10 -0700 Subject: [PATCH 29/29] Hopefully better way to do orientation --- sensor/sensor.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/sensor/sensor.c b/sensor/sensor.c index 8e7fc09..04f44ef 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -52,7 +52,6 @@ ------------------------------------------------------------------------------*/ extern GPS_DATA gps_data; extern IMU_OFFSET imu_offset; -extern PRESET_DATA preset_data; /* Timing (sensors) */ uint64_t imu_velo_tick = 0; @@ -373,13 +372,13 @@ imu_converted->gyro_x = sensor_gyro_conv(imu_raw->gyro_x); imu_converted->gyro_y = sensor_gyro_conv(imu_raw->gyro_y); imu_converted->gyro_z = sensor_gyro_conv(imu_raw->gyro_z); -sensor_axis_remap( &(imu_converted->gyro_x), &(imu_converted->gyro_y), &(imu_converted->gyro_z) ); - -/* Remove gyro bias */ +/* Remove gyro bias BEFORE applying axis conversion */ imu_converted->gyro_x -= imu_offset.gyro_x; imu_converted->gyro_y -= imu_offset.gyro_y; imu_converted->gyro_z -= imu_offset.gyro_z; +sensor_axis_remap( &(imu_converted->gyro_x), &(imu_converted->gyro_y), &(imu_converted->gyro_z) ); + sensor_conv_mag(imu_converted, imu_raw); } @@ -399,7 +398,6 @@ void set_mount_orientation ) { mount_orientation = orientation; -preset_data.last_orientation = orientation; }