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 */
|
||||
#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_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 */
|
||||
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) */
|
||||
#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) {
|
||||
/* 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) {
|
||||
/* 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 */
|
||||
check_heartbeat(); /* Do heartbeat stuffz */
|
||||
update_drives(); /* Do motorcontroller shit */
|
||||
|
||||
/* TODO: check drive faults */
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue