From 98b0966e31b9becbd9afda267e227d4f82b9ad40 Mon Sep 17 00:00:00 2001 From: Davide Basso <davide.basso@skywarder.eu> Date: Sun, 1 Dec 2024 10:55:21 +0100 Subject: [PATCH] [Parafoil] [WIP] Add NAS controller --- src/Parafoil/BoardScheduler.h | 1 + src/Parafoil/Configs/NASConfig.h | 81 ++++ .../NASController/NASController.cpp | 356 ++++++++++++++++++ .../NASController/NASController.h | 100 +++++ .../NASController/NASControllerData.h | 54 +++ src/Parafoil/parafoil-entry.cpp | 5 + 6 files changed, 597 insertions(+) create mode 100644 src/Parafoil/Configs/NASConfig.h create mode 100644 src/Parafoil/StateMachines/NASController/NASController.cpp create mode 100644 src/Parafoil/StateMachines/NASController/NASController.h create mode 100644 src/Parafoil/StateMachines/NASController/NASControllerData.h diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h index 1b9b1d2de..2843c6cc6 100644 --- a/src/Parafoil/BoardScheduler.h +++ b/src/Parafoil/BoardScheduler.h @@ -52,6 +52,7 @@ public: }; }; + Boardcore::TaskScheduler& nasController() { return critical; } Boardcore::TaskScheduler& sensors() { return high; } static Priority::PriorityLevel flightModeManagerPriority() diff --git a/src/Parafoil/Configs/NASConfig.h b/src/Parafoil/Configs/NASConfig.h new file mode 100644 index 000000000..d5b49789f --- /dev/null +++ b/src/Parafoil/Configs/NASConfig.h @@ -0,0 +1,81 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Davide Mor, Niccolò Betto, Davide Basso + * + * 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/NAS/NASConfig.h> +#include <common/ReferenceConfig.h> +#include <units/Frequency.h> + +namespace Parafoil +{ +namespace Config +{ +namespace NAS +{ + +/* linter off */ using namespace Boardcore::Units::Frequency; + +constexpr Hertz UPDATE_RATE = 50_hz; +constexpr float UPDATE_RATE_SECONDS = 0.02; // [s] + +constexpr int CALIBRATION_SAMPLES_COUNT = 20; +constexpr unsigned int CALIBRATION_SLEEP_TIME = 100; // [ms] + +static const Boardcore::NASConfig CONFIG = { + .T = UPDATE_RATE_SECONDS, + .SIGMA_BETA = 0.0001, + .SIGMA_W = 0.0019, + .SIGMA_ACC = 0.202, + .SIGMA_MAG = 0.0047, + .SIGMA_GPS = {0.0447f, 0.0447f, 1.0f / 30.0f, 1.0f / 30.0f}, + .SIGMA_BAR = 400.0f, + .SIGMA_POS = 2.0, + .SIGMA_VEL = 1.0, + .SIGMA_PITOT = 1e-3, + .P_POS = 0.0, + .P_POS_VERTICAL = 0.0, + .P_VEL = 0.0, + .P_VEL_VERTICAL = 0.0, + .P_ATT = 0.1, + .P_BIAS = 0.01, + .SATS_NUM = 6.0, + .NED_MAG = Common::ReferenceConfig::nedMag}; + +// TODO: make sure this is correct! + +// Only use one out of every 50 samples (1 Hz) +constexpr int MAGNETOMETER_DECIMATE = 50; + +// Maximum allowed acceleration to correct with GPS +constexpr float DISABLE_GPS_ACCELERATION = 34.0f; // [m/s^2] + +// How much confidence (in m/s^2) to apply to the accelerometer to check if it +// is 1g +constexpr float ACCELERATION_1G_CONFIDENCE = 0.5; +// How many samples will determine that we are in fact measuring gravity +// acceleration +constexpr int ACCELERATION_1G_SAMPLES = 20; + +} // namespace NAS +} // namespace Config +} // namespace Parafoil diff --git a/src/Parafoil/StateMachines/NASController/NASController.cpp b/src/Parafoil/StateMachines/NASController/NASController.cpp new file mode 100644 index 000000000..872f0c5b6 --- /dev/null +++ b/src/Parafoil/StateMachines/NASController/NASController.cpp @@ -0,0 +1,356 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Niccolò Betto, Davide Mor, Davide Basso + * + * 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 <Parafoil/BoardScheduler.h> +#include <Parafoil/Configs/NASConfig.h> +#include <Parafoil/Sensors/Sensors.h> +#include <Parafoil/StateMachines/NASController/NASController.h> +#include <algorithms/NAS/StateInitializer.h> +#include <common/Events.h> +#include <common/ReferenceConfig.h> +#include <events/EventBroker.h> +#include <utils/SkyQuaternion/SkyQuaternion.h> + +using namespace Boardcore; +using namespace Eigen; +using namespace Common; +namespace config = Parafoil::Config::NAS; + +namespace Parafoil +{ + +NASController::NASController() + : FSM(&NASController::Init, miosix::STACK_DEFAULT_FOR_PTHREAD, + BoardScheduler::nasControllerPriority()), + nas(config::CONFIG) +{ + 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(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 initial reference values from the default ones + nas.setReferenceValues(ReferenceConfig::defaultReferenceValues); + + auto& scheduler = getModule<BoardScheduler>()->nasController(); + // Add the task to the scheduler + auto task = scheduler.addTask([this] { update(); }, config::UPDATE_RATE, + TaskScheduler::Policy::RECOVER); + + if (task == 0) + { + LOG_ERR(logger, "Failed to add NAS update task"); + return false; + } + + if (!FSM::start()) + { + LOG_ERR(logger, "Failed to start NASController FSM active object"); + return false; + } + + started = true; + return true; +} + +NASState NASController::getNasState() +{ + miosix::Lock<miosix::FastMutex> l(nasMutex); + return nas.getState(); +} + +ReferenceValues NASController::getReferenceValues() +{ + miosix::Lock<miosix::FastMutex> l(nasMutex); + return nas.getReferenceValues(); +} + +NASControllerState NASController::getState() { return state; } + +void NASController::setOrientation(const Eigen::Quaternionf& quat) +{ + Lock<FastMutex> lock{nasMutex}; + + auto x = nas.getX(); + x(NAS::IDX_QUAT + 0) = quat.x(); + x(NAS::IDX_QUAT + 1) = quat.y(); + x(NAS::IDX_QUAT + 2) = quat.z(); + x(NAS::IDX_QUAT + 3) = quat.w(); + nas.setX(x); +} + +void NASController::Init(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(NASControllerState::INIT); + break; + } + + case NAS_CALIBRATE: + { + transition(&NASController::Calibrating); + break; + } + } +} + +void NASController::Calibrating(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(NASControllerState::CALIBRATING); + calibrate(); + break; + } + + case NAS_READY: + { + transition(&NASController::Ready); + break; + } + } +} + +void NASController::Ready(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(NASControllerState::READY); + break; + } + + case NAS_CALIBRATE: + { + transition(&NASController::Calibrating); + break; + } + + case NAS_FORCE_START: + case FLIGHT_ARMED: + { + transition(&NASController::Active); + break; + } + } +} + +void NASController::Active(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(NASControllerState::ACTIVE); + break; + } + + case FLIGHT_LANDING_DETECTED: + { + transition(&NASController::End); + break; + } + + case NAS_FORCE_STOP: + case FLIGHT_DISARMED: + { + transition(&NASController::Ready); + break; + } + } +} + +void NASController::End(const Event& event) +{ + switch (event) + { + case EV_ENTRY: + { + updateState(NASControllerState::END); + break; + } + } +} + +void NASController::calibrate() +{ + Sensors* sensors = getModule<Sensors>(); + + Vector3f accSum = Vector3f::Zero(); + Vector3f magSum = Vector3f::Zero(); + float baroSum = 0.0f; + + for (int i = 0; i < config::CALIBRATION_SAMPLES_COUNT; i++) + { + BMX160Data imu = sensors->getBMX160WithCorrectionLastSample(); + PressureData baro = sensors->getLPS22DFLastSample(); + + accSum += + Vector3f{imu.accelerationX, imu.accelerationY, imu.accelerationZ}; + magSum += Vector3f{imu.magneticFieldX, imu.magneticFieldY, + imu.magneticFieldZ}; + + baroSum += baro.pressure; + + Thread::sleep(config::CALIBRATION_SLEEP_TIME); + } + + Vector3f meanAcc = accSum / config::CALIBRATION_SAMPLES_COUNT; + meanAcc.normalize(); + Vector3f meanMag = magSum / config::CALIBRATION_SAMPLES_COUNT; + meanMag.normalize(); + float meanBaro = baroSum / config::CALIBRATION_SAMPLES_COUNT; + + // Use the triad to compute initial state + StateInitializer init; + init.triad(meanAcc, meanMag, ReferenceConfig::nedMag); + + miosix::Lock<miosix::FastMutex> l(nasMutex); + + // Compute reference values + ReferenceValues reference = nas.getReferenceValues(); + reference.refPressure = meanBaro; + reference.refAltitude = Aeroutils::relAltitude( + reference.refPressure, reference.mslPressure, reference.mslTemperature); + + // Also update the reference with the GPS if we have fix + UBXGPSData gps = sensors->getUBXGPSLastSample(); + if (gps.fix == 3) + { + // Don't use the GPS altitude because it is not reliable + reference.refLatitude = gps.latitude; + reference.refLongitude = gps.longitude; + } + + // Update the algorithm reference values + nas.setX(init.getInitX()); + nas.setReferenceValues(reference); + + EventBroker::getInstance().post(NAS_READY, TOPIC_NAS); +} + +void NASController::update() +{ + // Update the NAS state only if the FSM is active + if (state != NASControllerState::ACTIVE) + { + return; + } + + auto* sensors = getModule<Sensors>(); + + auto imu = sensors->getBMX160LastSample(); + auto gps = sensors->getUBXGPSLastSample(); + auto baro = sensors->getLPS22DFLastSample(); + + // Calculate acceleration + Vector3f acc = static_cast<AccelerometerData>(imu); + float accLength = acc.norm(); + + miosix::Lock<miosix::FastMutex> l(nasMutex); + + // 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); + } + + 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); + } + + // 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) + { + acc1gSamplesCount++; + } + else + { + acc1g = true; + } + } + else + { + acc1gSamplesCount = 0; + acc1g = false; + } + + lastGyroTimestamp = imu.angularSpeedTimestamp; + lastAccTimestamp = imu.accelerationTimestamp; + lastMagTimestamp = imu.magneticFieldTimestamp; + lastGpsTimestamp = gps.gpsTimestamp; + lastBaroTimestamp = baro.pressureTimestamp; + + auto state = nas.getState(); + auto ref = nas.getReferenceValues(); + + Logger::getInstance().log(state); +} + +void NASController::updateState(NASControllerState newState) +{ + state = newState; + + auto status = NASControllerStatus{ + .timestamp = TimestampTimer::getTimestamp(), + .state = newState, + }; + Logger::getInstance().log(status); +} + +} // namespace Parafoil diff --git a/src/Parafoil/StateMachines/NASController/NASController.h b/src/Parafoil/StateMachines/NASController/NASController.h new file mode 100644 index 000000000..6c75c5830 --- /dev/null +++ b/src/Parafoil/StateMachines/NASController/NASController.h @@ -0,0 +1,100 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Niccolò Betto, Davide Basso + * + * 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/NAS/NAS.h> +#include <diagnostic/PrintLogger.h> +#include <events/FSM.h> +#include <utils/DependencyManager/DependencyManager.h> + +#include "NASControllerData.h" + +namespace Parafoil +{ +class BoardScheduler; +class Sensors; + +class NASController + : public Boardcore::FSM<NASController>, + public Boardcore::InjectableWithDeps<BoardScheduler, Sensors> +{ +public: + /** + * @brief Initializes the NAS controller. + * + * Sets the initial FSM state to idle, subscribes to NAS and flight events + * and initializes the NAS algorithm. + */ + NASController(); + + /** + * @brief Adds the NAS update function into the scheduler and starts the FSM + * thread + */ + bool start() override; + + Boardcore::NASState getNasState(); + Boardcore::ReferenceValues getReferenceValues(); + + NASControllerState getState(); + + void setOrientation(const Eigen::Quaternionf& orientation); + +private: + void calibrate(); + + /** + * @brief Update the NAS estimation + */ + void update(); + + // FSM states + void Init(const Boardcore::Event& event); + void Calibrating(const Boardcore::Event& event); + void Ready(const Boardcore::Event& event); + void Active(const Boardcore::Event& event); + void End(const Boardcore::Event& event); + + void updateState(NASControllerState newState); + + std::atomic<NASControllerState> state{NASControllerState::INIT}; + + Boardcore::NAS nas; ///< The NAS algorithm instance + miosix::FastMutex nasMutex; + + int magDecimateCount = 0; + 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; + + std::atomic<bool> started{false}; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("NAS"); +}; + +} // namespace Parafoil diff --git a/src/Parafoil/StateMachines/NASController/NASControllerData.h b/src/Parafoil/StateMachines/NASController/NASControllerData.h new file mode 100644 index 000000000..020f69541 --- /dev/null +++ b/src/Parafoil/StateMachines/NASController/NASControllerData.h @@ -0,0 +1,54 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Authors: Niccolò Betto, Davide Basso + * + * 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 <cstdint> +#include <ostream> +#include <string> + +namespace Parafoil +{ + +enum class NASControllerState : uint8_t +{ + INIT = 0, + CALIBRATING, + READY, + ACTIVE, + END +}; + +struct NASControllerStatus +{ + uint64_t timestamp = 0; + NASControllerState state = NASControllerState::INIT; + + static std::string header() { return "timestamp,state\n"; } + + void print(std::ostream& os) const + { + os << timestamp << "," << (int)state << "\n"; + } +}; + +} // namespace Parafoil diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp index a38d1413b..acbf3e24b 100644 --- a/src/Parafoil/parafoil-entry.cpp +++ b/src/Parafoil/parafoil-entry.cpp @@ -22,6 +22,7 @@ #include <Parafoil/BoardScheduler.h> #include <Parafoil/Buses.h> +#include <Parafoil/Sensors/Sensors.h> #include <diagnostic/PrintLogger.h> #include <utils/DependencyManager/DependencyManager.h> @@ -54,5 +55,9 @@ int main() auto scheduler = new BoardScheduler(); initResult &= depman.insert(scheduler); + // Sensors + auto sensors = new Sensors(); + initResult &= depman.insert(sensors); + return 0; } \ No newline at end of file -- GitLab