finished code to process apps and update erpm
This commit is contained in:
parent
b4eebea7de
commit
7c6b8c6404
164
src/main.cpp
164
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<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 */
|
||||
#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() {
|
||||
|
|
|
|||
Loading…
Reference in New Issue