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 <[email protected]>
PurdueElectricRacing · May 5, 2024 · 182ad3c · 182ad3c
1 parent b9a49e0
commit 182ad3c
Show file tree

Hide file tree

Showing 11 changed files with 121 additions and 61 deletions.
diff --git 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_ */
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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);
 }
 
 /**