diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp index 46c417315dda5a652638139e75384be62f08dc41..1cf296d05eb8cb101bd62486c85af8a3981e0a8f 100644 --- a/src/boards/Main/StateMachines/NASController/NASController.cpp +++ b/src/boards/Main/StateMachines/NASController/NASController.cpp @@ -135,16 +135,17 @@ void NASController::update() nas.predictAcc(imu); // Then perform necessary corrections - if (lastMagTimestamp < imu.magneticFieldTimestamp && - magDecimateCount == Config::NAS::MAGNETOMETER_DECIMATE) - { - nas.correctMag(imu); - magDecimateCount = 0; - } - else - { - magDecimateCount++; - } + // Disable magnetometer correction + // if (lastMagTimestamp < imu.magneticFieldTimestamp && + // magDecimateCount == Config::NAS::MAGNETOMETER_DECIMATE) + // { + // nas.correctMag(imu); + // magDecimateCount = 0; + // } + // else + // { + // magDecimateCount++; + // } if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3 && accLength < Config::NAS::DISABLE_GPS_ACCELERATION) @@ -158,14 +159,15 @@ void NASController::update() } // 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); - } + // Disable pitot correction + // 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) diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp index 0b1569529fc9b496db22fde35ca2174f82f661fd..0a6f5cf0ac51de5246a4f0d14b9fd70c8df57c9f 100644 --- a/src/boards/Payload/StateMachines/NASController/NASController.cpp +++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp @@ -297,16 +297,17 @@ void NASController::update() nas.predictAcc(imu); // Then perform necessary corrections - if (lastMagTimestamp < imu.magneticFieldTimestamp && - magDecimateCount == Config::NAS::MAGNETOMETER_DECIMATE) - { - nas.correctMag(imu); - magDecimateCount = 0; - } - else - { - magDecimateCount++; - } + // Disable magnetometer correction + // if (lastMagTimestamp < imu.magneticFieldTimestamp && + // magDecimateCount == Config::NAS::MAGNETOMETER_DECIMATE) + // { + // nas.correctMag(imu); + // magDecimateCount = 0; + // } + // else + // { + // magDecimateCount++; + // } if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3 && accLength < Config::NAS::DISABLE_GPS_ACCELERATION) @@ -320,14 +321,15 @@ void NASController::update() } // 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); - } + // Disable pitot correction + // 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)