From 182ad3c6528f9bd0b62cd8a2a81c126c00dba037 Mon Sep 17 00:00:00 2001 From: Aditya Anand <90068582+AdityaAsGithub@users.noreply.github.com> Date: Sat, 4 May 2024 22:58:28 -0400 Subject: [PATCH] Bug/anand89/clock fix (#124) * clock cal update * TV implementation, apps fix * TV fix, added calibrated shockpots * Moved defines for shockpot calibration --------- Co-authored-by: AdityaAsGithub --- common/common_defs/common_defs.h | 12 ++--- common/daq/can_config.json | 8 +-- common/daq/per_dbc.dbc | 8 +-- common/faults/fault_config.json | 8 +-- common/faults/faults.c | 8 +-- common/faults/faults.h | 16 +++--- source/dashboard/main.c | 13 ++--- source/dashboard/main.h | 13 ++++- source/main_module/car/car.c | 79 ++++++++++++++++++++-------- source/main_module/car/car.h | 14 +++++ source/torque_vector/can/can_parse.c | 3 ++ 11 files changed, 121 insertions(+), 61 deletions(-) diff --git a/common/common_defs/common_defs.h b/common/common_defs/common_defs.h index c6fabc44..9262c64c 100644 --- a/common/common_defs/common_defs.h +++ b/common/common_defs/common_defs.h @@ -4,9 +4,9 @@ * @brief Common defs for the entire firmware repository. Dont let this get too out of control please. * @version 0.1 * @date 2022-01-26 - * + * * @copyright Copyright (c) 2022 - * + * */ #ifndef COMMON_DEFS_H_ @@ -50,9 +50,9 @@ /* Per-Node HSI RCC Trim Constants */ #define HSI_TRIM_TORQUE_VECTOR 15 #define HSI_TRIM_MAIN_MODULE 15 -#define HSI_TRIM_PDU 17 -#define HSI_TRIM_DASHBOARD 16 -#define HSI_TRIM_DAQ 19 -#define HSI_TRIM_A_BOX 17 +#define HSI_TRIM_PDU 16 +#define HSI_TRIM_DASHBOARD 15 +#define HSI_TRIM_DAQ 17 +#define HSI_TRIM_A_BOX 16 #endif /* COMMON_DEFS_H_ */ \ No newline at end of file diff --git a/common/daq/can_config.json b/common/daq/can_config.json index cda56814..e2c2aba4 100644 --- a/common/daq/can_config.json +++ b/common/daq/can_config.json @@ -70,8 +70,8 @@ "msg_name": "shock_rear", "msg_desc": "Load Sensor readings from left and right", "signals": [ - {"sig_name": "left_shock", "type": "uint16_t", "unit": "mm"}, - {"sig_name": "right_shock", "type": "uint16_t", "unit": "mm"} + {"sig_name": "left_shock", "type": "int16_t", "unit": "mm"}, + {"sig_name": "right_shock", "type": "int16_t", "unit": "mm"} ], "msg_period": 15, "msg_hlp": 4, @@ -636,8 +636,8 @@ "msg_name": "shock_front", "msg_desc": "Load Sensor readings from left and right", "signals": [ - {"sig_name": "left_shock", "type": "uint16_t", "unit": "mm"}, - {"sig_name": "right_shock", "type": "uint16_t", "unit": "mm"} + {"sig_name": "left_shock", "type": "int16_t", "unit": "mm"}, + {"sig_name": "right_shock", "type": "int16_t", "unit": "mm"} ], "msg_period": 15, "msg_hlp": 4, diff --git a/common/daq/per_dbc.dbc b/common/daq/per_dbc.dbc index 3312323c..7902d317 100644 --- a/common/daq/per_dbc.dbc +++ b/common/daq/per_dbc.dbc @@ -60,8 +60,8 @@ BO_ 2415983105 load_sensor_readings: 8 Main_Module SG_ left_load_sensor : 0|32@1- (1,0) [0|0] "N" Vector__XXX BO_ 2415984385 shock_rear: 4 Main_Module - SG_ right_shock : 16|16@1+ (1,0) [0|0] "mm" Vector__XXX - SG_ left_shock : 0|16@1+ (1,0) [0|0] "mm" Vector__XXX + SG_ right_shock : 16|16@1- (1,0) [0|0] "mm" Vector__XXX + SG_ left_shock : 0|16@1- (1,0) [0|0] "mm" Vector__XXX BO_ 2415925633 mcu_status: 4 Main_Module SG_ sched_error : 24|8@1+ (1,0) [0|0] "" Vector__XXX @@ -329,8 +329,8 @@ BO_ 2415919749 raw_throttle_brake: 8 Dashboard SG_ throttle : 0|12@1+ (0.02442,0) [0|0] "%" Vector__XXX BO_ 2415984069 shock_front: 4 Dashboard - SG_ right_shock : 16|16@1+ (1,0) [0|0] "mm" Vector__XXX - SG_ left_shock : 0|16@1+ (1,0) [0|0] "mm" Vector__XXX + SG_ right_shock : 16|16@1- (1,0) [0|0] "mm" Vector__XXX + SG_ left_shock : 0|16@1- (1,0) [0|0] "mm" Vector__XXX BO_ 2415983749 load_sensor_readings_dash: 8 Dashboard SG_ right_load_sensor : 32|32@1- (1,0) [0|0] "N" Vector__XXX diff --git a/common/faults/fault_config.json b/common/faults/fault_config.json index acb95e87..7c1960ca 100644 --- a/common/faults/fault_config.json +++ b/common/faults/fault_config.json @@ -100,13 +100,13 @@ "node_name": "main_module", "faults": [ { - "fault_name": "pchg_implaus", + "fault_name": "remap_unreliable", "max": 1, "min": 0, - "priority": "error", + "priority": "warning", "time_to_latch": 50, "time_to_unlatch": 1000, - "lcd_message": "Precharge Implausibility" + "lcd_message": "Remap Disabled" }, { "fault_name": "rtd_exit", @@ -416,7 +416,7 @@ "min": 0, "priority": "warning", "time_to_latch": 10, - "time_to_unlatch": 1000, + "time_to_unlatch": 10, "lcd_message": "APPS Brake Fault" } ] diff --git a/common/faults/faults.c b/common/faults/faults.c index ff25fb86..2d6c431f 100644 --- a/common/faults/faults.c +++ b/common/faults/faults.c @@ -20,7 +20,7 @@ //BEGIN AUTO FAULT INFO ARRAY DEFS uint16_t faultLatchTime[TOTAL_NUM_FAULTS] = { PDU_MCU_TEMP_HIGH_LATCH_TIME, DASH_RAIL_LATCH_TIME, ABOX_RAIL_LATCH_TIME, MAIN_RAIL_LATCH_TIME, DAQ_RAIL_LATCH_TIME, V_CRIT_LATCH_TIME, - V_NONCRIT_LATCH_TIME, FAN1_LATCH_TIME, BULLET_RAIL_LATCH_TIME, FAN2_LATCH_TIME, PCHG_IMPLAUS_LATCH_TIME, RTD_EXIT_LATCH_TIME, LEFT_MC_CONN_LATCH_TIME, + V_NONCRIT_LATCH_TIME, FAN1_LATCH_TIME, BULLET_RAIL_LATCH_TIME, FAN2_LATCH_TIME, REMAP_UNRELIABLE_LATCH_TIME, RTD_EXIT_LATCH_TIME, LEFT_MC_CONN_LATCH_TIME, RIGHT_MC_CONN_LATCH_TIME, MCU_TEMP_HIGH_LATCH_TIME, DT_L_TEMP_OT_LATCH_TIME, DT_R_TEMP_OT_LATCH_TIME, DT_L_TEMP_HIGH_LATCH_TIME, DT_R_TEMP_HIGH_LATCH_TIME, BAT_COOL_LOOP_HIGH_LATCH_TIME, DT_COOL_LOOP_HIGH_LATCH_TIME, DT_R_THERM_DISC_LATCH_TIME, DT_L_THERM_DISC_LATCH_TIME, BATT_CL_DISC_LATCH_TIME, DT_CL_DISC_LATCH_TIME, BSPD_LATCHED_LATCH_TIME, BOTS_FAIL_LATCH_TIME, INERTIA_FAIL_LATCH_TIME, COCKPIT_ESTOP_LATCH_TIME, RIGHT_ESTOP_LATCH_TIME, LEFT_ESTOP_LATCH_TIME, HVD_DISC_LATCH_TIME, HUB_DISC_LATCH_TIME, TSMS_DISC_LATCH_TIME, @@ -32,7 +32,7 @@ uint16_t faultLatchTime[TOTAL_NUM_FAULTS] = { PDU_MCU_TEMP_HIGH_LATCH_TIME, DASH PACK_TEMP_LATCH_TIME, PACK_TEMP_EXCEEDED_LATCH_TIME, MIN_PACK_TEMP_LATCH_TIME, IMD_LATCH_TIME, TMU_POWER_LOST_LATCH_TIME, TV_DISABLED_LATCH_TIME, TV_UNCALIBRATED_LATCH_TIME, NO_GPS_FIX_LATCH_TIME, TEST_FAULT_1_LATCH_TIME, TEST_FAULT_2_LATCH_TIME, TEST_FAULT_3_LATCH_TIME, TEST_FAULT_4_LATCH_TIME,}; uint16_t faultULatchTime[TOTAL_NUM_FAULTS] = { PDU_MCU_TEMP_HIGH_UNLATCH_TIME, DASH_RAIL_UNLATCH_TIME, ABOX_RAIL_UNLATCH_TIME, MAIN_RAIL_UNLATCH_TIME, DAQ_RAIL_UNLATCH_TIME, V_CRIT_UNLATCH_TIME, - V_NONCRIT_UNLATCH_TIME, FAN1_UNLATCH_TIME, BULLET_RAIL_UNLATCH_TIME, FAN2_UNLATCH_TIME, PCHG_IMPLAUS_UNLATCH_TIME, RTD_EXIT_UNLATCH_TIME, LEFT_MC_CONN_UNLATCH_TIME, + V_NONCRIT_UNLATCH_TIME, FAN1_UNLATCH_TIME, BULLET_RAIL_UNLATCH_TIME, FAN2_UNLATCH_TIME, REMAP_UNRELIABLE_UNLATCH_TIME, RTD_EXIT_UNLATCH_TIME, LEFT_MC_CONN_UNLATCH_TIME, RIGHT_MC_CONN_UNLATCH_TIME, MCU_TEMP_HIGH_UNLATCH_TIME, DT_L_TEMP_OT_UNLATCH_TIME, DT_R_TEMP_OT_UNLATCH_TIME, DT_L_TEMP_HIGH_UNLATCH_TIME, DT_R_TEMP_HIGH_UNLATCH_TIME, BAT_COOL_LOOP_HIGH_UNLATCH_TIME, DT_COOL_LOOP_HIGH_UNLATCH_TIME, DT_R_THERM_DISC_UNLATCH_TIME, DT_L_THERM_DISC_UNLATCH_TIME, BATT_CL_DISC_UNLATCH_TIME, DT_CL_DISC_UNLATCH_TIME, BSPD_LATCHED_UNLATCH_TIME, BOTS_FAIL_UNLATCH_TIME, INERTIA_FAIL_UNLATCH_TIME, COCKPIT_ESTOP_UNLATCH_TIME, RIGHT_ESTOP_UNLATCH_TIME, LEFT_ESTOP_UNLATCH_TIME, HVD_DISC_UNLATCH_TIME, HUB_DISC_UNLATCH_TIME, TSMS_DISC_UNLATCH_TIME, @@ -55,7 +55,7 @@ fault_status_t statusArray[TOTAL_NUM_FAULTS] = { (fault_status_t){false, ID_FAN1_FAULT}, (fault_status_t){false, ID_BULLET_RAIL_FAULT}, (fault_status_t){false, ID_FAN2_FAULT}, - (fault_status_t){false, ID_PCHG_IMPLAUS_FAULT}, + (fault_status_t){false, ID_REMAP_UNRELIABLE_FAULT}, (fault_status_t){false, ID_RTD_EXIT_FAULT}, (fault_status_t){false, ID_LEFT_MC_CONN_FAULT}, (fault_status_t){false, ID_RIGHT_MC_CONN_FAULT}, @@ -138,7 +138,7 @@ fault_attributes_t faultArray[TOTAL_NUM_FAULTS] = { (fault_attributes_t){false, false, FAN1_PRIORITY, 0, 0, FAN1_MAX, FAN1_MIN, &statusArray[7], 0, FAN1_MSG}, (fault_attributes_t){false, false, BULLET_RAIL_PRIORITY, 0, 0, BULLET_RAIL_MAX, BULLET_RAIL_MIN, &statusArray[8], 0, BULLET_RAIL_MSG}, (fault_attributes_t){false, false, FAN2_PRIORITY, 0, 0, FAN2_MAX, FAN2_MIN, &statusArray[9], 0, FAN2_MSG}, - (fault_attributes_t){false, false, PCHG_IMPLAUS_PRIORITY, 0, 0, PCHG_IMPLAUS_MAX, PCHG_IMPLAUS_MIN, &statusArray[10], 0, PCHG_IMPLAUS_MSG}, + (fault_attributes_t){false, false, REMAP_UNRELIABLE_PRIORITY, 0, 0, REMAP_UNRELIABLE_MAX, REMAP_UNRELIABLE_MIN, &statusArray[10], 0, REMAP_UNRELIABLE_MSG}, (fault_attributes_t){false, false, RTD_EXIT_PRIORITY, 0, 0, RTD_EXIT_MAX, RTD_EXIT_MIN, &statusArray[11], 0, RTD_EXIT_MSG}, (fault_attributes_t){false, false, LEFT_MC_CONN_PRIORITY, 0, 0, LEFT_MC_CONN_MAX, LEFT_MC_CONN_MIN, &statusArray[12], 0, LEFT_MC_CONN_MSG}, (fault_attributes_t){false, false, RIGHT_MC_CONN_PRIORITY, 0, 0, RIGHT_MC_CONN_MAX, RIGHT_MC_CONN_MIN, &statusArray[13], 0, RIGHT_MC_CONN_MSG}, diff --git a/common/faults/faults.h b/common/faults/faults.h index c7c87007..9f0ec5ce 100644 --- a/common/faults/faults.h +++ b/common/faults/faults.h @@ -41,7 +41,7 @@ #define ID_FAN1_FAULT 0x7 #define ID_BULLET_RAIL_FAULT 0x8 #define ID_FAN2_FAULT 0x9 -#define ID_PCHG_IMPLAUS_FAULT 0x100a +#define ID_REMAP_UNRELIABLE_FAULT 0x100a #define ID_RTD_EXIT_FAULT 0x100b #define ID_LEFT_MC_CONN_FAULT 0x100c #define ID_RIGHT_MC_CONN_FAULT 0x100d @@ -133,7 +133,7 @@ #define FAN1_PRIORITY 0 #define BULLET_RAIL_PRIORITY 0 #define FAN2_PRIORITY 0 -#define PCHG_IMPLAUS_PRIORITY 1 +#define REMAP_UNRELIABLE_PRIORITY 0 #define RTD_EXIT_PRIORITY 0 #define LEFT_MC_CONN_PRIORITY 1 #define RIGHT_MC_CONN_PRIORITY 1 @@ -217,7 +217,7 @@ #define FAN1_MAX 1 #define BULLET_RAIL_MAX 1 #define FAN2_MAX 1 -#define PCHG_IMPLAUS_MAX 1 +#define REMAP_UNRELIABLE_MAX 1 #define RTD_EXIT_MAX 1 #define LEFT_MC_CONN_MAX 1 #define RIGHT_MC_CONN_MAX 1 @@ -301,7 +301,7 @@ #define FAN1_MIN 0 #define BULLET_RAIL_MIN 0 #define FAN2_MIN 0 -#define PCHG_IMPLAUS_MIN 0 +#define REMAP_UNRELIABLE_MIN 0 #define RTD_EXIT_MIN 0 #define LEFT_MC_CONN_MIN 0 #define RIGHT_MC_CONN_MIN 0 @@ -385,7 +385,7 @@ #define FAN1_LATCH_TIME 100 #define BULLET_RAIL_LATCH_TIME 100 #define FAN2_LATCH_TIME 100 -#define PCHG_IMPLAUS_LATCH_TIME 50 +#define REMAP_UNRELIABLE_LATCH_TIME 50 #define RTD_EXIT_LATCH_TIME 100 #define LEFT_MC_CONN_LATCH_TIME 3000 #define RIGHT_MC_CONN_LATCH_TIME 3000 @@ -469,7 +469,7 @@ #define FAN1_UNLATCH_TIME 1000 #define BULLET_RAIL_UNLATCH_TIME 1000 #define FAN2_UNLATCH_TIME 1000 -#define PCHG_IMPLAUS_UNLATCH_TIME 1000 +#define REMAP_UNRELIABLE_UNLATCH_TIME 1000 #define RTD_EXIT_UNLATCH_TIME 1000 #define LEFT_MC_CONN_UNLATCH_TIME 1000 #define RIGHT_MC_CONN_UNLATCH_TIME 1000 @@ -503,7 +503,7 @@ #define BSE_UNLATCH_TIME 1000 #define BSPD_UNLATCH_TIME 1000 #define IMPLAUS_DETECTED_UNLATCH_TIME 100 -#define APPS_BRAKE_UNLATCH_TIME 1000 +#define APPS_BRAKE_UNLATCH_TIME 10 #define DISCHARGE_LIMIT_ENFORCE_UNLATCH_TIME 5 #define CHARGER_SAFETY_RELAY_UNLATCH_TIME 5 #define INTERNAL_HARDWARE_UNLATCH_TIME 5 @@ -553,7 +553,7 @@ #define FAN1_MSG "Fan 1 Rail Down\0" #define BULLET_RAIL_MSG "Bullet Rail Down\0" #define FAN2_MSG "Fan 2 Rail Down\0" -#define PCHG_IMPLAUS_MSG "Precharge Implausibility\0" +#define REMAP_UNRELIABLE_MSG "Remap Disabled\0" #define RTD_EXIT_MSG "HV not detected, idling\0" #define LEFT_MC_CONN_MSG "LEFT MC CONN FAIL\0" #define RIGHT_MC_CONN_MSG "RIGHT MC CONN FAIL\0" diff --git a/source/dashboard/main.c b/source/dashboard/main.c index 531995b8..f0d83ec6 100644 --- a/source/dashboard/main.c +++ b/source/dashboard/main.c @@ -276,17 +276,18 @@ void preflightChecks(void) { } -#define POT_VOLT_MAX_DIST_MM 0 -#define POT_VOLT_MIN_DIST_MM 4095 + void send_shockpots() { uint16_t shock_l = raw_adc_values.shock_left; uint16_t shock_r = raw_adc_values.shock_right; - //Scale from raw 12bit adc to mm * 10 of linear pot travel - // shock_l = (POT_VOLT_MIN_DIST_MM * 10 - ((uint32_t) shock_l) * (POT_VOLT_MIN_DIST_MM - POT_VOLT_MAX_DIST_MM) * 10 / 4095); - // shock_r = (POT_VOLT_MIN_DIST_MM * 10 - ((uint32_t) shock_r) * (POT_VOLT_MIN_DIST_MM - POT_VOLT_MAX_DIST_MM) * 10 / 4095); - SEND_SHOCK_FRONT(shock_l, shock_r); + int16_t shock_l_parsed; + int16_t shock_r_parsed; + // Will scale linearly from 0 - 3744. so 75 - (percent of 3744 * 75) + shock_l_parsed = -1 * ((POT_MAX_DIST - (int16_t)((shock_l / (POT_VOLT_MIN_L - POT_VOLT_MAX_L)) * POT_MAX_DIST)) - POT_DIST_DROOP_L); + shock_r_parsed = -1 * ((POT_MAX_DIST - (int16_t)((shock_r / (POT_VOLT_MIN_R - POT_VOLT_MAX_R)) * POT_MAX_DIST)) - POT_DIST_DROOP_R); + SEND_SHOCK_FRONT(shock_l_parsed, shock_r_parsed); } void preflightAnimation(void) { diff --git a/source/dashboard/main.h b/source/dashboard/main.h index 9d8135f2..d6027249 100644 --- a/source/dashboard/main.h +++ b/source/dashboard/main.h @@ -19,6 +19,15 @@ //STM32F407 +// Shockpot Calibration +#define POT_VOLT_MAX_L 4.0f +#define POT_VOLT_MIN_L 4090.0f +#define POT_VOLT_MAX_R 4.0f +#define POT_VOLT_MIN_R 4060.0f +#define POT_MAX_DIST 75 +#define POT_DIST_DROOP_L 55 +#define POT_DIST_DROOP_R 57 + typedef struct __attribute__((packed)) { // Do not modify this struct unless @@ -40,7 +49,7 @@ typedef struct __attribute__((packed)) volatile extern raw_adc_values_t raw_adc_values; -typedef enum +typedef enum { DASH_INPUT_NONE, DASH_INPUT_ROT_ENC, @@ -152,7 +161,7 @@ typedef enum #define LCD_UART_TX_Pin (9) #define LCD_UART_RX_GPIO_Port (GPIOA) #define LCD_UART_RX_Pin (10) -#define LCD_NUM_PAGES (8) // Number encoder selectable pages +#define LCD_NUM_PAGES (8) // Number encoder selectable pages // LV Status #define LV_5V_V_SENSE_GPIO_Port (GPIOC) diff --git a/source/main_module/car/car.c b/source/main_module/car/car.c index ea29e339..19a11e95 100644 --- a/source/main_module/car/car.c +++ b/source/main_module/car/car.c @@ -7,6 +7,7 @@ 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 uint16_t num_failed_msgs_l, num_failed_msgs_r; +extern WheelSpeeds_t wheel_speeds; // TODO: Just to remove errors for now // usart_rx_buf_t huart_l_rx_buf, huart_r_rx_buf; uint8_t daq_buzzer; @@ -35,7 +36,7 @@ bool carInit() /* Set initial states */ car = (Car_t) {0}; // Everything to zero car.state = CAR_STATE_IDLE; - car.torque_src = CAR_TORQUE_THROT_MAP; + car.torque_src = CAR_TORQUE_TV; car.regen_enabled = false; car.sdc_close = true; // We want to initialize SDC as "good" PHAL_writeGPIO(SDC_CTRL_GPIO_Port, SDC_CTRL_Pin, car.sdc_close); @@ -235,7 +236,12 @@ void carPeriodic() } else if (car.state == CAR_STATE_READY2DRIVE) { - PHAL_readGPIO(PRCHG_STAT_GPIO_Port, PRCHG_STAT_Pin); + car.pchg.pchg_complete = PHAL_readGPIO(PRCHG_STAT_GPIO_Port, PRCHG_STAT_Pin); + if (!car.pchg.pchg_complete) + { + car.state = CAR_STATE_IDLE; + } + // Check if requesting to exit ready2drive if (car.start_btn_debounced) { @@ -246,12 +252,18 @@ void carPeriodic() float t_req_pedal = 0; float t_req_pedal_l = 0; float t_req_pedal_r = 0; + float t_req_equal_l = 0; + float t_req_equal_r = 0; if (!can_data.filt_throttle_brake.stale) t_req_pedal = (float) CLAMP(can_data.filt_throttle_brake.throttle, 0, 4095); if (!can_data.throttle_vcu.stale) t_req_pedal_l = (float) CLAMP(can_data.throttle_vcu.vcu_k_rl, 0, 4095); if (!can_data.throttle_vcu.stale) t_req_pedal_r = (float) CLAMP(can_data.throttle_vcu.vcu_k_rr, 0, 4095); + // if (!can_data.throttle_vcu.stale) + // t_req_pedal_l = (float) CLAMP(can_data.throttle_vcu.equal_k_rl, 0, 4095); + // if (!can_data.throttle_vcu.stale) + // t_req_pedal_r = (float) CLAMP(can_data.throttle_vcu.equal_k_rr, 0, 4095); t_req_pedal = t_req_pedal * 100.0f / 4095.0f; t_req_pedal_l = t_req_pedal_l * 100.0f / 4095.0f; @@ -272,25 +284,46 @@ void carPeriodic() temp_t_req.torque_right = t_req_pedal; break; case CAR_TORQUE_TV: - temp_t_req.torque_left = t_req_pedal_l; - temp_t_req.torque_right = t_req_pedal_r; - // EV.4.2.3 - Torque algorithm - // Any algorithm or electronic control unit that can adjust the - // requested wheel torque may only lower the total driver - // requested torque and must not increase it + if ((wheel_speeds.left_rad_s_x100 == 0 || wheel_speeds.right_rad_s_x100 == 0) && can_data.orion_currents_volts.pack_current > 10) + { + setFault(ID_REMAP_UNRELIABLE_FAULT, 1); + } + else + { + setFault(ID_REMAP_UNRELIABLE_FAULT, 0); + } + if (checkFault(ID_TV_DISABLED_FAULT)) + { + temp_t_req.torque_left = t_req_pedal_l; + temp_t_req.torque_right = t_req_pedal_r; + // EV.4.2.3 - Torque algorithm + // Any algorithm or electronic control unit that can adjust the + // requested wheel torque may only lower the total driver + // requested torque and must not increase it + if (temp_t_req.torque_left > t_req_equal_l) + { + temp_t_req.torque_left = t_req_equal_l; + } + if (temp_t_req.torque_right > t_req_equal_r) + { + temp_t_req.torque_right = t_req_equal_r; + } + } + else if (!checkFault(ID_REMAP_UNRELIABLE_FAULT)) + { + temp_t_req.torque_left = t_req_equal_l; + temp_t_req.torque_right = t_req_equal_r; + } + else + { + temp_t_req.torque_left = t_req_pedal; + temp_t_req.torque_right = t_req_pedal; + } if (t_req_pedal == 0) { temp_t_req.torque_left = 0; temp_t_req.torque_right = 0; } - // if (temp_t_req.torque_left > t_req_pedal_l) - // { - // temp_t_req.torque_left = t_req_pedal_l; - // } - // if (temp_t_req.torque_right > t_req_pedal_r) - // { - // temp_t_req.torque_right = t_req_pedal_r; - // } break; case CAR_TORQUE_THROT_MAP: temp_t_req.torque_left = t_req_pedal_l; @@ -458,17 +491,17 @@ void parseMCDataPeriodic(void) } } -#define POT_VOLT_MAX_DIST_MM 0 -#define POT_VOLT_MIN_DIST_MM 4095 - void send_shockpots() { uint16_t shock_l = adc_readings.shock_l; uint16_t shock_r = adc_readings.shock_r; - //Scale from raw 12bit adc to mm * 10 of linear pot travel - // shock_l = (POT_VOLT_MIN_DIST_MM * 10 - ((uint32_t) shock_l) * (POT_VOLT_MIN_DIST_MM - POT_VOLT_MAX_DIST_MM) * 10 / 4095); - // shock_r = (POT_VOLT_MIN_DIST_MM * 10 - ((uint32_t) shock_r) * (POT_VOLT_MIN_DIST_MM - POT_VOLT_MAX_DIST_MM) * 10 / 4095); - SEND_SHOCK_REAR(shock_l, shock_r); + int16_t shock_l_parsed; + int16_t shock_r_parsed; + // Will scale linearly from 0 - 3744. so 75 - (percent of 3744 * 75) + shock_l_parsed = -1 * ((POT_MAX_DIST - (int16_t)((shock_l / (POT_VOLT_MIN_L - POT_VOLT_MAX_L)) * POT_MAX_DIST)) - POT_DIST_DROOP_L); + shock_r_parsed = -1 * ((POT_MAX_DIST - (int16_t)((shock_r / (POT_VOLT_MIN_R - POT_VOLT_MAX_R)) * POT_MAX_DIST)) - POT_DIST_DROOP_R); + + SEND_SHOCK_REAR(shock_l_parsed, shock_r_parsed); } /** diff --git a/source/main_module/car/car.h b/source/main_module/car/car.h index 38bf608c..a4a8348d 100644 --- a/source/main_module/car/car.h +++ b/source/main_module/car/car.h @@ -31,6 +31,20 @@ #define BRAKE_PRESSED_THRESHOLD (BRAKE_LIGHT_ON_THRESHOLD) +// Shock Pot Calibration +#define POT_TOTAL_RES 3000 +#define POT_MAX_RES 3300 +#define POT_MIN_RES 300 + +#define POT_VOLT_MAX_L 4.0f +#define POT_VOLT_MIN_L 4077.0f +#define POT_VOLT_MAX_R 4.0f +#define POT_VOLT_MIN_R 4090.0f +#define POT_MAX_DIST 75 +#define POT_DIST_DROOP_L 57 +#define POT_DIST_DROOP_R 54 + + //Defines to guess a BSPD fault #define NUM_HIST_BSPD 16 typedef struct diff --git a/source/torque_vector/can/can_parse.c b/source/torque_vector/can/can_parse.c index 271d7a3c..42d51ecb 100644 --- a/source/torque_vector/can/can_parse.c +++ b/source/torque_vector/can/can_parse.c @@ -195,6 +195,9 @@ bool initCANFilter() CAN1->FA1R |= (1 << 7); // configure bank 7 CAN1->sFilterRegister[7].FR1 = (ID_RETURN_FAULT_CONTROL << 3) | 4; /* END AUTO FILTER */ + // Adding LWS standard to bank 8 since it needs to be ExtID + CAN1->FA1R |= (1 << 8); // configure bank 8 + CAN1->sFilterRegister[8].FR1 = (ID_LWS_STANDARD << 3) | 4; CAN1->FMR &= ~CAN_FMR_FINIT; // Enable Filters (exit filter init mode)