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

added handling of MC CAN messsages

This commit is contained in:
bitscuit 2023-04-22 14:13:50 +02:00
parent 7c6b8c6404
commit c2d6166e9f
1 changed files with 144 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
src/main.cpp
View File

@ -11,8 +11,13 @@ void sendERPM(); // function to change the speed DON'T FORGET TO EDIT THE ID OF
#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 */
#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 */
}