From fd04ee8731a151b4d8797924d8f9facc48c06c94 Mon Sep 17 00:00:00 2001 From: bitscuit Date: Sat, 22 Apr 2023 14:41:38 +0200 Subject: [PATCH] added code to send periodic info broadcaast on LV can --- src/main.cpp | 74 +++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 70 insertions(+), 4 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 958b6f9..e4e5c48 100644 --- a/src/main.cpp +++ b/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 */ }