mirror of https://github.com/rusefi/bldc.git
UAVCAN update to latest dronecan libcanard, bug fixes, redundant CAN support, send highest of motor and fet temperature in escstatus message
This commit is contained in:
Benjamin Vedder
parent
7a2c212ff1
commit
4d2adfd4b3
177
comm_can.c
177
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,6 +268,7 @@ void comm_can_transmit_eid_replace(uint32_t id, const uint8_t *data, uint8_t len
|
|||
|
||||
chMtxLock(&can_mtx);
|
||||
#ifdef HW_CAN2_DEV
|
||||
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);
|
||||
|
|
@ -260,7 +277,13 @@ void comm_can_transmit_eid_replace(uint32_t id, const uint8_t *data, uint8_t len
|
|||
}
|
||||
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++];
|
||||
|
||||
chMtxUnlock(&can_rx_mtx);
|
||||
return res;
|
||||
} else {
|
||||
chMtxUnlock(&can_rx_mtx);
|
||||
return 0;
|
||||
if (m_rx_state2.frame_read == RX_FRAMES_SIZE) {
|
||||
m_rx_state2.frame_read = 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:
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -1,11 +1,8 @@
|
|||
# Libcanard
|
||||
[](https://forum.uavcan.org)
|
||||
[](https://travis-ci.org/UAVCAN/libcanard)
|
||||
[](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:<coverity-build-tool-directory>/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.
|
||||
|
|
|
|||
|
|
@ -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<padding; i++) {
|
||||
crc = crcAddByte(crc, empty);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
int16_t canardRequestOrRespond(CanardInstance* ins,
|
||||
uint8_t destination_node_id,
|
||||
uint64_t data_type_signature,
|
||||
|
|
@ -196,7 +234,11 @@ int16_t canardRequestOrRespond(CanardInstance* ins,
|
|||
uint8_t priority,
|
||||
CanardRequestResponse kind,
|
||||
const void* payload,
|
||||
uint16_t payload_len)
|
||||
uint16_t payload_len
|
||||
#if CANARD_ENABLE_CANFD
|
||||
,bool canfd
|
||||
#endif
|
||||
)
|
||||
{
|
||||
if (payload == NULL && payload_len > 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++;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -72,12 +85,26 @@ extern "C" {
|
|||
#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,8 +471,10 @@ 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,
|
||||
int16_t canardHandleRxFrame(CanardInstance* ins,
|
||||
const CanardCANFrame* frame,
|
||||
uint64_t timestamp_usec);
|
||||
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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; i<len; i++) {
|
||||
canardEncodeScalar(buffer, offset, 8, &fw_update.path[i]);
|
||||
canardEncodeScalar(msg_buffer, offset, 8, &fw_update.path[i]);
|
||||
offset += 8;
|
||||
}
|
||||
uint32_t total_size = (offset+7)/8;
|
||||
|
||||
canardRequestOrRespond(&canard,
|
||||
canardRequestOrRespond(ins,
|
||||
fw_update.node_id,
|
||||
UAVCAN_PROTOCOL_FILE_READ_SIGNATURE,
|
||||
UAVCAN_PROTOCOL_FILE_READ_ID,
|
||||
&fw_update.transfer_id,
|
||||
CANARD_TRANSFER_PRIORITY_HIGH,
|
||||
CanardRequest,
|
||||
&buffer[0],
|
||||
&msg_buffer[0],
|
||||
total_size);
|
||||
}
|
||||
|
||||
|
|
@ -942,11 +937,10 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer*
|
|||
}
|
||||
}
|
||||
|
||||
uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE];
|
||||
uavcan_protocol_file_BeginFirmwareUpdateResponse reply;
|
||||
reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK;
|
||||
|
||||
uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer);
|
||||
uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, msg_buffer);
|
||||
canardRequestOrRespond(ins,
|
||||
transfer->source_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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue