diff --git a/CMakeLists.txt b/CMakeLists.txt index 2436e264..7e40e7a4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,7 +58,7 @@ add_subdirectory(common/common_defs) add_subdirectory(common/log) add_subdirectory(common/faults) add_subdirectory(common/daq) -add_subdirectory(common/plettenberg) +add_subdirectory(common/amk) add_subdirectory(common/modules/wheel_speeds) add_subdirectory(common/modules/Wiznet) diff --git a/common/amk/CMakeLists.txt b/common/amk/CMakeLists.txt new file mode 100644 index 00000000..f9cbef87 --- /dev/null +++ b/common/amk/CMakeLists.txt @@ -0,0 +1,13 @@ +set(TARGET_NAME AMK) +add_library(${TARGET_NAME}) + +# Find all .c sources in project +#file(GLOB_RECURSE glob_sources "*.c") +target_sources(${TARGET_NAME} PRIVATE "amk.c") + +# Find directories for '#include' +# For libraries, these directories are all referenced to the top level firmware directory, CMAKE_SOURCE_DIR +target_include_directories(${TARGET_NAME} PUBLIC ${CMAKE_SOURCE_DIR}) + +target_link_libraries(${TARGET_NAME} "QUEUE;common_defs;PHAL_F407") + diff --git a/common/amk/amk.c b/common/amk/amk.c new file mode 100644 index 00000000..ea7f5cef --- /dev/null +++ b/common/amk/amk.c @@ -0,0 +1,429 @@ +#include "amk.h" +#include "source/main_module/can/can_parse.h" + +/* NOTE: + * Step 1 is turning on LV + * Step 3 happens when HV is turned on and precharging starts (I think this is actually step 2) + * I can check when this is done with precharge complete and then move + * onto other steps + * MAYBE we can check when AMK_bSystemReady is on and display a message + * on LCD so they know when to turn on the HV to start the precharging. + * Not sure if I need that + */ + +static void turnMotorOn(amk_motor_t* motor); +static void motorRunning(amk_motor_t* motor); +static void turnMotorOff(amk_motor_t* motor); +static void motorGetData(amk_motor_t* motor); +static void motorReset(amk_motor_t* motor); + +/* NOTE: As of now this is just setting everything to 0, but it may make sense + * to have it in case something changes down the line while I learn more, so + * this may end up being deleted if everything just inits to 0 */ +void motorInit(amk_motor_t* motor, bool* pchg_complete) +{ + *motor = (amk_motor_t){ + /* States */ + .states.stage = MOTOR_STAGE_INIT, + .states.init_stage = MOTOR_INIT_POWER_ON, + .states.deinit_stage = MOTOR_DEINIT_SETPOINTS_DEINIT, + .states.running_stage = MOTOR_RUNNING_GOOD, + .states.reset_state = MOTOR_RESET_INVERTER_OFF, + + /* Values */ + .torque_setpoint = DEFAULT_TORQUE_SETPOINT, + .torque_limit_positive = DEFAULT_POSITIVE_TORQUE_LIMIT, + .torque_limit_negative = DEFAULT_NEGATIVE_TORQUE_LIMIT, + + .pchg_complete = pchg_complete + }; +} + +void amkCanTesting(amk_motor_t* motor) +{ + SEND_AMK_TESTING(motor->states.init_stage, motor->control.bits, motor->status.bits, *motor->pchg_complete); +} + +/* TODO: Determine period of this. Should be pretty often of course. The control + * word needs to be sent every 50ms or the motors will shut off. Plettenberg did + * every 15ms so maybe we will just do that? + */ +void motorPeriodic(amk_motor_t* motor) +{ + motorGetData(motor); + + amkCanTesting(motor); + + switch(motor->states.stage) { + case MOTOR_STAGE_INIT: + turnMotorOn(motor); + break; + case MOTOR_STAGE_RUNNING: + motorRunning(motor); + break; + case MOTOR_STAGE_DEINIT: + turnMotorOff(motor); + break; + } +} + +/* Sets the torque setpoint from -1000% to 1000% of nominal torque. + * But this function just takes in -100% to 100% to stay consistent */ +void motorSetTorque(amk_motor_t* motor, int16_t torque_setpoint) +{ + if (torque_setpoint > MAX_POSITIVE_TORQUE_SETPOINT + || torque_setpoint < MAX_NEGATIVE_TORQUE_SETPOINT) { + return; + } + + /* Scale up since unit is 0.1% of nominal torque */ + torque_setpoint *= 10; + + motor->torque_setpoint = torque_setpoint; + + if (torque_setpoint < 0) { + motor->torque_limit_negative = -100; + motor->torque_limit_positive = 0; + } else { + /* FIXME: Was told 9.8Nm is nominal, and to set to limit to 17Nm for now, + * but in the mean time I am going to go less than 100% as I am unsure */ + motor->torque_limit_positive = 500; + + /* FIXME: Should this actually be negative? */ + /* NOTE: For some reason it cannot be 0, so do -0.1% (according to UIUC's team) */ + motor->torque_limit_negative = -1; + } + +} + +static void motorGetData(amk_motor_t* motor) +{ + if (!can_data.AMK_Actual_Values_1.stale) { + motor->status.bits = can_data.AMK_Actual_Values_1.AMK_Status; + motor->actual_torque = can_data.AMK_Actual_Values_1.AMK_ActualTorque; + motor->serial_num = can_data.AMK_Actual_Values_1.AMK_MotorSerialNumber; + } + + if (!can_data.AMK_Actual_Values_2.stale) { + motor->actual_speed = can_data.AMK_Actual_Values_2.AMK_ActualSpeed; + motor->dc_bus_voltage = can_data.AMK_Actual_Values_2.AMK_DCBusVoltage; + motor->system_reset = can_data.AMK_Actual_Values_2.AMK_SystemReset; + /* FIXME: Will this grab diagnostic number in time? It should? */ + motor->diagnostic_num = can_data.AMK_Actual_Values_2.AMK_DiagnosticNumber; + } + + if (!can_data.AMK_Temperatures_1.stale) { + motor->motor_temp = can_data.AMK_Temperatures_1.AMK_MotorTemp; + motor->inverter_temp = can_data.AMK_Temperatures_1.AMK_InverterTemp; + motor->igbt_temp = can_data.AMK_Temperatures_1.AMK_IGBTTemp; + } +} + +static void motorReset(amk_motor_t* motor) +{ + /* State machine here based on 8.2.6 */ + switch (motor->states.reset_state) { + case MOTOR_RESET_INVERTER_OFF: + /* 1. Set AMK_bInverterOn = 0 */ + motor->control.fields.AMK_bInverterOn = false; + + SEND_AMK_SETPOINTS(motor->control.bits, + motor->torque_setpoint, + motor->torque_limit_positive, + motor->torque_limit_negative); + + motor->states.reset_state = MOTOR_RESET_ERROR_RESET_ON; + + break; + case MOTOR_RESET_ERROR_RESET_ON: + motor->control.fields.AMK_bErrorReset = true; + + SEND_AMK_SETPOINTS(motor->control.bits, + 0, + 0, + 0); + + motor->states.reset_state = MOTOR_RESET_ERROR_RESET_OFF; + + break; + case MOTOR_RESET_ERROR_RESET_OFF: + motor->control.fields.AMK_bErrorReset = false; + + SEND_AMK_SETPOINTS(motor->control.bits, + 0, + 0, + 0); + + motor->states.reset_state = MOTOR_RESET_CHECK_SYSTEM_READY; + + break; + case MOTOR_RESET_CHECK_SYSTEM_READY: + if (motor->status.fields.AMK_bSystemReady) { + motor->states.reset_state = MOTOR_RESET_INVERTER_OFF; + motor->states.running_stage = MOTOR_RUNNING_GOOD; + } + + break; + } +} + +/* TODO: Not really sure what needs to be done here. We just need to push + * these values that are determined in car.c state machine (READY2DRIVE state) + */ +static void motorRunning(amk_motor_t* motor) +{ + switch (motor->states.running_stage) { + case MOTOR_RUNNING_GOOD: + /* Set setpoints as needed */ + SEND_AMK_SETPOINTS(motor->control.bits, + motor->torque_setpoint, + motor->torque_limit_positive, + motor->torque_limit_negative); + + /* Check for errors */ + if (motor->status.fields.AMK_bError) { + motor->states.running_stage = MOTOR_RUNNING_ERROR; + } + + break; + case MOTOR_RUNNING_ERROR: + /* FIXME: We should also send a message to dash, so we can see + * when this happens, or do we just latch an error in the fault + * library? */ + motorReset(motor); + + break; + } +} + +static void turnMotorOn(amk_motor_t* motor) +{ + /* + * Motor Datasheet: + * https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/pdk_205481_kw26-s5-fse-4q_en_.pdf + * + * Section 9.4 goes over turning the motors on and off + * + * Steps with the "r" suffix are requirement steps, the requirement needs to + * be met before moving onto the next step. + */ + + switch (motor->states.init_stage) { + case MOTOR_INIT_POWER_ON: + /* 1. Turn on 24V DC to inverters */ + /* 1r. Check AMK_bSystemReady = 1*/ + if (motor->status.fields.AMK_bSystemReady) { + motor->states.init_stage = MOTOR_INIT_PRECHARGE; + } + + break; + case MOTOR_INIT_PRECHARGE: + /* 2. Charge DC caps; QUE should be set (is this just DcOn?) */ + /* This step happens when HV turns on. I can check the precharge + * complete GPIO pin to see when this is finished. When finished + * I move onto the next state. */ + + /* if precharge complete pin is high */ + /* NOTE: This is found for us in car.c. Can check the pin ourselves + * if we should not be touching this struct outside of car.c */ + if (*motor->pchg_complete) { + motor->states.init_stage = MOTOR_INIT_DC_ON; + } + + break; + case MOTOR_INIT_DC_ON: + /* 3. Set AMK_bDcOn = 1 */ + motor->control.fields.AMK_bDcOn = true; + + SEND_AMK_SETPOINTS(motor->control.bits, + motor->torque_setpoint, + motor->torque_limit_positive, + motor->torque_limit_negative); + + motor->states.init_stage = MOTOR_INIT_DC_ON_CHECK; + + break; + case MOTOR_INIT_DC_ON_CHECK: + /* 3r. AMK_bDcOn is mirrored in AMK_Status, so should be on there */ + /* When will AMK_bQuitDcOn go on? Does it take some time after + * DcOn is set?? */ + /* 3r. (QUE & AMK_bDcOn) -> Check AMK_bQuitDcOn = 1 */ + /* Does where do I check QUE??? */ + + if (motor->status.fields.AMK_bQuitDcOn) { + motor->states.init_stage = MOTOR_INIT_TORQUE_INIT; + } + + break; + case MOTOR_INIT_TORQUE_INIT: + /* 4. Set AMK_TorqueLimitNegativ = 0 and AMK_TorqueLimitPositiv = 0 */ + /* Should already be done, just doing again to confirm */ + motor->torque_limit_positive = 0; + motor->torque_limit_negative = 0; + + /* Says I also need to set act speed to 0? */ + SEND_AMK_SETPOINTS(motor->control.bits, + motor->torque_setpoint, + motor->torque_limit_positive, + motor->torque_limit_negative); + + motor->states.init_stage = MOTOR_INIT_ENABLE; + + break; + case MOTOR_INIT_ENABLE: + /* 7. Set AMK_bEnable = 1 */ + motor->control.fields.AMK_bEnable = true; + + SEND_AMK_SETPOINTS(motor->control.bits, + motor->torque_setpoint, + motor->torque_limit_positive, + motor->torque_limit_negative); + + motor->states.init_stage = MOTOR_INIT_INVERTER_ON; + + break; + case MOTOR_INIT_INVERTER_ON: + /* 8 Set AMK_bInverterOn = 1 */ + motor->control.fields.AMK_bInverterOn = true; + + SEND_AMK_SETPOINTS(motor->control.bits, + motor->torque_setpoint, + motor->torque_limit_positive, + motor->torque_limit_negative); + + motor->states.init_stage = MOTOR_INIT_INVERTER_ON_CHECK; + + break; + case MOTOR_INIT_INVERTER_ON_CHECK: + /* 8r. AMK_bInverterOn is mirrored in AMK_Status, so should be on there */ + /* Same with AMK_bQuitDcOn, do we need seperate states for these quits?? */ + /* 9. Check AMK_bQuitInverterOn = 1 */ + + /* This should be the last init state, so now we move onto the stage for + * running the motors */ + if (motor->status.fields.AMK_bQuitInverterOn) { + motor->states.init_stage = MOTOR_INIT_DONE; + motor->states.stage = MOTOR_STAGE_RUNNING; + } + + break; + } +} + +static void turnMotorOff(amk_motor_t* motor) +{ + /* + * Motor Datasheet: + * https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/pdk_205481_kw26-s5-fse-4q_en_.pdf + * + * Section 9.4 goes over turning the motors on and off + * + * Steps with the "r" suffix are requirement steps, the requirement needs to + * be met before moving onto the next step. + */ + + /* NOTE: For some reason in the deinit state machine, we have to turn the + * inverter off, then disable, and then check for the inverter being off. + * This is different to how it is done in the init state machine but it + * is correct based on the datasheet. + */ + + switch(motor->states.deinit_stage) { + case MOTOR_DEINIT_SETPOINTS_DEINIT: + /* 1. Set setpoint settings to 0 (AMK_TargetVelocity, AMK_TorqueLimitNegativ, AMK_TorqueLimitPositiv) */ + motor->torque_setpoint = 0; + motor->torque_limit_positive = 0; + motor->torque_limit_negative = 0; + + SEND_AMK_SETPOINTS(motor->control.bits, + motor->torque_setpoint, + motor->torque_limit_positive, + motor->torque_limit_negative); + + motor->states.deinit_stage = MOTOR_DEINIT_INVERTER_OFF; + + break; + + case MOTOR_DEINIT_INVERTER_OFF: + /* 2. Set AMK_bInverterOn = 0 */ + motor->control.fields.AMK_bInverterOn = false; + + SEND_AMK_SETPOINTS(motor->control.bits, + motor->torque_setpoint, + motor->torque_limit_positive, + motor->torque_limit_negative); + + motor->states.deinit_stage = MOTOR_DEINIT_DISABLE; + + break; + case MOTOR_DEINIT_DISABLE: + /* 3. Set AMK_bEnable = 0 */ + + motor->control.fields.AMK_bEnable = false; + + SEND_AMK_SETPOINTS(motor->control.bits, + motor->torque_setpoint, + motor->torque_limit_positive, + motor->torque_limit_negative); + + motor->states.deinit_stage = MOTOR_DEINIT_QUIT_INVERTER_CHECK; + + break; + case MOTOR_DEINIT_QUIT_INVERTER_CHECK: + /* 4. Check AMK_bQuitInverterOn = 0 */ + if (!(motor->status.fields.AMK_bQuitInverterOn)) { + motor->states.deinit_stage = MOTOR_DEINIT_DISABLE; + } + + motor->states.deinit_stage = MOTOR_DEINIT_DC_OFF; + + break; + case MOTOR_DEINIT_DC_OFF: + /* 5. Set AMK_bDcOn = 0 */ + + motor->control.fields.AMK_bDcOn = false; + + SEND_AMK_SETPOINTS(motor->control.bits, + motor->torque_setpoint, + motor->torque_limit_positive, + motor->torque_limit_negative); + + motor->states.deinit_stage = MOTOR_DEINIT_DC_OFF_CHECK; + + break; + case MOTOR_DEINIT_DC_OFF_CHECK: + /* 6r. AMK_bDcOn is mirrored in AMK_Status, so should be on there */ + /* 6r. Check AMK_bQuitDcOn = 0 */ + /* When will AMK_bQuitDcOn go on? Does it take some time after + * DcOn is set?? */ + /* 3r. (QUE & AMK_bDcOn) -> Check AMK_bQuitDcOn = 1 */ + /* Does where do I check QUE??? */ + + if (!(motor->status.fields.AMK_bQuitDcOn)) { + motor->states.init_stage = MOTOR_INIT_TORQUE_INIT; + } + + motor->states.deinit_stage = MOTOR_DEINIT_PRECHARGE; + + break; + case MOTOR_DEINIT_PRECHARGE: + /* 7. Discharge DC caps; QUE should be reset (is this just DcOn?) */ + + /* FIXME: Will this work? Not sure if this goes low when discharged */ + if (!(*motor->pchg_complete)) { + motor->states.init_stage = MOTOR_DEINIT_POWER_OFF; + } + + /* If discharged, move on */ + motor->states.deinit_stage = MOTOR_DEINIT_POWER_OFF; + + break; + case MOTOR_DEINIT_POWER_OFF: + /* 8. Turn off 24v DC to inverters */ + /* We don't do much here, just have to turn off the car I guess */ + motor->states.deinit_stage = MOTOR_DEINIT_DONE; + motor->states.stage = MOTOR_STAGE_OFF; + + break; + } +} diff --git a/common/amk/amk.h b/common/amk/amk.h new file mode 100644 index 00000000..bb1b7fd4 --- /dev/null +++ b/common/amk/amk.h @@ -0,0 +1,144 @@ +/** + * @file amk.h + * @author Cole Roberts (rober638@purdue.edu) + * @brief Vroom + * @version 0.1 + * @date 2024-10-11 + * + * @copyright Copyright (c) 2024 + * + */ +#ifndef _AMK_H_ +#define _AMK_H_ + +#include +#include + +/* Inverter -> CAN */ +/* In AMK_Actual_Values_1 */ +typedef union +{ + struct { + uint16_t AMK_bReserve : 8; + uint16_t AMK_bSystemReady : 1; + uint16_t AMK_bError : 1; + uint16_t AMK_bWarn : 1; + uint16_t AMK_bQuitDcOn : 1; + uint16_t AMK_bDcOn : 1; /* Same as QUE ?? */ + uint16_t AMK_bQuitInverterOn : 1; + uint16_t AMK_bInverterOn : 1; + uint16_t AMK_bDerating : 1; + } fields; + uint16_t bits; +} AMK_Status_t; + +/* CAN -> Inverter */ +/* In AMK_Setpoints */ +/* THIS NEEDS TO BE SENT EVERY 50ms */ +typedef union +{ + struct { + uint16_t AMK_bReserve1 : 8; + uint16_t AMK_bInverterOn : 1; + uint16_t AMK_bDcOn : 1; + uint16_t AMK_bEnable : 1; + uint16_t AMK_bErrorReset : 1; + uint16_t AMK_bReserve2 : 1; + } fields; + uint16_t bits; +} AMK_Control_t; + +typedef struct { + uint8_t stage; + uint8_t running_stage; + uint8_t init_stage; + uint8_t deinit_stage; + uint8_t reset_state; +} amk_motor_states_t; + +typedef struct { + amk_motor_states_t states; + AMK_Status_t status; + AMK_Control_t control; + + /* Scaling: 0.1% */ + int16_t torque_setpoint; + int16_t torque_limit_positive; + int16_t torque_limit_negative; + + /* Scaling: 0.1% */ + int16_t actual_torque; + int16_t actual_speed; + + uint32_t serial_num; // for sanity checking + + /* Scaling: 0.1% */ + int16_t motor_temp; + int16_t inverter_temp; + int16_t igbt_temp; + + uint16_t dc_bus_voltage; + uint16_t system_reset; + + uint16_t diagnostic_num; + + bool* pchg_complete; +} amk_motor_t; + +void motorInit(amk_motor_t* motor, bool* pchg_complete); +void motorPeriodic(amk_motor_t* motor); +void motorSetTorque(amk_motor_t* motor, int16_t torque_setpoint); + +typedef enum { + MOTOR_INIT_POWER_ON = 0, + MOTOR_INIT_PRECHARGE, + MOTOR_INIT_DC_ON, + MOTOR_INIT_DC_ON_CHECK, + MOTOR_INIT_TORQUE_INIT, + MOTOR_INIT_ENABLE, + MOTOR_INIT_INVERTER_ON, + MOTOR_INIT_INVERTER_ON_CHECK, + MOTOR_INIT_DONE, +} amk_motor_init_state_t; + +typedef enum { + MOTOR_DEINIT_SETPOINTS_DEINIT = 0, + MOTOR_DEINIT_INVERTER_OFF, + MOTOR_DEINIT_DISABLE, + MOTOR_DEINIT_QUIT_INVERTER_CHECK, + MOTOR_DEINIT_DC_OFF, + MOTOR_DEINIT_DC_OFF_CHECK, + MOTOR_DEINIT_PRECHARGE, + MOTOR_DEINIT_POWER_OFF, + MOTOR_DEINIT_DONE, +} amk_motor_deinit_state_t; + +typedef enum { + MOTOR_RUNNING_GOOD = 0, + MOTOR_RUNNING_ERROR, +} amk_motor_running_state_t; + +typedef enum { + MOTOR_RESET_INVERTER_OFF = 0, + MOTOR_RESET_ERROR_RESET_ON, + MOTOR_RESET_ERROR_RESET_OFF, + MOTOR_RESET_CHECK_SYSTEM_READY, +} amk_motor_reset_state_t; + +typedef enum { + MOTOR_STAGE_OFF = 0, + MOTOR_STAGE_INIT, + MOTOR_STAGE_RUNNING, + MOTOR_STAGE_DEINIT +} amk_motor_stage_t; + +#define DEFAULT_TORQUE_SETPOINT (0) +#define DEFAULT_POSITIVE_TORQUE_LIMIT (0) +#define DEFAULT_NEGATIVE_TORQUE_LIMIT (0) + +#define MAX_POSITIVE_TORQUE_SETPOINT (1000) +#define MAX_NEGATIVE_TORQUE_SETPOINT (-1000) +#define MAX_POSITIVE_TORQUE_LIMIT (1000) +#define MAX_NEGATIVE_TORQUE_LIMIT (-1000) + +#endif /* _AMK_H_ */ diff --git a/common/daq/can_config.json b/common/daq/can_config.json index 89a5eba5..8959f019 100644 --- a/common/daq/can_config.json +++ b/common/daq/can_config.json @@ -174,10 +174,12 @@ "msg_name": "rear_motor_temps", "msg_desc": "Temperature of motor controllers and motors", "signals": [ - {"sig_name": "left_mot_temp", "type":"uint8_t", "length": 8, "unit": "C"}, - {"sig_name": "right_mot_temp", "type":"uint8_t", "length": 8, "unit": "C"}, - {"sig_name": "left_ctrl_temp", "type":"uint8_t", "length":8, "unit": "C"}, - {"sig_name": "right_ctrl_temp", "type":"uint8_t", "length":8, "unit": "C"} + {"sig_name": "left_mot_temp", "type":"uint8_t", "length": 8, "unit": "C", "scale":0.1}, + {"sig_name": "right_mot_temp", "type":"uint8_t", "length": 8, "unit": "C", "scale":0.1}, + {"sig_name": "left_inv_temp", "type":"uint8_t", "length":8, "unit": "C", "scale":0.1}, + {"sig_name": "right_inv_temp", "type":"uint8_t", "length":8, "unit": "C", "scale":0.1}, + {"sig_name": "left_igbt_temp", "type":"uint8_t", "length":8, "unit": "C", "scale":0.1}, + {"sig_name": "right_igbt_temp", "type":"uint8_t", "length":8, "unit": "C", "scale":0.1} ], "msg_period": 1000, "msg_hlp": 4, @@ -195,6 +197,30 @@ "msg_period": 15, "msg_hlp": 1, "msg_pgn": 14 + }, + { + "msg_name": "AMK_Setpoints", + "msg_desc": "Contains control word and setpoints", + "signals": [ + {"sig_name": "AMK_Control", "sig_desc": "Control word", "type":"uint16_t"}, + {"sig_name": "AMK_TorqueSetpoint", "sig_desc": "Torque setpoint", "type":"int16_t", "unit":"Nm", "scale":0.1}, + {"sig_name": "AMK_PositiveTorqueLimit", "sig_desc": "Positive Torque Limit", "type":"int16_t", "unit":"Nm", "scale":0.1}, + {"sig_name": "AMK_NegativeTorqueLimit", "sig_desc": "Negative Torque Limit", "type":"int16_t", "unit":"Nm", "scale":0.1} + ], + "msg_period": 5, + "msg_id_override": "0x182" + }, + { + "msg_name": "AMK_Testing", + "msg_desc": "Contains control word and setpoints", + "signals": [ + {"sig_name": "AMK_InitStage", "sig_desc": "init stage", "type":"uint8_t"}, + {"sig_name": "AMK_Control", "sig_desc": "Control word", "type":"uint16_t"}, + {"sig_name": "AMK_Status_from_motor", "sig_desc": "Control word", "type":"uint16_t"}, + {"sig_name": "precharge", "sig_desc": "Control word", "type":"uint8_t"} + ], + "msg_period": 5, + "msg_id_override": "0x384" } ], "rx":[ @@ -206,7 +232,52 @@ {"msg_name": "main_module_bl_cmd" , "callback": true}, {"msg_name": "orion_currents_volts"}, {"msg_name": "throttle_vcu"}, - {"msg_name": "throttle_vcu_equal"} + {"msg_name": "throttle_vcu_equal"}, + { + "msg_name": "AMK_Actual_Values_1", + "msg_desc": "Contains the status word and actual values", + "signals": [ + {"sig_name": "AMK_Status", "sig_desc": "Status word, bit field", "type":"uint16_t"}, + {"sig_name": "AMK_ActualTorque", "sig_desc": "Actual torque value", "type":"int16_t", "unit":"Nm", "scale":0.1}, + {"sig_name": "AMK_MotorSerialNumber", "sig_desc": "Serial number of the motor", "type":"uint32_t"} + ], + "msg_period": 5, + "msg_id_override": "0x282" + }, + { + "msg_name": "AMK_Actual_Values_2", + "msg_desc": "Contains actual values", + "signals": [ + {"sig_name": "AMK_ActualSpeed", "sig_desc": "Actual speed value", "type":"int16_t", "unit": "rpm"}, + {"sig_name": "AMK_DCBusVoltage", "sig_desc": "Voltage of the DC bus", "type":"uint16_t", "unit": "V"}, + {"sig_name": "AMK_SystemReset", "sig_desc": "System reset", "type":"uint16_t"}, + {"sig_name": "AMK_DiagnosticNumber", "sig_desc": "Error number", "type":"uint16_t"} + ], + "msg_period": 5, + "msg_id_override": "0x284" + }, + { + "msg_name": "AMK_Temperatures_1", + "msg_desc": "Contains temperatures", + "signals": [ + {"sig_name": "AMK_MotorTemp", "sig_desc": "Motor temperature", "type":"int16_t", "unit":"C", "scale":0.1}, + {"sig_name": "AMK_InverterTemp", "sig_desc": "Inverter cold plate temperature", "type":"int16_t", "unit":"C", "scale":0.1}, + {"sig_name": "AMK_IGBTTemp", "sig_desc": "IGBT temperature", "type":"int16_t", "unit":"C", "scale":0.1} + ], + "msg_period": 5, + "msg_id_override": "0x286" + }, + { + "msg_name": "AMK_Temperatures_2", + "msg_desc": "Contains temperatures", + "signals": [ + {"sig_name": "AMK_InternalTemp", "sig_desc": "Internal temperature", "type":"int16_t", "unit":"C", "scale":0.1}, + {"sig_name": "AMK_ExternalTemp", "sig_desc": "External temperature", "type":"int16_t", "unit":"C", "scale":0.1}, + {"sig_name": "AMK_TempSensorMotor", "sig_desc": "???", "type":"int16_t", "unit":"C", "scale":0.1} + ], + "msg_period": 5, + "msg_id_override": "0x288" + } ] }, { @@ -997,6 +1068,83 @@ {"msg_name": "daq_bl_cmd" , "callback": true} ] }, + { + "node_name": "Inverter1", + "node_ssa": 63, + "tx": [ + { + "msg_name": "AMK_Actual_Values_1", + "msg_desc": "Contains the status word and actual values", + "signals": [ + {"sig_name": "AMK_Status", "sig_desc": "Status word, bit field", "type":"uint16_t"}, + {"sig_name": "AMK_ActualTorque", "sig_desc": "Actual torque value", "type":"int16_t", "unit":"Nm", "scale":0.1}, + {"sig_name": "AMK_MotorSerialNumber", "sig_desc": "Serial number of the motor", "type":"uint32_t"} + ], + "msg_period": 5, + "msg_id_override": "0x282" + }, + { + "msg_name": "AMK_Actual_Values_2", + "msg_desc": "Contains actual values", + "signals": [ + {"sig_name": "AMK_ActualSpeed", "sig_desc": "Actual speed value", "type":"int16_t", "unit": "rpm"}, + {"sig_name": "AMK_DCBusVoltage", "sig_desc": "Voltage of the DC bus", "type":"uint16_t", "unit": "V"}, + {"sig_name": "AMK_SystemReset", "sig_desc": "System reset", "type":"uint16_t"}, + {"sig_name": "AMK_DiagnosticNumber", "sig_desc": "Error number", "type":"uint16_t"} + ], + "msg_period": 5, + "msg_id_override": "0x284" + }, + { + "msg_name": "AMK_Temperatures_1", + "msg_desc": "Contains temperatures", + "signals": [ + {"sig_name": "AMK_MotorTemp", "sig_desc": "Motor temperature", "type":"int16_t", "unit":"C", "scale":0.1}, + {"sig_name": "AMK_InverterTemp", "sig_desc": "Inverter cold plate temperature", "type":"int16_t", "unit":"C", "scale":0.1}, + {"sig_name": "AMK_IGBTTemp", "sig_desc": "IGBT temperature", "type":"int16_t", "unit":"C", "scale":0.1} + ], + "msg_period": 5, + "msg_id_override": "0x286" + }, + { + "msg_name": "AMK_Temperatures_2", + "msg_desc": "Contains temperatures", + "signals": [ + {"sig_name": "AMK_InternalTemp", "sig_desc": "Internal temperature", "type":"int16_t", "unit":"C", "scale":0.1}, + {"sig_name": "AMK_ExternalTemp", "sig_desc": "External temperature", "type":"int16_t", "unit":"C", "scale":0.1}, + {"sig_name": "AMK_TempSensorMotor", "sig_desc": "???", "type":"int16_t", "unit":"C", "scale":0.1} + ], + "msg_period": 5, + "msg_id_override": "0x288" + } + ], + "rx": [ + { + "msg_name": "AMK_Setpoints", + "msg_desc": "Contains control word and setpoints", + "signals": [ + {"sig_name": "AMK_Control", "sig_desc": "Control word", "type":"uint16_t"}, + {"sig_name": "AMK_TorqueSetpoint", "sig_desc": "Torque setpoint", "type":"int16_t", "unit":"Nm", "scale":1}, + {"sig_name": "AMK_PositiveTorqueLimit", "sig_desc": "Positive Torque Limit", "type":"int16_t", "unit":"Nm", "scale":0.1}, + {"sig_name": "AMK_NegativeTorqueLimit", "sig_desc": "Negative Torque Limit", "type":"int16_t", "unit":"Nm", "scale":0.1} + ], + "msg_period": 5, + "msg_id_override": "0x182" + }, + { + "msg_name": "AMK_Testing", + "msg_desc": "Contains control word and setpoints", + "signals": [ + {"sig_name": "AMK_InitStage", "sig_desc": "init stage", "type":"uint8_t", "choices":["power_on","percharge","dc_on","dc_on_check"]}, + {"sig_name": "AMK_Control", "sig_desc": "Control word", "type":"uint16_t"}, + {"sig_name": "AMK_Status_from_motor", "sig_desc": "Control word", "type":"uint16_t"}, + {"sig_name": "precharge", "sig_desc": "Control word", "type":"uint8_t"} + ], + "msg_period": 5, + "msg_id_override": "0x384" + } + ] + }, { "node_name":"BITSTREAM", "node_ssa":62, diff --git a/common/daq/can_parse_base.c b/common/daq/can_parse_base.c index 4bc682a0..749c1cf1 100644 --- a/common/daq/can_parse_base.c +++ b/common/daq/can_parse_base.c @@ -8,16 +8,21 @@ can_stats_t can_stats; // TODO: support CAN2 +// CAN2 cannot happen in here, move it over to MAIN stuff q_handle_t q_tx_can1_s[CAN_TX_MAILBOX_CNT]; +q_handle_t q_tx_can2_s[CAN_TX_MAILBOX_CNT]; q_handle_t q_rx_can; -uint32_t mbx_last_send_time[CAN_TX_MAILBOX_CNT]; +uint32_t can1_mbx_last_send_time[CAN_TX_MAILBOX_CNT]; +uint32_t can2_mbx_last_send_time[CAN_TX_MAILBOX_CNT]; void initCANParseBase() { for (uint8_t i = 0; i < CAN_TX_MAILBOX_CNT; ++i) { qConstruct(&q_tx_can1_s[i], sizeof(CanMsgTypeDef_t)); - mbx_last_send_time[i] = 0; + qConstruct(&q_tx_can2_s[i], sizeof(CanMsgTypeDef_t)); + can1_mbx_last_send_time[i] = 0; + can2_mbx_last_send_time[i] = 0; } qConstruct(&q_rx_can, sizeof(CanMsgTypeDef_t)); can_stats = (can_stats_t){0}; @@ -33,20 +38,38 @@ void canTxSendToBack(CanMsgTypeDef_t *msg) { case 0: case 1: - qh = &q_tx_can1_s[0]; + if (msg->Bus == CAN1) { + qh = &q_tx_can1_s[0]; + } else { + qh = &q_tx_can2_s[0]; + } break; case 2: case 3: - qh = &q_tx_can1_s[1]; + if (msg->Bus == CAN1) { + qh = &q_tx_can1_s[1]; + } else { + qh = &q_tx_can2_s[1]; + } break; default: - qh = &q_tx_can1_s[2]; + if (msg->Bus == CAN1) { + qh = &q_tx_can1_s[2]; + } else { + qh = &q_tx_can2_s[2]; + } break; } } else { - qh = &q_tx_can1_s[0]; // IDE = 0 doesn't have an HLP + // IDE = 0 doesn't have an HLP + if (msg->Bus == CAN1) { + qh = &q_tx_can1_s[0]; + } else { + qh = &q_tx_can2_s[0]; + } + // qh = &q_tx_can1_s[0]; // IDE = 0 doesn't have an HLP } if (qSendToBack(qh, msg) != SUCCESS_G) { @@ -66,10 +89,10 @@ void canTxUpdate(void) if (qReceive(&q_tx_can1_s[i], &tx_msg) == SUCCESS_G) // Check queue for items and take if there is one { PHAL_txCANMessage(&tx_msg, i); - mbx_last_send_time[i] = sched.os_ticks; + can1_mbx_last_send_time[i] = sched.os_ticks; } } - else if (sched.os_ticks - mbx_last_send_time[i] > CAN_TX_TIMEOUT_MS) + else if (sched.os_ticks - can1_mbx_last_send_time[i] > CAN_TX_TIMEOUT_MS) { PHAL_txCANAbort(CAN1, i); // aborts tx and empties the mailbox can_stats.tx_fail++; @@ -124,4 +147,4 @@ void canParseIRQHandler(CAN_TypeDef *can_h) can_stats.rx_of++; } } -} \ No newline at end of file +} diff --git a/common/daq/can_parse_base.h b/common/daq/can_parse_base.h index 76373e85..df7b3817 100644 --- a/common/daq/can_parse_base.h +++ b/common/daq/can_parse_base.h @@ -33,6 +33,7 @@ typedef struct { extern can_stats_t can_stats; extern q_handle_t q_tx_can1_s[CAN_TX_MAILBOX_CNT ]; +extern q_handle_t q_tx_can2_s[CAN_TX_MAILBOX_CNT ]; extern q_handle_t q_rx_can; void initCANParseBase(); diff --git a/common/daq/per_dbc.dbc b/common/daq/per_dbc.dbc index 4af356c4..1a0b6f63 100644 --- a/common/daq/per_dbc.dbc +++ b/common/daq/per_dbc.dbc @@ -33,7 +33,7 @@ NS_ : BS_: -BU_: Main_Module a_box OrionBMS torque_vector Dashboard Steering PDU daq bootloader BITSTREAM Charger DAQ TEST_NODE TEST_NODE_2 DAQ +BU_: Main_Module a_box OrionBMS torque_vector Dashboard Steering PDU daq bootloader Inverter1 BITSTREAM Charger DAQ TEST_NODE TEST_NODE_2 DAQ BO_ 2348816641 main_hb: 2 Main_Module @@ -107,11 +107,13 @@ BO_ 2348811137 sdc_status: 2 Main_Module SG_ BMS : 1|1@1+ (1,0) [0|0] "" Vector__XXX SG_ IMD : 0|1@1+ (1,0) [0|0] "" Vector__XXX -BO_ 2415919873 rear_motor_temps: 4 Main_Module - SG_ right_ctrl_temp : 24|8@1+ (1,0) [0|0] "C" Vector__XXX - SG_ left_ctrl_temp : 16|8@1+ (1,0) [0|0] "C" Vector__XXX - SG_ right_mot_temp : 8|8@1+ (1,0) [0|0] "C" Vector__XXX - SG_ left_mot_temp : 0|8@1+ (1,0) [0|0] "C" Vector__XXX +BO_ 2415919873 rear_motor_temps: 6 Main_Module + SG_ right_igbt_temp : 40|8@1+ (0.1,0) [0|0] "C" Vector__XXX + SG_ left_igbt_temp : 32|8@1+ (0.1,0) [0|0] "C" Vector__XXX + SG_ right_inv_temp : 24|8@1+ (0.1,0) [0|0] "C" Vector__XXX + SG_ left_inv_temp : 16|8@1+ (0.1,0) [0|0] "C" Vector__XXX + SG_ right_mot_temp : 8|8@1+ (0.1,0) [0|0] "C" Vector__XXX + SG_ left_mot_temp : 0|8@1+ (0.1,0) [0|0] "C" Vector__XXX BO_ 2214593409 rear_wheel_speeds: 8 Main_Module SG_ right_speed_sensor : 48|16@1+ (0.01,0) [0|0] "rad/s" Vector__XXX @@ -119,6 +121,18 @@ BO_ 2214593409 rear_wheel_speeds: 8 Main_Module SG_ right_speed_mc : 16|16@1+ (1,0) [0|0] "RPM" Vector__XXX SG_ left_speed_mc : 0|16@1+ (1,0) [0|0] "RPM" Vector__XXX +BO_ 2147484034 AMK_Setpoints: 8 Main_Module + SG_ AMK_NegativeTorqueLimit : 48|16@1- (0.1,0) [0|0] "Nm" Vector__XXX + SG_ AMK_PositiveTorqueLimit : 32|16@1- (0.1,0) [0|0] "Nm" Vector__XXX + SG_ AMK_TorqueSetpoint : 16|16@1- (0.1,0) [0|0] "Nm" Vector__XXX + SG_ AMK_Control : 0|16@1+ (1,0) [0|0] "" Vector__XXX + +BO_ 2147484548 AMK_Testing: 6 Main_Module + SG_ precharge : 40|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ AMK_Status_from_motor : 24|16@1+ (1,0) [0|0] "" Vector__XXX + SG_ AMK_Control : 8|16@1+ (1,0) [0|0] "" Vector__XXX + SG_ AMK_InitStage : 0|8@1+ (1,0) [0|0] "" Vector__XXX + BO_ 2148059649 fault_sync_main_module: 3 Main_Module SG_ latched : 16|1@1+ (1,0) [0|0] "" Vector__XXX SG_ idx : 0|16@1+ (1,0) [0|0] "" Vector__XXX @@ -468,6 +482,27 @@ BO_ 2217793084 daq_bl_resp: 5 bootloader SG_ data : 8|32@1+ (1,0) [0|0] "" Vector__XXX SG_ cmd : 0|8@1+ (1,0) [0|0] "" Vector__XXX +BO_ 2147484290 AMK_Actual_Values_1: 8 Inverter1 + SG_ AMK_MotorSerialNumber : 32|32@1+ (1,0) [0|0] "" Vector__XXX + SG_ AMK_ActualTorque : 16|16@1- (0.1,0) [0|0] "Nm" Vector__XXX + SG_ AMK_Status : 0|16@1+ (1,0) [0|0] "" Vector__XXX + +BO_ 2147484292 AMK_Actual_Values_2: 8 Inverter1 + SG_ AMK_DiagnosticNumber : 48|16@1+ (1,0) [0|0] "" Vector__XXX + SG_ AMK_SystemReset : 32|16@1+ (1,0) [0|0] "" Vector__XXX + SG_ AMK_DCBusVoltage : 16|16@1+ (1,0) [0|0] "V" Vector__XXX + SG_ AMK_ActualSpeed : 0|16@1- (1,0) [0|0] "rpm" Vector__XXX + +BO_ 2147484294 AMK_Temperatures_1: 6 Inverter1 + SG_ AMK_IGBTTemp : 32|16@1- (0.1,0) [0|0] "C" Vector__XXX + SG_ AMK_InverterTemp : 16|16@1- (0.1,0) [0|0] "C" Vector__XXX + SG_ AMK_MotorTemp : 0|16@1- (0.1,0) [0|0] "C" Vector__XXX + +BO_ 2147484296 AMK_Temperatures_2: 6 Inverter1 + SG_ AMK_TempSensorMotor : 32|16@1- (0.1,0) [0|0] "C" Vector__XXX + SG_ AMK_ExternalTemp : 16|16@1- (0.1,0) [0|0] "C" Vector__XXX + SG_ AMK_InternalTemp : 0|16@1- (0.1,0) [0|0] "C" Vector__XXX + BO_ 2214598974 bitstream_data: 8 BITSTREAM SG_ d7 : 56|8@1+ (1,0) [0|0] "" Vector__XXX SG_ d6 : 48|8@1+ (1,0) [0|0] "" Vector__XXX @@ -628,6 +663,7 @@ CM_ BU_ Steering ""; CM_ BU_ PDU ""; CM_ BU_ daq ""; CM_ BU_ bootloader ""; +CM_ BU_ Inverter1 ""; CM_ BU_ BITSTREAM ""; CM_ BU_ Charger ""; CM_ BU_ DAQ ""; @@ -699,8 +735,10 @@ CM_ SG_ 2348811137 BSPD ""; CM_ SG_ 2348811137 BMS ""; CM_ SG_ 2348811137 IMD ""; CM_ BO_ 2415919873 "Temperature of motor controllers and motors"; -CM_ SG_ 2415919873 right_ctrl_temp ""; -CM_ SG_ 2415919873 left_ctrl_temp ""; +CM_ SG_ 2415919873 right_igbt_temp ""; +CM_ SG_ 2415919873 left_igbt_temp ""; +CM_ SG_ 2415919873 right_inv_temp ""; +CM_ SG_ 2415919873 left_inv_temp ""; CM_ SG_ 2415919873 right_mot_temp ""; CM_ SG_ 2415919873 left_mot_temp ""; CM_ BO_ 2214593409 "Rear Wheel speeds from motor controllers"; @@ -708,6 +746,16 @@ CM_ SG_ 2214593409 right_speed_sensor ""; CM_ SG_ 2214593409 left_speed_sensor ""; CM_ SG_ 2214593409 right_speed_mc ""; CM_ SG_ 2214593409 left_speed_mc ""; +CM_ BO_ 2147484034 "Contains control word and setpoints"; +CM_ SG_ 2147484034 AMK_NegativeTorqueLimit "Negative Torque Limit"; +CM_ SG_ 2147484034 AMK_PositiveTorqueLimit "Positive Torque Limit"; +CM_ SG_ 2147484034 AMK_TorqueSetpoint "Torque setpoint"; +CM_ SG_ 2147484034 AMK_Control "Control word"; +CM_ BO_ 2147484548 "Contains control word and setpoints"; +CM_ SG_ 2147484548 precharge "Control word"; +CM_ SG_ 2147484548 AMK_Status_from_motor "Control word"; +CM_ SG_ 2147484548 AMK_Control "Control word"; +CM_ SG_ 2147484548 AMK_InitStage "init stage"; CM_ BO_ 2148059649 "Fault status message"; CM_ SG_ 2148059649 latched ""; CM_ SG_ 2148059649 idx ""; @@ -992,6 +1040,23 @@ CM_ SG_ 2217793020 cmd ""; CM_ BO_ 2217793084 "DAQ Bootloader response"; CM_ SG_ 2217793084 data ""; CM_ SG_ 2217793084 cmd ""; +CM_ BO_ 2147484290 "Contains the status word and actual values"; +CM_ SG_ 2147484290 AMK_MotorSerialNumber "Serial number of the motor"; +CM_ SG_ 2147484290 AMK_ActualTorque "Actual torque value"; +CM_ SG_ 2147484290 AMK_Status "Status word, bit field"; +CM_ BO_ 2147484292 "Contains actual values"; +CM_ SG_ 2147484292 AMK_DiagnosticNumber "Error number"; +CM_ SG_ 2147484292 AMK_SystemReset "System reset"; +CM_ SG_ 2147484292 AMK_DCBusVoltage "Voltage of the DC bus"; +CM_ SG_ 2147484292 AMK_ActualSpeed "Actual speed value"; +CM_ BO_ 2147484294 "Contains temperatures"; +CM_ SG_ 2147484294 AMK_IGBTTemp "IGBT temperature"; +CM_ SG_ 2147484294 AMK_InverterTemp "Inverter cold plate temperature"; +CM_ SG_ 2147484294 AMK_MotorTemp "Motor temperature"; +CM_ BO_ 2147484296 "Contains temperatures"; +CM_ SG_ 2147484296 AMK_TempSensorMotor "???"; +CM_ SG_ 2147484296 AMK_ExternalTemp "External temperature"; +CM_ SG_ 2147484296 AMK_InternalTemp "Internal temperature"; CM_ BO_ 2214598974 "Bitstream download command"; CM_ SG_ 2214598974 d7 "Bitstream data word 7"; CM_ SG_ 2214598974 d6 "Bitstream data word 6"; diff --git a/common/phal_F4_F7/can/can.h b/common/phal_F4_F7/can/can.h index a690da3f..d72f2389 100644 --- a/common/phal_F4_F7/can/can.h +++ b/common/phal_F4_F7/can/can.h @@ -73,4 +73,4 @@ bool PHAL_txCANMessage(CanMsgTypeDef_t* msg, uint8_t mbx); bool PHAL_txMailboxFree(CAN_TypeDef* bus, uint8_t mbx); void PHAL_txCANAbort(CAN_TypeDef* bus, uint8_t mbx); -#endif // _PHAL_CAN_H \ No newline at end of file +#endif // _PHAL_CAN_H diff --git a/source/dashboard/can/can_parse.c b/source/dashboard/can/can_parse.c index 71e6eb2d..daa9b6e0 100644 --- a/source/dashboard/can/can_parse.c +++ b/source/dashboard/can/can_parse.c @@ -120,8 +120,10 @@ void canRxUpdate() case ID_REAR_MOTOR_TEMPS: can_data.rear_motor_temps.left_mot_temp = msg_data_a->rear_motor_temps.left_mot_temp; can_data.rear_motor_temps.right_mot_temp = msg_data_a->rear_motor_temps.right_mot_temp; - can_data.rear_motor_temps.left_ctrl_temp = msg_data_a->rear_motor_temps.left_ctrl_temp; - can_data.rear_motor_temps.right_ctrl_temp = msg_data_a->rear_motor_temps.right_ctrl_temp; + can_data.rear_motor_temps.left_inv_temp = msg_data_a->rear_motor_temps.left_inv_temp; + can_data.rear_motor_temps.right_inv_temp = msg_data_a->rear_motor_temps.right_inv_temp; + can_data.rear_motor_temps.left_igbt_temp = msg_data_a->rear_motor_temps.left_igbt_temp; + can_data.rear_motor_temps.right_igbt_temp = msg_data_a->rear_motor_temps.right_igbt_temp; can_data.rear_motor_temps.stale = 0; can_data.rear_motor_temps.last_rx = sched.os_ticks; break; diff --git a/source/dashboard/can/can_parse.h b/source/dashboard/can/can_parse.h index 9d0f1351..b306e50d 100644 --- a/source/dashboard/can/can_parse.h +++ b/source/dashboard/can/can_parse.h @@ -87,7 +87,7 @@ typedef union { #define DLC_ORION_CURRENTS_VOLTS 4 #define DLC_ORION_ERRORS 4 #define DLC_MAX_CELL_TEMP 2 -#define DLC_REAR_MOTOR_TEMPS 4 +#define DLC_REAR_MOTOR_TEMPS 6 #define DLC_PRECHARGE_HB 2 #define DLC_REAR_WHEEL_SPEEDS 8 #define DLC_COOLANT_TEMPS 4 @@ -375,8 +375,10 @@ typedef union { struct { uint64_t left_mot_temp: 8; uint64_t right_mot_temp: 8; - uint64_t left_ctrl_temp: 8; - uint64_t right_ctrl_temp: 8; + uint64_t left_inv_temp: 8; + uint64_t right_inv_temp: 8; + uint64_t left_igbt_temp: 8; + uint64_t right_igbt_temp: 8; } rear_motor_temps; struct { uint64_t IMD: 8; @@ -556,8 +558,10 @@ typedef struct { struct { uint8_t left_mot_temp; uint8_t right_mot_temp; - uint8_t left_ctrl_temp; - uint8_t right_ctrl_temp; + uint8_t left_inv_temp; + uint8_t right_inv_temp; + uint8_t left_igbt_temp; + uint8_t right_igbt_temp; uint8_t stale; uint32_t last_rx; } rear_motor_temps; diff --git a/source/f4_testing/can/can_parse.c b/source/f4_testing/can/can_parse.c new file mode 100644 index 00000000..c0383077 --- /dev/null +++ b/source/f4_testing/can/can_parse.c @@ -0,0 +1,111 @@ +/** + * @file can_parse.c + * @author Cole Roberts (rober638@purdue.edu) + * @brief Parsing of CAN messages using auto-generated structures with bit-fields + * @version 0.1 + * @date 2024-09-27 + * + * @copyright Copyright (c) 2024 + * + */ +#include "can_parse.h" + +// prototypes +bool initCANFilter(); + +can_data_t can_data; +volatile uint32_t last_can_rx_time_ms = 0; + +void initCANParse(void) +{ + initCANParseBase(); + initCANFilter(); +} + +void canRxUpdate(void) +{ + CanMsgTypeDef_t msg_header; + CanParsedData_t* msg_data_a; + + if(qReceive(&q_rx_can, &msg_header) == SUCCESS_G) + { + msg_data_a = (CanParsedData_t *) &msg_header.Data; + last_can_rx_time_ms = sched.os_ticks; + /* BEGIN AUTO CASES */ + switch(msg_header.ExtId) + { + case ID_AMK_SETPOINTS: + can_data.AMK_Setpoints.AMK_Control = msg_data_a->AMK_Setpoints.AMK_Control; + can_data.AMK_Setpoints.AMK_TorqueSetpoint = (int16_t) msg_data_a->AMK_Setpoints.AMK_TorqueSetpoint; + can_data.AMK_Setpoints.AMK_PositiveTorqueLimit = (int16_t) msg_data_a->AMK_Setpoints.AMK_PositiveTorqueLimit; + can_data.AMK_Setpoints.AMK_NegativeTorqueLimit = (int16_t) msg_data_a->AMK_Setpoints.AMK_NegativeTorqueLimit; + can_data.AMK_Setpoints.stale = 0; + can_data.AMK_Setpoints.last_rx = sched.os_ticks; + break; + case ID_AMK_TESTING: + can_data.AMK_Testing.AMK_InitStage = msg_data_a->AMK_Testing.AMK_InitStage; + can_data.AMK_Testing.AMK_Control = msg_data_a->AMK_Testing.AMK_Control; + can_data.AMK_Testing.AMK_Status_from_motor = msg_data_a->AMK_Testing.AMK_Status_from_motor; + can_data.AMK_Testing.precharge = msg_data_a->AMK_Testing.precharge; + can_data.AMK_Testing.stale = 0; + can_data.AMK_Testing.last_rx = sched.os_ticks; + break; + default: + __asm__("nop"); + } + /* END AUTO CASES */ + } + + /* BEGIN AUTO STALE CHECKS */ + CHECK_STALE(can_data.AMK_Setpoints.stale, + sched.os_ticks, can_data.AMK_Setpoints.last_rx, + UP_AMK_SETPOINTS); + CHECK_STALE(can_data.AMK_Testing.stale, + sched.os_ticks, can_data.AMK_Testing.last_rx, + UP_AMK_TESTING); + /* END AUTO STALE CHECKS */ +} + +bool initCANFilter() +{ + CAN1->MCR |= CAN_MCR_INRQ; // Enter back into INIT state (required for changing scale) + uint32_t timeout = 0; + while(!(CAN1->MSR & CAN_MSR_INAK) && ++timeout < PHAL_CAN_INIT_TIMEOUT) + ; + if (timeout == PHAL_CAN_INIT_TIMEOUT) + return false; + + CAN1->FMR |= CAN_FMR_FINIT; // Enter init mode for filter banks + CAN1->FM1R |= 0x07FFFFFF; // Set banks 0-27 to id mode + CAN1->FS1R |= 0x07FFFFFF; // Set banks 0-27 to 32-bit scale + + /* BEGIN AUTO FILTER */ + CAN1->FA1R |= (1 << 0); // configure bank 0 + CAN1->sFilterRegister[0].FR1 = (ID_AMK_SETPOINTS << 3) | 4; + CAN1->sFilterRegister[0].FR2 = (ID_AMK_TESTING << 3) | 4; + /* END AUTO FILTER */ + + CAN1->FMR &= ~CAN_FMR_FINIT; // Enable Filters (exit filter init mode) + + // Enter back into NORMAL mode + CAN1->MCR &= ~CAN_MCR_INRQ; + while((CAN1->MSR & CAN_MSR_INAK) && ++timeout < PHAL_CAN_INIT_TIMEOUT) + ; + + return timeout != PHAL_CAN_INIT_TIMEOUT; +} + + +void canProcessRxIRQs(CanMsgTypeDef_t* rx) +{ + CanParsedData_t* msg_data_a; + + msg_data_a = (CanParsedData_t *) rx->Data; + switch(rx->ExtId) + { + /* BEGIN AUTO RX IRQ */ + /* END AUTO RX IRQ */ + default: + __asm__("nop"); + } +} diff --git a/source/f4_testing/can/can_parse.h b/source/f4_testing/can/can_parse.h new file mode 100644 index 00000000..6aeb8623 --- /dev/null +++ b/source/f4_testing/can/can_parse.h @@ -0,0 +1,198 @@ +/** + * @file can_parse.h + * @author Cole Roberts (rober638@purdue.edu) + * @brief Parsing of CAN messages using auto-generated structures with bit-fields + * @version 0.1 + * @date 2024-09-27 + * + * @copyright Copyright (c) 2024 + * + */ +#ifndef _CAN_PARSE_H_ +#define _CAN_PARSE_H_ + +#include "common/queue/queue.h" +#include "common/psched/psched.h" +#include "common/phal_F4_F7/can/can.h" +#include "common/daq/can_parse_base.h" + +// Make this match the node name within the can_config.json +#define NODE_NAME "Inverter1" + + +// Used to represent a float as 32 bits +typedef union { + float f; + uint32_t u; +} FloatConvert_t; +#define FLOAT_TO_UINT32(float_) (((FloatConvert_t) float_).u) +#define UINT32_TO_FLOAT(uint32_) (((FloatConvert_t) ((uint32_t) uint32_)).f) + +// Message ID definitions +/* BEGIN AUTO ID DEFS */ +#define ID_AMK_ACTUAL_VALUES_1 0x282 +#define ID_AMK_ACTUAL_VALUES_2 0x284 +#define ID_AMK_TEMPERATURES_1 0x286 +#define ID_AMK_TEMPERATURES_2 0x288 +#define ID_AMK_SETPOINTS 0x182 +#define ID_AMK_TESTING 0x384 +/* END AUTO ID DEFS */ + +// Message DLC definitions +/* BEGIN AUTO DLC DEFS */ +#define DLC_AMK_ACTUAL_VALUES_1 8 +#define DLC_AMK_ACTUAL_VALUES_2 8 +#define DLC_AMK_TEMPERATURES_1 6 +#define DLC_AMK_TEMPERATURES_2 6 +#define DLC_AMK_SETPOINTS 8 +#define DLC_AMK_TESTING 6 +/* END AUTO DLC DEFS */ + +// Message sending macros +/* BEGIN AUTO SEND MACROS */ +#define SEND_AMK_ACTUAL_VALUES_1(AMK_Status_, AMK_ActualTorque_, AMK_MotorSerialNumber_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_AMK_ACTUAL_VALUES_1, .DLC=DLC_AMK_ACTUAL_VALUES_1, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->AMK_Actual_Values_1.AMK_Status = AMK_Status_;\ + data_a->AMK_Actual_Values_1.AMK_ActualTorque = AMK_ActualTorque_;\ + data_a->AMK_Actual_Values_1.AMK_MotorSerialNumber = AMK_MotorSerialNumber_;\ + canTxSendToBack(&msg);\ + } while(0) +#define SEND_AMK_ACTUAL_VALUES_2(AMK_ActualSpeed_, AMK_DCBusVoltage_, AMK_SystemReset_, AMK_DiagnosticNumber_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_AMK_ACTUAL_VALUES_2, .DLC=DLC_AMK_ACTUAL_VALUES_2, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->AMK_Actual_Values_2.AMK_ActualSpeed = AMK_ActualSpeed_;\ + data_a->AMK_Actual_Values_2.AMK_DCBusVoltage = AMK_DCBusVoltage_;\ + data_a->AMK_Actual_Values_2.AMK_SystemReset = AMK_SystemReset_;\ + data_a->AMK_Actual_Values_2.AMK_DiagnosticNumber = AMK_DiagnosticNumber_;\ + canTxSendToBack(&msg);\ + } while(0) +#define SEND_AMK_TEMPERATURES_1(AMK_MotorTemp_, AMK_InverterTemp_, AMK_IGBTTemp_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_AMK_TEMPERATURES_1, .DLC=DLC_AMK_TEMPERATURES_1, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->AMK_Temperatures_1.AMK_MotorTemp = AMK_MotorTemp_;\ + data_a->AMK_Temperatures_1.AMK_InverterTemp = AMK_InverterTemp_;\ + data_a->AMK_Temperatures_1.AMK_IGBTTemp = AMK_IGBTTemp_;\ + canTxSendToBack(&msg);\ + } while(0) +#define SEND_AMK_TEMPERATURES_2(AMK_InternalTemp_, AMK_ExternalTemp_, AMK_TempSensorMotor_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_AMK_TEMPERATURES_2, .DLC=DLC_AMK_TEMPERATURES_2, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->AMK_Temperatures_2.AMK_InternalTemp = AMK_InternalTemp_;\ + data_a->AMK_Temperatures_2.AMK_ExternalTemp = AMK_ExternalTemp_;\ + data_a->AMK_Temperatures_2.AMK_TempSensorMotor = AMK_TempSensorMotor_;\ + canTxSendToBack(&msg);\ + } while(0) +/* END AUTO SEND MACROS */ + +// Stale Checking +#define STALE_THRESH 30 / 2 // 5 / 2 would be 250% of period +/* BEGIN AUTO UP DEFS (Update Period)*/ +#define UP_AMK_SETPOINTS 5 +#define UP_AMK_TESTING 5 +/* END AUTO UP DEFS */ + +#define CHECK_STALE(stale, curr, last, period) if(!stale && \ + (curr - last) > period * STALE_THRESH) stale = 1 + +/* BEGIN AUTO CAN ENUMERATIONS */ +/* END AUTO CAN ENUMERATIONS */ + +// Message Raw Structures +/* BEGIN AUTO MESSAGE STRUCTURE */ +typedef union { + struct { + uint64_t AMK_Status: 16; + uint64_t AMK_ActualTorque: 16; + uint64_t AMK_MotorSerialNumber: 32; + } AMK_Actual_Values_1; + struct { + uint64_t AMK_ActualSpeed: 16; + uint64_t AMK_DCBusVoltage: 16; + uint64_t AMK_SystemReset: 16; + uint64_t AMK_DiagnosticNumber: 16; + } AMK_Actual_Values_2; + struct { + uint64_t AMK_MotorTemp: 16; + uint64_t AMK_InverterTemp: 16; + uint64_t AMK_IGBTTemp: 16; + } AMK_Temperatures_1; + struct { + uint64_t AMK_InternalTemp: 16; + uint64_t AMK_ExternalTemp: 16; + uint64_t AMK_TempSensorMotor: 16; + } AMK_Temperatures_2; + struct { + uint64_t AMK_Control: 16; + uint64_t AMK_TorqueSetpoint: 16; + uint64_t AMK_PositiveTorqueLimit: 16; + uint64_t AMK_NegativeTorqueLimit: 16; + } AMK_Setpoints; + struct { + uint64_t AMK_InitStage: 8; + uint64_t AMK_Control: 16; + uint64_t AMK_Status_from_motor: 16; + uint64_t precharge: 8; + } AMK_Testing; + uint8_t raw_data[8]; +} __attribute__((packed)) CanParsedData_t; +/* END AUTO MESSAGE STRUCTURE */ + +// contains most up to date received +// type for each variable matches that defined in JSON +/* BEGIN AUTO CAN DATA STRUCTURE */ +typedef struct { + struct { + uint16_t AMK_Control; + int16_t AMK_TorqueSetpoint; + int16_t AMK_PositiveTorqueLimit; + int16_t AMK_NegativeTorqueLimit; + uint8_t stale; + uint32_t last_rx; + } AMK_Setpoints; + struct { + uint8_t AMK_InitStage; + uint16_t AMK_Control; + uint16_t AMK_Status_from_motor; + uint8_t precharge; + uint8_t stale; + uint32_t last_rx; + } AMK_Testing; +} can_data_t; +/* END AUTO CAN DATA STRUCTURE */ + +extern can_data_t can_data; +extern volatile uint32_t last_can_rx_time_ms; + +/* BEGIN AUTO EXTERN CALLBACK */ +extern void handleCallbacks(uint16_t id, bool latched); +extern void set_fault_daq(uint16_t id, bool value); +extern void return_fault_control(uint16_t id); +extern void send_fault(uint16_t id, bool latched); +/* END AUTO EXTERN CALLBACK */ + +/* BEGIN AUTO EXTERN RX IRQ */ +/* END AUTO EXTERN RX IRQ */ + +/** + * @brief Setup queue and message filtering + * + * @param q_rx_can RX buffer of CAN messages + */ +void initCANParse(void); + +/** + * @brief Pull message off of rx buffer, + * update can_data struct, + * check for stale messages + */ +void canRxUpdate(void); + +/** + * @brief Process any rx message callbacks from the CAN Rx IRQ + * + * @param rx rx data from message just recieved + */ +void canProcessRxIRQs(CanMsgTypeDef_t* rx); + +#endif diff --git a/source/f4_testing/main.c b/source/f4_testing/main.c index 88b09c1d..a39b2e24 100644 --- a/source/f4_testing/main.c +++ b/source/f4_testing/main.c @@ -4,6 +4,7 @@ #include "common/phal_F4_F7/dma/dma.h" #include "common/phal_F4_F7/spi/spi.h" #include "common/phal_F4_F7/usart/usart.h" +#include "common/phal_F4_F7/can/can.h" #include "common/psched/psched.h" #include "string.h" @@ -63,6 +64,10 @@ GPIOInitConfig_t gpio_config[] = { GPIO_INIT_AF(SPI_MOSI_PORT, SPI_MOSI_PIN, 5, GPIO_OUTPUT_HIGH_SPEED, GPIO_OUTPUT_PUSH_PULL, GPIO_INPUT_PULL_DOWN), GPIO_INIT_AF(SPI_MISO_PORT, SPI_MISO_PIN, 5, GPIO_OUTPUT_HIGH_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_OPEN_DRAIN), + // CAN + GPIO_INIT_CANRX_PA11, + GPIO_INIT_CANTX_PA12, + }; #define TargetCoreClockrateHz 16000000 @@ -138,6 +143,11 @@ int main() PHAL_startTxfer(&adc_dma_config); PHAL_startADC(ADC1); PHAL_usartRxDma(&lcd, (uint16_t *) msg, 5, 1); + if(!PHAL_initCAN(CAN1, false, VCAN_BPS)) + { + HardFault_Handler(); + } + NVIC_EnableIRQ(CAN1_RX0_IRQn); /* Task Creation */ schedInit(APB1ClockRateHz); taskCreate(ledblink, 50); @@ -204,4 +214,4 @@ void HardFault_Handler() { __asm__("nop"); } -} \ No newline at end of file +} diff --git a/source/main_module/CMakeLists.txt b/source/main_module/CMakeLists.txt index 0622fdc8..6d4644d6 100644 --- a/source/main_module/CMakeLists.txt +++ b/source/main_module/CMakeLists.txt @@ -12,6 +12,6 @@ set_target_properties(${TARGET_NAME} PROPERTIES COMPONENT_NAME ${COMPONENT_NAME} COMPONENT_DIR ${CMAKE_CURRENT_LIST_DIR} LINKER_SCRIPT "STM32F407VGTx_FLASH" - COMMON_LIBS "CMSIS_F407;PSCHED_F407;QUEUE;DAQ_F407;CAN_PARSE_F407;PHAL_F407;FAULTS_F407;PLETTENBERG;BOOTLOADER_COMMON_F407" + COMMON_LIBS "CMSIS_F407;PSCHED_F407;QUEUE;DAQ_F407;CAN_PARSE_F407;PHAL_F407;FAULTS_F407;AMK;BOOTLOADER_COMMON_F407" ) COMMON_FIRMWARE_COMPONENT(${TARGET_NAME}) diff --git a/source/main_module/can/can_parse.c b/source/main_module/can/can_parse.c index f10d03b4..1e619b4e 100644 --- a/source/main_module/can/can_parse.c +++ b/source/main_module/can/can_parse.c @@ -91,6 +91,35 @@ void canRxUpdate(void) can_data.throttle_vcu_equal.stale = 0; can_data.throttle_vcu_equal.last_rx = sched.os_ticks; break; + case ID_AMK_ACTUAL_VALUES_1: + can_data.AMK_Actual_Values_1.AMK_Status = msg_data_a->AMK_Actual_Values_1.AMK_Status; + can_data.AMK_Actual_Values_1.AMK_ActualTorque = (int16_t) msg_data_a->AMK_Actual_Values_1.AMK_ActualTorque; + can_data.AMK_Actual_Values_1.AMK_MotorSerialNumber = msg_data_a->AMK_Actual_Values_1.AMK_MotorSerialNumber; + can_data.AMK_Actual_Values_1.stale = 0; + can_data.AMK_Actual_Values_1.last_rx = sched.os_ticks; + break; + case ID_AMK_ACTUAL_VALUES_2: + can_data.AMK_Actual_Values_2.AMK_ActualSpeed = (int16_t) msg_data_a->AMK_Actual_Values_2.AMK_ActualSpeed; + can_data.AMK_Actual_Values_2.AMK_DCBusVoltage = msg_data_a->AMK_Actual_Values_2.AMK_DCBusVoltage; + can_data.AMK_Actual_Values_2.AMK_SystemReset = msg_data_a->AMK_Actual_Values_2.AMK_SystemReset; + can_data.AMK_Actual_Values_2.AMK_DiagnosticNumber = msg_data_a->AMK_Actual_Values_2.AMK_DiagnosticNumber; + can_data.AMK_Actual_Values_2.stale = 0; + can_data.AMK_Actual_Values_2.last_rx = sched.os_ticks; + break; + case ID_AMK_TEMPERATURES_1: + can_data.AMK_Temperatures_1.AMK_MotorTemp = (int16_t) msg_data_a->AMK_Temperatures_1.AMK_MotorTemp; + can_data.AMK_Temperatures_1.AMK_InverterTemp = (int16_t) msg_data_a->AMK_Temperatures_1.AMK_InverterTemp; + can_data.AMK_Temperatures_1.AMK_IGBTTemp = (int16_t) msg_data_a->AMK_Temperatures_1.AMK_IGBTTemp; + can_data.AMK_Temperatures_1.stale = 0; + can_data.AMK_Temperatures_1.last_rx = sched.os_ticks; + break; + case ID_AMK_TEMPERATURES_2: + can_data.AMK_Temperatures_2.AMK_InternalTemp = (int16_t) msg_data_a->AMK_Temperatures_2.AMK_InternalTemp; + can_data.AMK_Temperatures_2.AMK_ExternalTemp = (int16_t) msg_data_a->AMK_Temperatures_2.AMK_ExternalTemp; + can_data.AMK_Temperatures_2.AMK_TempSensorMotor = (int16_t) msg_data_a->AMK_Temperatures_2.AMK_TempSensorMotor; + can_data.AMK_Temperatures_2.stale = 0; + can_data.AMK_Temperatures_2.last_rx = sched.os_ticks; + break; case ID_FAULT_SYNC_PDU: can_data.fault_sync_pdu.idx = msg_data_a->fault_sync_pdu.idx; can_data.fault_sync_pdu.latched = msg_data_a->fault_sync_pdu.latched; @@ -157,6 +186,18 @@ void canRxUpdate(void) CHECK_STALE(can_data.throttle_vcu_equal.stale, sched.os_ticks, can_data.throttle_vcu_equal.last_rx, UP_THROTTLE_VCU_EQUAL); + CHECK_STALE(can_data.AMK_Actual_Values_1.stale, + sched.os_ticks, can_data.AMK_Actual_Values_1.last_rx, + UP_AMK_ACTUAL_VALUES_1); + CHECK_STALE(can_data.AMK_Actual_Values_2.stale, + sched.os_ticks, can_data.AMK_Actual_Values_2.last_rx, + UP_AMK_ACTUAL_VALUES_2); + CHECK_STALE(can_data.AMK_Temperatures_1.stale, + sched.os_ticks, can_data.AMK_Temperatures_1.last_rx, + UP_AMK_TEMPERATURES_1); + CHECK_STALE(can_data.AMK_Temperatures_2.stale, + sched.os_ticks, can_data.AMK_Temperatures_2.last_rx, + UP_AMK_TEMPERATURES_2); /* END AUTO STALE CHECKS */ } @@ -188,18 +229,24 @@ bool initCANFilter() CAN1->sFilterRegister[3].FR2 = (ID_THROTTLE_VCU << 3) | 4; CAN1->FA1R |= (1 << 4); // configure bank 4 CAN1->sFilterRegister[4].FR1 = (ID_THROTTLE_VCU_EQUAL << 3) | 4; - CAN1->sFilterRegister[4].FR2 = (ID_FAULT_SYNC_PDU << 3) | 4; + CAN1->sFilterRegister[4].FR2 = (ID_AMK_ACTUAL_VALUES_1 << 3) | 4; CAN1->FA1R |= (1 << 5); // configure bank 5 - CAN1->sFilterRegister[5].FR1 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4; - CAN1->sFilterRegister[5].FR2 = (ID_FAULT_SYNC_A_BOX << 3) | 4; + CAN1->sFilterRegister[5].FR1 = (ID_AMK_ACTUAL_VALUES_2 << 3) | 4; + CAN1->sFilterRegister[5].FR2 = (ID_AMK_TEMPERATURES_1 << 3) | 4; CAN1->FA1R |= (1 << 6); // configure bank 6 - CAN1->sFilterRegister[6].FR1 = (ID_FAULT_SYNC_TORQUE_VECTOR << 3) | 4; - CAN1->sFilterRegister[6].FR2 = (ID_FAULT_SYNC_TEST_NODE << 3) | 4; + CAN1->sFilterRegister[6].FR1 = (ID_AMK_TEMPERATURES_2 << 3) | 4; + CAN1->sFilterRegister[6].FR2 = (ID_FAULT_SYNC_PDU << 3) | 4; CAN1->FA1R |= (1 << 7); // configure bank 7 - CAN1->sFilterRegister[7].FR1 = (ID_SET_FAULT << 3) | 4; - CAN1->sFilterRegister[7].FR2 = (ID_RETURN_FAULT_CONTROL << 3) | 4; + CAN1->sFilterRegister[7].FR1 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4; + CAN1->sFilterRegister[7].FR2 = (ID_FAULT_SYNC_A_BOX << 3) | 4; CAN1->FA1R |= (1 << 8); // configure bank 8 - CAN1->sFilterRegister[8].FR1 = (ID_DAQ_COMMAND_MAIN_MODULE << 3) | 4; + CAN1->sFilterRegister[8].FR1 = (ID_FAULT_SYNC_TORQUE_VECTOR << 3) | 4; + CAN1->sFilterRegister[8].FR2 = (ID_FAULT_SYNC_TEST_NODE << 3) | 4; + CAN1->FA1R |= (1 << 9); // configure bank 9 + CAN1->sFilterRegister[9].FR1 = (ID_SET_FAULT << 3) | 4; + CAN1->sFilterRegister[9].FR2 = (ID_RETURN_FAULT_CONTROL << 3) | 4; + CAN1->FA1R |= (1 << 10); // configure bank 10 + CAN1->sFilterRegister[10].FR1 = (ID_DAQ_COMMAND_MAIN_MODULE << 3) | 4; /* END AUTO FILTER */ CAN1->FMR &= ~CAN_FMR_FINIT; // Enable Filters (exit filter init mode) diff --git a/source/main_module/can/can_parse.h b/source/main_module/can/can_parse.h index d1a87bc1..215c41d8 100644 --- a/source/main_module/can/can_parse.h +++ b/source/main_module/can/can_parse.h @@ -44,6 +44,8 @@ typedef union { #define ID_SDC_STATUS 0xc000381 #define ID_REAR_MOTOR_TEMPS 0x10000301 #define ID_REAR_WHEEL_SPEEDS 0x4000381 +#define ID_AMK_SETPOINTS 0x182 +#define ID_AMK_TESTING 0x384 #define ID_FAULT_SYNC_MAIN_MODULE 0x8ca01 #define ID_DAQ_RESPONSE_MAIN_MODULE 0x17ffffc1 #define ID_RAW_THROTTLE_BRAKE 0x10000285 @@ -55,6 +57,10 @@ typedef union { #define ID_ORION_CURRENTS_VOLTS 0x140006f8 #define ID_THROTTLE_VCU 0x40025b7 #define ID_THROTTLE_VCU_EQUAL 0x4002837 +#define ID_AMK_ACTUAL_VALUES_1 0x282 +#define ID_AMK_ACTUAL_VALUES_2 0x284 +#define ID_AMK_TEMPERATURES_1 0x286 +#define ID_AMK_TEMPERATURES_2 0x288 #define ID_FAULT_SYNC_PDU 0x8cb1f #define ID_FAULT_SYNC_DASHBOARD 0x8cac5 #define ID_FAULT_SYNC_A_BOX 0x8ca44 @@ -79,8 +85,10 @@ typedef union { #define DLC_REAR_MC_STATUS 6 #define DLC_REAR_MOTOR_CURRENTS_VOLTS 6 #define DLC_SDC_STATUS 2 -#define DLC_REAR_MOTOR_TEMPS 4 +#define DLC_REAR_MOTOR_TEMPS 6 #define DLC_REAR_WHEEL_SPEEDS 8 +#define DLC_AMK_SETPOINTS 8 +#define DLC_AMK_TESTING 6 #define DLC_FAULT_SYNC_MAIN_MODULE 3 #define DLC_DAQ_RESPONSE_MAIN_MODULE 8 #define DLC_RAW_THROTTLE_BRAKE 8 @@ -92,6 +100,10 @@ typedef union { #define DLC_ORION_CURRENTS_VOLTS 4 #define DLC_THROTTLE_VCU 4 #define DLC_THROTTLE_VCU_EQUAL 4 +#define DLC_AMK_ACTUAL_VALUES_1 8 +#define DLC_AMK_ACTUAL_VALUES_2 8 +#define DLC_AMK_TEMPERATURES_1 6 +#define DLC_AMK_TEMPERATURES_2 6 #define DLC_FAULT_SYNC_PDU 3 #define DLC_FAULT_SYNC_DASHBOARD 3 #define DLC_FAULT_SYNC_A_BOX 3 @@ -211,13 +223,15 @@ typedef union { data_a->sdc_status.pchg_out = pchg_out_;\ canTxSendToBack(&msg);\ } while(0) -#define SEND_REAR_MOTOR_TEMPS(left_mot_temp_, right_mot_temp_, left_ctrl_temp_, right_ctrl_temp_) do {\ +#define SEND_REAR_MOTOR_TEMPS(left_mot_temp_, right_mot_temp_, left_inv_temp_, right_inv_temp_, left_igbt_temp_, right_igbt_temp_) do {\ CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_REAR_MOTOR_TEMPS, .DLC=DLC_REAR_MOTOR_TEMPS, .IDE=1};\ CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ data_a->rear_motor_temps.left_mot_temp = left_mot_temp_;\ data_a->rear_motor_temps.right_mot_temp = right_mot_temp_;\ - data_a->rear_motor_temps.left_ctrl_temp = left_ctrl_temp_;\ - data_a->rear_motor_temps.right_ctrl_temp = right_ctrl_temp_;\ + data_a->rear_motor_temps.left_inv_temp = left_inv_temp_;\ + data_a->rear_motor_temps.right_inv_temp = right_inv_temp_;\ + data_a->rear_motor_temps.left_igbt_temp = left_igbt_temp_;\ + data_a->rear_motor_temps.right_igbt_temp = right_igbt_temp_;\ canTxSendToBack(&msg);\ } while(0) #define SEND_REAR_WHEEL_SPEEDS(left_speed_mc_, right_speed_mc_, left_speed_sensor_, right_speed_sensor_) do {\ @@ -229,6 +243,24 @@ typedef union { data_a->rear_wheel_speeds.right_speed_sensor = right_speed_sensor_;\ canTxSendToBack(&msg);\ } while(0) +#define SEND_AMK_SETPOINTS(AMK_Control_, AMK_TorqueSetpoint_, AMK_PositiveTorqueLimit_, AMK_NegativeTorqueLimit_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_AMK_SETPOINTS, .DLC=DLC_AMK_SETPOINTS, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->AMK_Setpoints.AMK_Control = AMK_Control_;\ + data_a->AMK_Setpoints.AMK_TorqueSetpoint = AMK_TorqueSetpoint_;\ + data_a->AMK_Setpoints.AMK_PositiveTorqueLimit = AMK_PositiveTorqueLimit_;\ + data_a->AMK_Setpoints.AMK_NegativeTorqueLimit = AMK_NegativeTorqueLimit_;\ + canTxSendToBack(&msg);\ + } while(0) +#define SEND_AMK_TESTING(AMK_InitStage_, AMK_Control_, AMK_Status_from_motor_, precharge_) do {\ + CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_AMK_TESTING, .DLC=DLC_AMK_TESTING, .IDE=1};\ + CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ + data_a->AMK_Testing.AMK_InitStage = AMK_InitStage_;\ + data_a->AMK_Testing.AMK_Control = AMK_Control_;\ + data_a->AMK_Testing.AMK_Status_from_motor = AMK_Status_from_motor_;\ + data_a->AMK_Testing.precharge = precharge_;\ + canTxSendToBack(&msg);\ + } while(0) #define SEND_FAULT_SYNC_MAIN_MODULE(idx_, latched_) do {\ CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_FAULT_SYNC_MAIN_MODULE, .DLC=DLC_FAULT_SYNC_MAIN_MODULE, .IDE=1};\ CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ @@ -254,6 +286,10 @@ typedef union { #define UP_ORION_CURRENTS_VOLTS 32 #define UP_THROTTLE_VCU 20 #define UP_THROTTLE_VCU_EQUAL 20 +#define UP_AMK_ACTUAL_VALUES_1 5 +#define UP_AMK_ACTUAL_VALUES_2 5 +#define UP_AMK_TEMPERATURES_1 5 +#define UP_AMK_TEMPERATURES_2 5 /* END AUTO UP DEFS */ #define CHECK_STALE(stale, curr, last, period) if(!stale && \ @@ -398,8 +434,10 @@ typedef union { struct { uint64_t left_mot_temp: 8; uint64_t right_mot_temp: 8; - uint64_t left_ctrl_temp: 8; - uint64_t right_ctrl_temp: 8; + uint64_t left_inv_temp: 8; + uint64_t right_inv_temp: 8; + uint64_t left_igbt_temp: 8; + uint64_t right_igbt_temp: 8; } rear_motor_temps; struct { uint64_t left_speed_mc: 16; @@ -407,6 +445,18 @@ typedef union { uint64_t left_speed_sensor: 16; uint64_t right_speed_sensor: 16; } rear_wheel_speeds; + struct { + uint64_t AMK_Control: 16; + uint64_t AMK_TorqueSetpoint: 16; + uint64_t AMK_PositiveTorqueLimit: 16; + uint64_t AMK_NegativeTorqueLimit: 16; + } AMK_Setpoints; + struct { + uint64_t AMK_InitStage: 8; + uint64_t AMK_Control: 16; + uint64_t AMK_Status_from_motor: 16; + uint64_t precharge: 8; + } AMK_Testing; struct { uint64_t idx: 16; uint64_t latched: 1; @@ -456,6 +506,27 @@ typedef union { uint64_t equal_k_rl: 16; uint64_t equal_k_rr: 16; } throttle_vcu_equal; + struct { + uint64_t AMK_Status: 16; + uint64_t AMK_ActualTorque: 16; + uint64_t AMK_MotorSerialNumber: 32; + } AMK_Actual_Values_1; + struct { + uint64_t AMK_ActualSpeed: 16; + uint64_t AMK_DCBusVoltage: 16; + uint64_t AMK_SystemReset: 16; + uint64_t AMK_DiagnosticNumber: 16; + } AMK_Actual_Values_2; + struct { + uint64_t AMK_MotorTemp: 16; + uint64_t AMK_InverterTemp: 16; + uint64_t AMK_IGBTTemp: 16; + } AMK_Temperatures_1; + struct { + uint64_t AMK_InternalTemp: 16; + uint64_t AMK_ExternalTemp: 16; + uint64_t AMK_TempSensorMotor: 16; + } AMK_Temperatures_2; struct { uint64_t idx: 16; uint64_t latched: 1; @@ -550,6 +621,35 @@ typedef struct { uint8_t stale; uint32_t last_rx; } throttle_vcu_equal; + struct { + uint16_t AMK_Status; + int16_t AMK_ActualTorque; + uint32_t AMK_MotorSerialNumber; + uint8_t stale; + uint32_t last_rx; + } AMK_Actual_Values_1; + struct { + int16_t AMK_ActualSpeed; + uint16_t AMK_DCBusVoltage; + uint16_t AMK_SystemReset; + uint16_t AMK_DiagnosticNumber; + uint8_t stale; + uint32_t last_rx; + } AMK_Actual_Values_2; + struct { + int16_t AMK_MotorTemp; + int16_t AMK_InverterTemp; + int16_t AMK_IGBTTemp; + uint8_t stale; + uint32_t last_rx; + } AMK_Temperatures_1; + struct { + int16_t AMK_InternalTemp; + int16_t AMK_ExternalTemp; + int16_t AMK_TempSensorMotor; + uint8_t stale; + uint32_t last_rx; + } AMK_Temperatures_2; struct { uint16_t idx; uint8_t latched; @@ -619,4 +719,4 @@ void canRxUpdate(void); */ void canProcessRxIRQs(CanMsgTypeDef_t* rx); -#endif \ No newline at end of file +#endif diff --git a/source/main_module/car/car.c b/source/main_module/car/car.c index b02ccfd8..e957ccb3 100644 --- a/source/main_module/car/car.c +++ b/source/main_module/car/car.c @@ -5,7 +5,7 @@ Car_t car; extern q_handle_t q_tx_can; extern q_handle_t q_tx_usart_l, q_tx_usart_r; -extern usart_rx_buf_t huart_l_rx_buf, huart_r_rx_buf; +// extern usart_rx_buf_t huart_l_rx_buf, huart_r_rx_buf; extern uint16_t num_failed_msgs_l, num_failed_msgs_r; extern WheelSpeeds_t wheel_speeds; // TODO: Just to remove errors for now @@ -48,9 +48,11 @@ bool carInit() const_tq_val = 0; hist_curr_idx = 0; /* Motor Controller Initialization */ - mcInit(&car.motor_l, MC_L_INVERT, &q_tx_usart_l, &huart_l_rx_buf, &car.pchg.pchg_complete); - mcInit(&car.motor_r, MC_R_INVERT, &q_tx_usart_r, &huart_r_rx_buf, &car.pchg.pchg_complete); + // mcInit(&car.motor_l, MC_L_INVERT, &q_tx_usart_l, &huart_l_rx_buf, &car.pchg.pchg_complete); + // mcInit(&car.motor_r, MC_R_INVERT, &q_tx_usart_r, &huart_r_rx_buf, &car.pchg.pchg_complete); + // motorInit(&car.motor_r, &car.pchg.pchg_complete); + motorInit(&car.motor_l, &car.pchg.pchg_complete); PHAL_writeGPIO(SDC_MUX_S0_GPIO_Port, SDC_MUX_S0_Pin, 0); PHAL_writeGPIO(SDC_MUX_S1_GPIO_Port, SDC_MUX_S1_Pin, 0); @@ -62,10 +64,10 @@ bool carInit() void carHeartbeat() { SEND_MAIN_HB(car.state, car.pchg.pchg_complete); - SEND_REAR_MC_STATUS(car.motor_l.motor_state, - car.motor_l.link_state, car.motor_l.last_link_error, - car.motor_r.motor_state, car.motor_r.link_state, - car.motor_r.last_link_error); + // SEND_REAR_MC_STATUS(car.motor_l.motor_state, + // car.motor_l.link_state, car.motor_l.last_link_error, + // car.motor_r.motor_state, car.motor_r.link_state, + // car.motor_r.last_link_error); static uint8_t n; } @@ -427,8 +429,10 @@ void carPeriodic() PHAL_writeGPIO(SDC_CTRL_GPIO_Port, SDC_CTRL_Pin, car.sdc_close); PHAL_writeGPIO(BRK_LIGHT_GPIO_Port, BRK_LIGHT_Pin, car.brake_light | daq_brake); PHAL_writeGPIO(BUZZER_GPIO_Port, BUZZER_Pin, car.buzzer); - mcSetPower(car.torque_r.torque_left, &car.motor_l); - mcSetPower(car.torque_r.torque_right, &car.motor_r); + // mcSetPower(car.torque_r.torque_left, &car.motor_l); + // mcSetPower(car.torque_r.torque_right, &car.motor_r); + // motorSetTorque(&car.motor_r, car.torque_r.torque_right); + motorSetTorque(&car.motor_l, car.torque_r.torque_left); } @@ -442,8 +446,11 @@ void parseMCDataPeriodic(void) uint16_t shock_l, shock_r; /* Update Motor Controller Data Structures */ - mcPeriodic(&car.motor_l); - mcPeriodic(&car.motor_r); + // mcPeriodic(&car.motor_l); + // mcPeriodic(&car.motor_r); + + motorPeriodic(&car.motor_l); + // motorPeriodic(&car.motor_r); // setFault(ID_LEFT_MC_CONN_FAULT, car.pchg.pchg_complete && // car.motor_l.motor_state != MC_CONNECTED); @@ -467,18 +474,19 @@ void parseMCDataPeriodic(void) // uint16_t l_speed = (wheel_speeds.l->rad_s / (2*PI)); // uint16_t r_speed = (wheel_speeds.l->rad_s / (2*PI)); wheelSpeedsPeriodic(); - SEND_REAR_WHEEL_SPEEDS(car.motor_l.rpm, car.motor_r.rpm, - wheel_speeds.left_rad_s_x100, - wheel_speeds.right_rad_s_x100); + // 2025 TODO Setup wheel speed stuff: + // SEND_REAR_WHEEL_SPEEDS(car.motor_l.rpm, car.motor_r.rpm, + // wheel_speeds.left_rad_s_x100, + // wheel_speeds.right_rad_s_x100); static uint32_t last_curr_t; - if (sched.os_ticks - last_curr_t >= 100) - { - SEND_REAR_MOTOR_CURRENTS_VOLTS( - (uint16_t) car.motor_l.current_x10, - (uint16_t) car.motor_r.current_x10, - (uint16_t) car.motor_r.voltage_x10); - last_curr_t = sched.os_ticks; - } + // if (sched.os_ticks - last_curr_t >= 100) + // { + // SEND_REAR_MOTOR_CURRENTS_VOLTS( + // (uint16_t) car.motor_l.current_x10, + // (uint16_t) car.motor_r.current_x10, + // (uint16_t) car.motor_r.voltage_x10); + // last_curr_t = sched.os_ticks; + // } // TODO: possibly move into cooling static uint32_t last_tmp_t; if (sched.os_ticks - last_tmp_t >= 500) @@ -486,9 +494,11 @@ void parseMCDataPeriodic(void) SEND_REAR_MOTOR_TEMPS( (uint8_t) car.motor_l.motor_temp, (uint8_t) car.motor_r.motor_temp, - (uint8_t) car.motor_l.controller_temp, - (uint8_t) car.motor_r.controller_temp); - SEND_NUM_MC_SKIPS(num_failed_msgs_r, num_failed_msgs_l); + (uint8_t) car.motor_l.inverter_temp, + (uint8_t) car.motor_r.inverter_temp, + (uint8_t) car.motor_l.igbt_temp, + (uint8_t) car.motor_r.igbt_temp); + // SEND_NUM_MC_SKIPS(num_failed_msgs_r, num_failed_msgs_l); last_tmp_t = sched.os_ticks; } } @@ -861,4 +871,4 @@ void monitorSDCPeriodic() PHAL_writeGPIO(SDC_MUX_S1_GPIO_Port, SDC_MUX_S1_Pin, (index & 0x02)); PHAL_writeGPIO(SDC_MUX_S2_GPIO_Port, SDC_MUX_S2_Pin, (index & 0x04)); PHAL_writeGPIO(SDC_MUX_S3_GPIO_Port, SDC_MUX_S3_Pin, (index & 0x08)); -} \ No newline at end of file +} diff --git a/source/main_module/car/car.h b/source/main_module/car/car.h index a4a8348d..a6ef4fd5 100644 --- a/source/main_module/car/car.h +++ b/source/main_module/car/car.h @@ -14,10 +14,11 @@ #include "can_parse.h" #include "common/faults/faults.h" #include "common/phal_F4_F7/gpio/gpio.h" -#include "common/plettenberg/plettenberg.h" +// #include "common/plettenberg/plettenberg.h" #include "common/psched/psched.h" #include "cooling.h" #include "main.h" +#include "common/amk/amk.h" #include #define BUZZER_DURATION_MS 2500 // EV.10.5: 1-3s @@ -80,8 +81,8 @@ typedef struct typedef struct { car_state_t state; - motor_t motor_l; - motor_t motor_r; + amk_motor_t motor_l; + amk_motor_t motor_r; torqueRequest_t torque_r; torqueSource_t torque_src; @@ -145,4 +146,4 @@ typedef struct __attribute__((packed)) extern sdc_nodes_t sdc_mux; -#endif \ No newline at end of file +#endif diff --git a/source/main_module/main.c b/source/main_module/main.c index 22101a64..d691ba90 100644 --- a/source/main_module/main.c +++ b/source/main_module/main.c @@ -14,6 +14,10 @@ #include "common/plettenberg/plettenberg.h" #include "common/psched/psched.h" #include "common/queue/queue.h" +#include "common/amk/amk.h" + +/* TODO: Move CAN2 stuff here since can parse base is dumb */ +#include "common/phal_F4_F7/can/can.h" /* Module Includes */ #include "car.h" @@ -32,7 +36,7 @@ GPIOInitConfig_t gpio_config[] = { GPIO_INIT_OUTPUT(BRK_LIGHT_GPIO_Port, BRK_LIGHT_Pin, GPIO_OUTPUT_LOW_SPEED), GPIO_INIT_OUTPUT(BUZZER_GPIO_Port, BUZZER_Pin, GPIO_OUTPUT_LOW_SPEED), GPIO_INIT_INPUT(BRK_BUZZER_STAT_GPIO_Port, BRK_BUZZER_STAT_Pin, GPIO_INPUT_OPEN_DRAIN), - GPIO_INIT_INPUT(TSAL_LVAL_STAT_GPIO_Port, TSAL_LVAL_STAT_Pin, GPIO_INPUT_OPEN_DRAIN), + // GPIO_INIT_INPUT(TSAL_LVAL_STAT_GPIO_Port, TSAL_LVAL_STAT_Pin, GPIO_INPUT_OPEN_DRAIN), // CAN GPIO_INIT_CANRX_PA11, @@ -66,10 +70,10 @@ GPIOInitConfig_t gpio_config[] = { GPIO_INIT_INPUT(PRCHG_STAT_GPIO_Port, PRCHG_STAT_Pin, GPIO_INPUT_OPEN_DRAIN), // Motor Controllers - GPIO_INIT_USART2TX_PA2, - GPIO_INIT_USART2RX_PA3, - GPIO_INIT_USART1TX_PA9, - GPIO_INIT_USART1RX_PA10, + // GPIO_INIT_USART2TX_PA2, + // GPIO_INIT_USART2RX_PA3, + // GPIO_INIT_USART1TX_PA9, + // GPIO_INIT_USART1RX_PA10, // Wheel Speed GPIO_INIT_AF(MOTOR_R_WS_GPIO_Port, MOTOR_R_WS_Pin, MOTOR_R_WS_AF, GPIO_OUTPUT_HIGH_SPEED, GPIO_OUTPUT_OPEN_DRAIN, GPIO_INPUT_PULL_DOWN), @@ -92,52 +96,52 @@ GPIOInitConfig_t gpio_config[] = { /* USART Configuration */ // Left Motor Controller UART -dma_init_t usart_l_tx_dma_config = USART1_TXDMA_CONT_CONFIG(NULL, 1); -dma_init_t usart_l_rx_dma_config = USART1_RXDMA_CONT_CONFIG(NULL, 2); -char usart_l_rx_array[MC_MAX_RX_LENGTH] = {'\0'}; -volatile usart_rx_buf_t huart_l_rx_buf = { - .last_msg_time = 0, .msg_size = MC_MAX_TX_LENGTH, - .last_msg_loc = 0, .last_rx_time = 0, - .rx_buf_size = MC_MAX_RX_LENGTH, .rx_buf = usart_l_rx_array -}; -usart_init_t huart_l = { - .baud_rate = 115200, - .word_length = WORD_8, - .stop_bits = SB_ONE, - .parity = PT_NONE, - .hw_flow_ctl = HW_DISABLE, - .ovsample = OV_16, - .obsample = OB_DISABLE, - .periph = USART1, - .wake_addr = false, - .usart_active_num = USART1_ACTIVE_IDX, - .tx_dma_cfg = &usart_l_tx_dma_config, - .rx_dma_cfg = &usart_l_rx_dma_config -}; +// dma_init_t usart_l_tx_dma_config = USART1_TXDMA_CONT_CONFIG(NULL, 1); +// dma_init_t usart_l_rx_dma_config = USART1_RXDMA_CONT_CONFIG(NULL, 2); +// char usart_l_rx_array[MC_MAX_RX_LENGTH] = {'\0'}; +// volatile usart_rx_buf_t huart_l_rx_buf = { +// .last_msg_time = 0, .msg_size = MC_MAX_TX_LENGTH, +// .last_msg_loc = 0, .last_rx_time = 0, +// .rx_buf_size = MC_MAX_RX_LENGTH, .rx_buf = usart_l_rx_array +// }; +// usart_init_t huart_l = { +// .baud_rate = 115200, +// .word_length = WORD_8, +// .stop_bits = SB_ONE, +// .parity = PT_NONE, +// .hw_flow_ctl = HW_DISABLE, +// .ovsample = OV_16, +// .obsample = OB_DISABLE, +// .periph = USART1, +// .wake_addr = false, +// .usart_active_num = USART1_ACTIVE_IDX, +// .tx_dma_cfg = &usart_l_tx_dma_config, +// .rx_dma_cfg = &usart_l_rx_dma_config +// }; // Right Motor Controller UART -dma_init_t usart_r_tx_dma_config = USART2_TXDMA_CONT_CONFIG(NULL, 1); -dma_init_t usart_r_rx_dma_config = USART2_RXDMA_CONT_CONFIG(NULL, 2); -usart_init_t huart_r = { - .baud_rate = 115200, - .word_length = WORD_8, - .stop_bits = SB_ONE, - .parity = PT_NONE, - .hw_flow_ctl = HW_DISABLE, - .ovsample = OV_16, - .obsample = OB_DISABLE, - .periph = USART2, - .wake_addr = false, - .usart_active_num = USART2_ACTIVE_IDX, - .tx_dma_cfg = &usart_r_tx_dma_config, - .rx_dma_cfg = &usart_r_rx_dma_config -}; -char usart_r_rx_array[MC_MAX_RX_LENGTH] = {'\0'}; -volatile usart_rx_buf_t huart_r_rx_buf = { - .last_msg_time = 0, .msg_size = MC_MAX_TX_LENGTH, - .last_msg_loc = 0, .last_rx_time = 0, - .rx_buf_size = MC_MAX_RX_LENGTH, .rx_buf = usart_r_rx_array -}; +// dma_init_t usart_r_tx_dma_config = USART2_TXDMA_CONT_CONFIG(NULL, 1); +// dma_init_t usart_r_rx_dma_config = USART2_RXDMA_CONT_CONFIG(NULL, 2); +// usart_init_t huart_r = { +// .baud_rate = 115200, +// .word_length = WORD_8, +// .stop_bits = SB_ONE, +// .parity = PT_NONE, +// .hw_flow_ctl = HW_DISABLE, +// .ovsample = OV_16, +// .obsample = OB_DISABLE, +// .periph = USART2, +// .wake_addr = false, +// .usart_active_num = USART2_ACTIVE_IDX, +// .tx_dma_cfg = &usart_r_tx_dma_config, +// .rx_dma_cfg = &usart_r_rx_dma_config +// }; +// char usart_r_rx_array[MC_MAX_RX_LENGTH] = {'\0'}; +// volatile usart_rx_buf_t huart_r_rx_buf = { +// .last_msg_time = 0, .msg_size = MC_MAX_TX_LENGTH, +// .last_msg_loc = 0, .last_rx_time = 0, +// .rx_buf_size = MC_MAX_RX_LENGTH, .rx_buf = usart_r_rx_array +// }; /* ADC Configuration */ ADCInitConfig_t adc_config = { @@ -218,23 +222,47 @@ ClockRateConfig_t clock_config = { void preflightAnimation(void); void preflightChecks(void); void heartBeatLED(); -void usartTxUpdate(void); -void usartIdleIRQ(volatile usart_init_t *huart, volatile usart_rx_buf_t *rx_buf); +// void usartTxUpdate(void); +// void usartIdleIRQ(volatile usart_init_t *huart, volatile usart_rx_buf_t *rx_buf); void send_fault(uint16_t, bool); extern void HardFault_Handler(); void interpretLoadSensor(void); float voltToForce(uint16_t load_read); -q_handle_t q_tx_usart_l; -q_handle_t q_tx_usart_r; +// q_handle_t q_tx_usart_l; +// q_handle_t q_tx_usart_r; uint16_t num_failed_msgs_r; uint16_t num_failed_msgs_l; +extern q_handle_t q_tx_can2_s[CAN_TX_MAILBOX_CNT]; +extern uint32_t can2_mbx_last_send_time[CAN_TX_MAILBOX_CNT]; + +void can2TxUpdate(void) +{ + CanMsgTypeDef_t tx_msg; + for (uint8_t i = 0; i < CAN_TX_MAILBOX_CNT; ++i) + { + if(PHAL_txMailboxFree(CAN2, i)) + { + if (qReceive(&q_tx_can2_s[i], &tx_msg) == SUCCESS_G) // Check queue for items and take if there is one + { + PHAL_txCANMessage(&tx_msg, i); + can2_mbx_last_send_time[i] = sched.os_ticks; + } + } + else if (sched.os_ticks - can2_mbx_last_send_time[i] > CAN_TX_TIMEOUT_MS) + { + PHAL_txCANAbort(CAN2, i); // aborts tx and empties the mailbox + can_stats.tx_fail++; + } + } +} + int main(void){ /* Data Struct Initialization */ - qConstruct(&q_tx_usart_l, MC_MAX_TX_LENGTH); - qConstruct(&q_tx_usart_r, MC_MAX_TX_LENGTH); + // qConstruct(&q_tx_usart_l, MC_MAX_TX_LENGTH); + // qConstruct(&q_tx_usart_r, MC_MAX_TX_LENGTH); /* HAL Initialization */ PHAL_trimHSI(HSI_TRIM_MAIN_MODULE); @@ -248,7 +276,6 @@ int main(void){ } PHAL_writeGPIO(SDC_CTRL_GPIO_Port, SDC_CTRL_Pin, 1); - /* Task Creation */ schedInit(APB1ClockRateHz); configureAnim(preflightAnimation, preflightChecks, 60, 750); @@ -266,8 +293,9 @@ int main(void){ taskCreate(daqPeriodic, DAQ_UPDATE_PERIOD); // taskCreate(memFg, MEM_FG_TIME); taskCreateBackground(canTxUpdate); + // taskCreateBackground(can2TxUpdate); taskCreateBackground(canRxUpdate); - taskCreateBackground(usartTxUpdate); + // taskCreateBackground(usartTxUpdate); // taskCreateBackground(memBg); // uint8_t i = 0; // calibrateSteeringAngle(&i); @@ -285,24 +313,30 @@ void preflightChecks(void) { switch (state++) { + /* TODO: Change this to init AMK CAN */ case 0: - huart_l.rx_dma_cfg->circular = true; - if(!PHAL_initUSART(&huart_l, APB2ClockRateHz)) - { - HardFault_Handler(); - } - huart_r.rx_dma_cfg->circular = true; - if(!PHAL_initUSART(&huart_r, APB1ClockRateHz)) + if(!PHAL_initCAN(CAN1, false, VCAN_BPS)) { HardFault_Handler(); } + NVIC_EnableIRQ(CAN1_RX0_IRQn); + // huart_l.rx_dma_cfg->circular = true; + // if(!PHAL_initUSART(&huart_l, APB2ClockRateHz)) + // { + // HardFault_Handler(); + // } + // huart_r.rx_dma_cfg->circular = true; + // if(!PHAL_initUSART(&huart_r, APB1ClockRateHz)) + // { + // HardFault_Handler(); + // } break; case 1: - if(!PHAL_initCAN(CAN1, false, VCAN_BPS)) + if(!PHAL_initCAN(CAN2, false, VCAN_BPS)) { HardFault_Handler(); } - NVIC_EnableIRQ(CAN1_RX0_IRQn); + NVIC_EnableIRQ(CAN2_RX0_IRQn); // spi_config.data_rate = APB2ClockRateHz / 16; // 5 MHz // if (!PHAL_SPI_init(&spi_config)) // HardFault_Handler(); @@ -324,12 +358,12 @@ void preflightChecks(void) { break; case 3: // initial rx request - PHAL_usartRxDma(&huart_r, - (uint16_t *) huart_r_rx_buf.rx_buf, - huart_r_rx_buf.rx_buf_size, 1); - PHAL_usartRxDma(&huart_l, - (uint16_t *) huart_l_rx_buf.rx_buf, - huart_l_rx_buf.rx_buf_size, 1); + // PHAL_usartRxDma(&huart_r, + // (uint16_t *) huart_r_rx_buf.rx_buf, + // huart_r_rx_buf.rx_buf_size, 1); + // PHAL_usartRxDma(&huart_l, + // (uint16_t *) huart_l_rx_buf.rx_buf, + // huart_l_rx_buf.rx_buf_size, 1); break; case 4: /* Module Initialization */ @@ -398,57 +432,57 @@ void heartBeatLED(void) /* USART Message Handling */ uint8_t tmp_left[MC_MAX_TX_LENGTH] = {'\0'}; uint8_t tmp_right[MC_MAX_TX_LENGTH] = {'\0'}; -void usartTxUpdate(void) -{ - if (!PHAL_usartTxBusy(&huart_l) && - qReceive(&q_tx_usart_l, tmp_left) == SUCCESS_G) - { - PHAL_usartTxDma(&huart_l, (uint16_t *) tmp_left, strlen(tmp_left)); - } - if (!PHAL_usartTxBusy(&huart_r) && - qReceive(&q_tx_usart_r, tmp_right) == SUCCESS_G) - { - PHAL_usartTxDma(&huart_r, (uint16_t *) tmp_right, strlen(tmp_right)); - } -} - -void usart_recieve_complete_callback(usart_init_t *handle) -{ - if (handle == &huart_r) - { - if (handle->rx_errors.noise_detected) - { - num_failed_msgs_r++; - return; - } - usartIdleIRQ(&huart_r, &huart_r_rx_buf); - } - else if (handle == &huart_l) - { - if (handle->rx_errors.noise_detected) - { - num_failed_msgs_l++; - return; - } - usartIdleIRQ(&huart_l, &huart_l_rx_buf); - } -} - -void usartIdleIRQ(volatile usart_init_t *huart, volatile usart_rx_buf_t *rx_buf) -{ - // TODO: check for overruns, framing errors, etc - uint16_t new_loc = 0; - rx_buf->last_rx_time = sched.os_ticks; - new_loc = rx_buf->rx_buf_size - huart->rx_dma_cfg->stream->NDTR; // extract last location from DMA - if (new_loc == rx_buf->rx_buf_size) new_loc = 0; // should never happen - else if (new_loc < rx_buf->last_rx_loc) new_loc += rx_buf->rx_buf_size; // wrap around - if (new_loc - rx_buf->last_rx_loc > rx_buf->msg_size) // status msg vs just an echo - { - rx_buf->last_msg_time = sched.os_ticks; - rx_buf->last_msg_loc = (rx_buf->last_rx_loc + 1) % rx_buf->rx_buf_size; - } - rx_buf->last_rx_loc = new_loc % rx_buf->rx_buf_size; -} +// void usartTxUpdate(void) +// { +// if (!PHAL_usartTxBusy(&huart_l) && +// qReceive(&q_tx_usart_l, tmp_left) == SUCCESS_G) +// { +// PHAL_usartTxDma(&huart_l, (uint16_t *) tmp_left, strlen(tmp_left)); +// } +// if (!PHAL_usartTxBusy(&huart_r) && +// qReceive(&q_tx_usart_r, tmp_right) == SUCCESS_G) +// { +// PHAL_usartTxDma(&huart_r, (uint16_t *) tmp_right, strlen(tmp_right)); +// } +// } + +// void usart_recieve_complete_callback(usart_init_t *handle) +// { +// if (handle == &huart_r) +// { +// if (handle->rx_errors.noise_detected) +// { +// num_failed_msgs_r++; +// return; +// } +// usartIdleIRQ(&huart_r, &huart_r_rx_buf); +// } +// else if (handle == &huart_l) +// { +// if (handle->rx_errors.noise_detected) +// { +// num_failed_msgs_l++; +// return; +// } +// usartIdleIRQ(&huart_l, &huart_l_rx_buf); +// } +// } + +// void usartIdleIRQ(volatile usart_init_t *huart, volatile usart_rx_buf_t *rx_buf) +// { +// // TODO: check for overruns, framing errors, etc +// uint16_t new_loc = 0; +// rx_buf->last_rx_time = sched.os_ticks; +// new_loc = rx_buf->rx_buf_size - huart->rx_dma_cfg->stream->NDTR; // extract last location from DMA +// if (new_loc == rx_buf->rx_buf_size) new_loc = 0; // should never happen +// else if (new_loc < rx_buf->last_rx_loc) new_loc += rx_buf->rx_buf_size; // wrap around +// if (new_loc - rx_buf->last_rx_loc > rx_buf->msg_size) // status msg vs just an echo +// { +// rx_buf->last_msg_time = sched.os_ticks; +// rx_buf->last_msg_loc = (rx_buf->last_rx_loc + 1) % rx_buf->rx_buf_size; +// } +// rx_buf->last_rx_loc = new_loc % rx_buf->rx_buf_size; +// } #define SCALE_F = (1 + (3.4/6.6)) @@ -492,6 +526,11 @@ void CAN1_RX0_IRQHandler() canParseIRQHandler(CAN1); } +void CAN2_RX0_IRQHandler() +{ + canParseIRQHandler(CAN2); +} + void main_module_bl_cmd_CALLBACK(CanParsedData_t *msg_data_a) { if (can_data.main_module_bl_cmd.cmd == BLCMD_RST) diff --git a/source/main_module/main.h b/source/main_module/main.h index 8192a3a6..87aae421 100644 --- a/source/main_module/main.h +++ b/source/main_module/main.h @@ -20,6 +20,18 @@ #define FAULT_NODE_NAME NODE_MAIN_MODULE +#define DIS_VOLT_DIAG_GPIO_Port (GPIOC) +#define DIS_VOLT_DIAG_GPIO_Pin (4) + +#define SAFE_STAT_DIAG_GPIO_Port (GPIOC) +#define SAFE_STAT_DIAG_GPIO_Pin (12) + +#define EX_OSC_GPIO_Port (GPIOC) +#define EX_OSC_GPIO_Pin (14) + +#define ABOX_VOLT_DIAG_GPIO_Port (GPIOE) +#define ABOX_VOLT_DIAG_GPIO_Pin (8) + // Internal Status Indicators #define ERR_LED_GPIO_Port (GPIOD) #define ERR_LED_Pin (4) @@ -38,8 +50,8 @@ #define BRK_BUZZER_STAT_GPIO_Port (GPIOB) #define BRK_BUZZER_STAT_Pin (15) -#define TSAL_LVAL_STAT_GPIO_Port (GPIOD) -#define TSAL_LVAL_STAT_Pin (10) +#define READY_TO_DRIVE_STAT_GPIO_Port (GPIOD) +#define READY_TO_DRIVE_STAT_GPIO_Pin (10) // CAN #define VCAN_RX_GPIO_Port (GPIOA) @@ -47,10 +59,10 @@ #define VCAN_TX_GPIO_Port (GPIOA) #define VCAN_TX_Pin (12) -#define EMCAN_RX_GPIO_Port (GPIOB) -#define EMCAN_RX_Pin (12) -#define EMCAN_TX_GPIO_Port (GPIOB) -#define EMCAN_TX_Pin (13) +#define AMK_RX_GPIO_Port (GPIOB) +#define AMK_RX_Pin (12) +#define AMK_TX_GPIO_Port (GPIOB) +#define AMK_TX_Pin (13) // SPI Peripherals #define SPI1_SCK_GPIO_Port (GPIOA) @@ -96,20 +108,10 @@ #define PRCHG_STAT_GPIO_Port (GPIOE) #define PRCHG_STAT_Pin (5) -// Motor Controllers -#define MC_L_INVERT (0) -#define MC_L_UART (USART1) -#define MC_L_UART_TX_GPIO_Port (GPIOA) -#define MC_L_UART_TX_GPIO_Pin (9) -#define MC_L_UART_RX_GPIO_Port (GPIOA) -#define MC_L_UART_RX_GPIO_Pin (10) - -#define MC_R_INVERT (0) -#define MC_R_UART (USART2) -#define MC_R_UART_TX_GPIO_Port (GPIOA) -#define MC_R_UART_TX_GPIO_Pin (2) -#define MC_R_UART_RX_GPIO_Port (GPIOA) -#define MC_R_UART_RX_GPIO_Pin (3) +#define SAFE_STAT_G_GPIO_Port (GPIOA) +#define SAFE_STAT_G__GPIO_Pin (2) +#define SAFE_STAT_R_GPIO_Port (GPIOA) +#define SAFE_STAT_R_GPIO_Pin (3) // Wheel Speed #define MOTOR_R_WS_PWM_TIM (TIM1) @@ -125,12 +127,12 @@ #define MOTOR_L_WS_AF (2) // Shock Pots -#define SHOCK_POT_L_GPIO_Port (GPIOC) -#define SHOCK_POT_L_Pin (3) +#define SHOCK_POT_R_GPIO_Port (GPIOC) +#define SHOCK_POT_R_Pin (3) #define SHOCK_POT_L_ADC_CHNL (13) -#define SHOCK_POT_R_GPIO_Port (GPIOC) -#define SHOCK_POT_R_Pin (2) +#define SHOCK_POT_L_GPIO_Port (GPIOC) +#define SHOCK_POT_L_Pin (2) #define SHOCK_POT_R_ADC_CHNL (12) // Load Sensors @@ -182,4 +184,4 @@ volatile extern ADCReadings_t adc_readings; void canTxSendToBack(CanMsgTypeDef_t *msg); -#endif \ No newline at end of file +#endif diff --git a/source/torque_vector/can/can_parse.c b/source/torque_vector/can/can_parse.c index f507a807..643a9e24 100644 --- a/source/torque_vector/can/can_parse.c +++ b/source/torque_vector/can/can_parse.c @@ -101,8 +101,10 @@ void canRxUpdate() case ID_REAR_MOTOR_TEMPS: can_data.rear_motor_temps.left_mot_temp = msg_data_a->rear_motor_temps.left_mot_temp; can_data.rear_motor_temps.right_mot_temp = msg_data_a->rear_motor_temps.right_mot_temp; - can_data.rear_motor_temps.left_ctrl_temp = msg_data_a->rear_motor_temps.left_ctrl_temp; - can_data.rear_motor_temps.right_ctrl_temp = msg_data_a->rear_motor_temps.right_ctrl_temp; + can_data.rear_motor_temps.left_inv_temp = msg_data_a->rear_motor_temps.left_inv_temp; + can_data.rear_motor_temps.right_inv_temp = msg_data_a->rear_motor_temps.right_inv_temp; + can_data.rear_motor_temps.left_igbt_temp = msg_data_a->rear_motor_temps.left_igbt_temp; + can_data.rear_motor_temps.right_igbt_temp = msg_data_a->rear_motor_temps.right_igbt_temp; can_data.rear_motor_temps.stale = 0; can_data.rear_motor_temps.last_rx = sched.os_ticks; break; diff --git a/source/torque_vector/can/can_parse.h b/source/torque_vector/can/can_parse.h index 8083af62..dfb4ea07 100644 --- a/source/torque_vector/can/can_parse.h +++ b/source/torque_vector/can/can_parse.h @@ -78,7 +78,7 @@ #define DLC_DASHBOARD_TV_PARAMETERS 7 #define DLC_MAIN_HB 2 #define DLC_REAR_WHEEL_SPEEDS 8 -#define DLC_REAR_MOTOR_TEMPS 4 +#define DLC_REAR_MOTOR_TEMPS 6 #define DLC_MAX_CELL_TEMP 2 #define DLC_FAULT_SYNC_PDU 3 #define DLC_FAULT_SYNC_MAIN_MODULE 3 @@ -349,8 +349,10 @@ typedef union { struct { uint64_t left_mot_temp: 8; uint64_t right_mot_temp: 8; - uint64_t left_ctrl_temp: 8; - uint64_t right_ctrl_temp: 8; + uint64_t left_inv_temp: 8; + uint64_t right_inv_temp: 8; + uint64_t left_igbt_temp: 8; + uint64_t right_igbt_temp: 8; } rear_motor_temps; struct { uint64_t max_temp: 16; @@ -442,8 +444,10 @@ typedef struct { struct { uint8_t left_mot_temp; uint8_t right_mot_temp; - uint8_t left_ctrl_temp; - uint8_t right_ctrl_temp; + uint8_t left_inv_temp; + uint8_t right_inv_temp; + uint8_t left_igbt_temp; + uint8_t right_igbt_temp; uint8_t stale; uint32_t last_rx; } rear_motor_temps;