diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 0a0905f194f5dad484d081d5f846a78577f50227..e09a68a5f03e998b41a80a44a8dd2cf709583994 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -26,6 +26,7 @@ set(OBSW_INCLUDE_DIRS
set(MAIN_COMPUTER
src/boards/Main/Sensors/Sensors.cpp
+ src/boards/Main/AlgoReference/AlgoReference.cpp
src/boards/Main/Radio/Radio.cpp
src/boards/Main/CanHandler/CanHandler.cpp
src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
diff --git a/src/boards/Main/AlgoReference/AlgoReference.cpp b/src/boards/Main/AlgoReference/AlgoReference.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6dcdf67e63b8de297baff5d70035487141f06769
--- /dev/null
+++ b/src/boards/Main/AlgoReference/AlgoReference.cpp
@@ -0,0 +1,78 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "AlgoReference.h"
+
+#include <Main/Configs/ReferenceConfig.h>
+#include <common/ReferenceConfig.h>
+#include <utils/AeroUtils/AeroUtils.h>
+
+using namespace Main;
+using namespace Boardcore;
+using namespace miosix;
+using namespace Common;
+
+AlgoReference::AlgoReference()
+ : reference{ReferenceConfig::defaultReferenceValues}
+{
+}
+
+void AlgoReference::calibrate()
+{
+ Sensors* sensors = getModule<Sensors>();
+
+ float baroAcc = 0.0f;
+
+ for (int i = 0; i < Config::Reference::CALIBRATION_SAMPLES_COUNT; i++)
+ {
+ PressureData baro = sensors->getAtmosPressureLastSample();
+ baroAcc += baro.pressure;
+
+ Thread::sleep(Config::Reference::CALIBRATION_SLEEP_TIME);
+ }
+
+ baroAcc /= Config::Reference::CALIBRATION_SAMPLES_COUNT;
+
+ Lock<FastMutex> lock{referenceMutex};
+
+ // Compute reference altitude
+ reference.refPressure = baroAcc;
+ reference.refAltitude = Aeroutils::relAltitude(
+ reference.refPressure, reference.mslPressure, reference.mslTemperature);
+
+ // Also updated the reference with the GPS if we have fix
+ UBXGPSData gps = sensors->getUBXGPSLastSample();
+ if (gps.fix == 3)
+ {
+ // We do not use the GPS altitude because it sucks
+ reference.refLatitude = gps.latitude;
+ reference.refLongitude = gps.longitude;
+ }
+
+ sdLogger.log(reference);
+}
+
+ReferenceValues AlgoReference::getReferenceValues()
+{
+ Lock<FastMutex> lock{referenceMutex};
+ return reference;
+}
\ No newline at end of file
diff --git a/src/boards/Main/AlgoReference/AlgoReference.h b/src/boards/Main/AlgoReference/AlgoReference.h
new file mode 100644
index 0000000000000000000000000000000000000000..1942f9bd2c301f191b07954e9a505b5ea2e97eff
--- /dev/null
+++ b/src/boards/Main/AlgoReference/AlgoReference.h
@@ -0,0 +1,50 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <Main/Sensors/Sensors.h>
+#include <algorithms/ReferenceValues.h>
+#include <miosix.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+namespace Main
+{
+
+class AlgoReference : public Boardcore::InjectableWithDeps<Sensors>
+{
+public:
+ AlgoReference();
+
+ void calibrate();
+
+ Boardcore::ReferenceValues getReferenceValues();
+
+private:
+ Boardcore::Logger& sdLogger = Boardcore::Logger::getInstance();
+ Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("reference");
+
+ miosix::FastMutex referenceMutex;
+ Boardcore::ReferenceValues reference;
+};
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Configs/ADAConfig.h b/src/boards/Main/Configs/ADAConfig.h
index 648c5a0b38007e3e16b8dbf1ea211b29853c5208..22cac1b0e87ec5af7e093123dfc7867255e97232 100644
--- a/src/boards/Main/Configs/ADAConfig.h
+++ b/src/boards/Main/Configs/ADAConfig.h
@@ -40,9 +40,6 @@ namespace ADA
constexpr Hertz UPDATE_RATE = 50_hz;
constexpr float UPDATE_RATE_SECONDS = 0.02; // [s]
-constexpr int CALIBRATION_SAMPLES_COUNT = 20;
-constexpr unsigned int CALIBRATION_SLEEP_TIME = 100; // [ms]
-
constexpr unsigned int SHADOW_MODE_TIMEOUT = 18000; // [ms]
constexpr float APOGEE_VERTICAL_SPEED_TARGET = 2.5; // [m/s]
diff --git a/src/boards/Main/Configs/ReferenceConfig.h b/src/boards/Main/Configs/ReferenceConfig.h
new file mode 100644
index 0000000000000000000000000000000000000000..b79656ed93f2067911ddebc4567fb92b6b6470ea
--- /dev/null
+++ b/src/boards/Main/Configs/ReferenceConfig.h
@@ -0,0 +1,41 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+namespace Main
+{
+
+namespace Config
+{
+
+namespace Reference
+{
+
+constexpr int CALIBRATION_SAMPLES_COUNT = 20;
+constexpr unsigned int CALIBRATION_SLEEP_TIME = 100; // [ms]
+
+} // namespace Reference
+
+} // namespace Config
+
+} // namespace Main
\ No newline at end of file
diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp
index 1c71d6a5c8bf23f3d2f8d66453bc6c56a458d630..be339c0de683efc46dbb140f974deef7a7914146 100644
--- a/src/boards/Main/Radio/Radio.cpp
+++ b/src/boards/Main/Radio/Radio.cpp
@@ -453,6 +453,30 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
return true;
}
+ case MAV_REFERENCE_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_reference_tm_t tm;
+
+ ReferenceValues ref =
+ getModule<AlgoReference>()->getReferenceValues();
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.ref_altitude = ref.refAltitude;
+ tm.ref_pressure = ref.refPressure;
+ tm.ref_temperature = ref.refTemperature;
+ tm.ref_latitude = ref.refLatitude;
+ tm.ref_longitude = ref.refLongitude;
+ tm.msl_pressure = ref.mslPressure;
+ tm.msl_temperature = ref.mslTemperature;
+
+ mavlink_msg_reference_tm_encode(Config::Radio::MAV_SYSTEM_ID,
+ Config::Radio::MAV_COMPONENT_ID,
+ &msg, &tm);
+ enqueuePacket(msg);
+ return true;
+ }
+
case MAV_ADA_ID:
{
mavlink_message_t msg;
@@ -461,8 +485,9 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
// Get the current ADA state
ADAController* ada = getModule<ADAController>();
- ADAState state = ada->getADAState();
- ReferenceValues ref = ada->getReferenceValues();
+ ADAState state = ada->getADAState();
+ ReferenceValues ref =
+ getModule<AlgoReference>()->getReferenceValues();
tm.timestamp = state.timestamp;
tm.state = static_cast<uint8_t>(ada->getState());
@@ -493,8 +518,9 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
// Get the current NAS state
NASController* nas = getModule<NASController>();
- NASState state = nas->getNASState();
- ReferenceValues ref = nas->getReferenceValues();
+ NASState state = nas->getNASState();
+ ReferenceValues ref =
+ getModule<AlgoReference>()->getReferenceValues();
tm.timestamp = state.timestamp;
tm.state = static_cast<uint8_t>(nas->getState());
@@ -534,7 +560,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
NASController* nas = getModule<NASController>();
MEAController* mea = getModule<MEAController>();
- auto pressDigi = sensors->getLPS22DFLastSample();
+ auto pressDigi = sensors->getLPS28DFWLastSample();
auto imu = sensors->getIMULastSample();
auto gps = sensors->getUBXGPSLastSample();
auto vn100 = sensors->getVN100LastSample();
@@ -611,6 +637,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
enqueuePacket(msg);
return true;
}
+
case MAV_STATS_ID:
{
mavlink_message_t msg;
@@ -647,11 +674,11 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
tm.dpl_bay_max_pressure_ts = stats.maxDplPressureTs;
tm.dpl_bay_max_pressure = stats.maxDplPressure;
- // NAS reference
- auto reference = nas->getReferenceValues();
- tm.ref_lat = reference.refLatitude;
- tm.ref_lon = reference.refLongitude;
- tm.ref_alt = reference.refAltitude;
+ // Algorithms reference
+ auto ref = getModule<AlgoReference>()->getReferenceValues();
+ tm.ref_lat = ref.refLatitude;
+ tm.ref_lon = ref.refLongitude;
+ tm.ref_alt = ref.refAltitude;
// Cpu stuff
CpuMeterData cpuStats = CpuMeter::getCpuStats();
@@ -715,6 +742,7 @@ bool Radio::enqueueSystemTm(uint8_t tmId)
enqueuePacket(msg);
return true;
}
+
case MAV_MOTOR_ID:
{
mavlink_message_t msg;
diff --git a/src/boards/Main/Radio/Radio.h b/src/boards/Main/Radio/Radio.h
index e107dedc9cd2ff7b01a35dd8fd281c94730a66e3..4a77c99b27b796be68d02bcabc572037ea82947b 100644
--- a/src/boards/Main/Radio/Radio.h
+++ b/src/boards/Main/Radio/Radio.h
@@ -23,6 +23,7 @@
#pragma once
#include <Main/Actuators/Actuators.h>
+#include <Main/AlgoReference/AlgoReference.h>
#include <Main/BoardScheduler.h>
#include <Main/Buses.h>
#include <Main/CanHandler/CanHandler.h>
@@ -38,7 +39,6 @@
#include <radio/MavlinkDriver/MavlinkDriver.h>
#include <radio/SX1278/SX1278Fsk.h>
#include <scheduler/TaskScheduler.h>
-
namespace Main
{
@@ -49,7 +49,7 @@ using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
class Radio : public Boardcore::InjectableWithDeps<
Buses, BoardScheduler, Actuators, PinHandler, CanHandler,
Sensors, FlightModeManager, ADAController, NASController,
- MEAController, StatsRecorder>
+ MEAController, StatsRecorder, AlgoReference>
{
public:
Radio() {}
diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp
index 7e979b1dc6026cb78bb74b930142f499110b9d22..48f6a7ea13bfae51b80d61062345315482abaf4b 100644
--- a/src/boards/Main/Sensors/Sensors.cpp
+++ b/src/boards/Main/Sensors/Sensors.cpp
@@ -26,8 +26,6 @@
#include <interfaces-impl/hwmapping.h>
#include <sensors/calibration/BiasCalibration/BiasCalibration.h>
-#include <utils/ModuleManager/ModuleManager.hpp>
-
#include "SensorsData.h"
using namespace Main;
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index 1c781cfecadfa8395cf658723422dda29157e790..d31713361b0a035caff93281cbbb17ea2c997b24 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -82,8 +82,6 @@ ADAController::ADAController()
{
EventBroker::getInstance().subscribe(this, TOPIC_ADA);
EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
-
- ada.setReferenceValues(ReferenceConfig::defaultReferenceValues);
}
bool ADAController::start()
@@ -105,6 +103,9 @@ bool ADAController::start()
return false;
}
+ ReferenceValues ref = getModule<AlgoReference>()->getReferenceValues();
+ ada.setReferenceValues(ref);
+
return true;
}
@@ -114,12 +115,6 @@ ADAState ADAController::getADAState()
return ada.getState();
}
-ReferenceValues ADAController::getReferenceValues()
-{
- Lock<FastMutex> lock{adaMutex};
- return ada.getReferenceValues();
-}
-
float ADAController::getDeploymentAltitude()
{
return Config::ADA::DEPLOYMENT_ALTITUDE_TARGET;
@@ -220,28 +215,14 @@ void ADAController::update()
void ADAController::calibrate()
{
- float baroAcc = 0.0f;
-
- for (int i = 0; i < Config::ADA::CALIBRATION_SAMPLES_COUNT; i++)
- {
- PressureData baro = getModule<Sensors>()->getAtmosPressureLastSample();
- baroAcc += baro.pressure;
-
- Thread::sleep(Config::ADA::CALIBRATION_SLEEP_TIME);
- }
-
- baroAcc /= Config::ADA::CALIBRATION_SAMPLES_COUNT;
+ ReferenceValues ref = getModule<AlgoReference>()->getReferenceValues();
Lock<FastMutex> lock{adaMutex};
+ ada.setReferenceValues(ref);
- ReferenceValues reference = ada.getReferenceValues();
- reference.refPressure = baroAcc;
- reference.refAltitude = Aeroutils::relAltitude(
- reference.refPressure, reference.mslPressure, reference.mslTemperature);
-
- ada.setReferenceValues(reference);
- ada.setKalmanConfig(computeADAKalmanConfig(reference.refPressure));
- ada.update(reference.refPressure);
+ // TODO: Should this be calculated by ADA at the moment?
+ ada.setKalmanConfig(computeADAKalmanConfig(ref.refPressure));
+ ada.update(ref.refPressure);
EventBroker::getInstance().post(ADA_READY, TOPIC_ADA);
}
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main/StateMachines/ADAController/ADAController.h
index 5b5e12334c023dbc6c75f70346e5aeffa7c8e90a..b95fffaecbfd3cfa5bdffa94ebf3084b7f1f8a4e 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.h
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.h
@@ -22,6 +22,7 @@
#pragma once
+#include <Main/AlgoReference/AlgoReference.h>
#include <Main/BoardScheduler.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/ADAController/ADAControllerData.h>
@@ -29,13 +30,12 @@
#include <algorithms/ADA/ADA.h>
#include <events/FSM.h>
#include <utils/DependencyManager/DependencyManager.h>
-
namespace Main
{
class ADAController
: public Boardcore::InjectableWithDeps<BoardScheduler, Sensors,
- StatsRecorder>,
+ StatsRecorder, AlgoReference>,
public Boardcore::FSM<ADAController>
{
public:
@@ -44,7 +44,6 @@ public:
[[nodiscard]] bool start() override;
Boardcore::ADAState getADAState();
- Boardcore::ReferenceValues getReferenceValues();
float getDeploymentAltitude();
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index fbf39835f68ab0b914e5dd2aa97e093da6a1bbb0..0bdb281ebbd7a43f21a543862adb0cff08af9ae6 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -195,6 +195,9 @@ State FlightModeManager::state_calibrate_sensors(const Event& event)
{
updateAndLogStatus(FlightModeManagerState::CALIBRATE_SENSORS);
+ // Wait a bit before calibrating
+ Thread::sleep(100);
+
getModule<Sensors>()->calibrate();
EventBroker::getInstance().post(FMM_SENSORS_CAL_DONE, TOPIC_FMM);
@@ -235,6 +238,12 @@ State FlightModeManager::state_calibrate_algorithms(const Event& event)
nasReady = false;
adaReady = false;
+ // Wait a bit after sensor calibration
+ Thread::sleep(100);
+
+ // First calibrate the reference
+ getModule<AlgoReference>()->calibrate();
+
// Post the calibration events
EventBroker::getInstance().post(NAS_CALIBRATE, TOPIC_NAS);
EventBroker::getInstance().post(ADA_CALIBRATE, TOPIC_ADA);
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
index ffe151608cdf94c750da6b800de1ef4980b8c099..1cdf0b25cdc782102a3767f11ba0d8d85b47dc4b 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h
@@ -23,6 +23,7 @@
#pragma once
#include <Main/Actuators/Actuators.h>
+#include <Main/AlgoReference/AlgoReference.h>
#include <Main/CanHandler/CanHandler.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StatsRecorder/StatsRecorder.h>
@@ -37,7 +38,7 @@ namespace Main
class FlightModeManager
: public Boardcore::InjectableWithDeps<Actuators, Sensors, CanHandler,
- StatsRecorder>,
+ StatsRecorder, AlgoReference>,
public Boardcore::HSM<FlightModeManager>
{
public:
diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.cpp b/src/boards/Main/StateMachines/MEAController/MEAController.cpp
index d1085ed8a4b57907badc941d72d97d5f46193849..334790bc809b588fcc84014f29f387aac5bb3cf1 100644
--- a/src/boards/Main/StateMachines/MEAController/MEAController.cpp
+++ b/src/boards/Main/StateMachines/MEAController/MEAController.cpp
@@ -129,11 +129,10 @@ void MEAController::update()
{
// Perform updates only during this phases
- PressureData baro = getModule<Sensors>()->getCanCCPressLastSample();
- IMUData imu = getModule<Sensors>()->getIMULastSample();
- NASState nas = getModule<NASController>()->getNASState();
- ReferenceValues reference =
- getModule<NASController>()->getReferenceValues();
+ PressureData baro = getModule<Sensors>()->getCanCCPressLastSample();
+ IMUData imu = getModule<Sensors>()->getIMULastSample();
+ NASState nas = getModule<NASController>()->getNASState();
+ ReferenceValues ref = getModule<AlgoReference>()->getReferenceValues();
// TODO: Is this even correct?
float aperture =
@@ -157,7 +156,7 @@ void MEAController::update()
if (nas.timestamp > lastNasTimestamp)
{
- step.withSpeedAndAlt(-nas.vd, reference.refAltitude - nas.d);
+ step.withSpeedAndAlt(-nas.vd, ref.refAltitude - nas.d);
}
mea.update(step);
diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.h b/src/boards/Main/StateMachines/MEAController/MEAController.h
index 54500cd2147d49facb08ed597e790ab23a5674f4..ac705ba925efac1425876f420c5d74da6a0a76b6 100644
--- a/src/boards/Main/StateMachines/MEAController/MEAController.h
+++ b/src/boards/Main/StateMachines/MEAController/MEAController.h
@@ -23,6 +23,7 @@
#pragma once
#include <Main/Actuators/Actuators.h>
+#include <Main/AlgoReference/AlgoReference.h>
#include <Main/BoardScheduler.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/MEAController/MEAController.h>
@@ -39,7 +40,7 @@ namespace Main
class MEAController
: public Boardcore::FSM<MEAController>,
public Boardcore::InjectableWithDeps<BoardScheduler, Actuators, Sensors,
- NASController>
+ NASController, AlgoReference>
{
public:
MEAController();
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index a50d4b7ba84e16c2bb0f2007b80c0870b229564b..d4968b8416734151ae99bbd3685c1cf377fa646f 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -47,18 +47,6 @@ NASController::NASController()
{
EventBroker::getInstance().subscribe(this, TOPIC_NAS);
EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
-
- // TODO: Review this code
- Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
- Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
-
- x(6) = q(0);
- x(7) = q(1);
- x(8) = q(2);
- x(9) = q(3);
-
- nas.setX(x);
- nas.setReferenceValues(ReferenceConfig::defaultReferenceValues);
}
bool NASController::start()
@@ -80,6 +68,21 @@ bool NASController::start()
return false;
}
+ // Initialize reference
+ ReferenceValues ref = getModule<AlgoReference>()->getReferenceValues();
+ nas.setReferenceValues(ref);
+
+ // Initialize state
+ Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
+ Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+
+ x(6) = q(0);
+ x(7) = q(1);
+ x(8) = q(2);
+ x(9) = q(3);
+
+ nas.setX(x);
+
return true;
}
@@ -91,12 +94,6 @@ NASState NASController::getNASState()
return nas.getState();
}
-ReferenceValues NASController::getReferenceValues()
-{
- Lock<FastMutex> lock{nasMutex};
- return nas.getReferenceValues();
-}
-
void NASController::update()
{
NASControllerState curState = state;
@@ -191,13 +188,11 @@ void NASController::calibrate()
Vector3f accAcc = Vector3f::Zero();
Vector3f magAcc = Vector3f::Zero();
- float baroAcc = 0.0f;
// First sample and average the data over a number of samples
for (int i = 0; i < Config::NAS::CALIBRATION_SAMPLES_COUNT; i++)
{
- IMUData imu = sensors->getIMULastSample();
- PressureData baro = sensors->getAtmosPressureLastSample();
+ IMUData imu = sensors->getIMULastSample();
Vector3f acc = static_cast<AccelerometerData>(imu);
Vector3f mag = static_cast<MagnetometerData>(imu);
@@ -205,8 +200,6 @@ void NASController::calibrate()
accAcc += acc;
magAcc += mag;
- baroAcc += baro.pressure;
-
Thread::sleep(Config::NAS::CALIBRATION_SLEEP_TIME);
}
@@ -214,31 +207,16 @@ void NASController::calibrate()
accAcc.normalize();
magAcc /= Config::NAS::CALIBRATION_SAMPLES_COUNT;
magAcc.normalize();
- baroAcc /= Config::NAS::CALIBRATION_SAMPLES_COUNT;
// Use the triad to compute initial state
StateInitializer init;
init.triad(accAcc, magAcc, ReferenceConfig::nedMag);
- Lock<FastMutex> lock{nasMutex};
-
- // Compute reference values
- ReferenceValues reference = nas.getReferenceValues();
- reference.refPressure = baroAcc;
- reference.refAltitude = Aeroutils::relAltitude(
- reference.refPressure, reference.mslPressure, reference.mslTemperature);
-
- // Also updated the reference with the GPS if we have fix
- UBXGPSData gps = sensors->getUBXGPSLastSample();
- if (gps.fix == 3)
- {
- // We do not use the GPS altitude because it sucks
- reference.refLatitude = gps.latitude;
- reference.refLongitude = gps.longitude;
- }
+ ReferenceValues ref = getModule<AlgoReference>()->getReferenceValues();
+ Lock<FastMutex> lock{nasMutex};
nas.setX(init.getInitX());
- nas.setReferenceValues(reference);
+ nas.setReferenceValues(ref);
EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
}
diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main/StateMachines/NASController/NASController.h
index 042c8f927a8e30035b9a5953646eed295d265585..8fac4ed9e7aaf442494e54729c5b7aa1783b6db5 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.h
+++ b/src/boards/Main/StateMachines/NASController/NASController.h
@@ -22,6 +22,7 @@
#pragma once
+#include <Main/AlgoReference/AlgoReference.h>
#include <Main/BoardScheduler.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/NASController/NASControllerData.h>
@@ -37,7 +38,7 @@ namespace Main
class NASController
: public Boardcore::FSM<NASController>,
public Boardcore::InjectableWithDeps<BoardScheduler, Sensors,
- StatsRecorder>
+ StatsRecorder, AlgoReference>
{
public:
NASController();
@@ -45,7 +46,6 @@ public:
[[nodiscard]] bool start() override;
Boardcore::NASState getNASState();
- Boardcore::ReferenceValues getReferenceValues();
NASControllerState getState();
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
index db4900fe5aba361704eab00801887567a1a5e6f0..1ce1f89fae616c48c04652903fb451648c3c0187 100644
--- a/src/entrypoints/Main/main-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -21,6 +21,7 @@
*/
#include <Main/Actuators/Actuators.h>
+#include <Main/AlgoReference/AlgoReference.h>
#include <Main/BoardScheduler.h>
#include <Main/Buses.h>
#include <Main/CanHandler/CanHandler.h>
@@ -60,6 +61,7 @@ int main()
CanHandler *canHandler = new CanHandler();
PinHandler *pinHandler = new PinHandler();
FlightModeManager *fmm = new FlightModeManager();
+ AlgoReference *ref = new AlgoReference();
ADAController *ada = new ADAController();
NASController *nas = new NASController();
MEAController *mea = new MEAController();
@@ -74,6 +76,7 @@ int main()
manager.insert<CanHandler>(canHandler) &&
manager.insert<PinHandler>(pinHandler) &&
manager.insert<FlightModeManager>(fmm) &&
+ manager.insert<AlgoReference>(ref) &&
manager.insert<ADAController>(ada) &&
manager.insert<NASController>(nas) &&
manager.insert<MEAController>(mea) &&