From ca45a11b70881c11d94ceb951d2508eb274bb5d0 Mon Sep 17 00:00:00 2001 From: Kevin Shen Date: Fri, 18 Sep 2026 19:42:43 -0400 Subject: [PATCH] changed imu data to classical CAN at 250hz --- src/embedded/sensor_board/src/app/can_task.c | 102 ++++++++++++++----- src/embedded/sensor_board/src/stm32/fdcan.c | 11 +- src/interfacing/dbc/humanoid.dbc | 18 ++-- 3 files changed, 87 insertions(+), 44 deletions(-) diff --git a/src/embedded/sensor_board/src/app/can_task.c b/src/embedded/sensor_board/src/app/can_task.c index d1880d35..d0e80a6f 100644 --- a/src/embedded/sensor_board/src/app/can_task.c +++ b/src/embedded/sensor_board/src/app/can_task.c @@ -9,8 +9,13 @@ #include #include -#define IMU_CAN_PAYLOAD_SIZE 20U -#define IMU_CAN_EXTENDED_ID 0x2B00U +#define IMU_QUATERNION_PAYLOAD_SIZE 8U +#define IMU_ANGULAR_VELOCITY_PAYLOAD_SIZE 6U +#define IMU_GRAVITY_PAYLOAD_SIZE 6U + +#define IMU_QUATERNION_CAN_ID 0x2B00U +#define IMU_ANGULAR_VELOCITY_CAN_ID 0x2D00U +#define IMU_GRAVITY_CAN_ID 0x2E00U #define IMU_STALE_TIMEOUT_MS 20U #define IMU_QUAT_SCALE 16384.0f @@ -91,41 +96,87 @@ static void pack_int16_big_endian(uint8_t *destination, int16_t value) destination[1] = (uint8_t)(raw & 0xFFU); } -static void pack_imu_data(uint8_t payload[IMU_CAN_PAYLOAD_SIZE], const ImuCanData *imu) +static void pack_quaternion( + uint8_t payload[IMU_QUATERNION_PAYLOAD_SIZE], + const ImuCanData *imu) { pack_int16_big_endian(&payload[0], imu->qx); pack_int16_big_endian(&payload[2], imu->qy); pack_int16_big_endian(&payload[4], imu->qz); pack_int16_big_endian(&payload[6], imu->qw); +} - pack_int16_big_endian(&payload[8], imu->angular_velocity_x); - pack_int16_big_endian(&payload[10], imu->angular_velocity_y); - pack_int16_big_endian(&payload[12], imu->angular_velocity_z); - - pack_int16_big_endian(&payload[14], imu->gravity_x); - pack_int16_big_endian(&payload[16], imu->gravity_y); - pack_int16_big_endian(&payload[18], imu->gravity_z); +static void pack_angular_velocity( + uint8_t payload[IMU_ANGULAR_VELOCITY_PAYLOAD_SIZE], + const ImuCanData *imu) +{ + pack_int16_big_endian(&payload[0], imu->angular_velocity_x); + pack_int16_big_endian(&payload[2], imu->angular_velocity_y); + pack_int16_big_endian(&payload[4], imu->angular_velocity_z); } -static bool send_imu_data(const ImuCanData *imu) +static void pack_gravity( + uint8_t payload[IMU_GRAVITY_PAYLOAD_SIZE], + const ImuCanData *imu) { - uint8_t payload[IMU_CAN_PAYLOAD_SIZE]; + pack_int16_big_endian(&payload[0], imu->gravity_x); + pack_int16_big_endian(&payload[2], imu->gravity_y); + pack_int16_big_endian(&payload[4], imu->gravity_z); +} +static bool send_classic_frame( + uint32_t identifier, + uint32_t dataLength, + uint8_t *payload) +{ FDCAN_TxHeaderTypeDef header = { - .Identifier = IMU_CAN_EXTENDED_ID, + .Identifier = identifier, .IdType = FDCAN_EXTENDED_ID, .TxFrameType = FDCAN_DATA_FRAME, - .DataLength = FDCAN_DLC_BYTES_20, + .DataLength = dataLength, .ErrorStateIndicator = FDCAN_ESI_ACTIVE, - .BitRateSwitch = FDCAN_BRS_ON, - .FDFormat = FDCAN_FD_CAN, + .BitRateSwitch = FDCAN_BRS_OFF, + .FDFormat = FDCAN_CLASSIC_CAN, .TxEventFifoControl = FDCAN_NO_TX_EVENTS, .MessageMarker = 0 }; - pack_imu_data(payload, imu); + return HAL_FDCAN_AddMessageToTxFifoQ( + &hfdcan1, + &header, + payload + ) == HAL_OK; +} - return HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &header, payload) == HAL_OK; +static bool send_imu_data(const ImuCanData *imu) +{ + uint8_t quaternionPayload[IMU_QUATERNION_PAYLOAD_SIZE] = {0}; + uint8_t angularVelocityPayload[IMU_ANGULAR_VELOCITY_PAYLOAD_SIZE] = {0}; + uint8_t gravityPayload[IMU_GRAVITY_PAYLOAD_SIZE] = {0}; + + pack_quaternion(quaternionPayload, imu); + pack_angular_velocity(angularVelocityPayload, imu); + pack_gravity(gravityPayload, imu); + + bool quaternionSent = send_classic_frame( + IMU_QUATERNION_CAN_ID, + FDCAN_DLC_BYTES_8, + quaternionPayload + ); + + bool angularVelocitySent = send_classic_frame( + IMU_ANGULAR_VELOCITY_CAN_ID, + FDCAN_DLC_BYTES_6, + angularVelocityPayload + ); + + bool gravitySent = send_classic_frame( + IMU_GRAVITY_CAN_ID, + FDCAN_DLC_BYTES_6, + gravityPayload + ); + + return quaternionSent && angularVelocitySent && gravitySent; } void CanTask(void *pvParameters) @@ -133,20 +184,17 @@ void CanTask(void *pvParameters) (void)pvParameters; bno085_sample sample; + TickType_t nextWakeTime = xTaskGetTickCount(); for (;;) { - if (xQueueReceive(bno085SampleQueue, &sample, pdMS_TO_TICKS(IMU_STALE_TIMEOUT_MS)) != pdTRUE) - { - continue; - } - - if (!sample_is_fresh(&sample)) + if ((xQueuePeek(bno085SampleQueue, &sample, 0) == pdTRUE) && + sample_is_fresh(&sample)) { - continue; + ImuCanData canData = sample_to_can_data(&sample); + send_imu_data(&canData); } - ImuCanData canData = sample_to_can_data(&sample); - send_imu_data(&canData); + vTaskDelayUntil(&nextWakeTime, pdMS_TO_TICKS(4U)); } } \ No newline at end of file diff --git a/src/embedded/sensor_board/src/stm32/fdcan.c b/src/embedded/sensor_board/src/stm32/fdcan.c index ecf167a2..290c073d 100644 --- a/src/embedded/sensor_board/src/stm32/fdcan.c +++ b/src/embedded/sensor_board/src/stm32/fdcan.c @@ -39,7 +39,7 @@ void MX_FDCAN1_Init(void) /* USER CODE END FDCAN1_Init 1 */ hfdcan1.Instance = FDCAN1; hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1; - hfdcan1.Init.FrameFormat = FDCAN_FRAME_FD_BRS; + hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC; hfdcan1.Init.Mode = FDCAN_MODE_NORMAL; hfdcan1.Init.AutoRetransmission = DISABLE; hfdcan1.Init.TransmitPause = DISABLE; @@ -60,19 +60,10 @@ void MX_FDCAN1_Init(void) Error_Handler(); } /* USER CODE BEGIN FDCAN1_Init 2 */ - if (HAL_FDCAN_ConfigTxDelayCompensation(&hfdcan1, 16, 0) != HAL_OK) { - Error_Handler(); - } - - if (HAL_FDCAN_EnableTxDelayCompensation(&hfdcan1) != HAL_OK) { - Error_Handler(); - } - if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) { Error_Handler(); } /* USER CODE END FDCAN1_Init 2 */ - } void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef* fdcanHandle) diff --git a/src/interfacing/dbc/humanoid.dbc b/src/interfacing/dbc/humanoid.dbc index 313d2251..2630876e 100644 --- a/src/interfacing/dbc/humanoid.dbc +++ b/src/interfacing/dbc/humanoid.dbc @@ -77,17 +77,21 @@ BO_ 2147500032 FingerPosLoopCmd: 8 Fingers SG_ PIP_x : 39|16@0+ (0.00549325,0) [0|360] "Degrees" Vector__XXX SG_ DIP_x : 55|16@0+ (0.00549325,0) [0|360] "Degrees" Vector__XXX -BO_ 2147494656 ImuStatusFeedback: 20 IMU +BO_ 2147494656 ImuQuaternionFeedback: 8 IMU SG_ QuaternionX : 7|16@0- (0.00006103515625,0) [-1|1] "" Vector__XXX SG_ QuaternionY : 23|16@0- (0.00006103515625,0) [-1|1] "" Vector__XXX SG_ QuaternionZ : 39|16@0- (0.00006103515625,0) [-1|1] "" Vector__XXX SG_ QuaternionW : 55|16@0- (0.00006103515625,0) [-1|1] "" Vector__XXX - SG_ AngularVelocityX : 71|16@0- (0.001953125,0) [-64|63.998046875] "rad/s" Vector__XXX - SG_ AngularVelocityY : 87|16@0- (0.001953125,0) [-64|63.998046875] "rad/s" Vector__XXX - SG_ AngularVelocityZ : 103|16@0- (0.001953125,0) [-64|63.998046875] "rad/s" Vector__XXX - SG_ GravityX : 119|16@0- (0.00390625,0) [-128|127.99609375] "m/s^2" Vector__XXX - SG_ GravityY : 135|16@0- (0.00390625,0) [-128|127.99609375] "m/s^2" Vector__XXX - SG_ GravityZ : 151|16@0- (0.00390625,0) [-128|127.99609375] "m/s^2" Vector__XXX + +BO_ 2147495168 ImuAngularVelocityFeedback: 6 IMU + SG_ AngularVelocityX : 7|16@0- (0.001953125,0) [-64|63.998046875] "rad/s" Vector__XXX + SG_ AngularVelocityY : 23|16@0- (0.001953125,0) [-64|63.998046875] "rad/s" Vector__XXX + SG_ AngularVelocityZ : 39|16@0- (0.001953125,0) [-64|63.998046875] "rad/s" Vector__XXX + +BO_ 2147495424 ImuGravityFeedback: 6 IMU + SG_ GravityX : 7|16@0- (0.00390625,0) [-128|127.99609375] "m/s^2" Vector__XXX + SG_ GravityY : 23|16@0- (0.00390625,0) [-128|127.99609375] "m/s^2" Vector__XXX + SG_ GravityZ : 39|16@0- (0.00390625,0) [-128|127.99609375] "m/s^2" Vector__XXX CM_ BU_ Motor "CubeMars AK-series actuator"; CM_ BU_ Fingers "STM devices controlling fingers";