diff --git a/comm_can.c b/comm_can.c index 490fd0b8..e3c0aac1 100644 --- a/comm_can.c +++ b/comm_can.c @@ -45,10 +45,17 @@ #endif // Settings -#define RX_FRAMES_SIZE 100 +#define RX_FRAMES_SIZE 50 #define RX_BUFFER_SIZE PACKET_MAX_PL_LEN #if CAN_ENABLE + +typedef struct { + CANRxFrame rx_frames[RX_FRAMES_SIZE]; + int frame_read; + int frame_write; +} rx_state; + // Threads static THD_WORKING_AREA(cancom_read_thread_wa, 256); static THD_WORKING_AREA(cancom_process_thread_wa, 2048); @@ -66,11 +73,13 @@ static THD_WORKING_AREA(cancom_status_internal_thread_wa, 512); static mutex_t can_mtx; static mutex_t can_rx_mtx; -static uint8_t rx_buffer[RX_BUFFER_SIZE]; -static unsigned int rx_buffer_last_id; -static CANRxFrame rx_frames[RX_FRAMES_SIZE]; -static int rx_frame_read; -static int rx_frame_write; +uint8_t rx_buffer[RX_BUFFER_SIZE]; +unsigned int rx_buffer_last_id; +static rx_state m_rx_state; +#ifdef HW_CAN2_DEV +static rx_state m_rx_state2; +#endif + static thread_t *process_tp = 0; static thread_t *ping_tp = 0; static volatile HW_TYPE ping_hw_last = HW_TYPE_VESC; @@ -131,8 +140,7 @@ void comm_can_init(void) { } #if CAN_ENABLE - rx_frame_read = 0; - rx_frame_write = 0; + memset(&m_rx_state, 0, sizeof(m_rx_state)); chMtxObjectInit(&can_mtx); chMtxObjectInit(&can_rx_mtx); @@ -147,6 +155,8 @@ void comm_can_init(void) { PAL_STM32_OSPEED_MID1); #ifdef HW_CAN2_DEV + memset(&m_rx_state2, 0, sizeof(m_rx_state2)); + palSetPadMode(HW_CAN2_RX_PORT, HW_CAN2_RX_PIN, PAL_MODE_ALTERNATE(HW_CAN2_GPIO_AF) | PAL_STM32_OTYPE_PUSHPULL | @@ -218,8 +228,14 @@ void comm_can_set_baud(CAN_BAUD baud) { * @param replace * Process packets for motor2 directly instead of sending them. Unused * on single motor hardware. + * + * @param interface + * CAN-interface + * 0: Both + * 1: CAN1 + * 2: CAN2 */ -void comm_can_transmit_eid_replace(uint32_t id, const uint8_t *data, uint8_t len, bool replace) { +void comm_can_transmit_eid_replace(uint32_t id, const uint8_t *data, uint8_t len, bool replace, int interface) { if (len > 8) { len = 8; } @@ -252,15 +268,22 @@ void comm_can_transmit_eid_replace(uint32_t id, const uint8_t *data, uint8_t len chMtxLock(&can_mtx); #ifdef HW_CAN2_DEV - for (int i = 0;i < 10;i++) { - msg_t ok = canTransmit(&HW_CAN_DEV, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE); - msg_t ok2 = canTransmit(&HW_CAN2_DEV, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE); - if (ok == MSG_OK || ok2 == MSG_OK) { - break; + if (interface == 0) { + for (int i = 0;i < 10;i++) { + msg_t ok = canTransmit(&HW_CAN_DEV, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE); + msg_t ok2 = canTransmit(&HW_CAN2_DEV, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE); + if (ok == MSG_OK || ok2 == MSG_OK) { + break; + } + chThdSleepMicroseconds(500); } - chThdSleepMicroseconds(500); + } else if (interface == 1) { + canTransmit(&HW_CAN_DEV, CAN_ANY_MAILBOX, &txmsg, MS2ST(5)); + } else if (interface == 2) { + canTransmit(&HW_CAN2_DEV, CAN_ANY_MAILBOX, &txmsg, MS2ST(5)); } #else + (void)interface; canTransmit(&HW_CAN_DEV, CAN_ANY_MAILBOX, &txmsg, MS2ST(5)); #endif chMtxUnlock(&can_mtx); @@ -273,7 +296,11 @@ void comm_can_transmit_eid_replace(uint32_t id, const uint8_t *data, uint8_t len } void comm_can_transmit_eid(uint32_t id, const uint8_t *data, uint8_t len) { - comm_can_transmit_eid_replace(id, data, len, false); + comm_can_transmit_eid_replace(id, data, len, false, 0); +} + +void comm_can_transmit_eid_if(uint32_t id, const uint8_t *data, uint8_t len, int interface) { + comm_can_transmit_eid_replace(id, data, len, false, interface); } void comm_can_transmit_sid(uint32_t id, uint8_t *data, uint8_t len) { @@ -370,7 +397,7 @@ void comm_can_send_buffer(uint8_t controller_id, uint8_t *data, unsigned int len memcpy(send_buffer + ind, data, len); ind += len; comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_PROCESS_SHORT_BUFFER << 8), send_buffer, ind, true); + ((uint32_t)CAN_PACKET_PROCESS_SHORT_BUFFER << 8), send_buffer, ind, true, 0); } else { unsigned int end_a = 0; for (unsigned int i = 0;i < len;i += 7) { @@ -391,7 +418,7 @@ void comm_can_send_buffer(uint8_t controller_id, uint8_t *data, unsigned int len } comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_FILL_RX_BUFFER << 8), send_buffer, send_len + 1, true); + ((uint32_t)CAN_PACKET_FILL_RX_BUFFER << 8), send_buffer, send_len + 1, true, 0); } for (unsigned int i = end_a;i < len;i += 6) { @@ -407,7 +434,7 @@ void comm_can_send_buffer(uint8_t controller_id, uint8_t *data, unsigned int len } comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_FILL_RX_BUFFER_LONG << 8), send_buffer, send_len + 2, true); + ((uint32_t)CAN_PACKET_FILL_RX_BUFFER_LONG << 8), send_buffer, send_len + 2, true, 0); } uint32_t ind = 0; @@ -420,7 +447,7 @@ void comm_can_send_buffer(uint8_t controller_id, uint8_t *data, unsigned int len send_buffer[ind++] = (uint8_t)(crc & 0xFF); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_PROCESS_RX_BUFFER << 8), send_buffer, ind++, true); + ((uint32_t)CAN_PACKET_PROCESS_RX_BUFFER << 8), send_buffer, ind++, true, 0); } } @@ -429,7 +456,7 @@ void comm_can_set_duty(uint8_t controller_id, float duty) { uint8_t buffer[4]; buffer_append_int32(buffer, (int32_t)(duty * 100000.0), &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_DUTY << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_DUTY << 8), buffer, send_index, true, 0); } void comm_can_set_current(uint8_t controller_id, float current) { @@ -437,7 +464,7 @@ void comm_can_set_current(uint8_t controller_id, float current) { uint8_t buffer[4]; buffer_append_int32(buffer, (int32_t)(current * 1000.0), &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_CURRENT << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_CURRENT << 8), buffer, send_index, true, 0); } void comm_can_set_current_off_delay(uint8_t controller_id, float current, float off_delay) { @@ -446,7 +473,7 @@ void comm_can_set_current_off_delay(uint8_t controller_id, float current, float buffer_append_int32(buffer, (int32_t)(current * 1000.0), &send_index); buffer_append_float16(buffer, off_delay, 1e3, &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_CURRENT << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_CURRENT << 8), buffer, send_index, true, 0); } void comm_can_set_current_brake(uint8_t controller_id, float current) { @@ -454,7 +481,7 @@ void comm_can_set_current_brake(uint8_t controller_id, float current) { uint8_t buffer[4]; buffer_append_int32(buffer, (int32_t)(current * 1000.0), &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE << 8), buffer, send_index, true, 0); } void comm_can_set_rpm(uint8_t controller_id, float rpm) { @@ -462,7 +489,7 @@ void comm_can_set_rpm(uint8_t controller_id, float rpm) { uint8_t buffer[4]; buffer_append_int32(buffer, (int32_t)rpm, &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_RPM << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_RPM << 8), buffer, send_index, true, 0); } void comm_can_set_pos(uint8_t controller_id, float pos) { @@ -470,7 +497,7 @@ void comm_can_set_pos(uint8_t controller_id, float pos) { uint8_t buffer[4]; buffer_append_int32(buffer, (int32_t)(pos * 1000000.0), &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_POS << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_POS << 8), buffer, send_index, true, 0); } /** @@ -487,7 +514,7 @@ void comm_can_set_current_rel(uint8_t controller_id, float current_rel) { uint8_t buffer[4]; buffer_append_float32(buffer, current_rel, 1e5, &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_CURRENT_REL << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_CURRENT_REL << 8), buffer, send_index, true, 0); } /** @@ -499,7 +526,7 @@ void comm_can_set_current_rel_off_delay(uint8_t controller_id, float current_rel buffer_append_float32(buffer, current_rel, 1e5, &send_index); buffer_append_float16(buffer, off_delay, 1e3, &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_CURRENT_REL << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_CURRENT_REL << 8), buffer, send_index, true, 0); } /** @@ -516,7 +543,7 @@ void comm_can_set_current_brake_rel(uint8_t controller_id, float current_rel) { uint8_t buffer[4]; buffer_append_float32(buffer, current_rel, 1e5, &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE_REL << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_CURRENT_BRAKE_REL << 8), buffer, send_index, true, 0); } /** @@ -533,7 +560,7 @@ void comm_can_set_handbrake(uint8_t controller_id, float current) { uint8_t buffer[4]; buffer_append_float32(buffer, current, 1e3, &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_CURRENT_HANDBRAKE << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_CURRENT_HANDBRAKE << 8), buffer, send_index, true, 0); } /** @@ -550,7 +577,7 @@ void comm_can_set_handbrake_rel(uint8_t controller_id, float current_rel) { uint8_t buffer[4]; buffer_append_float32(buffer, current_rel, 1e5, &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_SET_CURRENT_HANDBRAKE_REL << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_SET_CURRENT_HANDBRAKE_REL << 8), buffer, send_index, true, 0); } /** @@ -585,7 +612,7 @@ bool comm_can_ping(uint8_t controller_id, HW_TYPE *hw_type) { uint8_t buffer[1]; buffer[0] = app_get_configuration()->controller_id; comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_PING << 8), buffer, 1, true); + ((uint32_t)CAN_PACKET_PING << 8), buffer, 1, true, 0); int ret = chEvtWaitAnyTimeout(1 << 29, MS2ST(10)); ping_tp = 0; @@ -623,7 +650,7 @@ void comm_can_detect_apply_all_foc(uint8_t controller_id, bool activate_status_m buffer[send_index++] = activate_status_msgs; buffer_append_float32(buffer, max_power_loss, 1e3, &send_index); comm_can_transmit_eid_replace(controller_id | - ((uint32_t)CAN_PACKET_DETECT_APPLY_ALL_FOC << 8), buffer, send_index, true); + ((uint32_t)CAN_PACKET_DETECT_APPLY_ALL_FOC << 8), buffer, send_index, true, 0); } /** @@ -649,7 +676,7 @@ void comm_can_conf_current_limits(uint8_t controller_id, buffer_append_float32(buffer, max, 1e3, &send_index); comm_can_transmit_eid_replace(controller_id | ((uint32_t)(store ? CAN_PACKET_CONF_STORE_CURRENT_LIMITS : - CAN_PACKET_CONF_CURRENT_LIMITS) << 8), buffer, send_index, true); + CAN_PACKET_CONF_CURRENT_LIMITS) << 8), buffer, send_index, true, 0); } /** @@ -675,7 +702,7 @@ void comm_can_conf_current_limits_in(uint8_t controller_id, buffer_append_float32(buffer, max, 1e3, &send_index); comm_can_transmit_eid_replace(controller_id | ((uint32_t)(store ? CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN : - CAN_PACKET_CONF_CURRENT_LIMITS_IN) << 8), buffer, send_index, true); + CAN_PACKET_CONF_CURRENT_LIMITS_IN) << 8), buffer, send_index, true, 0); } /** @@ -701,7 +728,7 @@ void comm_can_conf_foc_erpms(uint8_t controller_id, buffer_append_float32(buffer, foc_sl_erpm, 1e3, &send_index); comm_can_transmit_eid_replace(controller_id | ((uint32_t)(store ? CAN_PACKET_CONF_STORE_FOC_ERPMS : - CAN_PACKET_CONF_FOC_ERPMS) << 8), buffer, send_index, true); + CAN_PACKET_CONF_FOC_ERPMS) << 8), buffer, send_index, true, 0); } int comm_can_detect_all_foc_res(unsigned int index) { @@ -728,14 +755,14 @@ void comm_can_conf_battery_cut(uint8_t controller_id, buffer_append_float32(buffer, end, 1e3, &send_index); comm_can_transmit_eid_replace(controller_id | ((uint32_t)(store ? CAN_PACKET_CONF_STORE_BATTERY_CUT : - CAN_PACKET_CONF_BATTERY_CUT) << 8), buffer, send_index, true); + CAN_PACKET_CONF_BATTERY_CUT) << 8), buffer, send_index, true, 0); } void comm_can_shutdown(uint8_t controller_id) { int32_t send_index = 0; uint8_t buffer[8]; comm_can_transmit_eid_replace(controller_id | - ((uint32_t)(CAN_PACKET_SHUTDOWN) << 8), buffer, send_index, true); + ((uint32_t)(CAN_PACKET_SHUTDOWN) << 8), buffer, send_index, true, 0); } /** @@ -1029,7 +1056,7 @@ void comm_can_io_board_set_output_digital(int id, int channel, bool on) { buffer[send_index++] = on ? 1 : 0; comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_IO_BOARD_SET_OUTPUT_DIGITAL << 8), - buffer, send_index, true); + buffer, send_index, true, 0); } void comm_can_io_board_set_output_pwm(int id, int channel, float duty) { @@ -1040,7 +1067,7 @@ void comm_can_io_board_set_output_pwm(int id, int channel, float duty) { buffer_append_float16(buffer, duty, 1e3, &send_index); comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_IO_BOARD_SET_OUTPUT_PWM << 8), - buffer, send_index, true); + buffer, send_index, true, 0); } psw_status *comm_can_get_psw_status_index(int index) { @@ -1069,7 +1096,7 @@ void comm_can_psw_switch(int id, bool is_on, bool plot) { buffer[send_index++] = plot ? 1 : 0; comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_PSW_SWITCH << 8), - buffer, send_index, true); + buffer, send_index, true, 0); } void comm_can_update_pid_pos_offset(int id, float angle_now, bool store) { @@ -1080,28 +1107,46 @@ void comm_can_update_pid_pos_offset(int id, float angle_now, bool store) { buffer[send_index++] = store; comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_UPDATE_PID_POS_OFFSET << 8), - buffer, send_index, true); + buffer, send_index, true, 0); } -CANRxFrame *comm_can_get_rx_frame(void) { +/* + * Get frame from RX buffer. Interface is the CAN-interface to read from. If + * no frames are available NULL is returned. + * + * Interface: 0: Any interface, 1: CAN1, 2: CAN2 + */ +CANRxFrame *comm_can_get_rx_frame(int interface) { + CANRxFrame *res = NULL; + #if CAN_ENABLE chMtxLock(&can_rx_mtx); - if (rx_frame_read != rx_frame_write) { - CANRxFrame *res = &rx_frames[rx_frame_read++]; + if (!res && interface != 2) { + if (m_rx_state.frame_read != m_rx_state.frame_write) { + res = &m_rx_state.rx_frames[m_rx_state.frame_read++]; - if (rx_frame_read == RX_FRAMES_SIZE) { - rx_frame_read = 0; + if (m_rx_state.frame_read == RX_FRAMES_SIZE) { + m_rx_state.frame_read = 0; + } + } + } +#ifdef HW_CAN2_DEV + if (!res && interface != 1) { + if (m_rx_state2.frame_read != m_rx_state2.frame_write) { + res = &m_rx_state2.rx_frames[m_rx_state2.frame_read++]; + + if (m_rx_state2.frame_read == RX_FRAMES_SIZE) { + m_rx_state2.frame_read = 0; + } } - - chMtxUnlock(&can_rx_mtx); - return res; - } else { - chMtxUnlock(&can_rx_mtx); - return 0; } -#else - return 0; #endif + chMtxUnlock(&can_rx_mtx); +#else + (void)interface; +#endif + + return res; } void comm_can_send_status1(uint8_t id, bool replace) { @@ -1111,7 +1156,7 @@ void comm_can_send_status1(uint8_t id, bool replace) { buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current_filtered() * 1e1), &send_index); buffer_append_int16(buffer, (int16_t)(mc_interface_get_duty_cycle_now() * 1e3), &send_index); comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS << 8), - buffer, send_index, replace); + buffer, send_index, replace, 0); } void comm_can_send_status2(uint8_t id, bool replace) { @@ -1120,7 +1165,7 @@ void comm_can_send_status2(uint8_t id, bool replace) { buffer_append_int32(buffer, (int32_t)(mc_interface_get_amp_hours(false) * 1e4), &send_index); buffer_append_int32(buffer, (int32_t)(mc_interface_get_amp_hours_charged(false) * 1e4), &send_index); comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_2 << 8), - buffer, send_index, replace); + buffer, send_index, replace, 0); } void comm_can_send_status3(uint8_t id, bool replace) { @@ -1129,7 +1174,7 @@ void comm_can_send_status3(uint8_t id, bool replace) { buffer_append_int32(buffer, (int32_t)(mc_interface_get_watt_hours(false) * 1e4), &send_index); buffer_append_int32(buffer, (int32_t)(mc_interface_get_watt_hours_charged(false) * 1e4), &send_index); comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_3 << 8), - buffer, send_index, replace); + buffer, send_index, replace, 0); } void comm_can_send_status4(uint8_t id, bool replace) { @@ -1140,7 +1185,7 @@ void comm_can_send_status4(uint8_t id, bool replace) { buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current_in_filtered() * 1e1), &send_index); buffer_append_int16(buffer, (int16_t)(mc_interface_get_pid_pos_now() * 50.0), &send_index); comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_4 << 8), - buffer, send_index, replace); + buffer, send_index, replace, 0); } void comm_can_send_status5(uint8_t id, bool replace) { @@ -1150,7 +1195,7 @@ void comm_can_send_status5(uint8_t id, bool replace) { buffer_append_int16(buffer, (int16_t)(mc_interface_get_input_voltage_filtered() * 1e1), &send_index); buffer_append_int16(buffer, 0, &send_index); // Reserved for now comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_5 << 8), - buffer, send_index, replace); + buffer, send_index, replace, 0); } void comm_can_send_status6(uint8_t id, bool replace) { @@ -1161,7 +1206,7 @@ void comm_can_send_status6(uint8_t id, bool replace) { buffer_append_float16(buffer, ADC_VOLTS(ADC_IND_EXT3), 1e3, &send_index); buffer_append_float16(buffer, servodec_get_servo(0), 1e3, &send_index); comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_6 << 8), - buffer, send_index, replace); + buffer, send_index, replace, 0); } #if CAN_ENABLE @@ -1190,9 +1235,9 @@ static THD_FUNCTION(cancom_read_thread, arg) { while (result == MSG_OK) { chMtxLock(&can_rx_mtx); - rx_frames[rx_frame_write++] = rxmsg; - if (rx_frame_write == RX_FRAMES_SIZE) { - rx_frame_write = 0; + m_rx_state.rx_frames[m_rx_state.frame_write++] = rxmsg; + if (m_rx_state.frame_write == RX_FRAMES_SIZE) { + m_rx_state.frame_write = 0; } chMtxUnlock(&can_rx_mtx); @@ -1206,9 +1251,9 @@ static THD_FUNCTION(cancom_read_thread, arg) { while (result == MSG_OK) { chMtxLock(&can_rx_mtx); - rx_frames[rx_frame_write++] = rxmsg; - if (rx_frame_write == RX_FRAMES_SIZE) { - rx_frame_write = 0; + m_rx_state2.rx_frames[m_rx_state2.frame_write++] = rxmsg; + if (m_rx_state2.frame_write == RX_FRAMES_SIZE) { + m_rx_state2.frame_write = 0; } chMtxUnlock(&can_rx_mtx); @@ -1238,7 +1283,7 @@ static THD_FUNCTION(cancom_process_thread, arg) { continue; } else if (app_get_configuration()->can_mode == CAN_MODE_COMM_BRIDGE) { CANRxFrame *rxmsg_tmp; - while ((rxmsg_tmp = comm_can_get_rx_frame()) != 0) { + while ((rxmsg_tmp = comm_can_get_rx_frame(0)) != 0) { CANRxFrame rxmsg = *rxmsg_tmp; commands_fwd_can_frame(rxmsg.DLC, rxmsg.data8, rxmsg.IDE == CAN_IDE_EXT ? rxmsg.EID : rxmsg.SID, @@ -1258,7 +1303,7 @@ static THD_FUNCTION(cancom_process_thread, arg) { } CANRxFrame *rxmsg_tmp; - while ((rxmsg_tmp = comm_can_get_rx_frame()) != 0) { + while ((rxmsg_tmp = comm_can_get_rx_frame(0)) != 0) { CANRxFrame rxmsg = *rxmsg_tmp; if (rxmsg.IDE == CAN_IDE_EXT) { @@ -1612,7 +1657,7 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced) buffer[0] = is_replaced ? utils_second_motor_id() : id; buffer[1] = HW_TYPE_VESC; comm_can_transmit_eid_replace(data8[0] | - ((uint32_t)CAN_PACKET_PONG << 8), buffer, 2, true); + ((uint32_t)CAN_PACKET_PONG << 8), buffer, 2, true, 0); } break; case CAN_PACKET_PONG: @@ -1651,7 +1696,7 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced) int8_t buffer[1]; buffer[0] = res; comm_can_transmit_eid_replace(data8[0] | - ((uint32_t)CAN_PACKET_DETECT_APPLY_ALL_FOC_RES << 8), (uint8_t*)buffer, 1, true); + ((uint32_t)CAN_PACKET_DETECT_APPLY_ALL_FOC_RES << 8), (uint8_t*)buffer, 1, true, 0); } break; case CAN_PACKET_DETECT_APPLY_ALL_FOC_RES: { @@ -1739,7 +1784,7 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced) case CAN_PACKET_POLL_TS5700N8501_STATUS: { comm_can_transmit_eid_replace(app_get_configuration()->controller_id | ((uint32_t)CAN_PACKET_POLL_TS5700N8501_STATUS << 8), - enc_ts5700n8501_get_raw_status(&encoder_cfg_TS5700N8501), 8, true); + enc_ts5700n8501_get_raw_status(&encoder_cfg_TS5700N8501), 8, true, 0); } break; case CAN_PACKET_CONF_BATTERY_CUT: @@ -1792,7 +1837,7 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced) int32_t index = 0; buffer_append_int32(buffer, (int32_t)(encoder_read_deg() * 100000.0), &index); comm_can_transmit_eid_replace(app_get_configuration()->controller_id | - ((uint32_t)CAN_PACKET_POLL_ROTOR_POS << 8), (uint8_t*)buffer, 4, true); + ((uint32_t)CAN_PACKET_POLL_ROTOR_POS << 8), (uint8_t*)buffer, 4, true, 0); } break; default: diff --git a/comm_can.h b/comm_can.h index a11ddbb0..a27a703f 100644 --- a/comm_can.h +++ b/comm_can.h @@ -30,7 +30,8 @@ void comm_can_init(void); void comm_can_set_baud(CAN_BAUD baud); void comm_can_transmit_eid(uint32_t id, const uint8_t *data, uint8_t len); -void comm_can_transmit_eid_replace(uint32_t id, const uint8_t *data, uint8_t len, bool replace); +void comm_can_transmit_eid_if(uint32_t id, const uint8_t *data, uint8_t len, int interface); +void comm_can_transmit_eid_replace(uint32_t id, const uint8_t *data, uint8_t len, bool replace, int interface); void comm_can_transmit_sid(uint32_t id, uint8_t *data, uint8_t len); void comm_can_set_sid_rx_callback(bool (*p_func)(uint32_t id, uint8_t *data, uint8_t len)); void comm_can_set_eid_rx_callback(bool (*p_func)(uint32_t id, uint8_t *data, uint8_t len)); @@ -85,7 +86,7 @@ psw_status *comm_can_get_psw_status_id(int id); void comm_can_psw_switch(int id, bool is_on, bool plot); void comm_can_update_pid_pos_offset(int id, float angle_now, bool store); -CANRxFrame *comm_can_get_rx_frame(void); +CANRxFrame *comm_can_get_rx_frame(int interface); void comm_can_send_status1(uint8_t id, bool replace); void comm_can_send_status2(uint8_t id, bool replace); diff --git a/libcanard/README.md b/libcanard/README.md index 277387c0..84b33430 100644 --- a/libcanard/README.md +++ b/libcanard/README.md @@ -1,11 +1,8 @@ # Libcanard -[![Forum](https://img.shields.io/discourse/https/forum.uavcan.org/users.svg)](https://forum.uavcan.org) -[![Build Status](https://travis-ci.org/UAVCAN/libcanard.svg?branch=master)](https://travis-ci.org/UAVCAN/libcanard) -[![Coverity Scan](https://scan.coverity.com/projects/uavcan-libcanard/badge.svg)](https://scan.coverity.com/projects/uavcan-libcanard) -Minimal implementation of the UAVCAN protocol stack in C for resource constrained applications. +Minimal implementation of the DroneCAN protocol stack in C for resource constrained applications. -Get help on the **[UAVCAN Forum](https://forum.uavcan.org)**. +Get help on the **[DroneCAN Forum](https://dronecan.org/discord)**. ## Usage @@ -14,7 +11,7 @@ If you're using Git, it is recommended to add Libcanard to your project as a Git like this: ```bash -git submodule add https://github.com/UAVCAN/libcanard +git submodule add https://github.com/DroneCAN/libcanard ``` The entire library is contained in three files: @@ -64,8 +61,6 @@ This section is intended only for library developers and contributors. The library design document can be found in [DESIGN.md](DESIGN.md) -Contributors, please follow the [Zubax C++ Coding Conventions](https://kb.zubax.com/x/84Ah). - ### Building and Running Tests ```bash @@ -74,18 +69,3 @@ cmake ../libcanard/tests # Adjust path if necessary make ./run_tests ``` - -### Submitting a Coverity Scan Build - -First, [get the Coverity build tool](https://scan.coverity.com/download?tab=cxx). -Then build the tests with it: - -```bash -export PATH=$PATH:/bin/ -mkdir build && cd build -cmake ../libcanard/tests -DCMAKE_BUILD_TYPE=Debug # Adjust path if necessary -cov-build --dir cov-int make -j8 -tar czvf libcanard.tgz cov-int -``` - -Then upload the resulting archive to Coverity. diff --git a/libcanard/canard.c b/libcanard/canard.c index 4efd01c6..73a72e39 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2016-2017 UAVCAN Team + * Copyright (c) 2016-2019 UAVCAN Team * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -94,7 +94,9 @@ void canardInit(CanardInstance* out_ins, out_ins->rx_states = NULL; out_ins->tx_queue = NULL; out_ins->user_reference = user_reference; - +#if CANARD_ENABLE_TAO_OPTION + out_ins->tao_disabled = false; +#endif size_t pool_capacity = mem_arena_size / CANARD_MEM_BLOCK_SIZE; if (pool_capacity > 0xFFFFU) { @@ -137,7 +139,11 @@ int16_t canardBroadcast(CanardInstance* ins, uint8_t* inout_transfer_id, uint8_t priority, const void* payload, - uint16_t payload_len) + uint16_t payload_len +#if CANARD_ENABLE_CANFD + ,bool canfd +#endif +) { if (payload == NULL && payload_len > 0) { @@ -173,21 +179,53 @@ int16_t canardBroadcast(CanardInstance* ins, else { can_id = ((uint32_t) priority << 24U) | ((uint32_t) data_type_id << 8U) | (uint32_t) canardGetLocalNodeID(ins); - - if (payload_len > 7) - { - crc = crcAddSignature(crc, data_type_signature); - crc = crcAdd(crc, payload, payload_len); - } + crc = calculateCRC(payload, payload_len, data_type_signature +#if CANARD_ENABLE_CANFD + , canfd +#endif + ); } - const int16_t result = enqueueTxFrames(ins, can_id, inout_transfer_id, crc, payload, payload_len); + const int16_t result = enqueueTxFrames(ins, can_id, inout_transfer_id, crc, payload, payload_len +#if CANARD_ENABLE_CANFD + , canfd +#endif +); incrementTransferID(inout_transfer_id); return result; } +CANARD_INTERNAL uint16_t calculateCRC(const void* payload, uint16_t payload_len, uint64_t data_type_signature +#if CANARD_ENABLE_CANFD + ,bool canfd +#endif +) +{ + uint16_t crc = 0xFFFFU; +#if CANARD_ENABLE_CANFD + if ((payload_len > 7 && !canfd) || (payload_len > 63 && canfd)) +#else + if (payload_len > 7) +#endif + { + crc = crcAddSignature(crc, data_type_signature); + crc = crcAdd(crc, payload, payload_len); +#if CANARD_ENABLE_CANFD + if (payload_len > 63 && canfd) { + uint8_t empty = 0; + uint8_t padding = dlcToDataLength(dataLengthToDlc(((payload_len+2) % 63)+1))-1; + padding-=((payload_len+2) % 63); + for (uint8_t i=0; i 0) { @@ -214,15 +256,19 @@ int16_t canardRequestOrRespond(CanardInstance* ins, const uint32_t can_id = ((uint32_t) priority << 24U) | ((uint32_t) data_type_id << 16U) | ((uint32_t) kind << 15U) | ((uint32_t) destination_node_id << 8U) | (1U << 7U) | (uint32_t) canardGetLocalNodeID(ins); - uint16_t crc = 0xFFFFU; - if (payload_len > 7) - { - crc = crcAddSignature(crc, data_type_signature); - crc = crcAdd(crc, payload, payload_len); - } + uint16_t crc = calculateCRC(payload, payload_len, data_type_signature +#if CANARD_ENABLE_CANFD + , canfd +#endif + ); - const int16_t result = enqueueTxFrames(ins, can_id, inout_transfer_id, crc, payload, payload_len); + + const int16_t result = enqueueTxFrames(ins, can_id, inout_transfer_id, crc, payload, payload_len +#if CANARD_ENABLE_CANFD + , canfd +#endif +); if (kind == CanardRequest) // Response Transfer ID must not be altered { @@ -248,7 +294,7 @@ void canardPopTxQueue(CanardInstance* ins) freeBlock(&ins->allocator, item); } -void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint64_t timestamp_usec) +int16_t canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint64_t timestamp_usec) { const CanardTransferType transfer_type = extractTransferType(frame->id); const uint8_t destination_node_id = (transfer_type == CanardTransferTypeBroadcast) ? @@ -262,13 +308,13 @@ void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint6 (frame->id & CANARD_CAN_FRAME_ERR) != 0 || (frame->data_len < 1)) { - return; // Unsupported frame, not UAVCAN - ignore + return -CANARD_ERROR_RX_INCOMPATIBLE_PACKET; } if (transfer_type != CanardTransferTypeBroadcast && destination_node_id != canardGetLocalNodeID(ins)) { - return; // Address mismatch + return -CANARD_ERROR_RX_WRONG_ADDRESS; } const uint8_t priority = PRIORITY_FROM_ID(frame->id); @@ -291,14 +337,14 @@ void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint6 if(rx_state == NULL) { - return; // No allocator room for this frame + return -CANARD_ERROR_OUT_OF_MEMORY; } rx_state->calculated_crc = crcAddSignature(0xFFFFU, data_type_signature); } else { - return; // The application doesn't want this transfer + return -CANARD_ERROR_RX_NOT_WANTED; } } else @@ -307,7 +353,7 @@ void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint6 if (rx_state == NULL) { - return; + return -CANARD_ERROR_RX_MISSED_START; } } @@ -330,10 +376,10 @@ void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint6 rx_state->transfer_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte); rx_state->next_toggle = 0; releaseStatePayload(ins, rx_state); - if (!IS_START_OF_TRANSFER(tail_byte)) // missed the first frame + if (!IS_START_OF_TRANSFER(tail_byte)) { rx_state->transfer_id++; - return; + return -CANARD_ERROR_RX_MISSED_START; } } @@ -345,33 +391,39 @@ void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint6 .payload_head = frame->data, .payload_len = (uint8_t)(frame->data_len - 1U), .data_type_id = data_type_id, - .transfer_type = transfer_type, + .transfer_type = (uint8_t)transfer_type, .transfer_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte), .priority = priority, - .source_node_id = source_node_id + .source_node_id = source_node_id, +#if CANARD_ENABLE_CANFD + .canfd = frame->canfd, + .tao = !(frame->canfd || ins->tao_disabled) +#elif CANARD_ENABLE_TAO_OPTION + .tao = !ins->tao_disabled +#endif }; ins->on_reception(ins, &rx_transfer); prepareForNextTransfer(rx_state); - return; + return CANARD_OK; } if (TOGGLE_BIT(tail_byte) != rx_state->next_toggle) { - return; // wrong toggle + return -CANARD_ERROR_RX_WRONG_TOGGLE; } if (TRANSFER_ID_FROM_TAIL_BYTE(tail_byte) != rx_state->transfer_id) { - return; // unexpected tid + return -CANARD_ERROR_RX_UNEXPECTED_TID; } if (IS_START_OF_TRANSFER(tail_byte) && !IS_END_OF_TRANSFER(tail_byte)) // Beginning of multi frame transfer { if (frame->data_len <= 3) { - return; // Not enough data + return -CANARD_ERROR_RX_SHORT_FRAME; } // take off the crc and store the payload @@ -382,7 +434,7 @@ void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint6 { releaseStatePayload(ins, rx_state); prepareForNextTransfer(rx_state); - return; + return CANARD_ERROR_OUT_OF_MEMORY; } rx_state->payload_crc = (uint16_t)(((uint16_t) frame->data[0]) | (uint16_t)((uint16_t) frame->data[1] << 8U)); rx_state->calculated_crc = crcAdd((uint16_t)rx_state->calculated_crc, @@ -396,7 +448,7 @@ void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint6 { releaseStatePayload(ins, rx_state); prepareForNextTransfer(rx_state); - return; + return CANARD_ERROR_OUT_OF_MEMORY; } rx_state->calculated_crc = crcAdd((uint16_t)rx_state->calculated_crc, frame->data, (uint8_t)(frame->data_len - 1)); @@ -450,10 +502,17 @@ void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint6 .payload_tail = (tail_offset >= frame_payload_size) ? NULL : (&frame->data[tail_offset]), .payload_len = (uint16_t)(rx_state->payload_len + frame_payload_size), .data_type_id = data_type_id, - .transfer_type = transfer_type, + .transfer_type = (uint8_t)transfer_type, .transfer_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte), .priority = priority, - .source_node_id = source_node_id + .source_node_id = source_node_id, + +#if CANARD_ENABLE_CANFD + .canfd = frame->canfd, + .tao = !(frame->canfd || ins->tao_disabled) +#elif CANARD_ENABLE_TAO_OPTION + .tao = !ins->tao_disabled +#endif }; rx_state->buffer_blocks = NULL; // Block list ownership has been transferred to rx_transfer! @@ -468,10 +527,19 @@ void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint6 // Making sure the payload is released even if the application didn't bother with it canardReleaseRxTransferPayload(ins, &rx_transfer); prepareForNextTransfer(rx_state); - return; + + if (rx_state->calculated_crc == rx_state->payload_crc) + { + return CANARD_OK; + } + else + { + return CANARD_ERROR_RX_BAD_CRC; + } } rx_state->next_toggle = rx_state->next_toggle ? 0 : 1; + return CANARD_OK; } void canardCleanupStaleTransfers(CanardInstance* ins, uint64_t current_time_usec) @@ -840,7 +908,7 @@ float canardConvertFloat16ToNativeFloat(uint16_t value) */ CANARD_INTERNAL int16_t computeTransferIDForwardDistance(uint8_t a, uint8_t b) { - int16_t d = (int16_t)(b - a); + int16_t d = (int16_t)(a - b); if (d < 0) { d = (int16_t)(d + (int16_t)(1U << TRANSFER_ID_BIT_LEN)); @@ -859,12 +927,58 @@ CANARD_INTERNAL void incrementTransferID(uint8_t* transfer_id) } } +CANARD_INTERNAL uint8_t dlcToDataLength(uint8_t dlc) { + /* + Data Length Code 9 10 11 12 13 14 15 + Number of data bytes 12 16 20 24 32 48 64 + */ + if (dlc <= 8) { + return dlc; + } else if (dlc == 9) { + return 12; + } else if (dlc == 10) { + return 16; + } else if (dlc == 11) { + return 20; + } else if (dlc == 12) { + return 24; + } else if (dlc == 13) { + return 32; + } else if (dlc == 14) { + return 48; + } + return 64; +} + +CANARD_INTERNAL uint8_t dataLengthToDlc(uint8_t data_length) { + if (data_length <= 8) { + return data_length; + } else if (data_length <= 12) { + return 9; + } else if (data_length <= 16) { + return 10; + } else if (data_length <= 20) { + return 11; + } else if (data_length <= 24) { + return 12; + } else if (data_length <= 32) { + return 13; + } else if (data_length <= 48) { + return 14; + } + return 15; +} + CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins, uint32_t can_id, uint8_t* transfer_id, uint16_t crc, const uint8_t* payload, - uint16_t payload_len) + uint16_t payload_len +#if CANARD_ENABLE_CANFD + ,bool canfd +#endif +) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT((can_id & CANARD_CAN_EXT_ID_MASK) == can_id); // Flags must be cleared @@ -880,8 +994,12 @@ CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins, } int16_t result = 0; - - if (payload_len < CANARD_CAN_FRAME_MAX_DATA_LEN) // Single frame transfer +#if CANARD_ENABLE_CANFD + uint8_t frame_max_data_len = canfd ? CANARD_CANFD_FRAME_MAX_DATA_LEN:CANARD_CAN_FRAME_MAX_DATA_LEN; +#else + uint8_t frame_max_data_len = CANARD_CAN_FRAME_MAX_DATA_LEN; +#endif + if (payload_len < frame_max_data_len) // Single frame transfer { CanardTxQueueItem* queue_item = createTxItem(&ins->allocator); if (queue_item == NULL) @@ -891,10 +1009,13 @@ CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins, memcpy(queue_item->frame.data, payload, payload_len); + payload_len = dlcToDataLength(dataLengthToDlc(payload_len+1))-1; queue_item->frame.data_len = (uint8_t)(payload_len + 1); queue_item->frame.data[payload_len] = (uint8_t)(0xC0U | (*transfer_id & 31U)); queue_item->frame.id = can_id | CANARD_CAN_FRAME_EFF; - +#if CANARD_ENABLE_CANFD + queue_item->frame.canfd = canfd; +#endif pushTxQueue(ins, queue_item); result++; } @@ -927,16 +1048,20 @@ CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins, i = 0; } - for (; i < (CANARD_CAN_FRAME_MAX_DATA_LEN - 1) && data_index < payload_len; i++, data_index++) + for (; i < (frame_max_data_len - 1) && data_index < payload_len; i++, data_index++) { queue_item->frame.data[i] = payload[data_index]; } // tail byte sot_eot = (data_index == payload_len) ? (uint8_t)0x40 : sot_eot; - + + i = dlcToDataLength(dataLengthToDlc(i+1))-1; queue_item->frame.data[i] = (uint8_t)(sot_eot | ((uint32_t)toggle << 5U) | ((uint32_t)*transfer_id & 31U)); queue_item->frame.id = can_id | CANARD_CAN_FRAME_EFF; queue_item->frame.data_len = (uint8_t)(i + 1); +#if CANARD_ENABLE_CANFD + queue_item->frame.canfd = canfd; +#endif pushTxQueue(ins, queue_item); result++; @@ -1120,7 +1245,7 @@ CANARD_INTERNAL CanardRxState* traverseRxStates(CanardInstance* ins, uint32_t tr if (states == NULL) // initialize CanardRxStates { states = createRxState(&ins->allocator, transfer_descriptor); - + if(states == NULL) { return NULL; diff --git a/libcanard/canard.h b/libcanard/canard.h index 39a27b30..1e36c2e2 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2016-2018 UAVCAN Team + * Copyright (c) 2016-2019 UAVCAN Team * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -45,9 +45,22 @@ extern "C" { #define CANARD_VERSION_MAJOR 0 #define CANARD_VERSION_MINOR 2 + +#ifndef CANARD_ENABLE_CANFD +#define CANARD_ENABLE_CANFD 0 +#endif + + +#ifndef CANARD_ENABLE_TAO_OPTION +#if CANARD_ENABLE_CANFD +#define CANARD_ENABLE_TAO_OPTION 1 +#else +#define CANARD_ENABLE_TAO_OPTION 0 +#endif +#endif + /// By default this macro resolves to the standard assert(). The user can redefine this if necessary. #ifndef CANARD_ASSERT -//# define CANARD_ASSERT(x) assert(x) #define CANARD_ASSERT(x) #endif @@ -66,18 +79,32 @@ extern "C" { #endif /// Error code definitions; inverse of these values may be returned from API calls. -#define CANARD_OK 0 +#define CANARD_OK 0 // Value 1 is omitted intentionally, since -1 is often used in 3rd party code -#define CANARD_ERROR_INVALID_ARGUMENT 2 -#define CANARD_ERROR_OUT_OF_MEMORY 3 -#define CANARD_ERROR_NODE_ID_NOT_SET 4 -#define CANARD_ERROR_INTERNAL 9 +#define CANARD_ERROR_INVALID_ARGUMENT 2 +#define CANARD_ERROR_OUT_OF_MEMORY 3 +#define CANARD_ERROR_NODE_ID_NOT_SET 4 +#define CANARD_ERROR_INTERNAL 9 +#define CANARD_ERROR_RX_INCOMPATIBLE_PACKET 10 +#define CANARD_ERROR_RX_WRONG_ADDRESS 11 +#define CANARD_ERROR_RX_NOT_WANTED 12 +#define CANARD_ERROR_RX_MISSED_START 13 +#define CANARD_ERROR_RX_WRONG_TOGGLE 14 +#define CANARD_ERROR_RX_UNEXPECTED_TID 15 +#define CANARD_ERROR_RX_SHORT_FRAME 16 +#define CANARD_ERROR_RX_BAD_CRC 17 /// The size of a memory block in bytes. +#if CANARD_ENABLE_CANFD +#define CANARD_MEM_BLOCK_SIZE 128U +#else #define CANARD_MEM_BLOCK_SIZE 32U +#endif -/// This will be changed when the support for CAN FD is added #define CANARD_CAN_FRAME_MAX_DATA_LEN 8U +#if CANARD_ENABLE_CANFD +#define CANARD_CANFD_FRAME_MAX_DATA_LEN 64U +#endif /// Node ID values. Refer to the specification for more info. #define CANARD_BROADCAST_NODE_ID 0 @@ -123,8 +150,15 @@ typedef struct * - CANARD_CAN_FRAME_ERR */ uint32_t id; +#if CANARD_ENABLE_CANFD + uint8_t data[CANARD_CANFD_FRAME_MAX_DATA_LEN]; +#else uint8_t data[CANARD_CAN_FRAME_MAX_DATA_LEN]; +#endif uint8_t data_len; +#if CANARD_ENABLE_CANFD + bool canfd; +#endif } CanardCANFrame; /** @@ -261,6 +295,10 @@ struct CanardInstance CanardTxQueueItem* tx_queue; ///< TX frames awaiting transmission void* user_reference; ///< User pointer that can link this instance with other objects + +#if CANARD_ENABLE_TAO_OPTION + bool tao_disabled; ///< True if TAO is disabled +#endif }; /** @@ -312,6 +350,12 @@ struct CanardRxTransfer uint8_t transfer_id; ///< 0 to 31 uint8_t priority; ///< 0 to 31 uint8_t source_node_id; ///< 1 to 127, or 0 if the source is anonymous +#if CANARD_ENABLE_TAO_OPTION + bool tao; +#endif +#if CANARD_ENABLE_CANFD + bool canfd; ///< frame canfd +#endif }; /** @@ -370,7 +414,12 @@ int16_t canardBroadcast(CanardInstance* ins, ///< Library instance uint8_t* inout_transfer_id, ///< Pointer to a persistent variable containing the transfer ID uint8_t priority, ///< Refer to definitions CANARD_TRANSFER_PRIORITY_* const void* payload, ///< Transfer payload - uint16_t payload_len); ///< Length of the above, in bytes + uint16_t payload_len ///< Length of the above, in bytes +#if CANARD_ENABLE_CANFD + ,bool canfd ///< Is the frame canfd +#endif + ); + /** * Sends a request or a response transfer. @@ -397,7 +446,11 @@ int16_t canardRequestOrRespond(CanardInstance* ins, ///< Library ins uint8_t priority, ///< Refer to definitions CANARD_TRANSFER_PRIORITY_* CanardRequestResponse kind, ///< Refer to CanardRequestResponse const void* payload, ///< Transfer payload - uint16_t payload_len); ///< Length of the above, in bytes + uint16_t payload_len ///< Length of the above, in bytes +#if CANARD_ENABLE_CANFD + ,bool canfd ///< Is the frame canfd +#endif + ); /** * Returns a pointer to the top priority frame in the TX queue. @@ -418,10 +471,12 @@ void canardPopTxQueue(CanardInstance* ins); /** * Processes a received CAN frame with a timestamp. * The application will call this function when it receives a new frame from the CAN bus. + * + * Return value will report any errors in decoding packets. */ -void canardHandleRxFrame(CanardInstance* ins, - const CanardCANFrame* frame, - uint64_t timestamp_usec); +int16_t canardHandleRxFrame(CanardInstance* ins, + const CanardCANFrame* frame, + uint64_t timestamp_usec); /** * Traverses the list of transfers and removes those that were last updated more than timeout_usec microseconds ago. @@ -521,9 +576,15 @@ uint16_t canardConvertNativeFloatToFloat16(float value); float canardConvertFloat16ToNativeFloat(uint16_t value); /// Abort the build if the current platform is not supported. +#if CANARD_ENABLE_CANFD +CANARD_STATIC_ASSERT(((uint32_t)CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) < 128, + "Platforms where sizeof(void*) > 4 are not supported. " + "On AMD64 use 32-bit mode (e.g. GCC flag -m32)."); +#else CANARD_STATIC_ASSERT(((uint32_t)CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) < 32, "Platforms where sizeof(void*) > 4 are not supported. " "On AMD64 use 32-bit mode (e.g. GCC flag -m32)."); +#endif #ifdef __cplusplus } diff --git a/libcanard/canard_driver.c b/libcanard/canard_driver.c index 7a33cf61..83152e84 100755 --- a/libcanard/canard_driver.c +++ b/libcanard/canard_driver.c @@ -45,6 +45,7 @@ #include "crc.h" #include "nrf_driver.h" #include "buffer.h" +#include "utils.h" // Constants #define CAN_APP_NODE_NAME "org.vesc." HW_NAME @@ -74,8 +75,13 @@ typedef struct { } status_msg_wrapper_t; // Private variables -static CanardInstance canard; +static CanardInstance canard_ins; static uint8_t canard_memory_pool[1024]; +#ifdef HW_CAN2_DEV +static CanardInstance canard_ins_if2; +static uint8_t canard_memory_pool_if2[1024]; +#endif +static uint8_t msg_buffer[512]; static uint8_t node_health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; static uint8_t node_mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; static int debug_level; @@ -89,7 +95,7 @@ static THD_FUNCTION(canard_thread, arg); // Private functions static void calculateTotalCurrent(void); -static void sendEscStatus(void); +static void sendEscStatus(CanardInstance *ins); static void readUniqueID(uint8_t* out_uid); static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer); static bool shouldAcceptTransfer(const CanardInstance* ins, @@ -110,6 +116,7 @@ static struct { uint8_t path[UAVCAN_PROTOCOL_FILE_PATH_PATH_MAX_LENGTH+1]; uint8_t sector; uint32_t sector_ofs; + CanardInstance *ins; } fw_update; systime_t last_read_file_req = 0; @@ -196,12 +203,10 @@ static void write_app_config(void); */ static param_t parameters[] = { - {"uavcan_mode", AP_PARAM_INT8, 0, 0, 2, 1}, - {"can_baud_rate", AP_PARAM_INT8, 0, 0, 8, 3}, - {"can_status_rate", AP_PARAM_INT32, 0, 0, 100, 50}, - {"can_send_status", AP_PARAM_INT8, 0, 0, 5, 1}, - {"can_esc_index", AP_PARAM_INT8, 0, 0, 255, 0}, - {"controller_id", AP_PARAM_INT8, 0, 0, 255, 0} + {"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} }; /* @@ -234,12 +239,10 @@ static void write_app_config(void) { app_configuration *appconf = mempools_alloc_appconf(); *appconf = *app_get_configuration(); - appconf->can_mode = (uint8_t)getParamByName("uavcan_mode")->val; appconf->can_baud_rate = (uint8_t)getParamByName("can_baud_rate")->val; - appconf->can_status_rate_1 = (uint32_t)getParamByName("can_status_rate_1")->val; - appconf->can_status_msgs_r1 = (uint8_t)getParamByName("can_status_msgs_r1")->val; - appconf->uavcan_esc_index = (uint8_t)getParamByName("can_esc_index")->val; - appconf->controller_id = (uint8_t)getParamByName("controller_id")->val; + appconf->can_status_rate_1 = (uint32_t)getParamByName("can_status_rate")->val; + appconf->uavcan_esc_index = (uint16_t)getParamByName("can_esc_index")->val; + appconf->controller_id = (uint16_t)getParamByName("controller_id")->val; conf_general_store_app_configuration(appconf); app_set_configuration(appconf); @@ -258,10 +261,8 @@ static void write_app_config(void) { static void refresh_parameters(void){ const app_configuration *appconf = app_get_configuration(); - updateParamByName((uint8_t *)"uavcan_mode", appconf->can_mode ); 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_send_status", appconf->can_status_msgs_r1 ); + 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 ); } @@ -273,7 +274,7 @@ static void refresh_parameters(void){ */ static param_t* getParamByIndex(uint16_t index) { - if (index >= (sizeof(parameters)/40)) + if (index >= (sizeof(parameters) / sizeof(parameters[0]))) { if (debug_level == 2) { commands_printf("Index is out of range"); @@ -281,7 +282,7 @@ static param_t* getParamByIndex(uint16_t index) return NULL; } if (debug_level == 2) { - commands_printf("Size of parameter array is: %d",sizeof(parameters)/40); + commands_printf("Size of parameter array is: %d", (sizeof(parameters) / sizeof(parameters[0]))); } return ¶meters[index]; } @@ -293,7 +294,7 @@ static param_t* getParamByIndex(uint16_t index) */ static param_t* getParamByName(char * name) { - for (uint16_t i = 0; i < sizeof(parameters)/40; i++) + for (uint16_t i = 0; i < (sizeof(parameters) / sizeof(parameters[0])); i++) { if (debug_level == 2) { commands_printf("name: %s paramname: %s", name, parameters[i].name); @@ -373,10 +374,9 @@ static void calculateTotalCurrent(void) { /* * Send Node Status Message */ -static void sendNodeStatus(void) { +static void sendNodeStatus(CanardInstance *ins) { node_mode = fw_update.node_id?UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE:UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; - uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; node_status.health = node_health; @@ -386,7 +386,7 @@ static void sendNodeStatus(void) { // status.vendor_specific_status_code is filled in the firmware update loop uavcan_protocol_NodeStatus_encode(&node_status, buffer); static uint8_t transfer_id; - canardBroadcast(&canard, + canardBroadcast(ins, UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, UAVCAN_PROTOCOL_NODESTATUS_ID, &transfer_id, @@ -398,8 +398,7 @@ static void sendNodeStatus(void) { /* * Send ESC Status Message */ -static void sendEscStatus(void) { - uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE]; +static void sendEscStatus(CanardInstance *ins) { uavcan_equipment_esc_Status status; status.current = mc_interface_get_tot_current(); status.error_count = mc_interface_get_fault(); @@ -408,10 +407,10 @@ static void sendEscStatus(void) { mc_interface_get_configuration()->l_current_max * mc_interface_get_configuration()->l_current_max_scale) * 100.0; status.rpm = mc_interface_get_rpm(); - status.temperature = mc_interface_temp_fet_filtered() + 273.15; + status.temperature = fmaxf(mc_interface_temp_fet_filtered(), mc_interface_temp_motor_filtered()) + 273.15; status.voltage = mc_interface_get_input_voltage_filtered(); - uavcan_equipment_esc_Status_encode(&status, buffer); + uavcan_equipment_esc_Status_encode(&status, msg_buffer); static uint8_t transfer_id; @@ -419,12 +418,12 @@ static void sendEscStatus(void) { commands_printf("UAVCAN sendESCStatus"); } - canardBroadcast(&canard, + canardBroadcast(ins, UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ESC_STATUS_ID, &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, - buffer, + msg_buffer, UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE); } @@ -446,7 +445,6 @@ static void readUniqueID(uint8_t* out_uid) { * Node status is syncronize with the data from the node status message. */ static void handle_get_node_info(CanardInstance* ins, CanardRxTransfer* transfer) { - uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; uavcan_protocol_GetNodeInfoResponse pkt; node_status.uptime_sec = ST2S(chVTGetSystemTimeX()); @@ -477,7 +475,7 @@ static void handle_get_node_info(CanardInstance* ins, CanardRxTransfer* transfer pkt.name.len = strlen(CAN_APP_NODE_NAME); pkt.name.data = (uint8_t *)name; - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, msg_buffer); const int16_t resp_res = canardRequestOrRespond(ins, transfer->source_node_id, @@ -486,7 +484,7 @@ static void handle_get_node_info(CanardInstance* ins, CanardRxTransfer* transfer &transfer->transfer_id, transfer->priority, CanardResponse, - &buffer[0], + &msg_buffer[0], total_size); if (resp_res <= 0) { if (debug_level > 1) { @@ -548,9 +546,7 @@ static void handle_esc_rpm_command(CanardInstance* ins, CanardRxTransfer* transf (void)ins; uavcan_equipment_esc_RPMCommand cmd; - uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_MAX_SIZE]; - memset(buffer, 0, sizeof(buffer)); - uint8_t *tmp = buffer; + uint8_t *tmp = msg_buffer; if (uavcan_equipment_esc_RPMCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0) >= 0) { if (cmd.rpm.len > app_get_configuration()->uavcan_esc_index) { @@ -598,8 +594,7 @@ static void handle_esc_status(CanardInstance* ins, CanardRxTransfer* transfer) { static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) { uavcan_protocol_param_GetSetRequest req; - uint8_t arraybuf[UAVCAN_PROTOCOL_PARAM_GETSET_REQUEST_NAME_MAX_LENGTH]; - uint8_t *arraybuf_ptr = arraybuf; + uint8_t *arraybuf_ptr = msg_buffer; if (uavcan_protocol_param_GetSetRequest_decode(transfer, transfer->payload_len, &req, &arraybuf_ptr) < 0) { return; } @@ -732,8 +727,7 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) if (debug_level == 2) { commands_printf("UAVCAN param_getset: Sending %s", pkt.name.data); } - uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE]; - uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, msg_buffer); const int16_t resp_res = canardRequestOrRespond(ins, transfer->source_node_id, @@ -742,7 +736,7 @@ static void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer) &transfer->transfer_id, transfer->priority, CanardResponse, - &buffer[0], + &msg_buffer[0], total_size); if ((resp_res <= 0) && (debug_level > 1)) { @@ -768,8 +762,10 @@ static void handle_restart_node(void) { * request before we send a new one. The value is cleared after procesing a response * so that we dont have to wait 250ms before sending the next request. */ -static void send_fw_read(void) +static void send_fw_read(CanardInstance *ins) { + fw_update.ins = ins; + uint32_t now = ST2MS(chVTGetSystemTimeX()); if (now - fw_update.last_ms < 250) { // the server may still be responding @@ -777,24 +773,23 @@ static void send_fw_read(void) } fw_update.last_ms = now; - uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE]; - canardEncodeScalar(buffer, 0, 40, &fw_update.ofs); + canardEncodeScalar(msg_buffer, 0, 40, &fw_update.ofs); uint32_t offset = 40; uint8_t len = strlen((const char *)fw_update.path); for (uint8_t i=0; isource_node_id, UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, @@ -954,7 +948,7 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* &transfer->transfer_id, transfer->priority, CanardResponse, - &buffer[0], + &msg_buffer[0], total_size); // Erase the reserved flash for new app @@ -966,7 +960,7 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* commands_printf("UAVCAN Begin firmware update from node_id: %d",fw_update.node_id); } - send_fw_read(); + send_fw_read(ins); } /** @@ -1123,7 +1117,15 @@ static THD_FUNCTION(canard_thread, arg) { (void)arg; chRegSetThreadName("UAVCAN"); - canardInit(&canard, canard_memory_pool, sizeof(canard_memory_pool), onTransferReceived, shouldAcceptTransfer, NULL); + getParamByName("controller_id")->defval = HW_DEFAULT_ID; + + canardInit(&canard_ins, canard_memory_pool, sizeof(canard_memory_pool), + onTransferReceived, shouldAcceptTransfer, NULL); + +#ifdef HW_CAN2_DEV + canardInit(&canard_ins_if2, canard_memory_pool_if2, sizeof(canard_memory_pool_if2), + onTransferReceived, shouldAcceptTransfer, NULL); +#endif systime_t last_status_time = 0; systime_t last_esc_status_time = 0; @@ -1139,10 +1141,14 @@ static THD_FUNCTION(canard_thread, arg) { } my_node_id = conf->controller_id; - canardSetLocalNodeID(&canard, conf->controller_id); + canardSetLocalNodeID(&canard_ins, conf->controller_id); + +#ifdef HW_CAN2_DEV + canardSetLocalNodeID(&canard_ins_if2, conf->controller_id); +#endif CANRxFrame *rxmsg; - while ((rxmsg = comm_can_get_rx_frame()) != 0) { + while ((rxmsg = comm_can_get_rx_frame(1)) != 0) { CanardCANFrame rx_frame; if (rxmsg->IDE == CAN_IDE_EXT) { @@ -1154,25 +1160,53 @@ static THD_FUNCTION(canard_thread, arg) { rx_frame.data_len = rxmsg->DLC; memcpy(rx_frame.data, rxmsg->data8, rxmsg->DLC); - canardHandleRxFrame(&canard, &rx_frame, ST2US(chVTGetSystemTimeX())); + canardHandleRxFrame(&canard_ins, &rx_frame, ST2US(chVTGetSystemTimeX())); } - for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { - comm_can_transmit_eid(txf->id, txf->data, txf->data_len); - canardPopTxQueue(&canard); + for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard_ins)) != NULL;) { + comm_can_transmit_eid_if(txf->id, txf->data, txf->data_len, 1); + canardPopTxQueue(&canard_ins); } +#ifdef HW_CAN2_DEV + while ((rxmsg = comm_can_get_rx_frame(2)) != 0) { + CanardCANFrame rx_frame; + + if (rxmsg->IDE == CAN_IDE_EXT) { + rx_frame.id = rxmsg->EID | CANARD_CAN_FRAME_EFF; + } else { + rx_frame.id = rxmsg->SID; + } + + rx_frame.data_len = rxmsg->DLC; + memcpy(rx_frame.data, rxmsg->data8, rxmsg->DLC); + + canardHandleRxFrame(&canard_ins_if2, &rx_frame, ST2US(chVTGetSystemTimeX())); + } + + for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard_ins_if2)) != NULL;) { + comm_can_transmit_eid_if(txf->id, txf->data, txf->data_len, 2); + canardPopTxQueue(&canard_ins_if2); + } +#endif + if (ST2MS(chVTTimeElapsedSinceX(last_status_time)) >= 1000) { last_status_time = chVTGetSystemTimeX(); - canardCleanupStaleTransfers(&canard, ST2US(chVTGetSystemTimeX())); - sendNodeStatus(); + canardCleanupStaleTransfers(&canard_ins, ST2US(chVTGetSystemTimeX())); + sendNodeStatus(&canard_ins); +#ifdef HW_CAN2_DEV + canardCleanupStaleTransfers(&canard_ins_if2, ST2US(chVTGetSystemTimeX())); + sendNodeStatus(&canard_ins_if2); +#endif } - if (conf->can_status_msgs_r1 > 0 && - ST2MS(chVTTimeElapsedSinceX(last_esc_status_time)) >= (1000 / conf->can_status_msgs_r1) && - conf->can_status_msgs_r1 != 0) { + 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(); + sendEscStatus(&canard_ins); +#ifdef HW_CAN2_DEV + sendEscStatus(&canard_ins_if2); +#endif } if (ST2MS(chVTTimeElapsedSinceX(last_tot_current_calc_time)) >= 1000 / CURRENT_CALC_FREQ_HZ) { @@ -1197,7 +1231,7 @@ static THD_FUNCTION(canard_thread, arg) { if ((ST2MS(chVTTimeElapsedSinceX(last_read_file_req)) >= 10) && (fw_update.node_id != 0)) { last_read_file_req = chVTGetSystemTimeX(); - send_fw_read(); + send_fw_read(fw_update.ins); } // delay jump to bootloader after receiving data for 0.5 sec diff --git a/libcanard/canard_internals.h b/libcanard/canard_internals.h index baf4b06e..79ff89a4 100644 --- a/libcanard/canard_internals.h +++ b/libcanard/canard_internals.h @@ -85,13 +85,20 @@ CANARD_INTERNAL void incrementTransferID(uint8_t* transfer_id); CANARD_INTERNAL uint64_t releaseStatePayload(CanardInstance* ins, CanardRxState* rxstate); +CANARD_INTERNAL uint8_t dlcToDataLength(uint8_t dlc); +CANARD_INTERNAL uint8_t dataLengthToDlc(uint8_t data_length); + /// Returns the number of frames enqueued CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins, uint32_t can_id, uint8_t* transfer_id, uint16_t crc, const uint8_t* payload, - uint16_t payload_len); + uint16_t payload_len +#if CANARD_ENABLE_CANFD + ,bool canfd +#endif + ); CANARD_INTERNAL void copyBitArray(const uint8_t* src, uint32_t src_offset, @@ -147,6 +154,13 @@ CANARD_INTERNAL void* allocateBlock(CanardPoolAllocator* allocator); CANARD_INTERNAL void freeBlock(CanardPoolAllocator* allocator, void* p); +CANARD_INTERNAL uint16_t calculateCRC(const void* payload, + uint16_t payload_len, + uint64_t data_type_signature +#if CANARD_ENABLE_CANFD + ,bool canfd +#endif +); #ifdef __cplusplus }