diff --git a/can_bus/quadruna/VC/VC_alerts.json b/can_bus/quadruna/VC/VC_alerts.json index b9b92e8280..8df8cd3c72 100644 --- a/can_bus/quadruna/VC/VC_alerts.json +++ b/can_bus/quadruna/VC/VC_alerts.json @@ -79,6 +79,10 @@ "BrakeAppsDisagreement": { "id": 564, "description": "Brakes and apps are active at same time." + }, + "SbgInitFailed": { + "id": 565, + "description": "Initializing the SBG Ellipse GPS failed" } }, "faults": { @@ -105,11 +109,6 @@ "RightInverterFault": { "id": 556, "description": "Right inverter entered fault state." - }, - "SBGModeFault": { - "disabled": true, - "id" :561, - "description" : "Ensure mode is Nominal mode for EKF data used" - } + } } } \ No newline at end of file diff --git a/can_bus/quadruna/VC/VC_enum.json b/can_bus/quadruna/VC/VC_enum.json index 205086cc0a..f4cd372f55 100644 --- a/can_bus/quadruna/VC/VC_enum.json +++ b/can_bus/quadruna/VC/VC_enum.json @@ -4,11 +4,11 @@ "VC_INVERTER_ON_STATE": 1, "VC_DRIVE_STATE": 2 }, - - "VcSbgStatus": { - "VC_SOLUTION_COMPUTED": 0, - "INSUFFICIENT_READINGS": 1, - "INTERNAL_ERROR": 2, - "LIMIT_EXCEEDED": 3 + "VcEkfStatus": { + "UNINITIALIZED": 0, + "VERTICAL_GYRO": 1, + "AHRS": 2, + "VELOCITY": 3, + "POSITION": 4 } -} +} \ No newline at end of file diff --git a/can_bus/quadruna/VC/VC_tx.json b/can_bus/quadruna/VC/VC_tx.json index 4860ddf77c..dfce441bdb 100644 --- a/can_bus/quadruna/VC/VC_tx.json +++ b/can_bus/quadruna/VC/VC_tx.json @@ -85,21 +85,21 @@ } }, "INVL_ReadWriteParamCommand": { - "msg_id": 33, + "msg_id": 33, "cycle_time": null, "signals": { "INVL_CommandParameterAddress": { - "start_bit": 0, + "start_bit": 0, "bits": 16, "start_value": 20 }, "INVL_CommandReadWrite": { - "start_bit": 16, + "start_bit": 16, "enum": "InverterReadWriteCommand", "start_value": 1 }, "INVL_CommandData": { - "start_bit": 32, + "start_bit": 32, "bits": 16, "start_value": 0 } @@ -165,21 +165,21 @@ } }, "INVR_ReadWriteParamCommand": { - "msg_id": 83, + "msg_id": 83, "cycle_time": null, "signals": { "INVR_CommandParameterAddress": { - "start_bit": 0, + "start_bit": 0, "bits": 16, "start_value": 20 }, "INVR_CommandReadWrite": { - "start_bit": 16, + "start_bit": 16, "enum": "InverterReadWriteCommand", "start_value": 1 }, "INVR_CommandData": { - "start_bit": 32, + "start_bit": 32, "bits": 16, "start_value": 0 } @@ -204,7 +204,6 @@ } }, "EllipseStatus": { - "disabled": true, "msg_id": 223, "cycle_time": 100, "description": "Status of the SBG Ellipse N sensor.", @@ -214,11 +213,13 @@ }, "EllipseComStatusBitmask": { "bits": 32 + }, + "EkfSolutionMode": { + "enum": "VcEkfStatus" } } }, "EllipseTime": { - "disabled": true, "msg_id": 209, "cycle_time": 100, "description": "Timestamp from the SBG Ellipse N sensor.", @@ -282,7 +283,6 @@ } }, "EllipseEulerAngles": { - "disabled": true, "msg_id": 222, "cycle_time": 1000, "description": "Euler angles from the SBG Ellipse N sensor.", @@ -488,20 +488,23 @@ "RegenEnabled": { "bits": 1 }, - "TorqueVectoringEnabled":{ + "TorqueVectoringEnabled": { "bits": 1 + }, + "VehicleVelocity": { + "resolution": 0.01, + "min": -150, + "max": 150, + "unit": "km/h" } } }, - "GpsPosInfo": { + "EllipseEkfNavPosition": { "disabled": true, "msg_id": 217, "cycle_time": 100, "description": "Car position info", "signals": { - "PositionStatus": { - "enum": "VcSbgStatus" - }, "Latitude": { "resolution": 0.00005, "min": 47, @@ -540,49 +543,45 @@ } } }, - "GpsVelInfo": { - "disabled": true, + "EllipseEkfNavVelocity": { "msg_id": 218, "cycle_time": 100, "description": "Car Velocity info", "signals": { - "VelocityStatus": { - "enum": "VcSbgStatus" - }, "VelocityNorth": { - "resolution": 0.1, - "min": 0, - "max": 120, + "resolution": 0.05, + "min": -35, + "max": 35, "unit": "m/s" }, "VelocityNorthAccuracy": { "resolution": 0.1, "min": 0, - "max": 20, + "max": 35, "unit": "m/s" }, "VelocityEast": { - "resolution": 0.1, - "min": 0, - "max": 120, + "resolution": 0.05, + "min": -35, + "max": 35, "unit": "m/s" }, "VelocityEastAccuracy": { "resolution": 0.1, "min": 0, - "max": 20, + "max": 35, "unit": "m/s" }, "VelocityDown": { - "resolution": 0.1, - "min": 0, - "max": 120, + "resolution": 0.05, + "min": -35, + "max": 35, "unit": "m/s" }, "VelocityDownAccuracy": { "resolution": 0.1, "min": 0, - "max": 20, + "max": 35, "unit": "m/s" } } @@ -764,7 +763,7 @@ "cycle_time": 100, "description": "Pedal percentage info", "signals": { - "MappedPedalPercentage" :{ + "MappedPedalPercentage": { "min": -1, "max": 1, "resolution": 0.01, diff --git a/firmware/quadruna/VC/src/app/app_sbgEllipse.c b/firmware/quadruna/VC/src/app/app_sbgEllipse.c index 49c0db8336..afd9c44825 100644 --- a/firmware/quadruna/VC/src/app/app_sbgEllipse.c +++ b/firmware/quadruna/VC/src/app/app_sbgEllipse.c @@ -1,23 +1,55 @@ #include #include "app_sbgEllipse.h" #include "app_canTx.h" +#include "app_utils.h" +#include "app_units.h" #include "io_sbgEllipse.h" -// TODO: Not using Ellipse GPS for Comp 2024 void app_sbgEllipse_broadcast() { - // // Status msg + /* Enable these back when you turn this on in the SBG, otherwise it's still sending + CAN messages because another message in the signal is being used */ + + // Status msg // const uint16_t general_status = io_sbgEllipse_getGeneralStatus(); // const uint32_t com_status = io_sbgEllipse_getComStatus(); // app_canTx_VC_EllipseGeneralStatusBitmask_set(general_status); // app_canTx_VC_EllipseComStatusBitmask_set(com_status); - // // Time msg + // Time msg // const uint32_t timestamp_us = io_sbgEllipse_getTimestampUs(); // app_canTx_VC_EllipseTimestamp_set(timestamp_us); - // // Acceleration msg + // Velocity EKF + const float ekf_vel_N = io_sbgEllipse_getEkfNavVelocityData()->north; + const float ekf_vel_E = io_sbgEllipse_getEkfNavVelocityData()->east; + const float ekf_vel_D = io_sbgEllipse_getEkfNavVelocityData()->down; + + const float ekf_vel_N_accuracy = io_sbgEllipse_getEkfNavVelocityData()->north_std_dev; + const float ekf_vel_E_accuracy = io_sbgEllipse_getEkfNavVelocityData()->east_std_dev; + const float ekf_vel_D_accuracy = io_sbgEllipse_getEkfNavVelocityData()->down_std_dev; + + app_canTx_VC_VelocityNorth_set(ekf_vel_N); + app_canTx_VC_VelocityEast_set(ekf_vel_E); + app_canTx_VC_VelocityDown_set(ekf_vel_D); + + app_canTx_VC_VelocityNorthAccuracy_set(ekf_vel_N_accuracy); + app_canTx_VC_VelocityEastAccuracy_set(ekf_vel_E_accuracy); + app_canTx_VC_VelocityDownAccuracy_set(ekf_vel_D_accuracy); + + const float vehicle_velocity = sqrtf(SQUARE(ekf_vel_N) + SQUARE(ekf_vel_E) + SQUARE(ekf_vel_D)); + + app_canTx_VC_VehicleVelocity_set(MPS_TO_KMH(vehicle_velocity)); + + // Position EKF + // const double ekf_pos_lat = io_sbgEllipse_getEkfNavPositionData()->latitude; + // const double ekf_pos_long = io_sbgEllipse_getEkfNavPositionData()->longitude; + + // app_canTx_VC_Latitude_set((float)ekf_pos_lat); + // app_canTx_VC_Longtitude_set((float)ekf_pos_long); + + // Acceleration msg // const float forward_accel = io_sbgEllipse_getImuAccelerations()->x; // const float lateral_accel = io_sbgEllipse_getImuAccelerations()->y; // const float vertical_accel = io_sbgEllipse_getImuAccelerations()->z; @@ -26,7 +58,7 @@ void app_sbgEllipse_broadcast() // app_canTx_VC_AccelerationLateral_set(lateral_accel); // app_canTx_VC_AccelerationVertical_set(vertical_accel); - // // Angular velocity msg + // Angular velocity msg // const float ang_vel_roll = io_sbgEllipse_getImuAngularVelocities()->roll; // const float ang_vel_pitch = io_sbgEllipse_getImuAngularVelocities()->pitch; // const float ang_vel_yaw = io_sbgEllipse_getImuAngularVelocities()->yaw; @@ -35,12 +67,12 @@ void app_sbgEllipse_broadcast() // app_canTx_VC_AngularVelocityPitch_set((int)ang_vel_pitch); // app_canTx_VC_AngularVelocityYaw_set((int)ang_vel_yaw); - // // Euler angles msg - // const float euler_roll = io_sbgEllipse_getEulerAngles()->roll; - // const float euler_pitch = io_sbgEllipse_getEulerAngles()->pitch; - // const float euler_yaw = io_sbgEllipse_getEulerAngles()->yaw; + // Euler angles msg + const float euler_roll = io_sbgEllipse_getEkfEulerAngles()->roll; + const float euler_pitch = io_sbgEllipse_getEkfEulerAngles()->pitch; + const float euler_yaw = io_sbgEllipse_getEkfEulerAngles()->yaw; - // app_canTx_VC_EulerAnglesRoll_set(euler_roll); - // app_canTx_VC_EulerAnglesPitch_set(euler_pitch); - // app_canTx_VC_EulerAnglesYaw_set(euler_yaw); + app_canTx_VC_EulerAnglesRoll_set(euler_roll); + app_canTx_VC_EulerAnglesPitch_set(euler_pitch); + app_canTx_VC_EulerAnglesYaw_set(euler_yaw); } diff --git a/firmware/quadruna/VC/src/app/app_sbgEllipse.h b/firmware/quadruna/VC/src/app/app_sbgEllipse.h index 4c3e62844f..0686ee44a3 100644 --- a/firmware/quadruna/VC/src/app/app_sbgEllipse.h +++ b/firmware/quadruna/VC/src/app/app_sbgEllipse.h @@ -1,7 +1,6 @@ #pragma once #include - /* * Broadcast sensor outputs over CAN. */ diff --git a/firmware/quadruna/VC/src/app/states/app_allStates.c b/firmware/quadruna/VC/src/app/states/app_allStates.c index 408a17d62a..203b0558fa 100644 --- a/firmware/quadruna/VC/src/app/states/app_allStates.c +++ b/firmware/quadruna/VC/src/app/states/app_allStates.c @@ -55,9 +55,8 @@ void app_allStates_runOnTick100Hz(void) else app_heartbeatMonitor_broadcastFaults(); - // Comment out for now - SBG Ellipse is not currently used. - // io_sbgEllipse_handleLogs(); - // app_sbgEllipse_broadcast(); + io_sbgEllipse_handleLogs(); + app_sbgEllipse_broadcast(); // Set status to false (which blocks drive) if either inverter is faulted, or another board has set a fault. } diff --git a/firmware/quadruna/VC/src/app/states/app_driveState.c b/firmware/quadruna/VC/src/app/states/app_driveState.c index 6722d0a9d1..4b67e9008e 100644 --- a/firmware/quadruna/VC/src/app/states/app_driveState.c +++ b/firmware/quadruna/VC/src/app/states/app_driveState.c @@ -115,6 +115,9 @@ static void driveStateRunOnTick100Hz(void) regen_switch_is_on = app_canRx_CRIT_RegenSwitch_get() == SWITCH_ON; bool turn_regen_led = regen_switch_is_on && !prev_regen_switch_val; + /* TODO: Vehicle dyanmics people need to make sure to do a check if sensor init failed + or not before using closed loop features */ + // Regen + TV LEDs and update warnings if (turn_regen_led) { diff --git a/firmware/quadruna/VC/src/io/io_sbgEllipse.c b/firmware/quadruna/VC/src/io/io_sbgEllipse.c index 051538647a..c5e8df5245 100644 --- a/firmware/quadruna/VC/src/io/io_sbgEllipse.c +++ b/firmware/quadruna/VC/src/io/io_sbgEllipse.c @@ -1,18 +1,22 @@ -#include "main.h" -#include -#include "sbgECom.h" -#include "interfaces/sbgInterfaceSerial.h" #include "io_sbgEllipse.h" -#include "app_units.h" + +#include #include "FreeRTOS.h" #include "cmsis_os.h" #include "queue.h" + +#include "main.h" +#include "app_units.h" +#include "sbgECom.h" +#include "interfaces/sbgInterfaceSerial.h" #include "app_canTx.h" +#include "io_time.h" +#include "io_log.h" /* ------------------------------------ Defines ------------------------------------- */ #define UART_RX_PACKET_SIZE 128 // Size of each received UART packet, in bytes -#define QUEUE_MAX_SIZE 4095 // 4kB +#define QUEUE_MAX_SIZE 32 // 128 * 32 = 4096 which is SBG_ECOM_MAX_BUFFER_SIZE /* --------------------------------- Variables ---------------------------------- */ extern UART_HandleTypeDef huart2; @@ -25,7 +29,7 @@ static SensorData sensor_data; // Struct of all senso static osMessageQueueId_t sensor_rx_queue_id; static StaticQueue_t rx_queue_control_block; -static uint8_t sensor_rx_queue_buf[QUEUE_MAX_SIZE]; +static uint8_t sensor_rx_queue_buf[QUEUE_MAX_SIZE * UART_RX_PACKET_SIZE]; static uint32_t sbg_queue_overflow_count; static const osMessageQueueAttr_t sensor_rx_queue_attr = { @@ -34,7 +38,7 @@ static const osMessageQueueAttr_t sensor_rx_queue_attr = { .cb_mem = &rx_queue_control_block, .cb_size = sizeof(StaticQueue_t), .mq_mem = sensor_rx_queue_buf, - .mq_size = QUEUE_MAX_SIZE, + .mq_size = QUEUE_MAX_SIZE * UART_RX_PACKET_SIZE, }; /* ------------------------- Static Function Prototypes -------------------------- */ @@ -85,30 +89,34 @@ static SbgErrorCode io_sbgEllipse_read(SbgInterface *interface, void *buffer, si // Disable interrupts so UART RX callback won't push data to the queue while we're reading from it vPortEnterCritical(); + *read_bytes = 0; // Read all available data from the RX queue, up to the requested amount - size_t i = 0; - while (i < bytes_to_read) - { - uint8_t data; + size_t i = 0; + uint8_t packet[UART_RX_PACKET_SIZE]; + while (*read_bytes < bytes_to_read) + { if (osMessageQueueGetCount(sensor_rx_queue_id) == 0) { break; } - if (osMessageQueueGet(sensor_rx_queue_id, &data, NULL, osWaitForever) != osOK) + if (osMessageQueueGet(sensor_rx_queue_id, packet, NULL, osWaitForever) != osOK) { break; } - ((uint8_t *)buffer)[i] = data; - (*read_bytes)++; - i++; + size_t bytes_to_copy = + (bytes_to_read - *read_bytes) < UART_RX_PACKET_SIZE ? (bytes_to_read - *read_bytes) : UART_RX_PACKET_SIZE; + + memcpy((uint8_t *)buffer + *read_bytes, packet, bytes_to_copy); + *read_bytes += bytes_to_copy; } vPortExitCritical(); + return SBG_NO_ERROR; } @@ -182,9 +190,9 @@ static void io_sbgEllipse_processMsg_Imu(const SbgBinaryLogData *log_data) static void io_sbgEllipse_processMsg_eulerAngles(const SbgBinaryLogData *log_data) { // Save euler angles, in deg - sensor_data.euler_data.euler_angles.roll = RAD_TO_DEG(log_data->ekfEulerData.euler[0]); - sensor_data.euler_data.euler_angles.pitch = RAD_TO_DEG(log_data->ekfEulerData.euler[1]); - sensor_data.euler_data.euler_angles.yaw = RAD_TO_DEG(log_data->ekfEulerData.euler[2]); + sensor_data.ekf_euler_data.euler_angles.roll = RAD_TO_DEG(log_data->ekfEulerData.euler[0]); + sensor_data.ekf_euler_data.euler_angles.pitch = RAD_TO_DEG(log_data->ekfEulerData.euler[1]); + sensor_data.ekf_euler_data.euler_angles.yaw = RAD_TO_DEG(log_data->ekfEulerData.euler[2]); } /* @@ -202,29 +210,44 @@ static void io_sbgEllipse_processMsg_status(const SbgBinaryLogData *log_data) */ static void io_sbgEllipse_processMsg_EkfNavVelandPos(const SbgBinaryLogData *log_data) { - // TODO: uncomment after initial testing + // TODO: uncomment after initial testing, if this occurs skip reading data // app_canAlerts_VC_Fault_SBGModeFault_set(sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status) != // SBG_ECOM_SOL_MODE_NAV_POSITION); + app_canTx_VC_EkfSolutionMode_set((VcEkfStatus)sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status)); + + // uint32_t status = log_data->ekfNavData.status; + + // if (sbgEComLogEkfGetSolutionMode(log_data->ekfNavData.status) != SBG_ECOM_SOL_MODE_NAV_POSITION) + // { + // uint32_t status = log_data->ekfNavData.status; + + // bool is_velocity_valid = (status & SBG_ECOM_SOL_VELOCITY_VALID) != 0; + // bool is_position_valid = (status & SBG_ECOM_SOL_POSITION_VALID) != 0; + + // if (!is_velocity_valid & !is_position_valid) + // return; + // } + // velocity data in m/s - sensor_data.ekf_data.ekf_nav_velocity.north = log_data->ekfNavData.velocity[0]; - sensor_data.ekf_data.ekf_nav_velocity.east = log_data->ekfNavData.velocity[1]; - sensor_data.ekf_data.ekf_nav_velocity.down = log_data->ekfNavData.velocity[2]; + sensor_data.ekf_nav_data.velocity.north = log_data->ekfNavData.velocity[0]; + sensor_data.ekf_nav_data.velocity.east = log_data->ekfNavData.velocity[1]; + sensor_data.ekf_nav_data.velocity.down = log_data->ekfNavData.velocity[2]; // velocity standard dev - sensor_data.ekf_data.ekf_nav_velocity.north_std_dev = log_data->ekfNavData.velocityStdDev[0]; - sensor_data.ekf_data.ekf_nav_velocity.east_std_dev = log_data->ekfNavData.velocityStdDev[1]; - sensor_data.ekf_data.ekf_nav_velocity.down_std_dev = log_data->ekfNavData.velocityStdDev[2]; + sensor_data.ekf_nav_data.velocity.north_std_dev = log_data->ekfNavData.velocityStdDev[0]; + sensor_data.ekf_nav_data.velocity.east_std_dev = log_data->ekfNavData.velocityStdDev[1]; + sensor_data.ekf_nav_data.velocity.down_std_dev = log_data->ekfNavData.velocityStdDev[2]; // position data in m - sensor_data.ekf_data.ekf_nav_position.latitude = log_data->ekfNavData.position[0]; - sensor_data.ekf_data.ekf_nav_position.longitude = log_data->ekfNavData.position[1]; - sensor_data.ekf_data.ekf_nav_position.altitude = log_data->ekfNavData.position[2]; + sensor_data.ekf_nav_data.position.latitude = log_data->ekfNavData.position[0]; + sensor_data.ekf_nav_data.position.longitude = log_data->ekfNavData.position[1]; + sensor_data.ekf_nav_data.position.altitude = log_data->ekfNavData.position[2]; // position standard dev - sensor_data.ekf_data.ekf_nav_position.altitude_std_dev = log_data->ekfNavData.positionStdDev[0]; - sensor_data.ekf_data.ekf_nav_position.latitude_std_dev = log_data->ekfNavData.positionStdDev[1]; - sensor_data.ekf_data.ekf_nav_position.longitude_std_dev = log_data->ekfNavData.positionStdDev[2]; + sensor_data.ekf_nav_data.position.altitude_std_dev = log_data->ekfNavData.positionStdDev[0]; + sensor_data.ekf_nav_data.position.latitude_std_dev = log_data->ekfNavData.positionStdDev[1]; + sensor_data.ekf_nav_data.position.longitude_std_dev = log_data->ekfNavData.positionStdDev[2]; } /* ------------------------- Public Function Definitions -------------------------- */ @@ -238,16 +261,19 @@ bool io_sbgEllipse_init(const UART *sbg_uart) // Initialize the SBG serial interface handle io_sbgEllipse_createSerialInterface(&sbg_interface); + SbgErrorCode sbgEComInitCode = sbgEComInit(&com_handle, &sbg_interface); + // Init SBG's communication protocol handle - if (sbgEComInit(&com_handle, &sbg_interface) != SBG_NO_ERROR) + if (sbgEComInitCode != SBG_NO_ERROR) { + LOG_INFO("%d", sbgEComInitCode); return false; } // Set the callback function (callback is called when a new log is successfully received and parsed) sbgEComSetReceiveLogCallback(&com_handle, io_sbgEllipse_logReceivedCallback, NULL); // Init RX queue for UART data - sensor_rx_queue_id = osMessageQueueNew(QUEUE_MAX_SIZE, sizeof(uint8_t), &sensor_rx_queue_attr); + sensor_rx_queue_id = osMessageQueueNew(QUEUE_MAX_SIZE, UART_RX_PACKET_SIZE, &sensor_rx_queue_attr); assert(sensor_rx_queue_id != NULL); @@ -262,7 +288,14 @@ void io_sbgEllipse_handleLogs(void) // Handle logs. Calls the pReadFunc set in sbgInterfaceSerialCreate to read data and parses // all logs found in the data. Upon successfully parsing a log, the the receive log callback function set in init is // triggered. Incomplete log data will be saved to a buffer in SBG's library to be used once more data is received. - sbgEComHandle(&com_handle); + SbgErrorCode errorCode = sbgEComHandle(&com_handle); + char buffer[256]; + sbgEComErrorToString(errorCode, buffer); + if (errorCode != SBG_NO_ERROR) + { + // handle error + LOG_INFO("%s", buffer); + } } uint32_t io_sbgEllipse_getTimestampUs(void) @@ -295,30 +328,27 @@ Attitude *io_sbgEllipse_getImuAngularVelocities() return &sensor_data.imu_data.angular_velocity; } -Attitude *io_sbgEllipse_getEulerAngles() +Attitude *io_sbgEllipse_getEkfEulerAngles() { - return &sensor_data.euler_data.euler_angles; + return &sensor_data.ekf_euler_data.euler_angles; } -EkfNavVelocityData *io_sbgEllipse_getEkfNavVelocityData() +VelocityData *io_sbgEllipse_getEkfNavVelocityData() { - return &sensor_data.ekf_data.ekf_nav_velocity; + return &sensor_data.ekf_nav_data.velocity; } -EkfNavPositionData *io_sbgEllipse_getEkfNavPositionData() +PositionData *io_sbgEllipse_getEkfNavPositionData() { - return &sensor_data.ekf_data.ekf_nav_position; + return &sensor_data.ekf_nav_data.position; } void io_sbgEllipse_msgRxCallback(void) { - for (int i = 0; i < UART_RX_PACKET_SIZE; i++) + sbg_queue_overflow_count = 0; + if (osMessageQueuePut(sensor_rx_queue_id, &uart_rx_buffer, 0, 0) != osOK) { - sbg_queue_overflow_count = 0; - - if (osMessageQueuePut(sensor_rx_queue_id, &uart_rx_buffer[i], 0, 0) != osOK) - { - sbg_queue_overflow_count++; - } + sbg_queue_overflow_count++; } + LOG_INFO("%d", sbg_queue_overflow_count); } diff --git a/firmware/quadruna/VC/src/io/io_sbgEllipse.h b/firmware/quadruna/VC/src/io/io_sbgEllipse.h index 34f586d85b..2ff387cc8a 100644 --- a/firmware/quadruna/VC/src/io/io_sbgEllipse.h +++ b/firmware/quadruna/VC/src/io/io_sbgEllipse.h @@ -32,24 +32,24 @@ typedef struct float latitude_std_dev; float longitude_std_dev; float altitude_std_dev; -} EkfNavPositionData; +} PositionData; typedef struct { uint32_t status; - float north; // North - float east; // East - float down; // Down + float north; + float east; + float down; float north_std_dev; float east_std_dev; float down_std_dev; -} EkfNavVelocityData; +} VelocityData; typedef struct { - EkfNavVelocityData ekf_nav_velocity; - EkfNavPositionData ekf_nav_position; -} EkfData; + VelocityData velocity; + PositionData position; +} EkfNavPacketData; typedef struct { @@ -60,7 +60,7 @@ typedef struct typedef struct { Attitude euler_angles; -} EulerPacketData; +} EkfEulerPacketData; typedef struct { @@ -71,10 +71,10 @@ typedef struct typedef struct { - ImuPacketData imu_data; - EulerPacketData euler_data; - StatusPacketData status_data; - EkfData ekf_data; + ImuPacketData imu_data; + EkfEulerPacketData ekf_euler_data; + StatusPacketData status_data; + EkfNavPacketData ekf_nav_data; } SensorData; #ifdef TARGET_EMBEDDED @@ -136,7 +136,7 @@ Attitude *io_sbgEllipse_getImuAngularVelocities(); * - float pitch: Pitch angle in rad * - float yaw: Yaw angle in rad */ -Attitude *io_sbgEllipse_getEulerAngles(); +Attitude *io_sbgEllipse_getEkfEulerAngles(); /* * Get the GPS velocity data as a struct pointer with fields: @@ -148,7 +148,7 @@ Attitude *io_sbgEllipse_getEulerAngles(); * - float velocity_accuracy_e: East velocity accuracy in m/s * - float velocity_accuracy_d: Down velocity accuracy in m/s */ -EkfNavVelocityData *io_sbgEllipse_getEkfNavVelocityData(); +VelocityData *io_sbgEllipse_getEkfNavVelocityData(); /* * Get the GPS position data as a struct pointer with fields: @@ -161,7 +161,7 @@ EkfNavVelocityData *io_sbgEllipse_getEkfNavVelocityData(); * - float altitude_accuracy: Altitude accuracy in meters * */ -EkfNavPositionData *io_sbgEllipse_getEkfNavPositionData(); +PositionData *io_sbgEllipse_getEkfNavPositionData(); /* * Handle SBG Ellipse UART Callbacks diff --git a/firmware/quadruna/VC/src/tasks.c b/firmware/quadruna/VC/src/tasks.c index 732973f0c1..223935cd1d 100644 --- a/firmware/quadruna/VC/src/tasks.c +++ b/firmware/quadruna/VC/src/tasks.c @@ -362,15 +362,15 @@ void tasks_init(void) io_efuse_init(efuse_configs); io_pcm_init(&pcm_config); - // Comment out for now, not using sbg - // if (!io_sbgEllipse_init(&sbg_uart)) - // { - // Error_Handler(); - // } - + if (!io_sbgEllipse_init(&sbg_uart)) + { + app_canAlerts_VC_Warning_SbgInitFailed_set(true); + LOG_INFO("Sbg initialization failed"); + } if (!io_imu_init()) { app_canAlerts_VC_Warning_ImuInitFailed_set(true); + LOG_INFO("Imu initialization failed"); } app_canTx_init(); diff --git a/firmware/quadruna/VC/test/test_vcBaseStateMachineTest.h b/firmware/quadruna/VC/test/test_vcBaseStateMachineTest.h index f1ce197705..ad6cf7c905 100644 --- a/firmware/quadruna/VC/test/test_vcBaseStateMachineTest.h +++ b/firmware/quadruna/VC/test/test_vcBaseStateMachineTest.h @@ -58,7 +58,9 @@ class VcBaseStateMachineTest : public BaseStateMachineTest fake_io_sbgEllipse_getImuAccelerations_returns(&fake_sensor_data.imu_data.acceleration); fake_io_sbgEllipse_getImuAngularVelocities_returns(&fake_sensor_data.imu_data.angular_velocity); - fake_io_sbgEllipse_getEulerAngles_returns(&fake_sensor_data.euler_data.euler_angles); + fake_io_sbgEllipse_getEkfEulerAngles_returns(&fake_sensor_data.ekf_euler_data.euler_angles); + fake_io_sbgEllipse_getEkfNavVelocityData_returns(&fake_sensor_data.ekf_nav_data.velocity); + fake_io_sbgEllipse_getEkfNavPositionData_returns(&fake_sensor_data.ekf_nav_data.position); } void TearDown() override @@ -79,7 +81,9 @@ class VcBaseStateMachineTest : public BaseStateMachineTest fake_io_pcm_set_reset(); fake_io_sbgEllipse_getImuAccelerations_reset(); fake_io_sbgEllipse_getImuAngularVelocities_reset(); - fake_io_sbgEllipse_getEulerAngles_reset(); + fake_io_sbgEllipse_getEkfEulerAngles_reset(); + fake_io_sbgEllipse_getEkfNavVelocityData_reset(); + fake_io_sbgEllipse_getEkfNavPositionData_reset(); } void SetInitialState(const State *const initial_state) diff --git a/firmware/shared/src/app/app_units.h b/firmware/shared/src/app/app_units.h index 224c1c1e5a..05a5091c63 100644 --- a/firmware/shared/src/app/app_units.h +++ b/firmware/shared/src/app/app_units.h @@ -24,3 +24,4 @@ #define MOTOR_KMH_TO_RPM(kmh) \ ((int)((kmh) / ((float)WHEEL_DIAMETER_IN * M_PI_F * INCH_TO_KM * MIN_TO_HOUR / GEAR_RATIO))) #define WHEEL_KMH_TO_RPM(kmh) ((int)((kmh) / ((float)WHEEL_DIAMETER_IN * M_PI_F * INCH_TO_KM * MIN_TO_HOUR))) +#define MPS_TO_KMH(mps) ((mps) * 3.6f) \ No newline at end of file diff --git a/firmware/shared/src/app/app_utils.h b/firmware/shared/src/app/app_utils.h index cc91525167..558db5520a 100644 --- a/firmware/shared/src/app/app_utils.h +++ b/firmware/shared/src/app/app_utils.h @@ -11,6 +11,7 @@ #define MIN3(x, y, z) (MIN(MIN((x), (y)), (z))) #define CLAMP(x, min, max) (MAX(MIN(x, max), min)) #define CLAMP_TO_ONE(x) (((x) <= 0) ? 1 : ((x) > 1 ? 1 : (x))) // initialize to 1 if value is <=0 +#define SQUARE(x) ((x) * (x)) #define MAX_4_BITS_VALUE (uint32_t)(15) #define MAX_6_BITS_VALUE (uint32_t)(63)