added handling of MC CAN messsages
This commit is contained in:
parent
7c6b8c6404
commit
c2d6166e9f
142
src/main.cpp
142
src/main.cpp
|
@ -13,6 +13,11 @@ void sendERPM(); // function to change the speed DON'T FORGET TO EDIT THE ID OF
|
||||||
/* Invertor CAN packet IDs */
|
/* Invertor CAN packet IDs */
|
||||||
#define CAN_MC_ID_ERPM 0x03 /* Message to set ERPM */
|
#define CAN_MC_ID_ERPM 0x03 /* Message to set ERPM */
|
||||||
#define CAN_MC_ID_ENABLE 0x0C /* Message to enable/disable drive */
|
#define CAN_MC_ID_ENABLE 0x0C /* Message to enable/disable drive */
|
||||||
|
#define CAN_MC_ID_GEN_DATA_1 0x20 /* Message that contains ERPM, duty cycle and input voltage of drives */
|
||||||
|
#define CAN_MC_ID_GEN_DATA_2 0x21 /* Message that contains AC and DC current of drives */
|
||||||
|
#define CAN_MC_ID_GEN_DATA_3 0x22 /* Message that contains temperature and fault codes of drives */
|
||||||
|
#define CAN_MC_ID_GEN_DATA_4 0x23 /* Message that contains FOC algorithm values of drives */
|
||||||
|
#define CAN_MC_ID_GEN_DATA_5 0x24 /* Message that contains various signals of drives */
|
||||||
|
|
||||||
/* 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 */
|
||||||
|
@ -51,6 +56,58 @@ uint64_t time_last_can_mc_send_enable = 0; /* Timestamp of last enable/disable
|
||||||
uint64_t time_last_can_mc_send_erpm = 0; /* Timestamp of last erpm message that was sent to motorcontrollers (to limit amount of messages) */
|
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 */
|
#define CAN_MC_ERPM_PERIOD 25 /* Time between ERPM messages */
|
||||||
|
|
||||||
|
/* Drive data (see datasheet or CAN manual) */
|
||||||
|
int32_t mcl_erpm, mcr_erpm;
|
||||||
|
int16_t mcl_duty, mcr_duty;
|
||||||
|
int16_t mcl_voltage, mcr_voltage;
|
||||||
|
int16_t mcl_ac_current, mcr_ac_current;
|
||||||
|
int16_t mcl_dc_current, mcr_dc_current;
|
||||||
|
int16_t mcl_ctrl_temp, mcr_ctrl_temp;
|
||||||
|
int16_t mcl_motor_temp, mcr_motor_temp;
|
||||||
|
char mcl_fault_code, mcr_fault_code;
|
||||||
|
int32_t mcl_foc_id, mcr_foc_id;
|
||||||
|
int32_t mcl_foc_iq, mcr_foc_iq;
|
||||||
|
int8_t mcl_throttle, mcr_throttle;
|
||||||
|
int8_t mcl_brake, mcr_brake;
|
||||||
|
char mcl_gpio, mcr_gpio;
|
||||||
|
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;
|
||||||
|
uint32_t time_last_mcl_data = 0, time_last_mcr_data = 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 */
|
||||||
|
#define FAULT_EXTERNAL_CRITICAL 0x00 /* CAN critical fault from other LVS module, will not be transmitted */
|
||||||
|
#define FAULT_INV_OVERVOLT 0x01 /* Fault from invertor: overvoltage */
|
||||||
|
#define FAULT_INV_UNDERVOLT 0x02 /* Fault from invertor: undervoltage */
|
||||||
|
#define FAULT_INV_DRV 0x03 /* Fault from invertor: transistor or drive error */
|
||||||
|
#define FAULT_INV_OVERCURRENT 0x04 /* Fault from invertor: ABS overcurrent */
|
||||||
|
#define FAULT_INV_CTLR_OVERTEMP 0x05 /* Fault from invertor: controller overtemp */
|
||||||
|
#define FAULT_INV_MOTOR_OVERTEMP 0x06 /* Fault from invertor: motor overtemp */
|
||||||
|
#define FAULT_INV_SENS_WIRE 0x07 /* Fault from invertor: sensor wire fault */
|
||||||
|
#define FAULT_INV_SENS_GENERAL 0x08 /* Fault from invertor: sensor general fault */
|
||||||
|
#define FAULT_INV_CAN_ERROR 0x09 /* Fault from invertor: CAN command 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_APPS_BPPS_TIMEOUT 0x12 /* Did not receive APPS/BPPS values in short enough time, assume data loss */
|
||||||
|
|
||||||
|
/* SAFETY */
|
||||||
|
void enter_safe_state(char fault_id) {
|
||||||
|
/* Set all state variables to safe value */
|
||||||
|
tc_state = TC_SAFE_STATE;
|
||||||
|
rtd_state = RTD_INACTIVE;
|
||||||
|
|
||||||
|
/* Send information to LVS */
|
||||||
|
if (fault_id == FAULT_EXTERNAL_CRITICAL) return; /* Do not resend fault that is not ours */
|
||||||
|
CAN_message_t m;
|
||||||
|
m.id = CAN_ID_CRITICAL_FAULT | CAN_MODULE_ID;
|
||||||
|
m.buf[0] = CAN_FAULT_ID;
|
||||||
|
m.buf[1] = fault_id;
|
||||||
|
can_lv.write(m);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* CAN */
|
||||||
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 */
|
||||||
|
|
||||||
|
@ -74,6 +131,89 @@ void handle_can_lv_message(const CAN_message_t &msg) {
|
||||||
|
|
||||||
void handle_can_mc_message(const CAN_message_t &msg) {
|
void handle_can_mc_message(const CAN_message_t &msg) {
|
||||||
/* Callback for MC CAN messages */
|
/* Callback for MC CAN messages */
|
||||||
|
|
||||||
|
uint8_t node_id = (msg.id & 0xff);
|
||||||
|
uint8_t packet_id = ((msg.id >> 8) & 0xff);
|
||||||
|
if (node_id == MC_L_ID) {
|
||||||
|
/* Left invertor */
|
||||||
|
if (packet_id == CAN_MC_ID_GEN_DATA_1) {
|
||||||
|
/* Parse data */
|
||||||
|
mcl_erpm = (msg.buf[0] << 24) | (msg.buf[1] << 16) | (msg.buf[2] << 8) | (msg.buf[3]);
|
||||||
|
mcl_duty = (msg.buf[4] << 8) | (msg.buf[5]);
|
||||||
|
mcl_voltage = (msg.buf[6] << 8) | (msg.buf[7]);
|
||||||
|
time_last_mcl_data = millis();
|
||||||
|
}
|
||||||
|
else if (packet_id == CAN_MC_ID_GEN_DATA_2) {
|
||||||
|
/* Parse data */
|
||||||
|
mcl_ac_current = (msg.buf[0] << 8) | (msg.buf[1]);
|
||||||
|
mcl_dc_current = (msg.buf[2] << 8) | (msg.buf[3]);
|
||||||
|
time_last_mcl_data = millis();
|
||||||
|
}
|
||||||
|
else if (packet_id == CAN_MC_ID_GEN_DATA_3) {
|
||||||
|
/* Parse data */
|
||||||
|
mcl_ctrl_temp = (msg.buf[0] << 8) | (msg.buf[1]);
|
||||||
|
mcl_motor_temp = (msg.buf[2] << 8) | (msg.buf[3]);
|
||||||
|
mcl_fault_code = msg.buf[4];
|
||||||
|
time_last_mcl_data = millis();
|
||||||
|
}
|
||||||
|
else if (packet_id == CAN_MC_ID_GEN_DATA_4) {
|
||||||
|
/* Parse data */
|
||||||
|
mcl_foc_id = (msg.buf[0] << 24) | (msg.buf[1] << 16) | (msg.buf[2] << 8) | (msg.buf[3]);
|
||||||
|
mcl_foc_iq = (msg.buf[4] << 24) | (msg.buf[5] << 16) | (msg.buf[6] << 8) | (msg.buf[7]);
|
||||||
|
time_last_mcl_data = millis();
|
||||||
|
}
|
||||||
|
else if (packet_id == CAN_MC_ID_GEN_DATA_5) {
|
||||||
|
/* Parse data */
|
||||||
|
mcl_throttle = msg.buf[0];
|
||||||
|
mcl_brake = msg.buf[1];
|
||||||
|
mcl_gpio = msg.buf[2];
|
||||||
|
mcl_enabled = msg.buf[3];
|
||||||
|
mcl_limits_1 = msg.buf[4];
|
||||||
|
mcl_limits_2 = msg.buf[5];
|
||||||
|
mcl_can_version = msg.buf[7];
|
||||||
|
time_last_mcl_data = millis();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (node_id == MC_R_ID) {
|
||||||
|
/* Right invertor */
|
||||||
|
if (packet_id == CAN_MC_ID_GEN_DATA_1) {
|
||||||
|
/* Parse data */
|
||||||
|
mcr_erpm = (msg.buf[0] << 24) | (msg.buf[1] << 16) | (msg.buf[2] << 8) | (msg.buf[3]);
|
||||||
|
mcr_duty = (msg.buf[4] << 8) | (msg.buf[5]);
|
||||||
|
mcr_voltage = (msg.buf[6] << 8) | (msg.buf[7]);
|
||||||
|
time_last_mcr_data = millis();
|
||||||
|
}
|
||||||
|
else if (packet_id == CAN_MC_ID_GEN_DATA_2) {
|
||||||
|
/* Parse data */
|
||||||
|
mcr_ac_current = (msg.buf[0] << 8) | (msg.buf[1]);
|
||||||
|
mcr_dc_current = (msg.buf[2] << 8) | (msg.buf[3]);
|
||||||
|
time_last_mcr_data = millis();
|
||||||
|
}
|
||||||
|
else if (packet_id == CAN_MC_ID_GEN_DATA_3) {
|
||||||
|
/* Parse data */
|
||||||
|
mcr_ctrl_temp = (msg.buf[0] << 8) | (msg.buf[1]);
|
||||||
|
mcr_motor_temp = (msg.buf[2] << 8) | (msg.buf[3]);
|
||||||
|
mcr_fault_code = msg.buf[4];
|
||||||
|
time_last_mcr_data = millis();
|
||||||
|
}
|
||||||
|
else if (packet_id == CAN_MC_ID_GEN_DATA_4) {
|
||||||
|
/* Parse data */
|
||||||
|
mcr_foc_id = (msg.buf[0] << 24) | (msg.buf[1] << 16) | (msg.buf[2] << 8) | (msg.buf[3]);
|
||||||
|
mcr_foc_iq = (msg.buf[4] << 24) | (msg.buf[5] << 16) | (msg.buf[6] << 8) | (msg.buf[7]);
|
||||||
|
time_last_mcr_data = millis();
|
||||||
|
}
|
||||||
|
else if (packet_id == CAN_MC_ID_GEN_DATA_5) {
|
||||||
|
/* Parse data */
|
||||||
|
mcr_throttle = msg.buf[0];
|
||||||
|
mcr_brake = msg.buf[1];
|
||||||
|
mcr_gpio = msg.buf[2];
|
||||||
|
mcr_enabled = msg.buf[3];
|
||||||
|
mcr_limits_1 = msg.buf[4];
|
||||||
|
mcr_limits_2 = msg.buf[5];
|
||||||
|
mcr_can_version = msg.buf[7];
|
||||||
|
time_last_mcr_data = millis();
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -197,5 +337,7 @@ void loop() {
|
||||||
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 */
|
update_drives(); /* Do motorcontroller shit */
|
||||||
|
|
||||||
|
/* TODO: check drive faults */
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue