diff --git a/commands/commands.c b/commands/commands.c index ed8762d..3cf44bb 100644 --- a/commands/commands.c +++ b/commands/commands.c @@ -165,29 +165,19 @@ 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 */ +dump_buffer_ptr->attitude = sensor_data.state_estimate.attitude; /* 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_z = sensor_data.imu_converted.accel_z; +dump_buffer_ptr->roll_rate = sensor_data.state_estimate.roll_rate; } #endif diff --git a/commands/commands.h b/commands/commands.h index 95e82b6..fadbfbf 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 /*------------------------------------------------------------------------------ @@ -95,30 +96,18 @@ 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 { - float acc_x; - float acc_y; + QUAT attitude; + float alt; + float latitude; + float longitude; 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) == DASHBOARD_DUMP_SIZE, "DASHBOARD_DUMP_TYPE size invalid."); /*------------------------------------------------------------------------------ Function Prototypes 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 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 1fdb8b7..2d05dfa 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 */ @@ -156,6 +156,22 @@ typedef struct TEXT_MESSAGE *******************************************************************************/ #define assert_return( condition, retval ) do { if ( !(condition) ) return retval; } 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_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 @@ -168,14 +184,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/math_sdr/math_sdr.c b/math_sdr/math_sdr.c index 2e1f724..272a993 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) @@ -60,6 +58,210 @@ return ~crc; } /* crc32 */ + +/** + * @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); +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; + +} /* 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, + 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_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. + * + * @param a The first quaternion. + * @param b The second quaternion. + * + * @return The quaternion sum a + b. + */ +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_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, + 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_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 + ) +{ +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 identity quaternion */ +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_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 + ) +{ +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 a716944..4f7c80d 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 -------------------------------------*/ @@ -40,107 +49,114 @@ 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 + { + float w, x, y, z; + } QUAT; /*------------------------------------------------------------------------------ - Macros and Inlines + Macros ------------------------------------------------------------------------------*/ /* Constants */ -#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] ) ) - -/******************************************************************************* -* * -* 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 ------------------------------------------------------------------------------*/ + uint32_t crc32 ( const uint8_t *data, size_t len ); + +QUAT eul_to_quat + ( + float yaw, + float pitch, + float roll + ); + +QUAT quat_mult + ( + QUAT a, + QUAT b + ); + +float quat_dot + ( + 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 68a4d72..04f44ef 100644 --- a/sensor/sensor.c +++ b/sensor/sensor.c @@ -50,14 +50,23 @@ /*------------------------------------------------------------------------------ Global Variables ------------------------------------------------------------------------------*/ +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; -extern GPS_DATA gps_data; -extern IMU_OFFSET imu_offset; +/* IMU */ +float velo_x_prev, velo_y_prev, velo_z_prev = 0.0; + +/* State estimation */ +QUAT attitude = { 1.0f, 0.0f, 0.0f, 0.0f }; + + +/*------------------------------------------------------------------------------ + Static Variables +------------------------------------------------------------------------------*/ +static MOUNT_ORIENTATION mount_orientation = MOUNT_ORIENTATION_IMU_INVERTED; /* Default assumption: antennta pointing up */ /*------------------------------------------------------------------------------ @@ -71,10 +80,30 @@ static SENSOR_STATUS sensor_get_it_ready static void sensor_conv_mag ( - IMU_DATA* imu_data, + IMU_CONVERTED* imu_converted, IMU_RAW* imu_raw ); +static QUAT quat_grav_attitude + ( + float ax, + float ay, + float az, + QUAT attitude + ); + +// ETS: Postponed +// static void gravity_comp_filter +// ( +// QUAT* gyro_attitude, +// QUAT g_orientation +// ); + +static float quat_to_yaw + ( + QUAT q + ); + /*------------------------------------------------------------------------------ API Functions @@ -240,16 +269,16 @@ 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), &(sensor_data_ptr->state_estimate) ); /* Calculated velocity and position */ -sensor_imu_velo( &(sensor_data_ptr->imu_data) ); +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 */ @@ -285,27 +314,34 @@ else /******************************************************************************* * * * 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; +imu_velo_tick = get_us_tick(); + +sensor_reset_velo(); -} /* sensor_initialize_tick */ +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_grav_attitude(ax, ay, az, attitude); + +} /* sensor_init */ /******************************************************************************* * * * PROCEDURE: * -* sensor_conv_imu * +* sensor_conv_imu * * * * DESCRIPTION: * * Conversion of IMU raw chip readouts into 9-axis Accelerometer and Gyro.* @@ -313,40 +349,62 @@ imu_velo_tick = baro_velo_tick; *******************************************************************************/ void sensor_conv_imu ( - IMU_DATA* imu_data, + IMU_CONVERTED* imu_converted, IMU_RAW* imu_raw ) { -/* Convert raw accel values */ -imu_data->imu_converted.accel_x = sensor_acc_conv(imu_raw->accel_x); -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); +/* Convert raw accel values */ +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_z); + +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 */ /* -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 */ -imu_data->imu_converted.gyro_x = sensor_gyro_conv(imu_raw->gyro_x); -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); +/* Convert raw gyroscope values to deg/s and remap axes */ +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); -/* 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; +/* 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_conv_mag(imu_data, imu_raw); +sensor_axis_remap( &(imu_converted->gyro_x), &(imu_converted->gyro_y), &(imu_converted->gyro_z) ); + +sensor_conv_mag(imu_converted, imu_raw); +} + + +MOUNT_ORIENTATION get_mount_orientation + ( + void + ) +{ +return mount_orientation; +} + + +void set_mount_orientation + ( + MOUNT_ORIENTATION orientation + ) +{ +mount_orientation = orientation; } /******************************************************************************* * * * PROCEDURE: * -* sensor_body_state * +* sensor_body_state * * * * DESCRIPTION: * * Perform sensor fusion on imu converted data to get body rate * @@ -355,65 +413,85 @@ sensor_conv_mag(imu_data, imu_raw); 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 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; +float gy = imu_converted->gyro_y; +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.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 */ +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); + +/* 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); + } -float gx = imu_data->imu_converted.gyro_x; -float gy = imu_data->imu_converted.gyro_y; -float gz = imu_data->imu_converted.gyro_z; +/* Scale back to unit quaternion to avoid drift */ +attitude = quat_normalize(attitude); -/* 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))); - -/* 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 = gx; /* Rate in deg/s */ } +/******************************************************************************* +* * +* PROCEDURE: * +* sensor_axis_remap * +* * +* DESCRIPTION: * +* Remaps sensor xyz readings so +X always points towards the nose in * +* the flight configuration * +* * +*******************************************************************************/ +void sensor_axis_remap + ( + float* x, + float* y, + float* z + ) +{ +*x *= mount_orientation; +*y *= mount_orientation; /* flip y too to maintain right-handedness */ +(void)z; /* Points down when FC is horizontal */ +} + + /******************************************************************************* * * * PROCEDURE: * @@ -464,67 +542,63 @@ 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; +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; } /******************************************************************************* * * * PROCEDURE: * -* sensor_baro_velo * +* sensor_baro_alt * * * * DESCRIPTION: * -* Calculate the velocity from pressure readings * +* Calculate the altitude from pressure readings * * * *******************************************************************************/ -float velo_prev, alt_prev = 0.0; -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; @@ -533,16 +607,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; } @@ -561,7 +626,6 @@ void sensor_reset_velo void ) { -velo_prev = 0; velo_x_prev = 0; velo_y_prev = 0; velo_z_prev = 0; @@ -646,6 +710,79 @@ 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. * +* * +*******************************************************************************/ +// 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); + +// } + + +/******************************************************************************* +* * +* 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 /******************************************************************************* * * @@ -734,7 +871,7 @@ return SENSOR_IT_TIMEOUT; *******************************************************************************/ static void sensor_conv_mag ( - IMU_DATA* imu_data, + IMU_CONVERTED* imu_converted, IMU_RAW* imu_raw ) { @@ -785,9 +922,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 43ba5d7..2e1c65a 100644 --- a/sensor/sensor.h +++ b/sensor/sensor.h @@ -41,6 +41,8 @@ Includes #include #endif +typedef struct _PRESET_DATA PRESET_DATA; /* From main.h */ + /*------------------------------------------------------------------------------ Macros @@ -57,8 +59,9 @@ Includes /* General */ #define NUM_SENSORS ( 38 ) -// #define IMU_DATA_SIZE ( 20 ) #define SENSOR_DATA_SIZE ( 128 ) +#define COMP_ALPHA ( 0.98f ) /* Used in sensor fusion */ + /*------------------------------------------------------------------------------ Typdefs @@ -84,14 +87,31 @@ typedef enum SENSOR_IT_TIMEOUT } SENSOR_STATUS; +/* Mount configuration of FC */ +typedef enum + { + MOUNT_ORIENTATION_IMU_INVERTED = -1, + MOUNT_ORIENTATION_IMU_NORMAL = 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; - float baro_velo; float gps_altitude_ft; float gps_speed_kmh; float gps_utc_time; @@ -126,12 +146,21 @@ SENSOR_STATUS sensor_dump SENSOR_DATA* sensor_data_ptr ); -/* Set the initial values for baro and imu tick at calibration */ -void sensor_initialize_tick +void sensor_init + ( + PRESET_DATA* preset_data + ); + +MOUNT_ORIENTATION get_mount_orientation ( void ); +void set_mount_orientation + ( + MOUNT_ORIENTATION orientation + ); + /* Reset velocity values to prevent accumulation of drift */ void sensor_reset_velo ( @@ -141,19 +170,30 @@ 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 + ); + +/* 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 ( - 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 ); @@ -170,7 +210,7 @@ float sensor_gyro_conv ); /* Calculate the velocity from pressure readings */ -void sensor_baro_velo +void sensor_baro_alt ( SENSOR_DATA* sensor_data_ptr ); diff --git a/telemetry/telemetry.c b/telemetry/telemetry.c index 2c905ea..7ee033a 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,215 +50,19 @@ 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 - ); - - /*------------------------------------------------------------------------------ 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 +79,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,15 +111,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); -get_uid( &(msg_buf->header.uid) ); +memset(msg_buf, 0, TELEMETRY_MESSAGE_SIZE); msg_buf->header.mid = message_type; msg_buf->header.timestamp = HAL_GetTick(); @@ -330,22 +127,16 @@ 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: - { - telemetry_build_text_message(msg_buf, message_type); - break; - } default: { error_fail_fast( ERROR_RECORD_FLIGHT_EVENTS_ERROR ); @@ -384,12 +175,13 @@ switch( message_type ) *********************************************************************************/ static void telemetry_build_msg_vehicle_id ( - LORA_MESSAGE* msg_buf + TELEMETRY_MESSAGE* msg_buf ) { /* 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 ); @@ -414,78 +206,10 @@ 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(); 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 - ( - LORA_MESSAGE* msg_buf, - LORA_MESSAGE_TYPES message_type - ) -{ -TEXT_MESSAGE text_message; /* extra copy is required due to packed struct */ -switch( message_type ) - { - case LORA_MSG_WARNING_MESSAGE: - { - /* return discarded; presence of warning checked earlier */ - error_get_warning( &text_message ); - break; - } - case LORA_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 */ - - -#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 4eba468..38b1959 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. @@ -36,7 +42,6 @@ extern "C" { /*------------------------------------------------------------------------------ Project Includes ------------------------------------------------------------------------------*/ -#include "math_sdr.h" #include "error_sdr.h" #include "commands.h" #include "main.h" @@ -49,9 +54,9 @@ extern "C" { /*------------------------------------------------------------------------------ Constants ------------------------------------------------------------------------------*/ -#define LORA_INTERNAL_HEADER_SIZE 20U -#define LORA_PAYLOAD_SIZE 76U -#define LORA_MESSAGE_SIZE (LORA_INTERNAL_HEADER_SIZE + LORA_PAYLOAD_SIZE) +#define LORA_INTERNAL_HEADER_SIZE 8U +#define LORA_PAYLOAD_SIZE 40U +#define TELEMETRY_MESSAGE_SIZE (LORA_INTERNAL_HEADER_SIZE + LORA_PAYLOAD_SIZE) /*------------------------------------------------------------------------------ Typedefs @@ -66,81 +71,88 @@ 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 + { + TELEMETRY_MSG_VEHICLE_ID = 0x00000001, + TELEMETRY_MSG_DASHBOARD_DATA = 0x00000002, + __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."); + +/* UID serial number (packed struct inhibits padding) */ +typedef struct __attribute__((packed)) _ST_UID_TYPE { - LORA_MSG_VEHICLE_ID = 0x00000001, - LORA_MSG_DASHBOARD_DATA = 0x00000002, - LORA_MSG_WARNING_MESSAGE = 0x00000003, - LORA_MSG_INFO_MESSAGE = 0x00000004, - __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."); + 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 { - ST_UID_TYPE uid; - LORA_MESSAGE_TYPES mid; - uint32_t timestamp; + 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 - { - uint8_t hw_opcode; - uint8_t fw_opcode; - VERSION_INFO_TYPE version; - char flight_id[16]; - uint8_t explicit_padding[54]; - } 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 +typedef struct __attribute((packed)) _TELEMETRY_MSG_VEHICLE_ID_TYPE { - FLIGHT_COMP_STATE_TYPE fsm_state; - DASHBOARD_DUMP_TYPE data; - uint8_t explicit_padding[3]; - } 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 + 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 */ + } 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)) _TELEMETRY_MSG_DASHBOARD_DUMP_TYPE { - TEXT_MESSAGE msg; - } LORA_MSG_TEXT_MESSAGE_TYPE; - _Static_assert( sizeof(LORA_MSG_TEXT_MESSAGE_TYPE) == LORA_PAYLOAD_SIZE, "LORA_MSG_TEXT_MESSAGE_TYPE size invalid."); + 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 */ + } TELEMETRY_MSG_DASHBOARD_DUMP_TYPE; + _Static_assert( sizeof(TELEMETRY_MSG_DASHBOARD_DUMP_TYPE) == LORA_PAYLOAD_SIZE, "TELEMETRY_MSG_DASHBOARD_DUMP_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; + 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; + 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) */ } 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."); + +/*------------------------------------------------------------------------------ + 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 */ /*------------------------------------------------------------------------------ @@ -148,27 +160,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 }