From 94502b72dbd3cfed3da1179a12fd3063db21205f Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Sat, 10 Sep 2022 15:00:09 +0000
Subject: [PATCH] [Algorithms] Updated reference values
---
src/boards/Main/Configs/NASConfig.h | 49 +++++-------
.../ADAController/ADAController.cpp | 2 +
.../NASController/NASController.cpp | 2 +
src/boards/Payload/Configs/NASConfig.h | 51 +++++-------
.../NASController/NASController.cpp | 2 +
src/boards/common/ReferenceConfig.h | 79 +++++++++++++++++++
.../test-hil/test-hil.cpp | 17 ++--
7 files changed, 132 insertions(+), 70 deletions(-)
create mode 100644 src/boards/common/ReferenceConfig.h
diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h
index 542026455..0da418ccf 100644
--- a/src/boards/Main/Configs/NASConfig.h
+++ b/src/boards/Main/Configs/NASConfig.h
@@ -24,6 +24,7 @@
#include <algorithms/NAS/NASConfig.h>
#include <algorithms/ReferenceValues.h>
+#include <common/ReferenceConfig.h>
namespace Main
{
@@ -36,38 +37,24 @@ constexpr uint32_t UPDATE_PERIOD = 20; // 50 hz
constexpr int CALIBRATION_SAMPLES_COUNT = 20;
constexpr int CALIBRATION_SLEEP_TIME = 100; // [ms]
-// Magnetic field in Milan
-const Eigen::Vector3f nedMag(0.4747, 0.0276, 0.8797);
-
static const Boardcore::NASConfig config = {
- UPDATE_PERIOD / 1000.0, // T
- 0.0001f, // SIGMA_BETA
- 0.3f, // SIGMA_W
- 0.1f, // SIGMA_MAG
- 10.0f, // SIGMA_GPS
- 4.3f, // SIGMA_BAR
- 10.0f, // SIGMA_POS
- 10.0f, // SIGMA_VEL
- 10.0f, // SIGMA_PITOT
- 1.0f, // P_POS
- 10.0f, // P_POS_VERTICAL
- 1.0f, // P_VEL
- 10.0f, // P_VEL_VERTICAL
- 0.01f, // P_ATT
- 0.01f, // P_BIAS
- 6.0f, // SATS_NUM
- nedMag // NED_MAG
-};
-
-// Reference values for EuRoC
-static const Boardcore::ReferenceValues defaultReferenceValues = {
- 160.0f, // [m] Altitude
- 99418.0f, // [Pa] Pressure
- 287.11f, // [K] Temperature
- 39.389733, // [deg] Start latitude
- -8.288992, // [deg] Start longitude
- Boardcore::Constants::MSL_PRESSURE,
- Boardcore::Constants::MSL_TEMPERATURE,
+ UPDATE_PERIOD / 1000.0, // T
+ 0.0001f, // SIGMA_BETA
+ 0.3f, // SIGMA_W
+ 0.1f, // SIGMA_MAG
+ 10.0f, // SIGMA_GPS
+ 4.3f, // SIGMA_BAR
+ 10.0f, // SIGMA_POS
+ 10.0f, // SIGMA_VEL
+ 10.0f, // SIGMA_PITOT
+ 1.0f, // P_POS
+ 10.0f, // P_POS_VERTICAL
+ 1.0f, // P_VEL
+ 10.0f, // P_VEL_VERTICAL
+ 0.01f, // P_ATT
+ 0.01f, // P_BIAS
+ 6.0f, // SATS_NUM
+ Common::ReferenceConfig::nedMag // NED_MAG
};
} // namespace NASConfig
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index 0a3509d85..58d70122a 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -27,6 +27,7 @@
#include <Main/Configs/NASConfig.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/AirBrakesController/AirBrakesController.h>
+#include <common/ReferenceConfig.h>
#include <common/events/Events.h>
#include <drivers/timer/TimestampTimer.h>
#include <events/EventBroker.h>
@@ -38,6 +39,7 @@ using namespace Main::ADAConfig;
using namespace Main::AirBrakesControllerConfig;
using namespace Main::NASConfig;
using namespace Common;
+using namespace Common::ReferenceConfig;
namespace Main
{
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index f3ee9c74a..8704d2bc7 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -26,6 +26,7 @@
#include <Main/Configs/NASConfig.h>
#include <Main/Sensors/Sensors.h>
#include <algorithms/NAS/StateInitializer.h>
+#include <common/ReferenceConfig.h>
#include <common/events/Events.h>
using namespace std;
@@ -33,6 +34,7 @@ using namespace Eigen;
using namespace Boardcore;
using namespace Main::NASConfig;
using namespace Common;
+using namespace Common::ReferenceConfig;
namespace Main
{
diff --git a/src/boards/Payload/Configs/NASConfig.h b/src/boards/Payload/Configs/NASConfig.h
index fea18ecf6..59b1b427e 100644
--- a/src/boards/Payload/Configs/NASConfig.h
+++ b/src/boards/Payload/Configs/NASConfig.h
@@ -24,6 +24,9 @@
#include <algorithms/NAS/NASConfig.h>
#include <algorithms/ReferenceValues.h>
+#include <common/ReferenceConfig.h>
+
+using namespace Common::ReferenceConfig;
namespace Payload
{
@@ -36,38 +39,24 @@ constexpr uint32_t UPDATE_PERIOD = 20; // 50 hz
constexpr int CALIBRATION_SAMPLES_COUNT = 20;
constexpr int CALIBRATION_SLEEP_TIME = 100; // [ms]
-// Magnetic field in Milan
-const Eigen::Vector3f nedMag(0.4747, 0.0276, 0.8797);
-
static const Boardcore::NASConfig config = {
- UPDATE_PERIOD / 1000.0, // T
- 0.0001f, // SIGMA_BETA
- 0.3f, // SIGMA_W
- 0.1f, // SIGMA_MAG
- 10.0f, // SIGMA_GPS
- 4.3f, // SIGMA_BAR
- 10.0f, // SIGMA_POS
- 10.0f, // SIGMA_VEL
- 10.0f, // SIGMA_PITOT
- 1.0f, // P_POS
- 10.0f, // P_POS_VERTICAL
- 1.0f, // P_VEL
- 10.0f, // P_VEL_VERTICAL
- 0.01f, // P_ATT
- 0.01f, // P_BIAS
- 6.0f, // SATS_NUM
- nedMag // NED_MAG
-};
-
-// Reference values for Milan
-static const Boardcore::ReferenceValues defaultReferenceValues = {
- 130.0f, // Altitude
- 100000.0f, // Pressure
- 25.0f + 275.15f, // Temperature
- 45.501077, // Start latitude
- 9.1563935, // Start longitude
- Boardcore::Constants::MSL_PRESSURE,
- Boardcore::Constants::MSL_TEMPERATURE,
+ UPDATE_PERIOD / 1000.0, // T
+ 0.0001f, // SIGMA_BETA
+ 0.3f, // SIGMA_W
+ 0.1f, // SIGMA_MAG
+ 10.0f, // SIGMA_GPS
+ 4.3f, // SIGMA_BAR
+ 10.0f, // SIGMA_POS
+ 10.0f, // SIGMA_VEL
+ 10.0f, // SIGMA_PITOT
+ 1.0f, // P_POS
+ 10.0f, // P_POS_VERTICAL
+ 1.0f, // P_VEL
+ 10.0f, // P_VEL_VERTICAL
+ 0.01f, // P_ATT
+ 0.01f, // P_BIAS
+ 6.0f, // SATS_NUM
+ Common::ReferenceConfig::nedMag // NED_MAG
};
} // namespace NASConfig
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp
index eddc8f042..2ea33a82c 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp
@@ -28,6 +28,7 @@
#include <Payload/FlightStatsRecorder/FlightStatsRecorder.h>
#include <Payload/Sensors/Sensors.h>
#include <algorithms/NAS/StateInitializer.h>
+#include <common/ReferenceConfig.h>
#include <common/events/Events.h>
using namespace std;
@@ -35,6 +36,7 @@ using namespace Eigen;
using namespace Boardcore;
using namespace Payload::NASConfig;
using namespace Common;
+using namespace Common::ReferenceConfig;
namespace Payload
{
diff --git a/src/boards/common/ReferenceConfig.h b/src/boards/common/ReferenceConfig.h
new file mode 100644
index 000000000..63b52635c
--- /dev/null
+++ b/src/boards/common/ReferenceConfig.h
@@ -0,0 +1,79 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <algorithms/ReferenceValues.h>
+
+namespace Common
+{
+
+namespace ReferenceConfig
+{
+
+#if defined(EUROC)
+
+static const Boardcore::ReferenceValues defaultReferenceValues = {
+ 160.0, // [m] Altitude
+ 99418.0, // [Pa] Pressure
+ 287.11, // [K] Temperature
+ 39.389733, // [deg] Start latitude
+ -8.288992, // [deg] Start longitude
+ Boardcore::Constants::MSL_PRESSURE,
+ Boardcore::Constants::MSL_TEMPERATURE,
+};
+
+const Eigen::Vector3f nedMag(0.5939, -0.0126, 0.8044);
+
+#elif defined(ROCCARASO)
+
+static const Boardcore::ReferenceValues defaultReferenceValues = {
+ 1414.0, // [m] Altitude
+ 85452.0, // [Pa] Pressure
+ 278.95, // [K] Temperature
+ 41.8086605, // [deg] Start latitude
+ 14.0543387, // [deg] Start longitude
+ Boardcore::Constants::MSL_PRESSURE,
+ Boardcore::Constants::MSL_TEMPERATURE,
+};
+
+const Eigen::Vector3f nedMag(0.5244, 0.0368, 0.8507);
+
+#else // Milan
+
+static const Boardcore::ReferenceValues defaultReferenceValues = {
+ 135.0, // [m] Altitude
+ 99714.0, // [Pa] Pressure
+ 278.27, // [K] Temperature
+ 45.50106793771145, // [deg] Start latitude
+ 9.156376900740167, // [deg] Start longitude
+ Boardcore::Constants::MSL_PRESSURE,
+ Boardcore::Constants::MSL_TEMPERATURE,
+};
+
+const Eigen::Vector3f nedMag(0.4732, 0.0272, 0.8805);
+
+#endif
+
+} // namespace ReferenceConfig
+
+} // namespace Common
diff --git a/src/tests/hardware_in_the_loop/test-hil/test-hil.cpp b/src/tests/hardware_in_the_loop/test-hil/test-hil.cpp
index 177ceaf0b..a3bee0bd2 100644
--- a/src/tests/hardware_in_the_loop/test-hil/test-hil.cpp
+++ b/src/tests/hardware_in_the_loop/test-hil/test-hil.cpp
@@ -19,6 +19,8 @@
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/
+#include <common/ReferenceConfig.h>
+
#include <cstdio>
#include <cstdlib>
@@ -75,6 +77,7 @@ using namespace std;
using namespace miosix;
using namespace Boardcore;
using namespace Common;
+using namespace Common::ReferenceConfig;
Thread* t;
@@ -253,10 +256,9 @@ int main()
{ HIL::getInstance().getElaboratedData()->addADAState(state); });
// setting initial reference values
- ada_controller.setReferenceValues(
- {Main::NASConfig::defaultReferenceValues.refAltitude,
- Main::NASConfig::defaultReferenceValues.refPressure,
- Main::NASConfig::defaultReferenceValues.refTemperature});
+ ada_controller.setReferenceValues({defaultReferenceValues.refAltitude,
+ defaultReferenceValues.refPressure,
+ defaultReferenceValues.refTemperature});
TRACE("Starting ada\n");
ada_controller.start();
@@ -273,10 +275,9 @@ int main()
{ HIL::getInstance().getElaboratedData()->addNASState(state); });
// setting initial reference values
- nas_controller.setReferenceValues(
- {Main::NASConfig::defaultReferenceValues.refAltitude,
- Main::NASConfig::defaultReferenceValues.refPressure,
- Main::NASConfig::defaultReferenceValues.refTemperature});
+ nas_controller.setReferenceValues({defaultReferenceValues.refAltitude,
+ defaultReferenceValues.refPressure,
+ defaultReferenceValues.refTemperature});
TRACE("Starting nas\n");
nas_controller.start();
--
GitLab