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

finished code to process apps and update erpm

This commit is contained in:
bitscuit 2023-04-22 12:14:24 +02:00
parent b4eebea7de
commit 7c6b8c6404
1 changed files with 103 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

164
src/main.cpp
View File

@ -5,42 +5,69 @@
void sendEnable(); // function to turn the inverter on DON'T FORGET TO EDIT THE ID OF THE DRIVE 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 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 */ /* CAN variables */
FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can_lv; /* LV CAN bus is connected to controller 1 */ FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can_lv; /* LV CAN bus is connected to controller 1 */
FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_16> can_mc; /* Motorcontroller/Invertor CAN bus is connected to controller 2 */ FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_16> 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_LV_BAUD 1000000 /* Data rate of the LV CAN = 1Mbps */
#define CAN_MC_BAUD 500000 /* Data rate of the MC CAN = 500kbps */ #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 */ /* CAN heartbeat stuff */
uint64_t time_last_heartbeat = 0; /* Timestamp of last heartbeat message that was sent */ 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_PERIOD 100 /* Send heartbeat message every 100ms */
#define CAN_ID_HEARTBEAT 0x080 /* CAN ID of heartbeat messages (see spreadsheet) */ #define HEARTBEAT_STATUS_OK 0x0 /* Everything is allright */
#define CAN_MODULE_ID 0x05 /* Module ID of this system (see spreadsheet) */ #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 */ /* CAN config continued */
#define CAN_ID_APPS 0x110 /* CAN ID of APPS values (not using Module ID of APPS) */ #define CAN_ID_CRITICAL_FAULT 0x000 /* CAN ID of critical fault message (not using any Module ID) */
#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_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 */ /* Torque Controller variables */
uint8_t apps_value = 0; /* Value of the APPS sensor (0-255) */ #define TC_OK 0b0
char rtd_state = 0; /* Whether or not RTD is actived */ #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) { void handle_can_lv_message(const CAN_message_t &msg) {
/* Callback for LV CAN messages */ /* 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 */
apps_value = msg.buf[0]; 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) { else if ((msg.id & 0xfff) == CAN_ID_BOOT_SEQ) {
/* Boot Sequence Status */ /* Boot Sequence Status */
char bs = msg.buf[0]; char bs = msg.buf[0];
rtd_state = (bs == 5) ? 0b1 : 0b0; rtd_state = (bs == 5) ? RTD_ACTIVE : RTD_INACTIVE;
time_last_rtd_state = millis(); time_last_rtd_state = millis();
} }
} }
@ -68,44 +95,49 @@ void can_setup() {
can_mc.onReceive(handle_can_mc_message); 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() { /* Update state */
CAN_message_t msg_DriveEnable; time_last_can_mc_send_enable = millis()
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();
} }
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() { /* Send ERPM to right drive */
if ( millis() - time_last_update_can > 100 ) { 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);
sendEnable(); /* Update state */
Serial.print("Enable"); time_last_can_mc_send_erpm = millis()
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();
}
} }
/* Subroutine to check if new heartbeat message needs to be sent and transmits it if needed */ /* 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 */ /* Send heartbeat */
CAN_message_t m; CAN_message_t m;
m.id = CAN_ID_HEARTBEAT | CAN_MODULE_ID; 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); can_lv.write(m);
/* Update stats */ /* Update stats */
@ -125,23 +157,33 @@ void check_heartbeat() {
/* Subroutine to update invertors */ /* Subroutine to update invertors */
void update_drives() { void update_drives() {
/* TODO: Return if not needed yet */ /* Check timeouts */
//if (millis() < time_last_heartbeat + HEARTBEAT_PERIOD) return; 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 */ /* 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 */ /* Send message */
//if (millis() > time_last_apps can_mc_send_erpm(erpm, erpm);
/* 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();
} }
void setup() { void setup() {