diff --git a/src/boards/Payload/Configs/NASConfig.h b/src/boards/Payload/Configs/NASConfig.h
index cc1a14ac7f9ef3573ea39a00460efc51a2e15c07..b6c0698651948266665abdc7ee10eddec802c2aa 100644
--- a/src/boards/Payload/Configs/NASConfig.h
+++ b/src/boards/Payload/Configs/NASConfig.h
@@ -47,11 +47,11 @@ static const Boardcore::NASConfig CONFIG = {
.SIGMA_W = 0.3,
.SIGMA_ACC = 0.1,
.SIGMA_MAG = 0.1,
- .SIGMA_GPS = {0.0447f, 0.0447f, 0.1f / 30.0f, 0.1f / 30.0f},
- .SIGMA_BAR = 4.3,
+ .SIGMA_GPS = {0.002f, 0.002f, 0.01f / 30.0f, 0.01f / 30.0f},
+ .SIGMA_BAR = 50.0,
.SIGMA_POS = 10.0,
.SIGMA_VEL = 10.0,
- .SIGMA_PITOT = 10.0,
+ .SIGMA_PITOT = 10e-3,
.P_POS = 1.0,
.P_POS_VERTICAL = 10.0,
.P_VEL = 1.0,
@@ -74,6 +74,12 @@ constexpr float ACCELERATION_1G_CONFIDENCE = 0.5;
// acceleration
constexpr int ACCELERATION_1G_SAMPLES = 20;
+// Altitude under which we make the pitot correction
+constexpr float PITOT_ALTITUDE_THRESHOLD = 800; // [m]
+
+// Vertical speed over which we make the pitot correction
+constexpr float PITOT_SPEED_THRESHOLD = 70; // [m/s]
+
} // namespace NAS
} // namespace Config
} // namespace Payload
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp
index e6e4a924049e6a9a7b53dbdb91f5fd1dca50bc10..c6b4b53f054e88f71226ff5ea44c3a4a444098e7 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp
@@ -44,33 +44,29 @@ NASController::NASController()
BoardScheduler::nasControllerPriority()),
nas(config::CONFIG)
{
- // Subscribe the class to the topics
EventBroker::getInstance().subscribe(this, TOPIC_NAS);
EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT);
+}
+bool NASController::start()
+{
// Setup the NAS
Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
-
// Create the initial quaternion
Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
// Set the initial quaternion inside the matrix
- x(6) = q(0);
- x(7) = q(1);
- x(8) = q(2);
- x(9) = q(3);
+ x(NAS::IDX_QUAT + 0) = q(0);
+ x(NAS::IDX_QUAT + 1) = q(1);
+ x(NAS::IDX_QUAT + 2) = q(2);
+ x(NAS::IDX_QUAT + 3) = q(3);
// Set the NAS x matrix
nas.setX(x);
-
- // Set the referenced values for the correct place on earth
+ // Set the initial reference values from the default ones
nas.setReferenceValues(ReferenceConfig::defaultReferenceValues);
-}
-bool NASController::start()
-{
auto& scheduler = getModule<BoardScheduler>()->nasController();
-
// Add the task to the scheduler
auto task = scheduler.addTask([this] { update(); }, config::UPDATE_RATE,
TaskScheduler::Policy::RECOVER);
@@ -265,88 +261,101 @@ void NASController::calibrate()
void NASController::update()
{
// Update the NAS state only if the FSM is active
- if (state == NASControllerState::ACTIVE)
+ if (state != NASControllerState::ACTIVE)
{
- Sensors* sensors = getModule<Sensors>();
+ return;
+ }
- IMUData imu = sensors->getIMULastSample();
- UBXGPSData gps = sensors->getUBXGPSLastSample();
- PressureData baro = sensors->getLPS28DFWLastSample();
+ auto* sensors = getModule<Sensors>();
- // Calculate acceleration
- Vector3f acc = static_cast<AccelerometerData>(imu);
- float accLength = acc.norm();
+ auto imu = sensors->getIMULastSample();
+ auto gps = sensors->getUBXGPSLastSample();
+ auto baro = sensors->getLPS28DFWLastSample();
+ auto staticPitot = sensors->getStaticPressureLastSample();
+ auto dynamicPitot = sensors->getDynamicPressureLastSample();
- miosix::Lock<miosix::FastMutex> l(nasMutex);
+ // Calculate acceleration
+ Vector3f acc = static_cast<AccelerometerData>(imu);
+ float accLength = acc.norm();
- // Perform initial NAS prediction
- // TODO: What about stale data?
- nas.predictGyro(imu);
- nas.predictAcc(imu);
+ miosix::Lock<miosix::FastMutex> l(nasMutex);
- // Then perform necessary corrections
- if (lastMagTimestamp < imu.magneticFieldTimestamp &&
- magDecimateCount == Config::NAS::MAGNETOMETER_DECIMATE)
- {
- nas.correctMag(imu);
- magDecimateCount = 0;
- }
- else
- {
- magDecimateCount++;
- }
+ // Perform initial NAS prediction
+ nas.predictGyro(imu);
+ nas.predictAcc(imu);
- if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3 &&
- accLength < Config::NAS::DISABLE_GPS_ACCELERATION)
- {
- nas.correctGPS(gps);
- }
+ // Then perform necessary corrections
+ if (lastMagTimestamp < imu.magneticFieldTimestamp &&
+ magDecimateCount == Config::NAS::MAGNETOMETER_DECIMATE)
+ {
+ nas.correctMag(imu);
+ magDecimateCount = 0;
+ }
+ else
+ {
+ magDecimateCount++;
+ }
- if (lastBaroTimestamp < baro.pressureTimestamp)
- {
- nas.correctBaro(baro.pressure);
- }
+ if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3 &&
+ accLength < Config::NAS::DISABLE_GPS_ACCELERATION)
+ {
+ nas.correctGPS(gps);
+ }
- // TODO: Correct with pitot
+ if (lastBaroTimestamp < baro.pressureTimestamp)
+ {
+ nas.correctBaro(baro.pressure);
+ }
- // Correct with accelerometer if the acceleration is in specs
- if (lastAccTimestamp < imu.accelerationTimestamp && acc1g)
- {
- nas.correctAcc(imu);
- }
+ // Correct with pitot if one pressure sample is new
+ if (dynamicPitot.pressure > 0 &&
+ (staticPitotTimestamp < staticPitot.pressureTimestamp ||
+ dynamicPitotTimestamp < dynamicPitot.pressureTimestamp) &&
+ (-nas.getState().d < Config::NAS::PITOT_ALTITUDE_THRESHOLD) &&
+ (-nas.getState().vd > Config::NAS::PITOT_SPEED_THRESHOLD))
+ {
+ nas.correctPitot(staticPitot.pressure, dynamicPitot.pressure);
+ }
+
+ // Correct with accelerometer if the acceleration is in specs
+ if (lastAccTimestamp < imu.accelerationTimestamp && acc1g)
+ {
+ nas.correctAcc(imu);
+ }
- // Check if the accelerometer is measuring 1g
- if (accLength <
- (Constants::g + Config::NAS::ACCELERATION_1G_CONFIDENCE / 2) &&
- accLength >
- (Constants::g - Config::NAS::ACCELERATION_1G_CONFIDENCE / 2))
+ // Check if the accelerometer is measuring 1g
+ if (accLength <
+ (Constants::g + Config::NAS::ACCELERATION_1G_CONFIDENCE / 2) &&
+ accLength >
+ (Constants::g - Config::NAS::ACCELERATION_1G_CONFIDENCE / 2))
+ {
+ if (acc1gSamplesCount < Config::NAS::ACCELERATION_1G_SAMPLES)
{
- if (acc1gSamplesCount < Config::NAS::ACCELERATION_1G_SAMPLES)
- {
- acc1gSamplesCount++;
- }
- else
- {
- acc1g = true;
- }
+ acc1gSamplesCount++;
}
else
{
- acc1gSamplesCount = 0;
- acc1g = false;
+ acc1g = true;
}
+ }
+ else
+ {
+ acc1gSamplesCount = 0;
+ acc1g = false;
+ }
- lastGyroTimestamp = imu.angularSpeedTimestamp;
- lastAccTimestamp = imu.accelerationTimestamp;
- lastMagTimestamp = imu.magneticFieldTimestamp;
- lastGpsTimestamp = gps.gpsTimestamp;
- lastBaroTimestamp = baro.pressureTimestamp;
+ lastGyroTimestamp = imu.angularSpeedTimestamp;
+ lastAccTimestamp = imu.accelerationTimestamp;
+ lastMagTimestamp = imu.magneticFieldTimestamp;
+ lastGpsTimestamp = gps.gpsTimestamp;
+ lastBaroTimestamp = baro.pressureTimestamp;
+ staticPitotTimestamp = staticPitot.pressureTimestamp;
+ dynamicPitotTimestamp = dynamicPitot.pressureTimestamp;
- auto state = nas.getState();
+ auto state = nas.getState();
- getModule<FlightStatsRecorder>()->updateNas(state);
- Logger::getInstance().log(state);
- }
+ getModule<FlightStatsRecorder>()->updateNas(state);
+ Logger::getInstance().log(state);
}
void NASController::updateState(NASControllerState newState)
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.h b/src/boards/Payload/StateMachines/NASController/NASController.h
index 9702a18203c62fc52c8e9117b7fa8276935e312b..8d3a7e19607d0b631fd8864c7e03942d833c88c4 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.h
+++ b/src/boards/Payload/StateMachines/NASController/NASController.h
@@ -86,11 +86,13 @@ private:
int acc1gSamplesCount = 0;
bool acc1g = false;
- uint64_t lastGyroTimestamp = 0;
- uint64_t lastAccTimestamp = 0;
- uint64_t lastMagTimestamp = 0;
- uint64_t lastGpsTimestamp = 0;
- uint64_t lastBaroTimestamp = 0;
+ uint64_t lastGyroTimestamp = 0;
+ uint64_t lastAccTimestamp = 0;
+ uint64_t lastMagTimestamp = 0;
+ uint64_t lastGpsTimestamp = 0;
+ uint64_t lastBaroTimestamp = 0;
+ uint64_t staticPitotTimestamp = 0;
+ uint64_t dynamicPitotTimestamp = 0;
std::atomic<bool> started{false};