Skip to content

changed imu bridge to classical CAN at 250hz #289

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
k44shen wants to merge 1 commit into
base: user/smirkomilov/imu_can_integration
Choose a base branch
from
Draft

changed imu bridge to classical CAN at 250hz #289

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
102 changes: 75 additions & 27 deletions src/embedded/sensor_board/src/app/can_task.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,13 @@
#include <stdbool.h>
#include <stdint.h>

#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
Expand Down Expand Up @@ -91,62 +96,105 @@ 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)
{
(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));
}
}
11 changes: 1 addition & 10 deletions src/embedded/sensor_board/src/stm32/fdcan.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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)
Expand Down
18 changes: 11 additions & 7 deletions src/interfacing/dbc/humanoid.dbc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down
Loading