Shayan/heartbeat fault fix (#1082)

### Summary
Removed heartbeat delays, fixed start up settle fault count and added
some safety measures for pedal angle calculation

### Changelist 
- Removed Hal delays from BMS, PDM and FSM as they were causing missing
heart beat faults at start up
- moved accumulator and traction system fault counting to after the
settle period (this way faults in the settle period will not be
recorded)
- added a clamping function to the input of acos in the calc angle
function of Io_AcceleratorPedal.C as a safety measure

### Testing Done
<!-- Outline the testing that was done to demonstrate the changes are
solid. This could be unit tests, integration tests, testing on the car,
etc. Include relevant code snippets, screenshots, etc as needed. -->

### Resolved Issues
Extensive testing was done, if you would like to see the results please
contact me and I will send you the CAN result screenshots (Will not
include here because there are more than 10)

### Checklist
- syntax
- Clamp implementation

firmware/thruna/BMS/Src/App/states/App_AllStates.c

@@ -157,8 +157,6 @@ bool App_AllStatesRunOnTick100Hz(struct StateMachine *const state_machine)
     App_CheckCellTemperatureRange(accumulator, state_machine);
     App_Accumulator_UpdateAuxThermistorTemps(accumulator);
 
-    const bool acc_fault = App_Accumulator_CheckFaults(accumulator, ts);
-    const bool ts_fault  = App_TractveSystem_CheckFaults(ts);
     App_Accumulator_BroadcastLatchedFaults(accumulator);
 
     if (App_Airs_IsAirNegativeClosed(airs) == CONTACTOR_STATE_CLOSED &&
@@ -188,11 +186,16 @@ bool App_AllStatesRunOnTick100Hz(struct StateMachine *const state_machine)
     {
         acc_meas_settle_count++;
     }
-    else if (acc_fault || ts_fault)
+    else
     {
-        status = false;
-        App_SharedStateMachine_SetNextState(state_machine, App_GetFaultState());
-    }
+        const bool acc_fault = App_Accumulator_CheckFaults(accumulator, ts);
+        const bool ts_fault  = App_TractveSystem_CheckFaults(ts);
 
+        if (acc_fault || ts_fault)
+        {
+            status = false;
+            App_SharedStateMachine_SetNextState(state_machine, App_GetFaultState());
+        }
+    }
     return status;
 }

firmware/thruna/BMS/Src/main.c

@@ -244,7 +244,7 @@ int main(void)
     SystemClock_Config();
 
     /* USER CODE BEGIN SysInit */
-    HAL_Delay(1000);
+
     /* USER CODE END SysInit */
 
     /* Initialize all configured peripherals */

firmware/thruna/FSM/Src/Io/Io_AcceleratorPedals.c

@@ -60,7 +60,10 @@ static float sapps_max_angle;
 // max and min angle calculation for PAPPS/SAPPS
 static float calcAppsAngle(float cos_law_coefficent, float pot_len, float cos_law_denominator)
 {
-    float angle = acosf(cos_law_coefficent - (powf(pot_len, 2) / cos_law_denominator));
+    // clamping input of acos from cos law ((a^2 + b^2 - c^2) / 2ab))
+    float acosf_input = CLAMP((cos_law_coefficent - (powf(pot_len, 2) / cos_law_denominator)), -1.0f, 1.0f);
+
+    float angle = acosf(acosf_input);
 
     return angle;
 }

firmware/thruna/FSM/Src/main.c

@@ -227,7 +227,7 @@ int main(void)
     SystemClock_Config();
 
     /* USER CODE BEGIN SysInit */
-    HAL_Delay(1000U);
+
     /* USER CODE END SysInit */
 
     /* Initialize all configured peripherals */

firmware/thruna/PDM/Src/main.c

@@ -225,7 +225,7 @@ int main(void)
     SystemClock_Config();
 
     /* USER CODE BEGIN SysInit */
-    HAL_Delay(500U);
+
     /* USER CODE END SysInit */
 
     /* Initialize all configured peripherals */