From 7c6b8c64041b88868dd1dbd64b2d9f0911657347 Mon Sep 17 00:00:00 2001 From: bitscuit Date: Sat, 22 Apr 2023 12:14:24 +0200 Subject: [PATCH] finished code to process apps and update erpm --- src/main.cpp | 164 ++++++++++++++++++++++++++++++++------------------- 1 file changed, 103 insertions(+), 61 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 36a4584..56f7334 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,42 +5,69 @@ void sendEnable(); // function to turn the inverter on DON'T FORGET TO EDIT THE ID OF THE DRIVE void sendERPM(); // function to change the speed DON'T FORGET TO EDIT THE ID OF THE DRIVE +/* Invertor config */ +#define MC_L_ID 0x4F /* ID of the left invertor, to be used in CAN communication */ +#define MC_R_ID 0x4F /* ID of the right invertor, to be used in CAN communication */ +#define CAN_MC_TIMEOUT 100 /* Timeout of the invertors */ + +/* Invertor CAN packet IDs */ +#define CAN_MC_ID_ERPM 0x03 /* Message to set ERPM */ +#define CAN_MC_ID_ENABLE 0x0C /* Message to enable/disable drive */ + /* CAN variables */ FlexCAN_T4 can_lv; /* LV CAN bus is connected to controller 1 */ FlexCAN_T4 can_mc; /* Motorcontroller/Invertor CAN bus is connected to controller 2 */ #define CAN_LV_BAUD 1000000 /* Data rate of the LV CAN = 1Mbps */ #define CAN_MC_BAUD 500000 /* Data rate of the MC CAN = 500kbps */ -uint64_t time_last_can_mc_send = 0; /* Timestamp of last message that was sent to motorcontrollers (because they have timeout safety shit) */ -uint64_t time_last_apps = 0; /* Timestamp of last message for APPS values */ -uint64_t time_last_rtd_state = 0; /* Timestamp of last message for Boot Sequence State stuff */ /* CAN heartbeat stuff */ -uint64_t time_last_heartbeat = 0; /* Timestamp of last heartbeat message that was sent */ -#define HEARTBEAT_PERIOD 100 /* Send heartbeat message every 100ms */ -#define CAN_ID_HEARTBEAT 0x080 /* CAN ID of heartbeat messages (see spreadsheet) */ -#define CAN_MODULE_ID 0x05 /* Module ID of this system (see spreadsheet) */ +uint64_t time_last_heartbeat = 0; /* Timestamp of last heartbeat message that was sent */ +#define HEARTBEAT_PERIOD 100 /* Send heartbeat message every 100ms */ +#define HEARTBEAT_STATUS_OK 0x0 /* Everything is allright */ +#define HEARTBEAT_STATUS_SAFESTATE 0x1 /* I'm in a safe state */ +#define CAN_ID_HEARTBEAT 0x080 /* CAN ID of heartbeat messages (see spreadsheet) */ +#define CAN_MODULE_ID 0x05 /* Module ID of this system (see spreadsheet) */ /* CAN config continued */ -#define CAN_ID_APPS 0x110 /* CAN ID of APPS values (not using Module ID of APPS) */ -#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_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) */ /* Torque Controller variables */ -uint8_t apps_value = 0; /* Value of the APPS sensor (0-255) */ -char rtd_state = 0; /* Whether or not RTD is actived */ +#define TC_OK 0b0 +#define TC_SAFE_STATE 0b1 +char tc_state = TC_OK; /* Status of the Torque Controller */ +uint8_t apps_value = 0; /* Value of the APPS sensor (0-255) */ +uint8_t bpps_value = 0; /* Value of the BPPS sensor (0-255) */ +uint64_t time_last_apps_bpps = 0; /* Timestamp of last message for APPS and BPPS values */ + +#define RTD_INACTIVE 0b0 +#define RTD_ACTIVE 0b1 +char rtd_state = RTD_INACTIVE; /* Whether or not RTD is actived */ +uint64_t time_last_rtd_state = 0; /* Timestamp of last message for Boot Sequence State stuff */ + +uint64_t time_last_can_mc_send_enable = 0; /* Timestamp of last enable/disable message that was sent to motorcontrollers (because they have timeout safety shit) */ +uint64_t time_last_can_mc_send_erpm = 0; /* Timestamp of last erpm message that was sent to motorcontrollers (to limit amount of messages) */ +#define CAN_MC_ERPM_PERIOD 25 /* Time between ERPM messages */ void handle_can_lv_message(const CAN_message_t &msg) { /* Callback for LV CAN messages */ - if ((msg.id & 0xff0) == CAN_ID_APPS) { + if ((msg.id & 0xff0) == CAN_ID_CRITICAL_FAULT) { + /* Enter safe state */ + enter_safe_state(FAULT_EXTERNAL_CRITICAL); + } + else if ((msg.id & 0xff0) == CAN_ID_APPS_BPPS) { /* APPS value */ apps_value = msg.buf[0]; - time_last_apps = millis(); + bpps_value = msg.buf[1]; + time_last_apps_bpps = millis(); } else if ((msg.id & 0xfff) == CAN_ID_BOOT_SEQ) { /* Boot Sequence Status */ char bs = msg.buf[0]; - rtd_state = (bs == 5) ? 0b1 : 0b0; + rtd_state = (bs == 5) ? RTD_ACTIVE : RTD_INACTIVE; time_last_rtd_state = millis(); } } @@ -68,44 +95,49 @@ void can_setup() { can_mc.onReceive(handle_can_mc_message); } +/* CAN MC subroutines */ +void can_mc_send_drive_allowed(char allowed) { + /* Enable or disable left drive */ + CAN_message_t ml; + ml.flags.extended = 1; + ml.id = CAN_MC_ID_ENABLE << 8 | MC_L_ID; + ml.buf[0] = allowed; + can_mc.write(ml); -// ID bij ons was 79 = 0x4F + /* Enable or disable right drive */ + CAN_message_t mr; + mr.flags.extended = 1; + mr.id = CAN_MC_ID_ENABLE << 8 | MC_R_ID; + mr.buf[0] = allowed; + can_mc.write(mr); -void sendEnable() { - CAN_message_t msg_DriveEnable; - msg_DriveEnable.flags.extended = 1; - msg_DriveEnable.id = 0x24 << 8 | 0x4F; // INVERTER ID INSTELLEN VOLGENS VOLGEND FORMAT: 0x24 << 8 | 0x"ID" - msg_DriveEnable.buf[0] = 1; - msg_DriveEnable.buf[1] = 255; - msg_DriveEnable.buf[2] = 0xFF; - msg_DriveEnable.buf[3] = 0xFF; - msg_DriveEnable.buf[4] = 0xFF; - msg_DriveEnable.buf[5] = 0xFF; - msg_DriveEnable.buf[6] = 0xFF; - msg_DriveEnable.buf[7] = 0xFF; - Can0.write(msg_DriveEnable); - Serial.println(); + /* Update state */ + time_last_can_mc_send_enable = millis() } +void can_mc_send_erpm(int32_t erpm_l, int32_t erpm_r) { + /* Send ERPM to left drive */ + CAN_message_t ml; + ml.flags.extended = 1; + ml.id = CAN_MC_ID_ERPM << 8 | MC_L_ID; + ml.buf[0] = (erpm_l >> 24) & 0xff; + ml.buf[1] = (erpm_l >> 16) & 0xff; + ml.buf[2] = (erpm_l >> 8) & 0xff; + ml.buf[3] = erpm_l & 0xff; + can_mc.write(ml); -void sendERPM() { - if ( millis() - time_last_update_can > 100 ) { - - sendEnable(); - Serial.print("Enable"); + /* Send ERPM to right drive */ + CAN_message_t mr; + mr.flags.extended = 1; + mr.id = CAN_MC_ID_ERPM << 8 | MC_R_ID; + ml.buf[0] = (erpm_r >> 24) & 0xff; + ml.buf[1] = (erpm_r >> 16) & 0xff; + ml.buf[2] = (erpm_r >> 8) & 0xff; + ml.buf[3] = erpm_r & 0xff; + can_mc.write(mr); - CAN_message_t msg_ERPM; - msg_ERPM.flags.extended = 1; - msg_ERPM.id = 0x1C << 8 | 0x4F; // INVERTER ID INSTELLEN VOLGENS VOLGEND FORMAT: 0x1C << 8 | 0x"ID" - msg_ERPM.buf[0] = (speed >> 24); - msg_ERPM.buf[1] = (speed >> 16) & 0xFF; - msg_ERPM.buf[2] = (speed >> 8) & 0xFF; - msg_ERPM.buf[3] = speed & 0xFF; - Can0.write(msg_ERPM); - Serial.println("ERPM"); - printMessage(msg_ERPM); - time_last_update_can = millis(); - } + /* Update state */ + time_last_can_mc_send_erpm = millis() } /* Subroutine to check if new heartbeat message needs to be sent and transmits it if needed */ @@ -116,7 +148,7 @@ void check_heartbeat() { /* Send heartbeat */ CAN_message_t m; m.id = CAN_ID_HEARTBEAT | CAN_MODULE_ID; - m.buf[0] = HEARTBEAT_STATUS_OK; + m.buf[0] = (tc_state == TC_SAFE_STATE) ? HEARTBEAT_STATUS_SAFESTATE : HEARTBEAT_STATUS_OK; can_lv.write(m); /* Update stats */ @@ -125,23 +157,33 @@ void check_heartbeat() { /* Subroutine to update invertors */ void update_drives() { - /* TODO: Return if not needed yet */ - //if (millis() < time_last_heartbeat + HEARTBEAT_PERIOD) return; + /* Check timeouts */ + if (millis() > time_last_rtd + RTD_TIMEOUT) enter_safe_state(FAULT_RTD_TIMEOUT); + if (millis() > time_last_apps_bpps + APPS_BPPS_TIMEOUT) enter_safe_state(FAULT_APPS_BPPS_TIMEOUT); + + /* Check if needs to send enable message */ + if (millis() > time_last_can_mc_send_enable + CAN_MC_TIMEOUT-50) { + /* Send enable signal to prevent safety timeout */ + char allowed = (tc_state == TC_OK && rtd_state == RTD_ACTIVE) ? 0b1 : 0b0; + can_mc_send_drive_allowed(allowed); + } + + /* Only continue if not recent enough */ + if (millis() < time_last_can_mc_send_erpm + CAN_MC_ERPM_PERIOD) return; /* Update speed */ - uint8_t speed = apps_value; + int32_t rpm; + if (rtd_state != RTD_ACTIVE) rpm = 0; + else if (tc_state != TC_OK) rpm = 0; + else if (apps_value < APPS_THRESHOLD) rpm = 0; + else { + /* Calculate RPM from APPS value (TODO: optimize this) */ + rpm = apps_value * 20; + } + int32_t erpm = rpm * 10; /* ERPM = RPM * Motor Poles, for Emrax this is equal to 10 */ - /* Check if data is recent enough */ - //if (millis() > time_last_apps - - /* Send heartbeat */ - CAN_message_t m; - m.id = CAN_ID_HEARTBEAT | CAN_MODULE_ID; - m.buf[0] = HEARTBEAT_STATUS_OK; - can_lv.write(m); - - /* Update stats */ - time_last_heartbeat = millis(); + /* Send message */ + can_mc_send_erpm(erpm, erpm); } void setup() {