bitscuit
/
UGR-Gen3-Torque-Controller
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

added code to send periodic info broadcaast on LV can

This commit is contained in:
bitscuit 2023-04-22 14:41:38 +02:00
parent c2d6166e9f
commit fd04ee8731
1 changed files with 70 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
src/main.cpp
View File

@ -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_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_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_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 */ /* Torque Controller variables */
#define TC_OK 0b0 #define TC_OK 0b0
@ -74,7 +77,10 @@ char mcl_enabled, mcr_enabled;
char mcl_limits_1, mcr_limits_1; char mcl_limits_1, mcr_limits_1;
char mcl_limits_2, mcr_limits_2; char mcl_limits_2, mcr_limits_2;
uint8_t mcl_can_version, mcr_can_version; 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; 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) */ /* 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 */ #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_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_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_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 */ /* SAFETY */
void enter_safe_state(char fault_id) { void enter_safe_state(char fault_id) {
@ -295,6 +302,64 @@ void check_heartbeat() {
time_last_heartbeat = millis(); 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 */ /* Subroutine to update invertors */
void update_drives() { void update_drives() {
/* Check timeouts */ /* Check timeouts */
@ -333,10 +398,11 @@ void setup() {
void loop() { void loop() {
/* Main event loop */ /* Main event loop */
can_lv.events(); /* Handle CAN LV events */ can_lv.events(); /* Handle CAN LV events */
can_mc.events(); /* Handle CAN MC events */ can_mc.events(); /* Handle CAN MC events */
check_heartbeat(); /* Do heartbeat stuffz */ check_heartbeat(); /* Do heartbeat stuffz */
update_drives(); /* Do motorcontroller shit */ analyse_drive_data(); /* Analyse the data from the drives duh */
update_drives(); /* Do motorcontroller shit */
/* TODO: check drive faults */ /* TODO: check drive faults */
} }