From 247fe8eefc5feae60f666cba35ac521a475313c5 Mon Sep 17 00:00:00 2001 From: Benjamin Vedder Date: Mon, 5 Sep 2022 20:05:39 +0200 Subject: [PATCH] Added custom UAVCAN message --- conf_general.h | 2 +- hwconf/hw.h | 7 + hwconf/trampa/60_alva/hw_60v2_alva_core.h | 2 + libcanard/canard.mk | 3 +- libcanard/canard_driver.c | 134 ++- .../dsdl/definitions/vesc/20601.RTData.uavcan | 50 + libcanard/dsdl/vesc/RTData.h | 175 ++++ libcanard/dsdl/vesc/vesc_RTData.c | 909 ++++++++++++++++++ 8 files changed, 1264 insertions(+), 18 deletions(-) create mode 100644 libcanard/dsdl/definitions/vesc/20601.RTData.uavcan create mode 100644 libcanard/dsdl/vesc/RTData.h create mode 100644 libcanard/dsdl/vesc/vesc_RTData.c diff --git a/conf_general.h b/conf_general.h index 4259afe3..a34a6598 100755 --- a/conf_general.h +++ b/conf_general.h @@ -24,7 +24,7 @@ #define FW_VERSION_MAJOR 6 #define FW_VERSION_MINOR 00 // Set to 0 for building a release and iterate during beta test builds -#define FW_TEST_VERSION_NUMBER 57 +#define FW_TEST_VERSION_NUMBER 58 #include "datatypes.h" diff --git a/hwconf/hw.h b/hwconf/hw.h index b4156dc6..e3669047 100644 --- a/hwconf/hw.h +++ b/hwconf/hw.h @@ -305,6 +305,13 @@ #define NTC_TEMP_MOS3_M2() 0.0 #endif +#ifndef TEMP_MOTOR_1 +#define TEMP_MOTOR_1() 0.0 +#endif +#ifndef TEMP_MOTOR_2 +#define TEMP_MOTOR_2() 0.0 +#endif + // Sin/Cos Encoder Signals. Override if available #ifndef ENCODER_SIN_VOLTS #if defined(ADC_IND_EXT) && defined(ADC_VOLTS) diff --git a/hwconf/trampa/60_alva/hw_60v2_alva_core.h b/hwconf/trampa/60_alva/hw_60v2_alva_core.h index e63cd304..d5b92947 100644 --- a/hwconf/trampa/60_alva/hw_60v2_alva_core.h +++ b/hwconf/trampa/60_alva/hw_60v2_alva_core.h @@ -154,6 +154,8 @@ #define NTC_RES_MOTOR(adc_val) (10000.0 / ((4095.0 / (float)adc_val) - 1.0)) // Motor temp sensor on low side #define NTC_TEMP_MOTOR(beta) alva_temp_motor_max(beta) +#define TEMP_MOTOR_1() (1.0 / ((logf(NTC_RES_MOTOR(ADC_Value[ADC_IND_TEMP_MOTOR]) / 10000.0) / beta) + (1.0 / 298.15)) - 273.15) +#define TEMP_MOTOR_2() (1.0 / ((logf(NTC_RES_MOTOR(ADC_Value[ADC_IND_TEMP_MOTOR_S2]) / 10000.0) / beta) + (1.0 / 298.15)) - 273.15) // Voltage on ADC channel #define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG) diff --git a/libcanard/canard.mk b/libcanard/canard.mk index cbb33e46..ecf55d1c 100644 --- a/libcanard/canard.mk +++ b/libcanard/canard.mk @@ -13,7 +13,8 @@ CANARDSRC = libcanard/canard.c \ libcanard/dsdl/uavcan/protocol/param/param_NumericValue.c \ libcanard/dsdl/uavcan/protocol/param/param_Value.c \ libcanard/dsdl/uavcan/protocol/file/file_BeginFirmwareUpdate.c \ - libcanard/dsdl/uavcan/protocol/file/file_Read.c + libcanard/dsdl/uavcan/protocol/file/file_Read.c \ + libcanard/dsdl/vesc/vesc_RTData.c CANARDINC = libcanard \ libcanard/dsdl diff --git a/libcanard/canard_driver.c b/libcanard/canard_driver.c index 32226d07..d8b0b015 100755 --- a/libcanard/canard_driver.c +++ b/libcanard/canard_driver.c @@ -29,8 +29,9 @@ #include "uavcan/protocol/param/GetSet.h" #include "uavcan/protocol/GetNodeInfo.h" #include "uavcan/protocol/RestartNode.h" -#include -#include +#include "uavcan/protocol/file/BeginFirmwareUpdate.h" +#include "uavcan/protocol/file/Read.h" +#include "vesc/RTData.h" #include "conf_general.h" #include "app.h" @@ -46,12 +47,14 @@ #include "nrf_driver.h" #include "buffer.h" #include "utils.h" +#include "mcpwm_foc.h" +#include "imu.h" // Constants #define CAN_APP_NODE_NAME "org.vesc." HW_NAME #define UNIQUE_ID_LENGTH_BYTES 12 #define STATUS_MSGS_TO_STORE 10 -#define AP_MAX_NAME_SIZE 16 +#define AP_MAX_NAME_SIZE 20 #define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_UNIQUE_ID_MAX_LENGTH 16 @@ -104,6 +107,7 @@ static THD_FUNCTION(canard_thread, arg); // Private functions static void calculateTotalCurrent(void); static void sendEscStatus(CanardInstance *ins); +static void sendRtData(CanardInstance *ins); static void readUniqueID(uint8_t* out_uid); static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer); static bool shouldAcceptTransfer(const CanardInstance* ins, @@ -211,11 +215,14 @@ static void write_app_config(void); */ static param_t parameters[] = { - {"can_baud_rate", AP_PARAM_INT8, 0, 0, 8, CAN_BAUD_500K}, - {"can_status_rate", AP_PARAM_INT32, 0, 0, 1000, 50}, - {"can_esc_index", AP_PARAM_INT16, 0, 0, 255, 0}, - {"controller_id", AP_PARAM_INT16, 0, 0, 253, 0}, - {"ctl_dir", AP_PARAM_INT8, 0, 0, 1, 0} + {"can_baud_rate", AP_PARAM_INT8, 0, 0, 8, CAN_BAUD_500K}, + {"can_status_rate_1", AP_PARAM_INT32, 0, 0, 1000, 50}, + {"can_status_rate_2", AP_PARAM_INT32, 0, 0, 1000, 5}, + {"can_status_msgs_r1", AP_PARAM_INT16, 0, 0, 255, 0}, + {"can_status_msgs_r2", AP_PARAM_INT16, 0, 0, 255, 0}, + {"can_esc_index", AP_PARAM_INT16, 0, 0, 255, 0}, + {"controller_id", AP_PARAM_INT16, 0, 0, 253, 0}, + {"ctl_dir", AP_PARAM_INT8, 0, 0, 1, 0} }; /* @@ -252,7 +259,10 @@ static void write_app_config(void) { *mcconf = *mc_interface_get_configuration(); appconf->can_baud_rate = (uint8_t)getParamByName("can_baud_rate")->val; - appconf->can_status_rate_1 = (uint32_t)getParamByName("can_status_rate")->val; + appconf->can_status_rate_1 = (uint32_t)getParamByName("can_status_rate_1")->val; + appconf->can_status_rate_2 = (uint32_t)getParamByName("can_status_rate_2")->val; + appconf->can_status_msgs_r1 = (uint16_t)getParamByName("can_status_msgs_r1")->val; + appconf->can_status_msgs_r2 = (uint16_t)getParamByName("can_status_msgs_r2")->val; appconf->uavcan_esc_index = (uint16_t)getParamByName("can_esc_index")->val; appconf->controller_id = (uint16_t)getParamByName("controller_id")->val; mcconf->m_invert_direction = (uint8_t)getParamByName("ctl_dir")->val;; @@ -279,11 +289,14 @@ static void refresh_parameters(void){ const app_configuration *appconf = app_get_configuration(); const volatile mc_configuration *mcconf = mc_interface_get_configuration(); - updateParamByName((uint8_t *)"can_baud_rate", appconf->can_baud_rate ); - updateParamByName((uint8_t *)"can_status_rate", appconf->can_status_rate_1 ); - updateParamByName((uint8_t *)"can_esc_index", appconf->uavcan_esc_index ); - updateParamByName((uint8_t *)"controller_id", appconf->controller_id ); - updateParamByName((uint8_t *)"ctl_dir", mcconf->m_invert_direction ); + updateParamByName((uint8_t *)"can_baud_rate", appconf->can_baud_rate); + updateParamByName((uint8_t *)"can_status_rate_1", appconf->can_status_rate_1); + updateParamByName((uint8_t *)"can_status_rate_2", appconf->can_status_rate_2); + updateParamByName((uint8_t *)"can_status_msgs_r1", appconf->can_status_msgs_r1); + updateParamByName((uint8_t *)"can_status_msgs_r2", appconf->can_status_msgs_r2); + updateParamByName((uint8_t *)"can_esc_index", appconf->uavcan_esc_index); + updateParamByName((uint8_t *)"controller_id", appconf->controller_id); + updateParamByName((uint8_t *)"ctl_dir", mcconf->m_invert_direction); } /* @@ -497,6 +510,76 @@ static void sendEscStatus(CanardInstance *ins) { UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE); } +static void sendRtData(CanardInstance *ins) { + vesc_RTData data; + memset(&data, 0, sizeof(data)); + + const volatile mc_configuration *conf = mc_interface_get_configuration(); + const app_configuration *appconf = app_get_configuration(); + + data.volt_in = mc_interface_get_input_voltage_filtered(); + data.volt_d = mcpwm_foc_get_vd(); + data.volt_q = mcpwm_foc_get_vq(); + + data.temp_mos_max = mc_interface_temp_fet_filtered(); + data.temp_mos_1 = NTC_TEMP_MOS1(); + data.temp_mos_2 = NTC_TEMP_MOS2(); + data.temp_mos_3 = NTC_TEMP_MOS3(); + data.temp_motor_max = mc_interface_temp_motor_filtered(); + data.temp_motor_1 = TEMP_MOTOR_1(); + data.temp_motor_2 = TEMP_MOTOR_2(); + + data.curr_motor = mc_interface_get_tot_current_filtered(); + data.curr_in = mc_interface_get_tot_current_in_filtered(); + data.curr_d = mcpwm_foc_get_id(); + data.curr_q = mcpwm_foc_get_iq(); + + float rpy[3], acc[3], gyro[3]; + imu_get_rpy(rpy); + imu_get_accel(acc); + imu_get_gyro(gyro); + + data.roll = rpy[0]; + data.pitch = rpy[1]; + data.yaw = rpy[2]; + data.acc_x = acc[0]; + data.acc_y = acc[1]; + data.acc_z = acc[2]; + data.gyro_x = gyro[0]; + data.gyro_y = gyro[1]; + data.gyro_z = gyro[2]; + + data.erpm = mc_interface_get_rpm(); + data.rpm = mc_interface_get_rpm() / ((float)conf->si_motor_poles / 2.0); + data.duty = mc_interface_get_duty_cycle_now(); + data.ah_used = mc_interface_get_amp_hours(false); + data.ah_charged = mc_interface_get_amp_hours_charged(false); + data.wh_used = mc_interface_get_watt_hours(false); + data.wh_charged = mc_interface_get_watt_hours_charged(false); + data.encoder_pos = mc_interface_get_pid_pos_now(); + float wh_left = 0.0; + data.battery_level = mc_interface_get_battery_level(&wh_left); + data.battery_wh_tot = wh_left / data.battery_level; + data.fault_code = mc_interface_get_fault(); + data.vesc_id = appconf->controller_id; + + vesc_RTData_encode(&data, msg_buffer, false); + + static uint8_t transfer_id; + + if (debug_level > 11) { + commands_printf("UAVCAN sendRtData"); + } + + canardBroadcast(ins, + VESC_RTDATA_SIGNATURE, + VESC_RTDATA_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + msg_buffer, + VESC_RTDATA_MAX_SIZE); +} + /* * Reads the STM32 Unique ID */ @@ -1228,6 +1311,7 @@ static THD_FUNCTION(canard_thread, arg) { systime_t last_status_time = 0; systime_t last_esc_status_time = 0; + systime_t last_esc_status_time_r2 = 0; systime_t last_tot_current_calc_time = 0; systime_t last_param_refresh = 0; bool was_running = false; @@ -1258,6 +1342,7 @@ static THD_FUNCTION(canard_thread, arg) { last_status_time = chVTGetSystemTimeX(); last_esc_status_time = chVTGetSystemTimeX(); + last_esc_status_time_r2 = chVTGetSystemTimeX(); last_tot_current_calc_time = chVTGetSystemTimeX(); last_param_refresh = chVTGetSystemTimeX(); @@ -1323,13 +1408,30 @@ static THD_FUNCTION(canard_thread, arg) { #endif } - if (conf->can_status_rate_1 > 0 && - UTILS_AGE_S(last_esc_status_time) >= (1.0 / (float)conf->can_status_rate_1)) { + if (conf->can_status_rate_1 > 0 && UTILS_AGE_S(last_esc_status_time) >= (1.0 / (float)conf->can_status_rate_1)) { last_esc_status_time = chVTGetSystemTimeX(); sendEscStatus(&canard_ins); #ifdef HW_CAN2_DEV sendEscStatus(&canard_ins_if2); #endif + + if ((conf->can_status_msgs_r1 >> 0) & 1) { + sendRtData(&canard_ins); +#ifdef HW_CAN2_DEV + sendRtData(&canard_ins_if2); +#endif + } + } + + if (conf->can_status_rate_2 > 0 && UTILS_AGE_S(last_esc_status_time_r2) >= (1.0 / (float)conf->can_status_rate_2)) { + last_esc_status_time_r2 = chVTGetSystemTimeX(); + + if ((conf->can_status_msgs_r2 >> 0) & 1) { + sendRtData(&canard_ins); +#ifdef HW_CAN2_DEV + sendRtData(&canard_ins_if2); +#endif + } } if (ST2MS(chVTTimeElapsedSinceX(last_tot_current_calc_time)) >= 1000 / CURRENT_CALC_FREQ_HZ) { diff --git a/libcanard/dsdl/definitions/vesc/20601.RTData.uavcan b/libcanard/dsdl/definitions/vesc/20601.RTData.uavcan new file mode 100644 index 00000000..6fd02870 --- /dev/null +++ b/libcanard/dsdl/definitions/vesc/20601.RTData.uavcan @@ -0,0 +1,50 @@ +# +# VESC Realtime Data +# + +# Voltages +float16 volt_in +float16 volt_d +float16 volt_q + +# Temperatures +float16 temp_mos_max +float16 temp_mos_1 +float16 temp_mos_2 +float16 temp_mos_3 +float16 temp_motor_max +float16 temp_motor_1 +float16 temp_motor_2 + +# Currents +float16 curr_motor +float16 curr_in +float16 curr_d +float16 curr_q + +# IMU +float16 roll +float16 pitch +float16 yaw +float16 acc_x +float16 acc_y +float16 acc_z +float16 gyro_x +float16 gyro_y +float16 gyro_z + +# Other +float16 erpm +float16 rpm +float16 duty +float16 ah_used +float16 ah_charged +float16 wh_used +float16 wh_charged +float16 encoder_pos +float16 battery_level +float16 battery_wh_tot + +uint8 fault_code +uint8 vesc_id + diff --git a/libcanard/dsdl/vesc/RTData.h b/libcanard/dsdl/vesc/RTData.h new file mode 100644 index 00000000..b49184b2 --- /dev/null +++ b/libcanard/dsdl/vesc/RTData.h @@ -0,0 +1,175 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /home/benjamin/Nextcloud/Dokument/ARM/STM_Eclipse/BLDC_4_ChibiOS/libcanard/dsdl/definitions/vesc/20601.RTData.uavcan + */ + +#ifndef __VESC_RTDATA +#define __VESC_RTDATA + +#include +#include "canard.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/******************************* Source text ********************************** +# +# VESC Realtime Data +# + +# Voltages +float16 volt_in +float16 volt_d +float16 volt_q + +# Temperatures +float16 temp_mos_max +float16 temp_mos_1 +float16 temp_mos_2 +float16 temp_mos_3 +float16 temp_motor_max +float16 temp_motor_1 +float16 temp_motor_2 + +# Currents +float16 curr_motor +float16 curr_in +float16 curr_d +float16 curr_q + +# IMU +float16 roll +float16 pitch +float16 yaw +float16 acc_x +float16 acc_y +float16 acc_z +float16 gyro_x +float16 gyro_y +float16 gyro_z + +# Other +float16 erpm +float16 rpm +float16 duty +float16 ah_used +float16 ah_charged +float16 wh_used +float16 wh_charged +float16 encoder_pos +float16 battery_level +float16 battery_wh_tot + +uint8 fault_code +uint8 vesc_id +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +vesc.RTData +saturated float16 volt_in +saturated float16 volt_d +saturated float16 volt_q +saturated float16 temp_mos_max +saturated float16 temp_mos_1 +saturated float16 temp_mos_2 +saturated float16 temp_mos_3 +saturated float16 temp_motor_max +saturated float16 temp_motor_1 +saturated float16 temp_motor_2 +saturated float16 curr_motor +saturated float16 curr_in +saturated float16 curr_d +saturated float16 curr_q +saturated float16 roll +saturated float16 pitch +saturated float16 yaw +saturated float16 acc_x +saturated float16 acc_y +saturated float16 acc_z +saturated float16 gyro_x +saturated float16 gyro_y +saturated float16 gyro_z +saturated float16 erpm +saturated float16 rpm +saturated float16 duty +saturated float16 ah_used +saturated float16 ah_charged +saturated float16 wh_used +saturated float16 wh_charged +saturated float16 encoder_pos +saturated float16 battery_level +saturated float16 battery_wh_tot +saturated uint8 fault_code +saturated uint8 vesc_id +******************************************************************************/ + +#define VESC_RTDATA_ID 20601 +#define VESC_RTDATA_NAME "vesc.RTData" +#define VESC_RTDATA_SIGNATURE (0xBC1F380DD6555010ULL) + +#define VESC_RTDATA_MAX_SIZE ((544 + 7)/8) + +// Constants + +typedef struct +{ + // FieldTypes + float volt_in; // float16 Saturate + float volt_d; // float16 Saturate + float volt_q; // float16 Saturate + float temp_mos_max; // float16 Saturate + float temp_mos_1; // float16 Saturate + float temp_mos_2; // float16 Saturate + float temp_mos_3; // float16 Saturate + float temp_motor_max; // float16 Saturate + float temp_motor_1; // float16 Saturate + float temp_motor_2; // float16 Saturate + float curr_motor; // float16 Saturate + float curr_in; // float16 Saturate + float curr_d; // float16 Saturate + float curr_q; // float16 Saturate + float roll; // float16 Saturate + float pitch; // float16 Saturate + float yaw; // float16 Saturate + float acc_x; // float16 Saturate + float acc_y; // float16 Saturate + float acc_z; // float16 Saturate + float gyro_x; // float16 Saturate + float gyro_y; // float16 Saturate + float gyro_z; // float16 Saturate + float erpm; // float16 Saturate + float rpm; // float16 Saturate + float duty; // float16 Saturate + float ah_used; // float16 Saturate + float ah_charged; // float16 Saturate + float wh_used; // float16 Saturate + float wh_charged; // float16 Saturate + float encoder_pos; // float16 Saturate + float battery_level; // float16 Saturate + float battery_wh_tot; // float16 Saturate + uint8_t fault_code; // bit len 8 + uint8_t vesc_id; // bit len 8 + +} vesc_RTData; + +extern +uint32_t vesc_RTData_encode(vesc_RTData* source, void* msg_buf, bool tao_enabled); + +extern +int32_t vesc_RTData_decode(const CanardRxTransfer* transfer, uint16_t payload_len, vesc_RTData* dest, uint8_t** dyn_arr_buf, bool tao_enabled); + +extern +uint32_t vesc_RTData_encode_internal(vesc_RTData* source, void* msg_buf, uint32_t offset, uint8_t root_item, bool tao_enabled); + +extern +int32_t vesc_RTData_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, vesc_RTData* dest, uint8_t** dyn_arr_buf, int32_t offset, bool tao_enabled); + +#ifdef __cplusplus +} // extern "C" +#endif +#endif // __VESC_RTDATA \ No newline at end of file diff --git a/libcanard/dsdl/vesc/vesc_RTData.c b/libcanard/dsdl/vesc/vesc_RTData.c new file mode 100644 index 00000000..bb83543c --- /dev/null +++ b/libcanard/dsdl/vesc/vesc_RTData.c @@ -0,0 +1,909 @@ +/* + * UAVCAN data structure definition for libcanard. + * + * Autogenerated, do not edit. + * + * Source file: /home/benjamin/Nextcloud/Dokument/ARM/STM_Eclipse/BLDC_4_ChibiOS/libcanard/dsdl/definitions/vesc/20601.RTData.uavcan + */ +#include "vesc/RTData.h" +#include "canard.h" + +#ifndef CANARD_INTERNAL_SATURATE +#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) ); +#endif + +#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED +#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) >= max) ? max : (x) ); +#endif + +#if defined(__GNUC__) +# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused)) +#else +# define CANARD_MAYBE_UNUSED(x) x +#endif + +/** + * @brief vesc_RTData_encode_internal + * @param source : pointer to source data struct + * @param msg_buf: pointer to msg storage + * @param offset: bit offset to msg storage + * @param root_item: for detecting if TAO should be used + * @retval returns offset + */ +uint32_t vesc_RTData_encode_internal(vesc_RTData* source, + void* msg_buf, + uint32_t offset, + uint8_t CANARD_MAYBE_UNUSED(root_item), + bool tao_enabled) +{ + (void)tao_enabled; +#ifndef CANARD_USE_FLOAT16_CAST + uint16_t tmp_float = 0; +#else + CANARD_USE_FLOAT16_CAST tmp_float = 0; +#endif + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->volt_in); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->volt_in; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->volt_d); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->volt_d; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->volt_q); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->volt_q; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->temp_mos_max); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->temp_mos_max; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->temp_mos_1); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->temp_mos_1; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->temp_mos_2); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->temp_mos_2; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->temp_mos_3); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->temp_mos_3; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->temp_motor_max); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->temp_motor_max; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->temp_motor_1); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->temp_motor_1; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->temp_motor_2); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->temp_motor_2; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->curr_motor); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->curr_motor; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->curr_in); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->curr_in; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->curr_d); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->curr_d; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->curr_q); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->curr_q; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->roll); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->roll; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->pitch); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->pitch; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->yaw); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->yaw; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->acc_x); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->acc_x; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->acc_y); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->acc_y; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->acc_z); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->acc_z; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->gyro_x); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->gyro_x; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->gyro_y); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->gyro_y; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->gyro_z); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->gyro_z; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->erpm); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->erpm; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->rpm); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->rpm; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->duty); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->duty; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->ah_used); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->ah_used; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->ah_charged); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->ah_charged; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->wh_used); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->wh_used; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->wh_charged); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->wh_charged; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->encoder_pos); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->encoder_pos; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->battery_level); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->battery_level; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + + // float16 special handling +#ifndef CANARD_USE_FLOAT16_CAST + tmp_float = canardConvertNativeFloatToFloat16(source->battery_wh_tot); +#else + tmp_float = (CANARD_USE_FLOAT16_CAST)source->battery_wh_tot; +#endif + canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767 + offset += 16; + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->fault_code); // 255 + offset += 8; + + canardEncodeScalar(msg_buf, offset, 8, (void*)&source->vesc_id); // 255 + offset += 8; + + return offset; +} + +/** + * @brief vesc_RTData_encode + * @param source : Pointer to source data struct + * @param msg_buf: Pointer to msg storage + * @retval returns message length as bytes + */ +uint32_t vesc_RTData_encode(vesc_RTData* source, void* msg_buf, bool tao_enabled) +{ + uint32_t offset = 0; + + offset = vesc_RTData_encode_internal(source, msg_buf, offset, 1, tao_enabled); + + return (offset + 7 ) / 8; +} + +/** + * @brief vesc_RTData_decode_internal + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * vesc_RTData dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @param offset: Call with 0, bit offset to msg storage + * @retval offset or ERROR value if < 0 + */ +int32_t vesc_RTData_decode_internal( + const CanardRxTransfer* transfer, + uint16_t CANARD_MAYBE_UNUSED(payload_len), + vesc_RTData* dest, + uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf), + int32_t offset, + bool tao_enabled) +{ + (void)tao_enabled; + int32_t ret = 0; +#ifndef CANARD_USE_FLOAT16_CAST + uint16_t tmp_float = 0; +#else + CANARD_USE_FLOAT16_CAST tmp_float = 0; +#endif + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->volt_in = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->volt_in = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->volt_d = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->volt_d = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->volt_q = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->volt_q = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->temp_mos_max = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->temp_mos_max = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->temp_mos_1 = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->temp_mos_1 = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->temp_mos_2 = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->temp_mos_2 = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->temp_mos_3 = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->temp_mos_3 = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->temp_motor_max = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->temp_motor_max = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->temp_motor_1 = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->temp_motor_1 = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->temp_motor_2 = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->temp_motor_2 = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->curr_motor = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->curr_motor = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->curr_in = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->curr_in = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->curr_d = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->curr_d = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->curr_q = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->curr_q = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->roll = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->roll = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->pitch = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->pitch = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->yaw = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->yaw = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->acc_x = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->acc_x = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->acc_y = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->acc_y = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->acc_z = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->acc_z = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->gyro_x = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->gyro_x = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->gyro_y = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->gyro_y = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->gyro_z = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->gyro_z = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->erpm = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->erpm = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->rpm = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->rpm = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->duty = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->duty = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->ah_used = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->ah_used = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->ah_charged = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->ah_charged = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->wh_used = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->wh_used = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->wh_charged = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->wh_charged = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->encoder_pos = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->encoder_pos = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->battery_level = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->battery_level = (float)tmp_float; +#endif + offset += 16; + + // float16 special handling + ret = canardDecodeScalar(transfer, (uint32_t)offset, 16, false, (void*)&tmp_float); + + if (ret != 16) + { + goto vesc_RTData_error_exit; + } +#ifndef CANARD_USE_FLOAT16_CAST + dest->battery_wh_tot = canardConvertFloat16ToNativeFloat(tmp_float); +#else + dest->battery_wh_tot = (float)tmp_float; +#endif + offset += 16; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->fault_code); + if (ret != 8) + { + goto vesc_RTData_error_exit; + } + offset += 8; + + ret = canardDecodeScalar(transfer, (uint32_t)offset, 8, false, (void*)&dest->vesc_id); + if (ret != 8) + { + goto vesc_RTData_error_exit; + } + offset += 8; + return offset; + +vesc_RTData_error_exit: + if (ret < 0) + { + return ret; + } + else + { + return -CANARD_ERROR_INTERNAL; + } +} + +/** + * @brief vesc_RTData_decode + * @param transfer: Pointer to CanardRxTransfer transfer + * @param payload_len: Payload message length + * @param dest: Pointer to destination struct + * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays + * vesc_RTData dyn memory will point to dyn_arr_buf memory. + * NULL will ignore dynamic arrays decoding. + * @retval offset or ERROR value if < 0 + */ +int32_t vesc_RTData_decode(const CanardRxTransfer* transfer, + uint16_t payload_len, + vesc_RTData* dest, + uint8_t** dyn_arr_buf, + bool tao_enabled) +{ + const int32_t offset = 0; + int32_t ret = 0; + + // Clear the destination struct + for (uint32_t c = 0; c < sizeof(vesc_RTData); c++) + { + ((uint8_t*)dest)[c] = 0x00; + } + + ret = vesc_RTData_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset, tao_enabled); + + return ret; +}