diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h index 54202645552ae077cbcb8071ff2575b33350733a..0da418ccf9fdd0b1620e72bbb034244703d45576 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 0a3509d85862fb46773a9a7f52addd78faa773c9..58d70122a398ecbbbc6b6745abdb1b3f5bd9a173 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 f3ee9c74a9e7842b14e7f692177544f7a0a0c3b7..8704d2bc7d05fa4378f5b5dffb894f9397a9490a 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 fea18ecf6b00cc44cfe95bbe0ac80bba6d220a80..59b1b427e98d276cdf6950e685d57dd4ab8eaac8 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 eddc8f04223c50cb3c326bc320eaffa252db3ae9..2ea33a82c6561bea2f8abf4683d7e2e63aa04f4a 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 0000000000000000000000000000000000000000..63b52635c24a676d9dafd1dce8a5f6c27b5ac106 --- /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 177ceaf0b4a64f3066bb3c549cdbde6f6676a741..a3bee0bd2dd78035f79a69c3892deebd10a15a0a 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();