added code to send periodic info broadcaast on LV can
This commit is contained in:
parent
c2d6166e9f
commit
fd04ee8731
74
src/main.cpp
74
src/main.cpp
|
@ -37,6 +37,9 @@ uint64_t time_last_heartbeat = 0; /* Timestamp of last heartbeat message t
|
|||
#define CAN_ID_CRITICAL_FAULT 0x000 /* CAN ID of critical fault message (not using any Module ID) */
|
||||
#define CAN_ID_APPS_BPPS 0x110 /* CAN ID of APPS and BPPS values (not using Module ID of APPS/BPPS) */
|
||||
#define CAN_ID_BOOT_SEQ 0x090 /* CAN ID of Boot Sequence Status (for RTD) values (using Module ID of ECU for safety) */
|
||||
#define CAN_ID_TC_INFO_GEN 0x480 /* CAN ID of TC general information */
|
||||
#define CAN_ID_TC_INFO_ERPM 0x490 /* CAN ID of TC ERPM information */
|
||||
#define CAN_ID_TC_INFO_TEMP 0x4A0 /* CAN ID of TC temperature information */
|
||||
|
||||
/* Torque Controller variables */
|
||||
#define TC_OK 0b0
|
||||
|
@ -74,7 +77,10 @@ char mcl_enabled, mcr_enabled;
|
|||
char mcl_limits_1, mcr_limits_1;
|
||||
char mcl_limits_2, mcr_limits_2;
|
||||
uint8_t mcl_can_version, mcr_can_version;
|
||||
#define MC_CAN_DATA_TIMEOUT 500 /* If no data received for this time, enter safe state */
|
||||
uint32_t time_last_mcl_data = 0, time_last_mcr_data = 0;
|
||||
#define CAN_TC_SUMMARY_PERIOD 500 /* Period of the information broadcasting on the LV can */
|
||||
uint32_t time_last_tc_summary = 0;
|
||||
|
||||
/* Fault IDs (used in 2nd data field of Critical Fault message on LV CAN) */
|
||||
#define CAN_FAULT_ID 0x05 /* Fault ID for D0 field of Critical Fault message, signifies that this is a TC fault */
|
||||
|
@ -91,6 +97,7 @@ uint32_t time_last_mcl_data = 0, time_last_mcr_data = 0;
|
|||
#define FAULT_INV_ANALOG_INPUT 0x0A /* Fault from invertor: analog input error */
|
||||
#define FAULT_RTD_TIMEOUT 0x11 /* Did not receive RTD state in short enough time, assume data loss */
|
||||
#define FAULT_APPS_BPPS_TIMEOUT 0x12 /* Did not receive APPS/BPPS values in short enough time, assume data loss */
|
||||
#define FAULT_INV_TIMEOUT 0x13 /* Did not receive INV values in short enough time, assume data loss */
|
||||
|
||||
/* SAFETY */
|
||||
void enter_safe_state(char fault_id) {
|
||||
|
@ -295,6 +302,64 @@ void check_heartbeat() {
|
|||
time_last_heartbeat = millis();
|
||||
}
|
||||
|
||||
/* Subroutine to analyse drive data and check safety stuff */
|
||||
void analyse_drive_data() {
|
||||
if (time_last_mcl_data == 0 || time_last_mcr_data == 0) return; /* No data yet :( */
|
||||
|
||||
/* Check timeouts */
|
||||
if (millis() > time_last_mcl_data + MC_CAN_DATA_TIMEOUT) enter_safe_state(FAULT_INV_TIMEOUT);
|
||||
if (millis() > time_last_mcr_data + MC_CAN_DATA_TIMEOUT) enter_safe_state(FAULT_INV_TIMEOUT);
|
||||
|
||||
/* TODO: check value boundaries */
|
||||
|
||||
/* Check faults */
|
||||
if (mcl_fault_code != 0x00) {
|
||||
/* Fault in left drive */
|
||||
enter_safe_state(mcl_fault_code);
|
||||
}
|
||||
if (mcr_fault_code != 0x00) {
|
||||
/* Fault in right drive */
|
||||
enter_safe_state(mcr_fault_code);
|
||||
}
|
||||
|
||||
/* Create summary and send on LV CAN */
|
||||
if (millis() < time_last_tc_summary + CAN_TC_SUMMARY_PERIOD) return;
|
||||
|
||||
CAN_message_t msg_gen;
|
||||
msg_gen.id = CAN_ID_TC_INFO_GEN | CAN_MODULE_ID;
|
||||
msg_gen.buf[0] = mcl_fault_code;
|
||||
msg_gen.buf[1] = mcr_fault_code;
|
||||
msg_gen.buf[2] = mcl_enabled;
|
||||
msg_gen.buf[3] = mcr_enabled;
|
||||
can_lv.write(msg_gen);
|
||||
|
||||
CAN_message_t msg_erpm;
|
||||
msg_erpm.id = CAN_ID_TC_INFO_ERPM | CAN_MODULE_ID;
|
||||
msg_erpm.buf[0] = (mcl_erpm > 24) & 0xff;
|
||||
msg_erpm.buf[1] = (mcl_erpm > 16) & 0xff;
|
||||
msg_erpm.buf[2] = (mcl_erpm > 8) & 0xff;
|
||||
msg_erpm.buf[3] = mcl_erpm & 0xff;
|
||||
msg_erpm.buf[4] = (mcr_erpm > 24) & 0xff;
|
||||
msg_erpm.buf[5] = (mcr_erpm > 16) & 0xff;
|
||||
msg_erpm.buf[6] = (mcr_erpm > 8) & 0xff;
|
||||
msg_erpm.buf[7] = mcr_erpm & 0xff;
|
||||
can_lv.write(msg_erpm);
|
||||
|
||||
CAN_message_t msg_temp;
|
||||
msg_temp.id = CAN_ID_TC_INFO_TEMP | CAN_MODULE_ID;
|
||||
msg_temp.buf[0] = (mcl_ctrl_temp > 8) & 0xff;
|
||||
msg_temp.buf[1] = mcl_ctrl_temp & 0xff;
|
||||
msg_temp.buf[2] = (mcr_ctrl_temp > 8) & 0xff;
|
||||
msg_temp.buf[3] = mcr_ctrl_temp & 0xff;
|
||||
msg_temp.buf[4] = (mcl_motor_temp > 8) & 0xff;
|
||||
msg_temp.buf[5] = mcl_motor_temp & 0xff;
|
||||
msg_temp.buf[6] = (mcr_motor_temp > 8) & 0xff;
|
||||
msg_temp.buf[7] = mcr_motor_temp & 0xff;
|
||||
can_lv.write(msg_temp);
|
||||
|
||||
time_last_tc_summary = millis();
|
||||
}
|
||||
|
||||
/* Subroutine to update invertors */
|
||||
void update_drives() {
|
||||
/* Check timeouts */
|
||||
|
@ -333,10 +398,11 @@ void setup() {
|
|||
|
||||
void loop() {
|
||||
/* Main event loop */
|
||||
can_lv.events(); /* Handle CAN LV events */
|
||||
can_mc.events(); /* Handle CAN MC events */
|
||||
check_heartbeat(); /* Do heartbeat stuffz */
|
||||
update_drives(); /* Do motorcontroller shit */
|
||||
can_lv.events(); /* Handle CAN LV events */
|
||||
can_mc.events(); /* Handle CAN MC events */
|
||||
check_heartbeat(); /* Do heartbeat stuffz */
|
||||
analyse_drive_data(); /* Analyse the data from the drives duh */
|
||||
update_drives(); /* Do motorcontroller shit */
|
||||
|
||||
/* TODO: check drive faults */
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue