diff --git a/can_bus/thruna/DCM/DCM_alerts.json b/can_bus/thruna/DCM/DCM_alerts.json index a8bfe1ba71..e1deb2cd6f 100644 --- a/can_bus/thruna/DCM/DCM_alerts.json +++ b/can_bus/thruna/DCM/DCM_alerts.json @@ -12,7 +12,8 @@ "WatchdogTimeout", "TxOverflow", "RxOverflow", - "RegenNotAvailable" + "RegenNotAvailable", + "SbgRxOverflow" ], "faults": [ "MissingBMSHeartbeat", diff --git a/can_bus/thruna/DCM/DCM_enum.json b/can_bus/thruna/DCM/DCM_enum.json index 204312b722..6adfc4629a 100644 --- a/can_bus/thruna/DCM/DCM_enum.json +++ b/can_bus/thruna/DCM/DCM_enum.json @@ -2,5 +2,12 @@ "DcmState": { "DCM_INIT_STATE": 0, "DCM_DRIVE_STATE": 1 + }, + + "DcmSbgStatus": { + "Solution_Computed": 0, + "Insufficent_Readings": 1, + "Internal_Error": 2, + "Limit_Exceeded": 3 } -} \ No newline at end of file +} diff --git a/can_bus/thruna/DCM/DCM_tx.json b/can_bus/thruna/DCM/DCM_tx.json index 6fc2c0d309..40a3c3e326 100644 --- a/can_bus/thruna/DCM/DCM_tx.json +++ b/can_bus/thruna/DCM/DCM_tx.json @@ -420,5 +420,97 @@ "resolution": 0.1 } } + }, + "GpsPosInfo":{ + "msg_id": 217, + "cycle_time": 100, + "description": "Car position info", + "signals":{ + "PositionStatus":{ + "enum": "DcmSbgStatus" + }, + "Latitude":{ + "resolution": 0.00005, + "min": 47, + "max": 50, + "unit": "deg" + }, + "LatitudeAccuracy":{ + "resolution": 0.1, + "min": 0, + "max": 2, + "unit": "m" + }, + "Longtitude":{ + "resolution": 0.00005, + "min": 122, + "max": 125, + "unit": "deg" + }, + "LongtitudeAccuracy":{ + "resolution": 0.1, + "min": 0, + "max": 2, + "unit": "m" + }, + "Altitude":{ + "resolution": 0.5, + "min": 0, + "max": 400, + "unit": "m" + }, + "AltitudeAccuracy":{ + "resolution": 0.1, + "min": 0, + "max": 20, + "unit": "m" + } + } + }, + "GpsVelInfo":{ + "msg_id": 218, + "cycle_time": 100, + "description": "Car Velocity info", + "signals":{ + "VelocityStatus":{ + "enum":"DcmSbgStatus" + }, + "VelocityNorth":{ + "resolution": 0.1, + "min": 0, + "max": 120, + "unit": "m/s" + }, + "VelocityNorthAccuracy":{ + "resolution": 0.1, + "min": 0, + "max": 20, + "unit": "m/s" + }, + "VelocityEast":{ + "resolution": 0.1, + "min": 0, + "max": 120, + "unit": "m/s" + }, + "VelocityEastAccuracy":{ + "resolution": 0.1, + "min": 0, + "max": 20, + "unit": "m/s" + }, + "VelocityDown":{ + "resolution": 0.1, + "min": 0, + "max": 120, + "unit": "m/s" + }, + "VelocityDownAccuracy":{ + "resolution": 0.1, + "min": 0, + "max": 20, + "unit": "m/s" + } + } } -} \ No newline at end of file +} diff --git a/firmware/shared/src/app/App_RingQueue.c b/firmware/shared/src/app/App_RingQueue.c deleted file mode 100644 index 9a087ca58d..0000000000 --- a/firmware/shared/src/app/App_RingQueue.c +++ /dev/null @@ -1,72 +0,0 @@ -#include -#include - -#include "App_RingQueue.h" - -void App_RingQueue_Init(RingQueue *queue, int size) -{ - // Queue size cannot exceed allocated buffer size - assert(size > 0 && size <= RING_QUEUE_MAX_SIZE); - assert(queue); - - memset(queue->elements, 0U, sizeof(queue->elements)); - queue->size = size; - queue->head = 0; - queue->tail = -1; - queue->count = 0; -} - -void App_RingQueue_Push(RingQueue *queue, uint8_t value) -{ - assert(queue); - - // Increment tail idx, with wrapping if necesssary - queue->tail += 1; - queue->tail %= queue->size; - - // Insert the new value - queue->elements[queue->tail] = value; - - // The queue has overflowed if the tail idx has "circled around" and caught up to the head idx, - // so increment the head idx. Also check there are elements in queue so this is not set for - // an empty queue (which has tail and head idx equal). - const bool queue_overflowed = (queue->tail == queue->head) && queue->count > 0; - if (queue_overflowed) - { - // Count doesn't change if overflowed, since a new element replaced an old element - queue->head += 1; - queue->head %= queue->size; - } - else - { - // Queue has space, so we increase the number of elements by 1 - queue->count += 1; - } -} - -bool App_RingQueue_Pop(RingQueue *queue, uint8_t *value) -{ - assert(queue); - assert(value); - - if (queue->count == 0) - { - // Queue is empty, return false - return false; - } - - // Write value at head idx to output - *value = queue->elements[queue->head]; - - // Move head to next-oldest value for future pop operations, and shrink size by 1 - queue->head += 1; - queue->head %= queue->size; - queue->count -= 1; - return true; -} - -int App_RingQueue_Count(RingQueue *queue) -{ - assert(queue); - return queue->count; -} diff --git a/firmware/shared/src/app/App_RingQueue.h b/firmware/shared/src/app/App_RingQueue.h deleted file mode 100644 index d5c2c16714..0000000000 --- a/firmware/shared/src/app/App_RingQueue.h +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -#include -#include -#include - -#define RING_QUEUE_MAX_SIZE 4096 // 4kB - -// Data struct for a ring queue: Do not interact with member vars directly! -typedef struct -{ - uint8_t elements[RING_QUEUE_MAX_SIZE]; - int size; - int head; - int tail; - int count; -} RingQueue; - -/** - * Initialize a ring queue. - * @param queue: Queue to initialize - * @param size: Desired size of queue, cannot exceed RING_QUEUE_MAX_SIZE - */ -void App_RingQueue_Init(RingQueue *queue, int size); - -/** - * @brief Push to queue. Will overwrite the oldest value if the ring buffer has - * exceeded its size. - * @param queue: Queue to push to - * @param value: Value to push - */ -void App_RingQueue_Push(RingQueue *queue, uint8_t value); - -/** - * Pop from queue. - * @param queue: Queue to pop from - * @param value: Pointer to the popped output - * @return True if an item was popped off, false otherwise (i.e. if queue was empty) - */ -bool App_RingQueue_Pop(RingQueue *queue, uint8_t *value); - -/** - * Get the number of elements in a queue. - * @param queue: Queue to read from - */ -int App_RingQueue_Count(RingQueue *queue); diff --git a/firmware/shared/test/Test_RingQueue.cpp b/firmware/shared/test/Test_RingQueue.cpp deleted file mode 100644 index dc2464f353..0000000000 --- a/firmware/shared/test/Test_RingQueue.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include - -extern "C" -{ -#include "App_RingQueue.h" -} - -TEST(RingQueueTest, test_push_and_pop) -{ - RingQueue queue; - App_RingQueue_Init(&queue, 10); - - // Queue is empty, no data - uint8_t popped; - ASSERT_FALSE(App_RingQueue_Pop(&queue, &popped)); - - // Push 1, 2, 3 - App_RingQueue_Push(&queue, 1); - App_RingQueue_Push(&queue, 2); - App_RingQueue_Push(&queue, 3); - - // Pop 1, 2, 3 - ASSERT_TRUE(App_RingQueue_Pop(&queue, &popped)); - ASSERT_EQ(popped, 1); - - ASSERT_TRUE(App_RingQueue_Pop(&queue, &popped)); - ASSERT_EQ(popped, 2); - - ASSERT_TRUE(App_RingQueue_Pop(&queue, &popped)); - ASSERT_EQ(popped, 3); - - // Queue is empty, no data - ASSERT_FALSE(App_RingQueue_Pop(&queue, &popped)); - - // Push 4, then pop 4 - App_RingQueue_Push(&queue, 4); - - ASSERT_TRUE(App_RingQueue_Pop(&queue, &popped)); - ASSERT_EQ(popped, 4); - - // Queue is empty, no data - ASSERT_FALSE(App_RingQueue_Pop(&queue, &popped)); -} - -TEST(RingQueueTest, test_buffer_overflow) -{ - RingQueue queue; - App_RingQueue_Init(&queue, 3); - - uint8_t popped; - ASSERT_FALSE(App_RingQueue_Pop(&queue, &popped)); - - // Push 1, 2, 3, 4 - App_RingQueue_Push(&queue, 1); - App_RingQueue_Push(&queue, 2); - App_RingQueue_Push(&queue, 3); - App_RingQueue_Push(&queue, 4); - - // Pop 2, 3, 4 (queue overflowed, 1 was overwritten) - ASSERT_TRUE(App_RingQueue_Pop(&queue, &popped)); - ASSERT_EQ(popped, 2); - - ASSERT_TRUE(App_RingQueue_Pop(&queue, &popped)); - ASSERT_EQ(popped, 3); - - // Push more elements - App_RingQueue_Push(&queue, 5); - App_RingQueue_Push(&queue, 6); - App_RingQueue_Push(&queue, 7); - App_RingQueue_Push(&queue, 8); - App_RingQueue_Push(&queue, 9); - App_RingQueue_Push(&queue, 10); - - // Pop 3 most recent elements (rest were overwritten) - ASSERT_TRUE(App_RingQueue_Pop(&queue, &popped)); - ASSERT_EQ(popped, 8); - - ASSERT_TRUE(App_RingQueue_Pop(&queue, &popped)); - ASSERT_EQ(popped, 9); - - ASSERT_TRUE(App_RingQueue_Pop(&queue, &popped)); - ASSERT_EQ(popped, 10); - - // Queue is empty, no data - ASSERT_FALSE(App_RingQueue_Pop(&queue, &popped)); -} diff --git a/firmware/thruna/DCM/CMakeLists.txt b/firmware/thruna/DCM/CMakeLists.txt index 92d58a7496..dca99b2cac 100644 --- a/firmware/thruna/DCM/CMakeLists.txt +++ b/firmware/thruna/DCM/CMakeLists.txt @@ -6,7 +6,6 @@ file(GLOB_RECURSE APP_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/src/app/*.c") list(APPEND APP_SRCS "${SHARED_APP_INCLUDE_DIR}/app_stateMachine.c" "${SHARED_APP_INCLUDE_DIR}/app_heartbeatMonitor.c" - "${SHARED_APP_INCLUDE_DIR}/App_RingQueue.c" "${SHARED_APP_INCLUDE_DIR}/app_timer.c" "${SHARED_APP_INCLUDE_DIR}/app_rangeCheck.c" ) diff --git a/firmware/thruna/DCM/src/app/app_sbgEllipse.c b/firmware/thruna/DCM/src/app/app_sbgEllipse.c index 93a959360c..aad312d41e 100644 --- a/firmware/thruna/DCM/src/app/app_sbgEllipse.c +++ b/firmware/thruna/DCM/src/app/app_sbgEllipse.c @@ -9,37 +9,75 @@ void app_sbgEllipse_broadcast() const uint16_t general_status = io_sbgEllipse_getGeneralStatus(); const uint32_t com_status = io_sbgEllipse_getComStatus(); + // Overflow msg + const uint32_t overflow_status = io_sbgEllipse_getOverflowCount(); + app_canTx_DCM_EllipseGeneralStatusBitmask_set(general_status); app_canTx_DCM_EllipseComStatusBitmask_set(com_status); + app_canTx_DCM_Warning_SbgRxOverflowCount_set(overflow_status); // Time msg const uint32_t timestamp_us = io_sbgEllipse_getTimestampUs(); app_canTx_DCM_EllipseTimestamp_set(timestamp_us); // Acceleration msg - const float forward_accel = io_sbgEllipse_getSensorOutput(SBG_ELLIPSE_OUT_ACCELERATION_X); - const float lateral_accel = io_sbgEllipse_getSensorOutput(SBG_ELLIPSE_OUT_ACCELERATION_Y); - const float vertical_accel = io_sbgEllipse_getSensorOutput(SBG_ELLIPSE_OUT_ACCELERATION_Z); + const float forward_accel = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ACCELERATION_X); + const float lateral_accel = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ACCELERATION_Y); + const float vertical_accel = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ACCELERATION_Z); app_canTx_DCM_AccelerationForward_set(forward_accel); app_canTx_DCM_AccelerationLateral_set(lateral_accel); app_canTx_DCM_AccelerationVertical_set(vertical_accel); // Angular velocity msg - const float ang_vel_roll = io_sbgEllipse_getSensorOutput(SBG_ELLIPSE_OUT_ANGULAR_VELOCITY_ROLL); - const float ang_vel_pitch = io_sbgEllipse_getSensorOutput(SBG_ELLIPSE_OUT_ANGULAR_VELOCITY_PITCH); - const float ang_vel_yaw = io_sbgEllipse_getSensorOutput(SBG_ELLIPSE_OUT_ANGULAR_VELOCITY_YAW); + const float ang_vel_roll = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ANGULAR_VELOCITY_ROLL); + const float ang_vel_pitch = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ANGULAR_VELOCITY_PITCH); + const float ang_vel_yaw = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_ANGULAR_VELOCITY_YAW); app_canTx_DCM_AngularVelocityRoll_set((int)ang_vel_roll); app_canTx_DCM_AngularVelocityPitch_set((int)ang_vel_pitch); app_canTx_DCM_AngularVelocityYaw_set((int)ang_vel_yaw); // Euler angles msg - const float euler_roll = io_sbgEllipse_getSensorOutput(SBG_ELLIPSE_OUT_EULER_ROLL); - const float euler_pitch = io_sbgEllipse_getSensorOutput(SBG_ELLIPSE_OUT_EULER_PITCH); - const float euler_yaw = io_sbgEllipse_getSensorOutput(SBG_ELLIPSE_OUT_EULER_YAW); + const float euler_roll = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_EULER_ROLL); + const float euler_pitch = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_EULER_PITCH); + const float euler_yaw = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_EULER_YAW); app_canTx_DCM_EulerAnglesRoll_set(euler_roll); app_canTx_DCM_EulerAnglesPitch_set(euler_pitch); app_canTx_DCM_EulerAnglesYaw_set(euler_yaw); + + // Gps position msg + const float position_status = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_POS_STATUS); + const float latitude = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_LAT); + const float latitude_acc = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_LAT_ACC); + const float longitude = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_LONG); + const float longitude_acc = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_LONG_ACC); + const float altitude = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_ALT); + const float altitude_acc = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_ALT_ACC); + + app_canTx_DCM_PositionStatus_set(position_status); + app_canTx_DCM_Latitude_set(latitude); + app_canTx_DCM_LatitudeAccuracy_set(latitude_acc); + app_canTx_DCM_Longtitude_set(longitude); + app_canTx_DCM_LongtitudeAccuracy_set(longitude_acc); + app_canTx_DCM_Altitude_set(altitude); + app_canTx_DCM_AltitudeAccuracy_set(altitude_acc); + + // Gps velocity msg + const float velocity_status = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_VEL_STATUS); + const float velocity_north = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_VEL_N); + const float velocity_north_acc = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_VEL_N_ACC); + const float velocity_east = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_VEL_E); + const float velocity_east_acc = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_VEL_E_ACC); + const float velocity_down = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_VEL_D); + const float velocity_down_acc = io_sbgEllipse_getSensorOutput(ELLIPSE_OUTPUT_GPS_VEL_D_ACC); + + app_canTx_DCM_VelocityStatus_set(velocity_status); + app_canTx_DCM_VelocityNorth_set(velocity_north); + app_canTx_DCM_VelocityNorthAccuracy_set(velocity_north_acc); + app_canTx_DCM_VelocityEast_set(velocity_east); + app_canTx_DCM_VelocityEastAccuracy_set(velocity_east_acc); + app_canTx_DCM_VelocityDown_set(velocity_down); + app_canTx_DCM_VelocityDownAccuracy_set(velocity_down_acc); } diff --git a/firmware/thruna/DCM/src/io/io_sbgEllipse.c b/firmware/thruna/DCM/src/io/io_sbgEllipse.c index 0a683ef168..19ca102f57 100644 --- a/firmware/thruna/DCM/src/io/io_sbgEllipse.c +++ b/firmware/thruna/DCM/src/io/io_sbgEllipse.c @@ -3,13 +3,16 @@ #include "sbgECom.h" #include "interfaces/sbgInterfaceSerial.h" #include "io_sbgEllipse.h" -#include "App_RingQueue.h" #include "app_units.h" #include "FreeRTOS.h" +#include "cmsis_os.h" +#include "queue.h" +#include "app_canTx.h" /* ------------------------------------ Defines ------------------------------------- */ #define UART_RX_PACKET_SIZE 128 // Size of each received UART packet, in bytes +#define QUEUE_MAX_SIZE 4095 // 4kB /* ------------------------------------ Typedefs ------------------------------------- */ @@ -27,6 +30,32 @@ typedef struct float yaw; } Attitude; +typedef struct +{ + SbgEComGpsPosStatus status; + double latitude; + double longitude; + double altitude; + float latitude_accuracy; + float longitude_accuracy; + float altitude_accuracy; +} GpsPositionData; +typedef struct +{ + SbgEComGpsVelStatus status; + float velocity_n; // North + float velocity_e; // East + float velocity_d; // Down + float velocity_accuracy_n; + float velocity_accuracy_e; + float velocity_accuracy_d; +} GpsVelocityData; +typedef struct +{ + GpsVelocityData gps1_velocity; + GpsPositionData gps1_position; +} Gps1Data; + typedef struct { Vector3 acceleration; @@ -50,32 +79,63 @@ typedef struct ImuPacketData imu_data; EulerPacketData euler_data; StatusPacketData status_data; + Gps1Data gps_data; } SensorData; /* --------------------------------- Variables ---------------------------------- */ +extern UART_HandleTypeDef huart1; static UART *uart; static SbgInterface sbg_interface; // Handle for interface static SbgEComHandle com_handle; // Handle for comms static uint8_t uart_rx_buffer[UART_RX_PACKET_SIZE]; // Buffer to hold last RXed UART packet -static RingQueue rx_queue; // FIFO queue of RXed UART bytes static SensorData sensor_data; // Struct of all sensor data +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 uint32_t sbg_queue_overflow_count; + +static const osMessageQueueAttr_t sensor_rx_queue_attr = { + .name = "SensorRxQueue", + .attr_bits = 0, + .cb_mem = &rx_queue_control_block, + .cb_size = sizeof(StaticQueue_t), + .mq_mem = sensor_rx_queue_buf, + .mq_size = QUEUE_MAX_SIZE, +}; + // Map each sensor output enum to a ptr to the actual value float *sensor_output_map[NUM_SBG_OUTPUTS] = { - [SBG_ELLIPSE_OUT_ACCELERATION_X] = &sensor_data.imu_data.acceleration.x, - [SBG_ELLIPSE_OUT_ACCELERATION_Y] = &sensor_data.imu_data.acceleration.y, - [SBG_ELLIPSE_OUT_ACCELERATION_Z] = &sensor_data.imu_data.acceleration.z, + [ELLIPSE_OUTPUT_ACCELERATION_X] = &sensor_data.imu_data.acceleration.x, + [ELLIPSE_OUTPUT_ACCELERATION_Y] = &sensor_data.imu_data.acceleration.y, + [ELLIPSE_OUTPUT_ACCELERATION_Z] = &sensor_data.imu_data.acceleration.z, + + [ELLIPSE_OUTPUT_ANGULAR_VELOCITY_ROLL] = &sensor_data.imu_data.angular_velocity.roll, + [ELLIPSE_OUTPUT_ANGULAR_VELOCITY_PITCH] = &sensor_data.imu_data.angular_velocity.pitch, + [ELLIPSE_OUTPUT_ANGULAR_VELOCITY_YAW] = &sensor_data.imu_data.angular_velocity.yaw, + + [ELLIPSE_OUTPUT_EULER_ROLL] = &sensor_data.euler_data.euler_angles.roll, + [ELLIPSE_OUTPUT_EULER_PITCH] = &sensor_data.euler_data.euler_angles.pitch, + [ELLIPSE_OUTPUT_EULER_YAW] = &sensor_data.euler_data.euler_angles.yaw, + + [ELLIPSE_OUTPUT_GPS_POS_STATUS] = (float *)&sensor_data.gps_data.gps1_position.status, + [ELLIPSE_OUTPUT_GPS_LAT] = (float *)&sensor_data.gps_data.gps1_position.latitude, + [ELLIPSE_OUTPUT_GPS_LAT_ACC] = &sensor_data.gps_data.gps1_position.latitude_accuracy, + [ELLIPSE_OUTPUT_GPS_LONG] = (float *)&sensor_data.gps_data.gps1_position.longitude, + [ELLIPSE_OUTPUT_GPS_LONG_ACC] = &sensor_data.gps_data.gps1_position.longitude_accuracy, + [ELLIPSE_OUTPUT_GPS_ALT] = (float *)&sensor_data.gps_data.gps1_position.altitude, + [ELLIPSE_OUTPUT_GPS_ALT_ACC] = &sensor_data.gps_data.gps1_position.altitude_accuracy, + + [ELLIPSE_OUTPUT_GPS_VEL_STATUS] = (float *)&sensor_data.gps_data.gps1_velocity.status, + [ELLIPSE_OUTPUT_GPS_VEL_N] = &sensor_data.gps_data.gps1_velocity.velocity_n, + [ELLIPSE_OUTPUT_GPS_VEL_N_ACC] = &sensor_data.gps_data.gps1_velocity.velocity_accuracy_n, + [ELLIPSE_OUTPUT_GPS_VEL_E] = &sensor_data.gps_data.gps1_velocity.velocity_e, + [ELLIPSE_OUTPUT_GPS_VEL_E_ACC] = &sensor_data.gps_data.gps1_velocity.velocity_accuracy_e, + [ELLIPSE_OUTPUT_GPS_VEL_D] = &sensor_data.gps_data.gps1_velocity.velocity_d, + [ELLIPSE_OUTPUT_GPS_VEL_D_ACC] = &sensor_data.gps_data.gps1_velocity.velocity_accuracy_d - [SBG_ELLIPSE_OUT_ANGULAR_VELOCITY_ROLL] = &sensor_data.imu_data.angular_velocity.roll, - [SBG_ELLIPSE_OUT_ANGULAR_VELOCITY_PITCH] = &sensor_data.imu_data.angular_velocity.pitch, - [SBG_ELLIPSE_OUT_ANGULAR_VELOCITY_YAW] = &sensor_data.imu_data.angular_velocity.yaw, - - [SBG_ELLIPSE_OUT_EULER_ROLL] = &sensor_data.euler_data.euler_angles.roll, - [SBG_ELLIPSE_OUT_EULER_PITCH] = &sensor_data.euler_data.euler_angles.pitch, - [SBG_ELLIPSE_OUT_EULER_YAW] = &sensor_data.euler_data.euler_angles.yaw, }; - /* ------------------------- Static Function Prototypes -------------------------- */ static void io_sbgEllipse_createSerialInterface(SbgInterface *interface); @@ -89,9 +149,10 @@ static SbgErrorCode io_sbgEllipse_logReceivedCallback( static void io_sbgEllipse_processMsg_imu(const SbgBinaryLogData *log_data); static void io_sbgEllipse_processMsg_eulerAngles(const SbgBinaryLogData *log_data); static void io_sbgEllipse_processMsg_status(const SbgBinaryLogData *log_data); +static void io_sbgEllipse_processMsg_gpsVel(const SbgBinaryLogData *log_data); +static void io_sbgEllipse_processMsg_gpsPos(const SbgBinaryLogData *log_data); /* ------------------------- Static Function Definitions -------------------------- */ - /* * Callback called when a UART packet is received. */ @@ -102,12 +163,17 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) // SBG's library anything was actually being malloced. This is why I push to a queue here and handle the logs later // in the 100Hz task. - assert(huart == uart->handle); + assert(huart == &huart1); // Push newly received data to queue for (int i = 0; i < UART_RX_PACKET_SIZE; i++) { - App_RingQueue_Push(&rx_queue, uart_rx_buffer[i]); + sbg_queue_overflow_count = 0; + + if (osMessageQueuePut(sensor_rx_queue_id, &uart_rx_buffer[i], 0, 0) != osOK) + { + sbg_queue_overflow_count++; + } } } @@ -146,9 +212,13 @@ static SbgErrorCode io_sbgEllipse_read(SbgInterface *interface, void *buffer, si while (i < bytes_to_read) { uint8_t data; - bool data_available = App_RingQueue_Pop(&rx_queue, &data); - if (!data_available) + if (osMessageQueueGetCount(sensor_rx_queue_id) == 0) + { + break; + } + + if (osMessageQueueGet(sensor_rx_queue_id, &data, NULL, osWaitForever) != osOK) { break; } @@ -196,6 +266,16 @@ SbgErrorCode io_sbgEllipse_logReceivedCallback( io_sbgEllipse_processMsg_status(log_data); break; } + case SBG_ECOM_LOG_GPS1_VEL: + { + io_sbgEllipse_processMsg_gpsVel(log_data); + break; + } + case SBG_ECOM_LOG_GPS1_POS: + { + io_sbgEllipse_processMsg_gpsPos(log_data); + break; + } default: { // Do nothing @@ -203,10 +283,8 @@ SbgErrorCode io_sbgEllipse_logReceivedCallback( } } } - return SBG_NO_ERROR; } - /* * Process and save a new IMU data msg. */ @@ -244,6 +322,43 @@ static void io_sbgEllipse_processMsg_status(const SbgBinaryLogData *log_data) sensor_data.status_data.com_status = log_data->statusData.comStatus; } +/* + * Process and save a relevant GPS Velocity information. + */ +static void io_sbgEllipse_processMsg_gpsVel(const SbgBinaryLogData *log_data) +{ + sensor_data.gps_data.gps1_velocity.status = sbgEComLogGpsVelGetStatus(log_data->gpsVelData.status); + + // velocity data in m/s + sensor_data.gps_data.gps1_velocity.velocity_n = log_data->gpsVelData.velocity[0]; + sensor_data.gps_data.gps1_velocity.velocity_e = log_data->gpsVelData.velocity[1]; + sensor_data.gps_data.gps1_velocity.velocity_d = log_data->gpsVelData.velocity[2]; + + // velocity accuracy + sensor_data.gps_data.gps1_velocity.velocity_accuracy_n = log_data->gpsVelData.velocityAcc[0]; + sensor_data.gps_data.gps1_velocity.velocity_accuracy_e = log_data->gpsVelData.velocityAcc[1]; + sensor_data.gps_data.gps1_velocity.velocity_accuracy_d = log_data->gpsVelData.velocityAcc[2]; +} + +/* + * Process and save a relevant GPS Position information. + */ +static void io_sbgEllipse_processMsg_gpsPos(const SbgBinaryLogData *log_data) +{ + // 0 means solution computed, 1-3 means an error occured (see binry log for more detail) + sensor_data.gps_data.gps1_position.status = sbgEComLogGpsPosGetStatus(log_data->gpsPosData.status); + + // lat and long measured in degrees, alt is measured in m + sensor_data.gps_data.gps1_position.altitude = log_data->gpsPosData.altitude; + sensor_data.gps_data.gps1_position.latitude = log_data->gpsPosData.latitude; + sensor_data.gps_data.gps1_position.longitude = log_data->gpsPosData.longitude; + + // accuracy in m + sensor_data.gps_data.gps1_position.altitude_accuracy = log_data->gpsPosData.altitudeAccuracy; + sensor_data.gps_data.gps1_position.latitude_accuracy = log_data->gpsPosData.latitudeAccuracy; + sensor_data.gps_data.gps1_position.longitude_accuracy = log_data->gpsPosData.longitudeAccuracy; +} + /* ------------------------- Public Function Definitions -------------------------- */ bool io_sbgEllipse_init(UART *imu_uart) @@ -263,9 +378,10 @@ bool io_sbgEllipse_init(UART *imu_uart) // 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 - App_RingQueue_Init(&rx_queue, RING_QUEUE_MAX_SIZE); + sensor_rx_queue_id = osMessageQueueNew(QUEUE_MAX_SIZE, sizeof(uint8_t), &sensor_rx_queue_attr); + + assert(sensor_rx_queue_id != NULL); // Start waiting for UART packets hw_uart_receiveDma(uart, uart_rx_buffer, UART_RX_PACKET_SIZE); @@ -301,3 +417,8 @@ uint32_t io_sbgEllipse_getComStatus() { return sensor_data.status_data.com_status; } + +uint32_t io_sbgEllipse_getOverflowCount() +{ + return sbg_queue_overflow_count; +} diff --git a/firmware/thruna/DCM/src/io/io_sbgEllipse.h b/firmware/thruna/DCM/src/io/io_sbgEllipse.h index 196bc036ad..7bfe1ba87b 100644 --- a/firmware/thruna/DCM/src/io/io_sbgEllipse.h +++ b/firmware/thruna/DCM/src/io/io_sbgEllipse.h @@ -11,19 +11,37 @@ typedef enum { // Transational acceleration (m/s^2) - SBG_ELLIPSE_OUT_ACCELERATION_X, - SBG_ELLIPSE_OUT_ACCELERATION_Y, - SBG_ELLIPSE_OUT_ACCELERATION_Z, + ELLIPSE_OUTPUT_ACCELERATION_X, + ELLIPSE_OUTPUT_ACCELERATION_Y, + ELLIPSE_OUTPUT_ACCELERATION_Z, // Angular velocity (deg/s) - SBG_ELLIPSE_OUT_ANGULAR_VELOCITY_ROLL, - SBG_ELLIPSE_OUT_ANGULAR_VELOCITY_PITCH, - SBG_ELLIPSE_OUT_ANGULAR_VELOCITY_YAW, + ELLIPSE_OUTPUT_ANGULAR_VELOCITY_ROLL, + ELLIPSE_OUTPUT_ANGULAR_VELOCITY_PITCH, + ELLIPSE_OUTPUT_ANGULAR_VELOCITY_YAW, // Euler angles (deg) - SBG_ELLIPSE_OUT_EULER_ROLL, - SBG_ELLIPSE_OUT_EULER_PITCH, - SBG_ELLIPSE_OUT_EULER_YAW, + ELLIPSE_OUTPUT_EULER_ROLL, + ELLIPSE_OUTPUT_EULER_PITCH, + ELLIPSE_OUTPUT_EULER_YAW, + + // Position Info + ELLIPSE_OUTPUT_GPS_POS_STATUS, + ELLIPSE_OUTPUT_GPS_LAT, + ELLIPSE_OUTPUT_GPS_LAT_ACC, + ELLIPSE_OUTPUT_GPS_LONG, + ELLIPSE_OUTPUT_GPS_LONG_ACC, + ELLIPSE_OUTPUT_GPS_ALT, + ELLIPSE_OUTPUT_GPS_ALT_ACC, + + // Velocity Info + ELLIPSE_OUTPUT_GPS_VEL_STATUS, + ELLIPSE_OUTPUT_GPS_VEL_N, + ELLIPSE_OUTPUT_GPS_VEL_N_ACC, + ELLIPSE_OUTPUT_GPS_VEL_E, + ELLIPSE_OUTPUT_GPS_VEL_E_ACC, + ELLIPSE_OUTPUT_GPS_VEL_D, + ELLIPSE_OUTPUT_GPS_VEL_D_ACC, NUM_SBG_OUTPUTS, } SbgEllipseOutput; @@ -65,3 +83,9 @@ uint16_t io_sbgEllipse_getGeneralStatus(void); * @return Bitmask of faults, 0 indiciates a fault */ uint32_t io_sbgEllipse_getComStatus(void); + +/* + * Get queue overflow status + * @return the overflow uint32_t + */ +uint32_t io_sbgEllipse_getOverflowCount(void);