From f641a376334bab5deeee1bcd2b8d3b28e7871e55 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Sat, 1 Oct 2022 20:23:37 +0200
Subject: [PATCH] [EuRoC] Added interp, updated hil and removed pitot
 correction

---
 CMakeLists.txt                                |  8 +++----
 .../Configs/AirBrakesControllerConfigInterp.h |  4 +++-
 src/boards/Main/Sensors/Sensors.cpp           | 24 ++++++++++++++++++-
 .../NASController/NASController.cpp           |  6 -----
 .../NASController/NASController.cpp           |  3 ---
 .../HIL/HILTransceiver.cpp                    |  2 +-
 6 files changed, 31 insertions(+), 16 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1bee016e7..f98a0eec0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,7 +39,7 @@ sbs_target(auxiliary-entry stm32f429zi_skyward_pyxis_auxiliary)
 
 add_executable(main-entry-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
 target_include_directories(main-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-euroc PRIVATE EUROC)
+target_compile_definitions(main-entry-euroc PRIVATE EUROC BUZZER_LOW INTERP)
 sbs_target(main-entry-euroc stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-entry-roccaraso src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
@@ -54,17 +54,17 @@ sbs_target(main-entry-milano stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-entry-hil-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL})
 target_include_directories(main-entry-hil-euroc PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-hil-euroc PRIVATE HILSimulation EUROC BUZZER_LOW USE_SERIAL_TRANSCEIVER INTERP)
+target_compile_definitions(main-entry-hil-euroc PRIVATE HILSimulation EUROC BUZZER_LOW INTERP)
 sbs_target(main-entry-hil-euroc stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-entry-hil-roccaraso src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL})
 target_include_directories(main-entry-hil-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-hil-roccaraso PRIVATE HILSimulation ROCCARASO BUZZER_LOW USE_SERIAL_TRANSCEIVER INTERP)
+target_compile_definitions(main-entry-hil-roccaraso PRIVATE HILSimulation ROCCARASO BUZZER_LOW)
 sbs_target(main-entry-hil-roccaraso stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-entry-hil-milano src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL})
 target_include_directories(main-entry-hil-milano PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-hil-milano PRIVATE HILSimulation MILANO BUZZER_LOW USE_SERIAL_TRANSCEIVER)
+target_compile_definitions(main-entry-hil-milano PRIVATE HILSimulation MILANO BUZZER_LOW)
 sbs_target(main-entry-hil-milano stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-entry-hil-maker-faire src/entrypoints/Main/main-entry-maker-faire.cpp ${MAIN_COMPUTER} ${HIL})
diff --git a/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h b/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h
index ae0d3aa52..22e27612f 100644
--- a/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h
+++ b/src/boards/Main/Configs/AirBrakesControllerConfigInterp.h
@@ -35,7 +35,9 @@ static const Boardcore::AirBrakesInterpConfig ABK_CONFIG_INTERP{
     .INITIAL_FILTER_COEFF = 0.3,
     .INITIAL_T_FILTER     = 12,
     .DELTA_T_FILTER       = 2.5,
-    .FILTER_RATIO         = 2};
+    .FILTER_RATIO         = 2,
+};
+
 }  // namespace AirBrakesControllerConfig
 
 }  // namespace Main
diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp
index 21a98a1cd..773637208 100644
--- a/src/boards/Main/Sensors/Sensors.cpp
+++ b/src/boards/Main/Sensors/Sensors.cpp
@@ -164,15 +164,37 @@ ADS131M04Data Sensors::getADS131M04LastSample()
 MPXH6115AData Sensors::getStaticPressureLastSample()
 {
     PauseKernelLock lock;
+
+#ifndef HILSimulation
     return staticPressure != nullptr ? staticPressure->getLastSample()
                                      : MPXH6115AData{};
+#else
+    auto baroData = state.barometer->getLastSample();
+
+    MPXH6115AData data;
+    data.pressureTimestamp = baroData.pressureTimestamp;
+    data.pressure          = baroData.pressure;
+
+    return data;
+#endif
 }
 
 MPXH6400AData Sensors::getDplPressureLastSample()
 {
     PauseKernelLock lock;
+
+#ifndef HILSimulation
     return dplPressure != nullptr ? dplPressure->getLastSample()
                                   : MPXH6400AData{};
+#else
+    auto baroData = state.barometer->getLastSample();
+
+    MPXH6400AData data;
+    data.pressureTimestamp = baroData.pressureTimestamp;
+    data.pressure          = baroData.pressure;
+
+    return data;
+#endif
 }
 
 Boardcore::PitotData Sensors::getPitotLastSample()
@@ -265,7 +287,7 @@ Sensors::Sensors()
 
     batteryVoltageInit();
     internalAdcInit();
-    vn100Init();
+    // vn100Init();
 
     // Moved down here because the bmx takes some times to start
     bmx160Init();
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index cf39b26e0..a212199ae 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -62,7 +62,6 @@ void NASController::update()
         auto gpsData = Sensors::getInstance().getUbxGpsLastSample();
         auto pressureData =
             Sensors::getInstance().getStaticPressureLastSample();
-        auto pitotData = Sensors::getInstance().getPitotLastSample();
 
         // Predict step
         nas.predictGyro(imuData);
@@ -73,11 +72,6 @@ void NASController::update()
         nas.correctGPS(gpsData);
         nas.correctBaro(pressureData.pressure);
 
-        // Correct the pitot only during ascending
-        if (FlightModeManager::getInstance().getStatus().state ==
-            FlightModeManagerState::ASCENDING)
-            nas.correctPitot(pitotData.deltaP, pressureData.pressure);
-
         Logger::getInstance().log(nas.getState());
         FlightStatsRecorder::getInstance().update(nas.getState());
     }
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp
index d5571e330..8ff21963b 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp
@@ -60,8 +60,6 @@ void NASController::update()
             Sensors::getInstance().getBMX160WithCorrectionLastSample();
         auto gpsData      = Sensors::getInstance().getUbxGpsLastSample();
         auto pressureData = Sensors::getInstance().getMS5803LastSample();
-        // auto pitotData =
-        //     Sensors::getInstance().getDifferentialPressureLastSample();
 
         // Predict step
         nas.predictGyro(imuData);
@@ -71,7 +69,6 @@ void NASController::update()
         nas.correctMag(imuData);
         nas.correctGPS(gpsData);
         nas.correctBaro(pressureData.pressure);
-        // nas.correctPitot(pitotData.pressure, pressureData.pressure);
 
         Logger::getInstance().log(nas.getState());
         FlightStatsRecorder::getInstance().update(nas.getState());
diff --git a/src/hardware_in_the_loop/HIL/HILTransceiver.cpp b/src/hardware_in_the_loop/HIL/HILTransceiver.cpp
index f5e238481..4077d3595 100644
--- a/src/hardware_in_the_loop/HIL/HILTransceiver.cpp
+++ b/src/hardware_in_the_loop/HIL/HILTransceiver.cpp
@@ -34,7 +34,7 @@ using namespace Main;
  * @brief Construct a serial connection attached to a control algorithm
  */
 HILTransceiver::HILTransceiver()
-    : hilSerial(Buses::getInstance().uart4), actuatorData(ActuatorData{})
+    : hilSerial(Buses::getInstance().usart3), actuatorData(ActuatorData{})
 {
 }
 
-- 
GitLab