From df8a2c2acf3cf428639f02b6e80636672a4b824b Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Thu, 1 Sep 2022 15:10:03 +0200
Subject: [PATCH] [ADA][NAS] Fixed altitude estimation
---
.vscode/settings.json | 2 +
src/boards/Main/Actuators/Actuators.cpp | 5 -
src/boards/Main/Actuators/Actuators.h | 9 +-
src/boards/Main/Configs/ADAConfig.h | 15 +-
.../Main/Configs/AirBrakesControllerConfig.h | 8 +-
src/boards/Main/Configs/NASConfig.h | 16 +-
src/boards/Main/Sensors/HILSensors.cpp | 279 ------------------
.../ADAController/ADAController.cpp | 86 ++++--
.../ADAController/ADAController.h | 4 +-
.../ADAController/ADAControllerData.h | 1 +
.../FlightModeManager/FlightModeManager.cpp | 32 +-
.../NASController/NASController.cpp | 94 +++---
.../NASController/NASController.h | 3 +-
src/boards/Main/TMRepository/TMRepository.cpp | 18 +-
.../Parafoil/TMRepository/TMRepository.cpp | 8 +-
src/boards/Payload/Configs/NASConfig.h | 4 +-
src/boards/Payload/Configs/WingConfig.h | 4 -
.../FlightModeManager/FlightModeManager.cpp | 15 +-
.../NASController/NASController.cpp | 146 ++++-----
.../NASController/NASController.h | 3 +-
.../Payload/TMRepository/TMRepository.cpp | 8 +-
.../Payload/Wing/AutomaticWingAlgorithm.cpp | 2 +-
src/boards/common/events/Events.h | 2 +
src/entrypoints/Main/main-entry.cpp | 6 -
.../HIL_actuators/HILServo.h | 81 +----
.../HILMockAerobrakeAlgorithm.h | 2 +-
.../test-hil/test-hil.cpp | 12 +-
27 files changed, 266 insertions(+), 599 deletions(-)
delete mode 100644 src/boards/Main/Sensors/HILSensors.cpp
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 268838c91..94e2159e5 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -125,7 +125,9 @@
"mavlink",
"miosix",
"Nidasio",
+ "pitot",
"telecommands",
+ "UBXGPS",
"usart",
"VREF",
"Xbee"
diff --git a/src/boards/Main/Actuators/Actuators.cpp b/src/boards/Main/Actuators/Actuators.cpp
index cb07950ed..faa1e06f4 100644
--- a/src/boards/Main/Actuators/Actuators.cpp
+++ b/src/boards/Main/Actuators/Actuators.cpp
@@ -161,15 +161,10 @@ Actuators::Actuators()
: ledRed(leds::red::getPin()), ledGreen(leds::green::getPin()),
ledBlue(leds::blue::getPin()), cutter1(cutter::enable::getPin()),
cutter1Backup(cutter::enable::getPin()), buzzer(buzzer::drive::getPin()),
-#ifndef HILSimulation
servoAirbrakes(ABK_SERVO_TIMER, ABK_SERVO_PWM_CH, ABK_SERVO_MIN_PULSE,
ABK_SERVO_MAX_PULSE),
servoExpulsion(DPL_SERVO_TIMER, DPL_SERVO_PWM_CH, DPL_SERVO_MIN_PULSE,
DPL_SERVO_MAX_PULSE)
-#else // HILSimulation
- servoAirbrakes(), servoExpulsion()
-#endif // HILSimulation
-
{
}
diff --git a/src/boards/Main/Actuators/Actuators.h b/src/boards/Main/Actuators/Actuators.h
index e83f167d7..9b11a48f7 100644
--- a/src/boards/Main/Actuators/Actuators.h
+++ b/src/boards/Main/Actuators/Actuators.h
@@ -23,12 +23,10 @@
#pragma once
#include <Singleton.h>
+#include <actuators/Servo/Servo.h>
#include <common/Mavlink.h>
#include <interfaces/gpio.h>
-#ifndef HILSimulation
-#include <actuators/Servo/Servo.h>
-#else // HILSimulation
-#include "HIL.h"
+#ifdef HILSimulation
#include "HIL_actuators/HILServo.h"
#endif // HILSimulation
@@ -92,11 +90,10 @@ private:
#ifndef HILSimulation
Boardcore::Servo servoAirbrakes;
- Boardcore::Servo servoExpulsion;
#else // HILSimulation
HILServo servoAirbrakes;
- HILServo servoExpulsion;
#endif // HILSimulation
+ Boardcore::Servo servoExpulsion;
};
} // namespace Main
diff --git a/src/boards/Main/Configs/ADAConfig.h b/src/boards/Main/Configs/ADAConfig.h
index 0f3a07690..c7d1f037b 100644
--- a/src/boards/Main/Configs/ADAConfig.h
+++ b/src/boards/Main/Configs/ADAConfig.h
@@ -37,25 +37,14 @@ static const float SAMPLING_PERIOD = UPDATE_PERIOD / 1000.0f; // in seconds
// to make it more responsive during the propulsive phase
static const float KALMAN_INITIAL_ACCELERATION = -500;
-#ifdef EUROC
-constexpr int SHADOW_MODE_TIMEOUT = 16 * 1000; // [ms]
-#else
-constexpr int SHADOW_MODE_TIMEOUT = 8 * 1000; // [ms]
-#endif
-
constexpr int PRES_STAB_TIMEOUT = 5 * 1000; // [ms]
-// Default reference values settings
#ifdef EUROC
-static const float DEFAULT_REFERENCE_ALTITUDE = 160.0f;
-static const float DEFAULT_REFERENCE_PRESSURE = 100022.4f;
+constexpr int SHADOW_MODE_TIMEOUT = 16 * 1000; // [ms]
#else
-static const float DEFAULT_REFERENCE_ALTITUDE = 1420.0f;
-static const float DEFAULT_REFERENCE_PRESSURE = 85389.4f;
+constexpr int SHADOW_MODE_TIMEOUT = 8 * 1000; // [ms]
#endif
-static const float DEFAULT_REFERENCE_TEMPERATURE = 288.15f;
-
// When the vertical speed is smaller than this value, apogee is detected.
// If equal to 0 -> Exact apogee
// If greater than 0 -> Apogee detected ahead of time (while still going up)
diff --git a/src/boards/Main/Configs/AirBrakesControllerConfig.h b/src/boards/Main/Configs/AirBrakesControllerConfig.h
index e4dbcf54b..c658c328a 100644
--- a/src/boards/Main/Configs/AirBrakesControllerConfig.h
+++ b/src/boards/Main/Configs/AirBrakesControllerConfig.h
@@ -28,9 +28,11 @@
namespace Main
{
-namespace AirBrakesControllerConfigs
+namespace AirBrakesControllerConfig
{
+constexpr uint32_t UPDATE_PERIOD = 100; // 10 hz
+
#ifdef EUROC
static constexpr int SHADOW_MODE_TIMEOUT = 5 * 1000;
#else
@@ -72,9 +74,9 @@ static const Boardcore::AirBrakesConfig ABK_CONFIG{
EIGEN_PI / 180.0,
.KP = 20,
.KI = 5,
- .TS = 0.1,
+ .TS = UPDATE_PERIOD / 1000.0,
};
-} // namespace AirBrakesControllerConfigs
+} // namespace AirBrakesControllerConfig
} // namespace Main
diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h
index 42a6899ae..542026455 100644
--- a/src/boards/Main/Configs/NASConfig.h
+++ b/src/boards/Main/Configs/NASConfig.h
@@ -32,7 +32,9 @@ namespace NASConfig
{
constexpr uint32_t UPDATE_PERIOD = 20; // 50 hz
-constexpr uint8_t MEAN_COUNT = 10;
+
+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);
@@ -57,13 +59,13 @@ static const Boardcore::NASConfig config = {
nedMag // NED_MAG
};
-// Reference values for Milan
+// Reference values for EuRoC
static const Boardcore::ReferenceValues defaultReferenceValues = {
- 130.0f, // [m] Altitude
- 99934.0f, // [Pa] Pressure
- 34.5f + 273.15f, // [K] Temperature
- 45.501077, // [deg] Start latitude
- 9.1563935, // [deg] Start longitude
+ 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,
};
diff --git a/src/boards/Main/Sensors/HILSensors.cpp b/src/boards/Main/Sensors/HILSensors.cpp
deleted file mode 100644
index 63e8e13ac..000000000
--- a/src/boards/Main/Sensors/HILSensors.cpp
+++ /dev/null
@@ -1,279 +0,0 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Emilio Corigliano
- *
- * 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 "HIL_sensors/HILSensors.h"
-
-#include <Main/Buses.h>
-#include <Main/Configs/SensorsConfig.h>
-#include <common/events/Events.h>
-#include <drivers/interrupt/external_interrupts.h>
-#include <events/EventBroker.h>
-
-#include "Main/BoardScheduler.h"
-#include "Sensors.h"
-
-using namespace std;
-using namespace miosix;
-using namespace Boardcore;
-using namespace Common;
-using namespace Main::SensorsConfig;
-
-namespace Main
-{
-
-bool Sensors::start()
-{
- sensorManager->enableAllSensors();
- return sensorManager->start();
-}
-
-bool Sensors::isStarted() { return sensorManager->areAllSensorsInitialized(); }
-
-BMX160Data Sensors::getBMX160LastSample()
-{
- PauseKernelLock lock;
-
- auto imuData = state.imu->getLastSample();
- BMX160Data data;
-
- data.accelerationTimestamp = imuData.accelerationTimestamp;
- data.accelerationX = imuData.accelerationX;
- data.accelerationY = imuData.accelerationY;
- data.accelerationZ = imuData.accelerationZ;
- data.angularVelocityTimestamp = imuData.angularVelocityTimestamp;
- data.angularVelocityX = imuData.angularVelocityX;
- data.angularVelocityY = imuData.angularVelocityY;
- data.angularVelocityZ = imuData.angularVelocityZ;
- data.magneticFieldTimestamp = imuData.magneticFieldTimestamp;
- data.magneticFieldX = imuData.magneticFieldX;
- data.magneticFieldY = imuData.magneticFieldY;
- data.magneticFieldZ = imuData.magneticFieldZ;
-
- return data;
-}
-
-BMX160WithCorrectionData Sensors::getBMX160WithCorrectionLastSample()
-{
- PauseKernelLock lock;
- auto imuData = state.imu->getLastSample();
- BMX160WithCorrectionData data;
-
- data.accelerationTimestamp = imuData.accelerationTimestamp;
- data.accelerationX = imuData.accelerationX;
- data.accelerationY = imuData.accelerationY;
- data.accelerationZ = imuData.accelerationZ;
- data.angularVelocityTimestamp = imuData.angularVelocityTimestamp;
- data.angularVelocityX = imuData.angularVelocityX;
- data.angularVelocityY = imuData.angularVelocityY;
- data.angularVelocityZ = imuData.angularVelocityZ;
- data.magneticFieldTimestamp = imuData.magneticFieldTimestamp;
- data.magneticFieldX = imuData.magneticFieldX;
- data.magneticFieldY = imuData.magneticFieldY;
- data.magneticFieldZ = imuData.magneticFieldZ;
-
- return data;
-}
-
-MPU9250Data Sensors::getMPU9250LastSample()
-{
- PauseKernelLock lock;
- auto imuData = state.imu->getLastSample();
- auto tempData = state.temperature->getLastSample();
- MPU9250Data data;
-
- data.accelerationTimestamp = imuData.accelerationTimestamp;
- data.accelerationX = imuData.accelerationX;
- data.accelerationY = imuData.accelerationY;
- data.accelerationZ = imuData.accelerationZ;
- data.angularVelocityTimestamp = imuData.angularVelocityTimestamp;
- data.angularVelocityX = imuData.angularVelocityX;
- data.angularVelocityY = imuData.angularVelocityY;
- data.angularVelocityZ = imuData.angularVelocityZ;
- data.magneticFieldTimestamp = imuData.magneticFieldTimestamp;
- data.magneticFieldX = imuData.magneticFieldX;
- data.magneticFieldY = imuData.magneticFieldY;
- data.magneticFieldZ = imuData.magneticFieldZ;
- data.temperatureTimestamp = tempData.temperatureTimestamp;
- data.temperature = tempData.temperature;
-
- return data;
-}
-
-MS5803Data Sensors::getMS5803LastSample()
-{
- PauseKernelLock lock;
- auto baroData = state.barometer->getLastSample();
- auto tempData = state.temperature->getLastSample();
-
- return MS5803Data(baroData.pressureTimestamp, baroData.pressure,
- tempData.temperatureTimestamp, tempData.temperature);
-}
-
-UBXGPSData Sensors::getUbxGpsLastSample()
-{
- miosix::PauseKernelLock lock;
- auto data = state.gps->getLastSample();
- UBXGPSData ubxData;
-
- ubxData.gpsTimestamp = data.gpsTimestamp;
- ubxData.latitude = data.latitude;
- ubxData.longitude = data.longitude;
- ubxData.height = data.height;
- ubxData.velocityNorth = data.velocityNorth;
- ubxData.velocityEast = data.velocityEast;
- ubxData.velocityDown = data.velocityDown;
- ubxData.speed = data.speed;
- ubxData.track = data.track;
- ubxData.positionDOP = data.positionDOP;
- ubxData.satellites = data.satellites;
- ubxData.fix = data.fix;
-
- return ubxData;
-}
-
-ADS131M04Data Sensors::getADS131M04LastSample()
-{
- PauseKernelLock lock;
- return ADS131M04Data{};
-}
-
-MPXH6115AData Sensors::getStaticPressureLastSample()
-{
- PauseKernelLock lock;
- auto baroData = state.barometer->getLastSample();
-
- MPXH6115AData data;
- data.pressureTimestamp = baroData.pressureTimestamp;
- data.pressure = baroData.pressure;
-
- return data;
-}
-
-Boardcore::SSCDRRN015PDAData Sensors::getDifferentialPressureLastSample()
-{
- PauseKernelLock lock;
- auto pitotData = state.pitot->getLastSample();
-
- Boardcore::SSCDRRN015PDAData data;
- data.pressureTimestamp = pitotData.pressureTimestamp;
- data.pressure = pitotData.pressure;
-
- return data;
-};
-
-MPXH6400AData Sensors::getDplPressureLastSample()
-{
- PauseKernelLock lock;
- return MPXH6400AData{};
-}
-
-AnalogLoadCellData Sensors::getLoadCellLastSample()
-{
- PauseKernelLock lock;
- return AnalogLoadCellData{};
-}
-
-BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample()
-{
- PauseKernelLock lock;
- return BatteryVoltageSensorData{};
-}
-
-InternalADCData Sensors::getInternalADCLastSample()
-{
- PauseKernelLock lock;
- return InternalADCData{};
-}
-
-void Sensors::calibrate() {}
-
-std::map<string, bool> Sensors::getSensorsState()
-{
- std::map<string, bool> sensorsState;
-
- for (auto sensor : sensorsMap)
- sensorsState[sensor.second.id] =
- sensorManager->getSensorInfo(sensor.first).isInitialized;
-
- return sensorsState;
-}
-
-Sensors::Sensors()
-{
- // Definition of the fake sensors for the simulation
- state.accelerometer = new HILAccelerometer(N_DATA_ACCEL);
- state.gyro = new HILGyroscope(N_DATA_GYRO);
- state.magnetometer = new HILMagnetometer(N_DATA_MAGN);
- state.imu = new HILImu(N_DATA_IMU);
- state.barometer = new HILBarometer(N_DATA_BARO);
- state.pitot = new HILPitot(N_DATA_PITOT);
- state.gps = new HILGps(N_DATA_GPS);
- state.temperature = new HILTemperature(N_DATA_TEMP);
- state.kalman = new HILKalman(N_DATA_KALM);
-
- sensorsMap = {{state.accelerometer, accelConfig},
- {state.gyro, gyroConfig},
- {state.magnetometer, magnConfig},
- {state.imu, imuConfig},
- {state.barometer, baroConfig},
- {state.pitot, pitotConfig},
- {state.gps, gpsConfig},
- {state.temperature, tempConfig},
- {state.kalman, kalmConfig}};
-
- // instantiate the sensor manager with the given scheduler
- sensorManager = new SensorManager(
- sensorsMap, &Main::BoardScheduler::getInstance().getScheduler());
-
- EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM);
-}
-
-Sensors::~Sensors()
-{
- delete state.accelerometer;
- delete state.gyro;
- delete state.magnetometer;
- delete state.imu;
- delete state.barometer;
- delete state.pitot;
- delete state.gps;
- delete state.temperature;
- delete state.kalman;
-
- sensorManager->stop();
- delete sensorManager;
-}
-
-void Sensors::bmx160Init() {}
-void Sensors::bmx160Callback() {}
-void Sensors::bmx160WithCorrectionInit() {}
-void Sensors::mpu9250Init() {}
-void Sensors::ms5803Init() {}
-void Sensors::ubxGpsInit() {}
-void Sensors::ads131m04Init() {}
-void Sensors::staticPressureInit() {}
-void Sensors::dplPressureInit() {}
-void Sensors::loadCellInit() {}
-void Sensors::batteryVoltageInit() {}
-void Sensors::internalAdcInit() {}
-
-} // namespace Main
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index 61e7f4f4b..d86908816 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -24,6 +24,7 @@
#include <Main/BoardScheduler.h>
#include <Main/Configs/AirBrakesControllerConfig.h>
+#include <Main/Configs/NASConfig.h>
#include <Main/Sensors/Sensors.h>
#include <Main/StateMachines/AirBrakesController/AirBrakesController.h>
#include <common/events/Events.h>
@@ -34,7 +35,8 @@
using namespace miosix;
using namespace Boardcore;
using namespace Main::ADAConfig;
-using namespace Main::AirBrakesControllerConfigs;
+using namespace Main::AirBrakesControllerConfig;
+using namespace Main::NASConfig;
using namespace Common;
namespace Main
@@ -56,13 +58,9 @@ void ADAController::update()
switch (status.state)
{
- case ADAControllerState::CALIBRATING:
+ case ADAControllerState::ARMED:
{
- // TODO: Implement calibration
-
- EventBroker::getInstance().post(ADA_READY, TOPIC_ADA);
-
- break;
+ ada.update(barometerData.pressure);
}
case ADAControllerState::SHADOW_MODE:
{
@@ -200,6 +198,7 @@ void ADAController::update()
case ADAControllerState::UNINIT:
case ADAControllerState::IDLE:
case ADAControllerState::READY:
+ case ADAControllerState::CALIBRATING:
case ADAControllerState::LANDED:
{
}
@@ -213,6 +212,31 @@ void ADAController::update()
#endif // HILSimulation
}
+void ADAController::calibrate()
+{
+ Stats pressure;
+
+ for (int i = 0; i < CALIBRATION_SAMPLES_COUNT; i++)
+ {
+ auto data = Sensors::getInstance().getMS5803LastSample();
+ pressure.add(data.pressure);
+
+ miosix::Thread::sleep(CALIBRATION_SLEEP_TIME);
+ }
+
+ // Set the pressure and temperature reference
+ ReferenceValues reference = ada.getReferenceValues();
+ reference.refPressure = pressure.getStats().mean;
+
+ // Update the algorithm reference values
+ {
+ miosix::PauseKernelLock l;
+ ada.setReferenceValues(reference);
+ }
+
+ EventBroker::getInstance().post(ADA_READY, TOPIC_ADA);
+}
+
ADAControllerStatus ADAController::getStatus()
{
PauseKernelLock lock;
@@ -237,7 +261,7 @@ void ADAController::setReferenceAltitude(float altitude)
miosix::PauseKernelLock l;
ReferenceValues reference = ada.getReferenceValues();
- reference.altitude = altitude;
+ reference.refAltitude = altitude;
ada.setReferenceValues(reference);
}
@@ -248,7 +272,7 @@ void ADAController::setReferenceTemperature(float temperature)
miosix::PauseKernelLock l;
ReferenceValues reference = ada.getReferenceValues();
- reference.temperature = temperature;
+ reference.refTemperature = temperature;
ada.setReferenceValues(reference);
}
@@ -287,9 +311,10 @@ void ADAController::state_calibrating(const Event& event)
{
case EV_ENTRY:
{
- logStatus(ADAControllerState::CALIBRATING);
+ // Calibrate the ADA
+ calibrate();
- return calibrate();
+ return logStatus(ADAControllerState::CALIBRATING);
}
case ADA_READY:
{
@@ -314,6 +339,31 @@ void ADAController::state_ready(const Event& event)
{
return transition(&ADAController::state_calibrating);
}
+ case ADA_FORCE_START:
+ case FLIGHT_ARMED:
+ {
+ return transition(&ADAController::state_armed);
+ }
+ case FLIGHT_MISSION_TIMEOUT:
+ {
+ return transition(&ADAController::state_landed);
+ }
+ }
+}
+
+void ADAController::state_armed(const Boardcore::Event& event)
+{
+ switch (event)
+ {
+ case EV_ENTRY:
+ {
+ return logStatus(ADAControllerState::ARMED);
+ }
+ case ADA_FORCE_STOP:
+ case FLIGHT_DISARMED:
+ {
+ return transition(&ADAController::state_ready);
+ }
case FLIGHT_LIFTOFF:
{
return transition(&ADAController::state_shadow_mode);
@@ -463,14 +513,13 @@ void ADAController::setUpdateDataFunction(
}
ADAController::ADAController()
- : FSM(&ADAController::state_idle),
- ada({DEFAULT_REFERENCE_ALTITUDE, DEFAULT_REFERENCE_PRESSURE,
- DEFAULT_REFERENCE_TEMPERATURE},
- getADAKalmanConfig()),
+ : FSM(&ADAController::state_idle), ada(getADAKalmanConfig()),
updateData([](Boardcore::ADAState) {})
{
EventBroker::getInstance().subscribe(this, TOPIC_ADA);
EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+
+ ada.setReferenceValues(defaultReferenceValues);
}
ADAController::~ADAController()
@@ -510,13 +559,8 @@ ADA::KalmanFilter::KalmanConfig ADAController::getADAKalmanConfig()
Q_INIT,
R_INIT,
P_INIT,
- ADA::KalmanFilter::CVectorN(DEFAULT_REFERENCE_PRESSURE, 0,
+ ADA::KalmanFilter::CVectorN(ada.getReferenceValues().refPressure, 0,
KALMAN_INITIAL_ACCELERATION)};
}
-void ADAController::calibrate()
-{
- // ...
-}
-
} // namespace Main
diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main/StateMachines/ADAController/ADAController.h
index e32a8e0c5..a6740a992 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.h
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.h
@@ -42,6 +42,7 @@ public:
bool start() override;
void update();
+ void calibrate();
void setDeploymentAltitude(float altitude);
void setReferenceAltitude(float altitude);
@@ -55,6 +56,7 @@ public:
void state_idle(const Boardcore::Event& event);
void state_calibrating(const Boardcore::Event& event);
void state_ready(const Boardcore::Event& event);
+ void state_armed(const Boardcore::Event& event);
void state_shadow_mode(const Boardcore::Event& event);
void state_active(const Boardcore::Event& event);
void state_pressure_stabilization(const Boardcore::Event& event);
@@ -75,8 +77,6 @@ private:
Boardcore::ADA::KalmanFilter::KalmanConfig getADAKalmanConfig();
- void calibrate();
-
Boardcore::ADA ada;
uint16_t detectedApogeeEvents = 0;
diff --git a/src/boards/Main/StateMachines/ADAController/ADAControllerData.h b/src/boards/Main/StateMachines/ADAController/ADAControllerData.h
index db8eee607..f0ee6ce62 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAControllerData.h
+++ b/src/boards/Main/StateMachines/ADAController/ADAControllerData.h
@@ -36,6 +36,7 @@ enum class ADAControllerState : uint8_t
IDLE,
CALIBRATING,
READY,
+ ARMED,
SHADOW_MODE,
ACTIVE,
PRESSURE_STABILIZATION,
diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
index 15a9ce79c..40aaf6889 100644
--- a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -189,6 +189,9 @@ State FlightModeManager::state_sensors_calibration(const Event& event)
State FlightModeManager::state_algos_calibration(const Event& event)
{
+ static bool nasReady = false;
+ static bool adaReady = false;
+
switch (event)
{
case EV_ENTRY:
@@ -196,6 +199,7 @@ State FlightModeManager::state_algos_calibration(const Event& event)
logStatus(FlightModeManagerState::ALGOS_CALIBRATION);
EventBroker::getInstance().post(NAS_CALIBRATE, TOPIC_NAS);
+ EventBroker::getInstance().post(ADA_CALIBRATE, TOPIC_ADA);
return HANDLED;
}
@@ -213,7 +217,21 @@ State FlightModeManager::state_algos_calibration(const Event& event)
}
case NAS_READY:
{
- return transition(&FlightModeManager::state_disarmed);
+ nasReady = true;
+
+ if (adaReady)
+ return transition(&FlightModeManager::state_disarmed);
+ else
+ return HANDLED;
+ }
+ case ADA_READY:
+ {
+ adaReady = true;
+
+ if (nasReady)
+ return transition(&FlightModeManager::state_disarmed);
+ else
+ return HANDLED;
}
default:
{
@@ -249,6 +267,7 @@ State FlightModeManager::state_disarmed(const Event& event)
}
case TMTC_ENTER_TEST_MODE:
{
+ Logger::getInstance().start();
return transition(&FlightModeManager::state_test_mode);
}
case TMTC_CALIBRATE:
@@ -273,10 +292,18 @@ State FlightModeManager::state_test_mode(const Event& event)
case EV_ENTRY:
{
logStatus(FlightModeManagerState::TEST_MODE);
+
+ EventBroker::getInstance().post(ADA_FORCE_START, TOPIC_NAS);
+ EventBroker::getInstance().post(NAS_FORCE_START, TOPIC_NAS);
+
return HANDLED;
}
case EV_EXIT:
{
+ EventBroker::getInstance().post(ADA_FORCE_STOP, TOPIC_NAS);
+ EventBroker::getInstance().post(NAS_FORCE_STOP, TOPIC_NAS);
+ Logger::getInstance().stop();
+
return HANDLED;
}
case EV_EMPTY:
@@ -382,7 +409,7 @@ State FlightModeManager::state_flying(const Event& event)
}
default:
{
- return tranSuper(&FlightModeManager::state_top);
+ return UNHANDLED;
}
}
}
@@ -537,6 +564,7 @@ FlightModeManager::FlightModeManager()
EventBroker::getInstance().subscribe(this, TOPIC_FMM);
EventBroker::getInstance().subscribe(this, TOPIC_TMTC);
EventBroker::getInstance().subscribe(this, TOPIC_NAS);
+ EventBroker::getInstance().subscribe(this, TOPIC_ADA);
}
FlightModeManager::~FlightModeManager()
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index bb21ab009..987fbfa97 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -27,12 +27,11 @@
#include <Main/Sensors/Sensors.h>
#include <algorithms/NAS/StateInitializer.h>
#include <common/events/Events.h>
-#include <events/EventBroker.h>
-#include <utils/SkyQuaternion/SkyQuaternion.h>
using namespace std;
using namespace Eigen;
using namespace Boardcore;
+using namespace Main::NASConfig;
using namespace Common;
namespace Main
@@ -42,7 +41,7 @@ bool NASController::start()
{
// Add the update task to the scheduler
BoardScheduler::getInstance().getScheduler().addTask(
- bind(&NASController::update, this), NASConfig::UPDATE_PERIOD,
+ bind(&NASController::update, this), UPDATE_PERIOD,
TaskScheduler::Policy::RECOVER);
return ActiveObject::start();
@@ -57,8 +56,7 @@ void NASController::update()
Sensors::getInstance().getBMX160WithCorrectionLastSample();
auto gpsData = Sensors::getInstance().getUbxGpsLastSample();
auto pressureData = Sensors::getInstance().getMS5803LastSample();
- auto pitotData =
- Sensors::getInstance().getDifferentialPressureLastSample();
+ auto pitotData = Sensors::getInstance().getPitotData();
// Predict step
nas.predictGyro(imuData);
@@ -68,7 +66,7 @@ void NASController::update()
nas.correctMag(imuData);
nas.correctGPS(gpsData);
nas.correctBaro(pressureData.pressure);
- // nas.correctPitot(pitotData.pressure, pressureData.pressure);
+ nas.correctPitot(pitotData.deltaP, pressureData.pressure);
Logger::getInstance().log(nas.getState());
}
@@ -79,82 +77,70 @@ void NASController::update()
#endif // HILSimulation
}
-void NASController::initializeOrientationAndPressure()
+void NASController::calibrate()
{
- // Mean 10 accelerometer values
- Eigen::Vector3f accelerometer;
- Eigen::Vector3f magnetometer;
- float pressure = 0;
- float temperature = 0;
- StateInitializer state;
- UBXGPSData gps = Sensors::getInstance().getUbxGpsLastSample();
- ReferenceValues reference = nas.getReferenceValues();
+ Vector3f acceleration, magneticField;
+ Stats pressure;
- // Mean the values
- for (int i = 0; i < NASConfig::MEAN_COUNT; i++)
+ for (int i = 0; i < CALIBRATION_SAMPLES_COUNT; i++)
{
// IMU
BMX160WithCorrectionData imuData =
Sensors::getInstance().getBMX160WithCorrectionLastSample();
- accelerometer +=
- Eigen::Vector3f(imuData.accelerationX, imuData.accelerationY,
- imuData.accelerationZ);
- magnetometer +=
- Eigen::Vector3f(imuData.magneticFieldX, imuData.magneticFieldY,
- imuData.magneticFieldZ);
+ acceleration += Vector3f(imuData.accelerationX, imuData.accelerationY,
+ imuData.accelerationZ);
+ magneticField +=
+ Vector3f(imuData.magneticFieldX, imuData.magneticFieldY,
+ imuData.magneticFieldZ);
// Barometer
- MS5803Data pressureData = Sensors::getInstance().getMS5803LastSample();
- pressure += pressureData.pressure;
- temperature += pressureData.temperature;
+ MS5803Data barometerData = Sensors::getInstance().getMS5803LastSample();
+ pressure.add(barometerData.pressure);
- // Wait for some time
- miosix::Thread::sleep(100);
+ miosix::Thread::sleep(CALIBRATION_SLEEP_TIME);
}
- accelerometer /= NASConfig::MEAN_COUNT;
- magnetometer /= NASConfig::MEAN_COUNT;
- pressure /= NASConfig::MEAN_COUNT;
- temperature /= NASConfig::MEAN_COUNT;
-
// Normalize the data
- accelerometer.normalize();
- magnetometer.normalize();
+ acceleration /= CALIBRATION_SAMPLES_COUNT;
+ magneticField /= CALIBRATION_SAMPLES_COUNT;
+ acceleration.normalize();
+ magneticField.normalize();
- // Triad the initial orientation
- state.triad(accelerometer, magnetometer, NASConfig::nedMag);
+ // Use the triad algorithm to estimate the initial orientation
+ StateInitializer state;
+ state.triad(acceleration, magneticField, nedMag);
// Set the pressure reference using an already existing reference values
- reference.pressure = pressure;
+ ReferenceValues reference = nas.getReferenceValues();
+ reference.refPressure = pressure.getStats().mean;
// If in this moment the GPS has fix i use that position as starting
+ UBXGPSData gps = Sensors::getInstance().getUbxGpsLastSample();
if (gps.fix != 0)
{
- reference.startLatitude = gps.latitude;
- reference.startLongitude = gps.longitude;
+ reference.refLatitude = gps.latitude;
+ reference.refLongitude = gps.longitude;
}
- // Set the values inside the NAS
+ // Update the algorithm reference values
{
miosix::PauseKernelLock l;
-
nas.setX(state.getInitX());
nas.setReferenceValues(reference);
}
- // At the end i publish on the nas topic the end
EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
}
-void NASController::setCoordinates(Eigen::Vector2f position)
+void NASController::setCoordinates(Vector2f position)
{
// Need to pause the kernel because the only invocation comes from the radio
// which is a separate thread
miosix::PauseKernelLock l;
ReferenceValues reference = nas.getReferenceValues();
- reference.startLatitude = position[0];
- reference.startLongitude = position[1];
+ reference.refLatitude = position[0];
+ reference.refLongitude = position[1];
nas.setReferenceValues(reference);
}
@@ -183,7 +169,7 @@ void NASController::setReferenceAltitude(float altitude)
miosix::PauseKernelLock l;
ReferenceValues reference = nas.getReferenceValues();
- reference.altitude = altitude;
+ reference.refAltitude = altitude;
nas.setReferenceValues(reference);
}
@@ -194,7 +180,7 @@ void NASController::setReferenceTemperature(float temperature)
miosix::PauseKernelLock l;
ReferenceValues reference = nas.getReferenceValues();
- reference.temperature = temperature;
+ reference.refTemperature = temperature;
nas.setReferenceValues(reference);
}
@@ -239,7 +225,7 @@ void NASController::state_calibrating(const Event &event)
case EV_ENTRY:
{
// Calibrate the NAS
- initializeOrientationAndPressure();
+ calibrate();
return logStatus(NASControllerState::CALIBRATING);
}
@@ -262,6 +248,7 @@ void NASController::state_ready(const Event &event)
{
return transition(&NASController::state_calibrating);
}
+ case NAS_FORCE_START:
case FLIGHT_ARMED:
{
return transition(&NASController::state_active);
@@ -282,6 +269,11 @@ void NASController::state_active(const Event &event)
{
return transition(&NASController::state_end);
}
+ case NAS_FORCE_STOP:
+ case FLIGHT_DISARMED:
+ {
+ return transition(&NASController::state_ready);
+ }
}
}
@@ -313,7 +305,7 @@ void NASController::logStatus(NASControllerState state)
}
NASController::NASController()
- : FSM(&NASController::state_idle), nas(NASConfig::config)
+ : FSM(&NASController::state_idle), nas(config)
#ifdef HILSimulation
,
updateData([](Boardcore::NASState) {})
@@ -332,7 +324,7 @@ NASController::NASController()
x(9) = q(3);
nas.setX(x);
- nas.setReferenceValues(NASConfig::defaultReferenceValues);
+ nas.setReferenceValues(defaultReferenceValues);
}
NASController::~NASController()
diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main/StateMachines/NASController/NASController.h
index 00fd3e10b..0f64d5efc 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.h
+++ b/src/boards/Main/StateMachines/NASController/NASController.h
@@ -41,8 +41,7 @@ public:
bool start() override;
void update();
-
- void initializeOrientationAndPressure();
+ void calibrate();
void setCoordinates(Eigen::Vector2f position);
void setOrientation(float yaw, float pitch, float roll);
diff --git a/src/boards/Main/TMRepository/TMRepository.cpp b/src/boards/Main/TMRepository/TMRepository.cpp
index 76ed1da98..8bc49eb75 100644
--- a/src/boards/Main/TMRepository/TMRepository.cpp
+++ b/src/boards/Main/TMRepository/TMRepository.cpp
@@ -162,9 +162,9 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
tm.kalman_x2 = state.x2;
tm.vertical_speed = state.verticalSpeed;
tm.msl_altitude = state.mslAltitude;
- tm.ref_pressure = ref.pressure;
- tm.ref_altitude = ref.altitude;
- tm.ref_temperature = ref.temperature;
+ tm.ref_pressure = ref.refPressure;
+ tm.ref_altitude = ref.refAltitude;
+ tm.ref_temperature = ref.refTemperature;
mavlink_msg_ada_tm_encode(RadioConfig::MAV_SYSTEM_ID,
RadioConfig::MAV_COMPONENT_ID, &msg, &tm);
@@ -194,10 +194,10 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
tm.nas_bias_x = state.bx;
tm.nas_bias_y = state.by;
tm.nas_bias_z = state.bz;
- tm.ref_pressure = ref.pressure;
- tm.ref_temperature = ref.temperature;
- tm.ref_latitude = ref.startLatitude;
- tm.ref_longitude = ref.startLongitude;
+ tm.ref_pressure = ref.refPressure;
+ tm.ref_temperature = ref.refTemperature;
+ tm.ref_latitude = ref.refLatitude;
+ tm.ref_longitude = ref.refLongitude;
mavlink_msg_nas_tm_encode(RadioConfig::MAV_SYSTEM_ID,
RadioConfig::MAV_COMPONENT_ID, &msg, &tm);
@@ -249,14 +249,14 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
tm.nas_state = static_cast<uint8_t>(nas.getStatus().state);
// Pressures
- tm.pressure_ada = ada.getAdaState().x0;
+ tm.pressure_ada = adaState.x0;
tm.pressure_digi = ms5803Data.pressure;
tm.pressure_static = sensors.getStaticPressureLastSample().pressure;
tm.pressure_dpl = sensors.getDplPressureLastSample().pressure;
tm.airspeed_pitot = 0; // TODO: Implement
// ADA estimation
- tm.altitude_agl = adaState.aglAltitude;
+ tm.altitude_agl = -adaState.aglAltitude;
tm.ada_vert_speed = adaState.verticalSpeed;
// IMU
diff --git a/src/boards/Parafoil/TMRepository/TMRepository.cpp b/src/boards/Parafoil/TMRepository/TMRepository.cpp
index f78f2820d..650e4c0cd 100644
--- a/src/boards/Parafoil/TMRepository/TMRepository.cpp
+++ b/src/boards/Parafoil/TMRepository/TMRepository.cpp
@@ -134,10 +134,10 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList reqTm)
tm.nas_bias_x = state.bx;
tm.nas_bias_y = state.by;
tm.nas_bias_z = state.bz;
- tm.ref_pressure = ref.pressure;
- tm.ref_temperature = ref.temperature;
- tm.ref_latitude = ref.startLatitude;
- tm.ref_longitude = ref.startLongitude;
+ tm.ref_pressure = ref.refPressure;
+ tm.ref_temperature = ref.refTemperature;
+ tm.ref_latitude = ref.refLatitude;
+ tm.ref_longitude = ref.refLongitude;
mavlink_msg_nas_tm_encode(RadioConfig::MAV_SYSTEM_ID,
RadioConfig::MAV_COMPONENT_ID, &msg, &tm);
diff --git a/src/boards/Payload/Configs/NASConfig.h b/src/boards/Payload/Configs/NASConfig.h
index 08c1ece0f..653cebc2a 100644
--- a/src/boards/Payload/Configs/NASConfig.h
+++ b/src/boards/Payload/Configs/NASConfig.h
@@ -32,7 +32,9 @@ namespace NASConfig
{
constexpr uint32_t UPDATE_PERIOD = 20; // 50 hz
-constexpr uint8_t MEAN_COUNT = 10;
+
+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);
diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h
index dac7c64b1..38ba5ea43 100644
--- a/src/boards/Payload/Configs/WingConfig.h
+++ b/src/boards/Payload/Configs/WingConfig.h
@@ -46,10 +46,6 @@ constexpr float WING_CALIBRATION_PRESSURE = 101325; // [Pa]
constexpr float WING_CALIBRATION_TEMPERATURE = 300; // [K]
constexpr uint8_t WING_PRESSURE_MEAN_COUNT = 20;
-// TODO change the values below
-constexpr float DEFAULT_GPS_INITIAL_LAT = 42.5;
-constexpr float DEFAULT_GPS_INITIAL_LON = 9.5;
-
constexpr float DEFAULT_TARGET_LAT = 42;
constexpr float DEFAULT_TARGET_LON = 9;
diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
index a8791eb92..a9f5b9930 100644
--- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -286,10 +286,7 @@ State FlightModeManager::state_test_mode(const Event& event)
{
logStatus(FlightModeManagerState::TEST_MODE);
- // Start the wing algorithm
WingController::getInstance().start();
-
- // Start also the NAS
EventBroker::getInstance().post(NAS_FORCE_START, TOPIC_NAS);
return HANDLED;
@@ -357,7 +354,6 @@ State FlightModeManager::state_armed(const Event& event)
return transition(&FlightModeManager::state_disarmed);
}
// TODO: Reviews the liftoff event
- case FLIGHT_LIFTOFF:
case TMTC_FORCE_LAUNCH:
{
return transition(&FlightModeManager::state_flying);
@@ -377,6 +373,7 @@ State FlightModeManager::state_flying(const Event& event)
{
case EV_ENTRY:
{
+ EventBroker::getInstance().post(FLIGHT_LIFTOFF, TOPIC_FLIGHT);
missionTimeoutEventId =
EventBroker::getInstance().postDelayed<MISSION_TIMEOUT>(
FLIGHT_MISSION_TIMEOUT, TOPIC_FLIGHT);
@@ -397,16 +394,12 @@ State FlightModeManager::state_flying(const Event& event)
}
case TMTC_FORCE_MAIN:
{
- // Activate the cutters
EventBroker::getInstance().post(DPL_CUT_DROGUE, TOPIC_DPL);
return HANDLED;
}
case FLIGHT_MISSION_TIMEOUT:
{
- // Stop eventual wing algorithm
WingController::getInstance().stop();
-
- // Stop recording
Actuators::getInstance().camOff();
return transition(&FlightModeManager::state_landed);
@@ -495,8 +488,6 @@ State FlightModeManager::state_terminal_descent(const Event& event)
logStatus(FlightModeManagerState::TERMINAL_DESCENT);
WingController::getInstance().start();
-
- // Activate the cutters
EventBroker::getInstance().post(DPL_CUT_DROGUE, TOPIC_DPL);
return HANDLED;
@@ -515,18 +506,14 @@ State FlightModeManager::state_terminal_descent(const Event& event)
}
case TMTC_FORCE_LANDING:
{
- // Stop the wing algorithm
WingController::getInstance().stop();
-
EventBroker::getInstance().post(FLIGHT_LANDING_DETECTED,
TOPIC_FLIGHT);
return HANDLED;
}
case FLIGHT_LANDING_DETECTED:
{
- // Stop the wing algorithm
WingController::getInstance().stop();
-
return transition(&FlightModeManager::state_landed);
}
default:
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp
index 5ce034723..e03f0c48e 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp
@@ -28,12 +28,11 @@
#include <Payload/Sensors/Sensors.h>
#include <algorithms/NAS/StateInitializer.h>
#include <common/events/Events.h>
-#include <sensors/MPU9250/MPU9250Data.h>
-#include <sensors/UBXGPS/UBXGPSData.h>
using namespace std;
using namespace Eigen;
using namespace Boardcore;
+using namespace Payload::NASConfig;
using namespace Common;
namespace Payload
@@ -43,7 +42,7 @@ bool NASController::start()
{
// Add the update task to the scheduler
BoardScheduler::getInstance().getScheduler().addTask(
- bind(&NASController::update, this), NASConfig::UPDATE_PERIOD,
+ bind(&NASController::update, this), UPDATE_PERIOD,
TaskScheduler::Policy::RECOVER);
return ActiveObject::start();
@@ -52,114 +51,93 @@ bool NASController::start()
void NASController::update()
{
// If the nas is not active i skip the step
- if (!this->testState(&NASController::state_active))
- return;
-
- auto imuData = Sensors::getInstance().getBMX160WithCorrectionLastSample();
- auto gpsData = Sensors::getInstance().getUbxGpsLastSample();
- auto pressureData = Sensors::getInstance().getMS5803LastSample();
-
- // Predict step
- nas.predictGyro(imuData);
- nas.predictAcc(imuData);
-
- // Correct step
- nas.correctMag(imuData);
- nas.correctGPS(gpsData);
- nas.correctBaro(pressureData.pressure);
-
- Logger::getInstance().log(nas.getState());
+ if (this->testState(&NASController::state_active))
+ {
+ auto imuData =
+ Sensors::getInstance().getBMX160WithCorrectionLastSample();
+ auto gpsData = Sensors::getInstance().getUbxGpsLastSample();
+ auto pressureData = Sensors::getInstance().getMS5803LastSample();
+ // auto pitotData =
+ // Sensors::getInstance().getDifferentialPressureLastSample();
+
+ // Predict step
+ nas.predictGyro(imuData);
+ nas.predictAcc(imuData);
+
+ // Correct step
+ nas.correctMag(imuData);
+ nas.correctGPS(gpsData);
+ nas.correctBaro(pressureData.pressure);
+ // nas.correctPitot(pitotData.pressure, pressureData.pressure);
+
+ Logger::getInstance().log(nas.getState());
+ }
}
-void NASController::initializeOrientationAndPressure()
+void NASController::calibrate()
{
- // Mean 10 accelerometer values
- Eigen::Vector3f accelerometer;
- Eigen::Vector3f magnetometer;
- float pressure = 0;
- float temperature = 0;
- StateInitializer state;
- UBXGPSData gps = Sensors::getInstance().getUbxGpsLastSample();
- ReferenceValues reference = nas.getReferenceValues();
+ Vector3f acceleration, magneticField;
+ Stats pressure;
- // Mean the values
- for (int i = 0; i < NASConfig::MEAN_COUNT; i++)
+ for (int i = 0; i < CALIBRATION_SAMPLES_COUNT; i++)
{
// IMU
BMX160WithCorrectionData imuData =
Sensors::getInstance().getBMX160WithCorrectionLastSample();
- accelerometer +=
- Eigen::Vector3f(imuData.accelerationX, imuData.accelerationY,
- imuData.accelerationZ);
- magnetometer +=
- Eigen::Vector3f(imuData.magneticFieldX, imuData.magneticFieldY,
- imuData.magneticFieldZ);
+ acceleration += Vector3f(imuData.accelerationX, imuData.accelerationY,
+ imuData.accelerationZ);
+ magneticField +=
+ Vector3f(imuData.magneticFieldX, imuData.magneticFieldY,
+ imuData.magneticFieldZ);
// Barometer
- MS5803Data pressureData = Sensors::getInstance().getMS5803LastSample();
- pressure += pressureData.pressure;
- temperature += pressureData.temperature;
+ MS5803Data barometerData = Sensors::getInstance().getMS5803LastSample();
+ pressure.add(barometerData.pressure);
- // Wait for some time
- miosix::Thread::sleep(100);
+ miosix::Thread::sleep(CALIBRATION_SLEEP_TIME);
}
- accelerometer /= NASConfig::MEAN_COUNT;
- magnetometer /= NASConfig::MEAN_COUNT;
- pressure /= NASConfig::MEAN_COUNT;
- temperature /= NASConfig::MEAN_COUNT;
-
// Normalize the data
- accelerometer.normalize();
- magnetometer.normalize();
+ acceleration /= CALIBRATION_SAMPLES_COUNT;
+ magneticField /= CALIBRATION_SAMPLES_COUNT;
+ acceleration.normalize();
+ magneticField.normalize();
- // Triad the initial orientation
- state.triad(accelerometer, magnetometer, NASConfig::nedMag);
+ // Use the triad algorithm to estimate the initial orientation
+ StateInitializer state;
+ state.triad(acceleration, magneticField, nedMag);
// Set the pressure reference using an already existing reference values
- reference.pressure = pressure;
- reference.temperature = temperature;
-
- // Set also these two because of differential measure in height (TODO
- // Discuss)
- reference.mslPressure = pressure;
- reference.mslTemperature = temperature;
+ ReferenceValues reference = nas.getReferenceValues();
+ reference.refPressure = pressure.getStats().mean;
// If in this moment the GPS has fix i use that position as starting
+ UBXGPSData gps = Sensors::getInstance().getUbxGpsLastSample();
if (gps.fix != 0)
{
- reference.startLatitude = gps.latitude;
- reference.startLongitude = gps.longitude;
- }
- else
- {
- // Change the initial position if not already set
- reference.startLatitude = reference.startLatitude == 0
- ? WingConfig::DEFAULT_GPS_INITIAL_LAT
- : reference.startLatitude;
- reference.startLongitude = reference.startLongitude == 0
- ? WingConfig::DEFAULT_GPS_INITIAL_LON
- : reference.startLongitude;
+ reference.refLatitude = gps.latitude;
+ reference.refLongitude = gps.longitude;
}
- // Set the values inside the NAS
+ // Update the algorithm reference values
{
miosix::PauseKernelLock l;
-
nas.setX(state.getInitX());
nas.setReferenceValues(reference);
}
+
+ EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
}
-void NASController::setCoordinates(Eigen::Vector2f position)
+void NASController::setCoordinates(Vector2f position)
{
// Need to pause the kernel because the only invocation comes from the radio
// which is a separate thread
miosix::PauseKernelLock l;
ReferenceValues reference = nas.getReferenceValues();
- reference.startLatitude = position[0];
- reference.startLongitude = position[1];
+ reference.refLatitude = position[0];
+ reference.refLongitude = position[1];
nas.setReferenceValues(reference);
}
@@ -188,7 +166,7 @@ void NASController::setReferenceAltitude(float altitude)
miosix::PauseKernelLock l;
ReferenceValues reference = nas.getReferenceValues();
- reference.altitude = altitude;
+ reference.refAltitude = altitude;
nas.setReferenceValues(reference);
}
@@ -199,7 +177,7 @@ void NASController::setReferenceTemperature(float temperature)
miosix::PauseKernelLock l;
ReferenceValues reference = nas.getReferenceValues();
- reference.temperature = temperature;
+ reference.refTemperature = temperature;
nas.setReferenceValues(reference);
}
@@ -243,15 +221,10 @@ void NASController::state_calibrating(const Event& event)
{
case EV_ENTRY:
{
- logStatus(NASControllerState::CALIBRATING);
-
// Calibrate the NAS
- initializeOrientationAndPressure();
-
- // At the end i publish on the nas topic the end
- EventBroker::getInstance().post(NAS_READY, TOPIC_NAS);
+ calibrate();
- break;
+ return logStatus(NASControllerState::CALIBRATING);
}
case NAS_READY:
{
@@ -320,8 +293,7 @@ void NASController::logStatus(NASControllerState state)
Logger::getInstance().log(status);
}
-NASController::NASController()
- : FSM(&NASController::state_idle), nas(NASConfig::config)
+NASController::NASController() : FSM(&NASController::state_idle), nas(config)
{
EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
EventBroker::getInstance().subscribe(this, TOPIC_NAS);
@@ -336,7 +308,7 @@ NASController::NASController()
x(9) = q(3);
nas.setX(x);
- nas.setReferenceValues(NASConfig::defaultReferenceValues);
+ nas.setReferenceValues(defaultReferenceValues);
}
NASController::~NASController()
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.h b/src/boards/Payload/StateMachines/NASController/NASController.h
index f354c7b48..db0eb9270 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.h
+++ b/src/boards/Payload/StateMachines/NASController/NASController.h
@@ -41,8 +41,7 @@ public:
bool start() override;
void update();
-
- void initializeOrientationAndPressure();
+ void calibrate();
void setCoordinates(Eigen::Vector2f position);
void setOrientation(float yaw, float pitch, float roll);
diff --git a/src/boards/Payload/TMRepository/TMRepository.cpp b/src/boards/Payload/TMRepository/TMRepository.cpp
index 5cf7c1df6..1fe1bb2b3 100644
--- a/src/boards/Payload/TMRepository/TMRepository.cpp
+++ b/src/boards/Payload/TMRepository/TMRepository.cpp
@@ -143,10 +143,10 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
tm.nas_bias_x = state.bx;
tm.nas_bias_y = state.by;
tm.nas_bias_z = state.bz;
- tm.ref_pressure = ref.pressure;
- tm.ref_temperature = ref.temperature;
- tm.ref_latitude = ref.startLatitude;
- tm.ref_longitude = ref.startLongitude;
+ tm.ref_pressure = ref.refPressure;
+ tm.ref_temperature = ref.refTemperature;
+ tm.ref_latitude = ref.refLatitude;
+ tm.ref_longitude = ref.refLongitude;
mavlink_msg_nas_tm_encode(RadioConfig::MAV_SYSTEM_ID,
RadioConfig::MAV_COMPONENT_ID, &msg, &tm);
diff --git a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
index 86d207e91..6873553aa 100644
--- a/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
+++ b/src/boards/Payload/Wing/AutomaticWingAlgorithm.cpp
@@ -62,7 +62,7 @@ void AutomaticWingAlgorithm::step()
ReferenceValues reference =
NASController::getInstance().getReferenceValues();
Vector2f startingPosition =
- Vector2f(reference.startLatitude, reference.startLongitude);
+ Vector2f(reference.refLatitude, reference.refLongitude);
Vector2f targetPosition = Aeroutils::geodetic2NED(
WingController::getInstance().getTargetPosition(), startingPosition);
Vector2f targetDirection = targetPosition - Vector2f(state.n, state.e);
diff --git a/src/boards/common/events/Events.h b/src/boards/common/events/Events.h
index d290d7f62..d51bfb74c 100644
--- a/src/boards/common/events/Events.h
+++ b/src/boards/common/events/Events.h
@@ -44,6 +44,8 @@ enum Events : uint8_t
ADA_CALIBRATE,
ADA_PRESS_STAB_TIMEOUT,
ADA_READY,
+ ADA_FORCE_START,
+ ADA_FORCE_STOP,
ADA_SHADOW_MODE_TIMEOUT,
DPL_CUT_DROGUE,
DPL_CUT_TIMEOUT,
diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp
index 128d26356..80b0822c0 100644
--- a/src/entrypoints/Main/main-entry.cpp
+++ b/src/entrypoints/Main/main-entry.cpp
@@ -104,12 +104,6 @@ int main()
ADAController::getInstance().setUpdateDataFunction(
[](Boardcore::ADAState state)
{ HIL::getInstance().getElaboratedData()->addADAState(state); });
-
- // Set reference values when starting calibration
- ADAController::getInstance().setReferenceValues(
- {Main::ADAConfig::DEFAULT_REFERENCE_ALTITUDE,
- Main::ADAConfig::DEFAULT_REFERENCE_PRESSURE,
- Main::ADAConfig::DEFAULT_REFERENCE_TEMPERATURE});
#endif
if (!ADAController::getInstance().start())
{
diff --git a/src/hardware_in_the_loop/HIL_actuators/HILServo.h b/src/hardware_in_the_loop/HIL_actuators/HILServo.h
index 6d8d2497d..8ab6b7072 100644
--- a/src/hardware_in_the_loop/HIL_actuators/HILServo.h
+++ b/src/hardware_in_the_loop/HIL_actuators/HILServo.h
@@ -22,86 +22,29 @@
#pragma once
+#include <actuators/Servo/Servo.h>
+
#include "HIL.h"
#include "HILConfig.h"
-class HILServo
+class HILServo : public Boardcore::Servo
{
public:
- /**
- * @brief constructor of the fake actuator used for the simulation.
- *
- * @param matlab reference of the MatlabTransceiver object that deals with
- * the simulator
- */
- HILServo() {}
-
- void enable()
+ explicit HILServo(TIM_TypeDef* const timer,
+ Boardcore::TimerUtils::Channel pwmChannel,
+ unsigned int minPulse = 1000,
+ unsigned int maxPulse = 2000, unsigned int frequency = 50)
+ : Servo(timer, pwmChannel, minPulse, maxPulse, frequency)
{
- // reset();
- isEnabled = true;
}
- void disable()
+ void setPosition(float position, bool limited = true)
{
- // reset();
- isEnabled = false;
- }
+ Servo::setPosition(position, limited);
- /**
- * @brief Initializes the fake actuator
- */
- bool init()
- {
- initialized = true;
- return true;
- }
-
- void selfTest() { return; }
-
- float getPosition()
- {
- miosix::Lock<FastMutex> l(mutex);
- return position;
- }
-
- /**
- * @brief sets the actuator data in the MatlabTransceiver object, then will
- * be sent to the simulator
- *
- * @param value opening in radiants
- */
- void setPosition(float value)
- {
- if (value < MinAlphaDegree)
- value = MinAlphaDegree;
- if (value > MaxAlphaDegree)
- value = MaxAlphaDegree;
-
- miosix::Lock<FastMutex> l(mutex);
- position = value;
- // TRACE("[HILServo] setting actuator\n");
- // actuatorData.print();
- // TRACE("[HILServo] didn't send abk opening\n");
- HIL::getInstance().send(value);
- }
-
- void sendToSimulator()
- {
- miosix::Lock<FastMutex> l(mutex);
+ // Send the position to MatLab
HIL::getInstance().send(position);
}
- /*
- * converts the value that the real servo driver accepts to the value the
- * matlab simulator accepts
- */
- // float convertToDegree(float x) { return (x * 180 / PI); }
-
-protected:
- miosix::FastMutex mutex;
- float position = 0;
-
- bool initialized = false;
- bool isEnabled = true;
+ void sendToSimulator() { HIL::getInstance().send(getPosition()); }
};
diff --git a/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h b/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h
index ef96ff373..67dba996c 100644
--- a/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h
+++ b/src/hardware_in_the_loop/HIL_algorithms/HILMockAerobrakeAlgorithm.h
@@ -76,7 +76,7 @@ protected:
// setting the opening to 50%
Main::Actuators::getInstance().setServo(ServosList::AIRBRAKES_SERVO,
0.5);
- Main::Actuators::getInstance().sendToSimulator();
+ // Main::Actuators::getInstance().sendToSimulator();
}
}
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 ae0639616..ead37287c 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
@@ -246,9 +246,9 @@ int main()
// setting initial reference values
ada_controller.setReferenceValues(
- {Main::ADAConfig::DEFAULT_REFERENCE_ALTITUDE,
- Main::ADAConfig::DEFAULT_REFERENCE_PRESSURE,
- Main::ADAConfig::DEFAULT_REFERENCE_TEMPERATURE});
+ {Main::NASConfig::defaultReferenceValues.refAltitude,
+ Main::NASConfig::defaultReferenceValues.refPressure,
+ Main::NASConfig::defaultReferenceValues.refTemperature});
TRACE("Starting ada\n");
ada_controller.start();
@@ -266,9 +266,9 @@ int main()
// setting initial reference values
nas_controller.setReferenceValues(
- {Main::ADAConfig::DEFAULT_REFERENCE_ALTITUDE,
- Main::ADAConfig::DEFAULT_REFERENCE_PRESSURE,
- Main::ADAConfig::DEFAULT_REFERENCE_TEMPERATURE});
+ {Main::NASConfig::defaultReferenceValues.refAltitude,
+ Main::NASConfig::defaultReferenceValues.refPressure,
+ Main::NASConfig::defaultReferenceValues.refTemperature});
// starting NAS only when simulation starts
--
GitLab