diff --git a/src/Parafoil/BoardScheduler.h b/src/Parafoil/BoardScheduler.h index 8f89928081b6b4dfc5c2594e8cc9c52dab05b779..1b9b1d2dee0f4561ca5377f013e3ef9620fa755b 100644 --- a/src/Parafoil/BoardScheduler.h +++ b/src/Parafoil/BoardScheduler.h @@ -52,6 +52,8 @@ public: }; }; + Boardcore::TaskScheduler& sensors() { return high; } + static Priority::PriorityLevel flightModeManagerPriority() { return Priority::MEDIUM; diff --git a/src/Parafoil/Configs/SensorsConfig.h b/src/Parafoil/Configs/SensorsConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..c7b8a5e536088dee32725c5e68f39eee7ba4e2cf --- /dev/null +++ b/src/Parafoil/Configs/SensorsConfig.h @@ -0,0 +1,129 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: 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 <drivers/adc/InternalADC.h> +#include <sensors/ADS131M08/ADS131M08Defs.h> +#include <sensors/BMX160/BMX160Config.h> +#include <sensors/H3LIS331DL/H3LIS331DL.h> +#include <sensors/LIS3MDL/LIS3MDL.h> +#include <sensors/LPS22DF/LPS22DF.h> +#include <sensors/calibration/AxisOrientation.h> +#include <units/Frequency.h> + +#include <chrono> + +namespace Parafoil +{ +namespace Config +{ +namespace Sensors +{ + +/* linter off */ using namespace Boardcore::Units::Frequency; +/* linter off */ using namespace std::chrono_literals; + +namespace BMX160 +{ +constexpr auto ENABLED = true; +constexpr auto SAMPLING_RATE = 100_hz; + +constexpr auto CALIBRATION_DURATION = 2000ms; +constexpr auto ACC_FSR = Boardcore::BMX160Config::AccelerometerRange::G_16; +constexpr auto GYRO_FSR = Boardcore::BMX160Config::GyroscopeRange::DEG_1000; +constexpr auto ACC_GYRO_ODR = Boardcore::BMX160Config::OutputDataRate::HZ_200; +constexpr auto MAG_ODR = Boardcore::BMX160Config::OutputDataRate::HZ_100; + +constexpr auto AXIS_ROTATION = {Boardcore::Direction::NEGATIVE_Y, + Boardcore::Direction::NEGATIVE_Z}; + +constexpr auto TEMP_DIVIDER = 1000; +constexpr auto FIFO_WATERMARK = 40; + +constexpr auto FIFO_HEADER_SIZE = 1; +constexpr auto ACC_DATA_SIZE = 6; +constexpr auto GYRO_DATA_SIZE = 6; +constexpr auto MAG_DATA_SIZE = 8; +} // namespace BMX160 + +namespace H3LIS331DL +{ +constexpr auto ENABLED = true; +constexpr auto SAMPLING_RATE = 100_hz; +constexpr auto ODR = Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400; +constexpr auto BDU = + Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE; +constexpr auto FSR = Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100; +} // namespace H3LIS331DL + +namespace LPS22DF +{ +constexpr auto ENABLED = true; +constexpr auto SAMPLING_RATE = 50_hz; +constexpr auto AVG = Boardcore::LPS22DF::AVG_4; +constexpr auto ODR = Boardcore::LPS22DF::ODR_100; +} // namespace LPS22DF + +namespace UBXGPS +{ +constexpr auto ENABLED = true; +constexpr auto SAMPLING_RATE = 10_hz; +} // namespace UBXGPS + +namespace ADS131M08 +{ +constexpr auto ENABLED = true; +constexpr auto SAMPLING_RATE = 100_hz; +constexpr auto OVERSAMPLING_RATIO = + Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192; +constexpr bool GLOBAL_CHOP_MODE = true; +} // namespace ADS131M08 + +namespace LIS3MDL +{ +constexpr auto ENABLED = true; +constexpr auto SAMPLING_RATE = 100_hz; +constexpr auto ODR = Boardcore::LIS3MDL::ODR_80_HZ; +constexpr auto FSR = Boardcore::LIS3MDL::FS_4_GAUSS; +} // namespace LIS3MDL + +namespace InternalADC +{ +constexpr auto ENABLED = true; +constexpr auto SAMPLING_RATE = 100_hz; +constexpr auto VBAT_CH = Boardcore::InternalADC::Channel::CH5; +constexpr auto VBAT_COEFF = (150 + 40.2) / 40.2; +} // namespace InternalADC + +namespace MagCalibration +{ +constexpr auto FILE_ENABLED = true; +constexpr auto SAMPLING_RATE = 10_hz; +constexpr auto CALIBRATION_PATH = "/sd/bmx160_magnetometer_calibration.csv"; +} // namespace MagCalibration + +} // namespace Sensors + +} // namespace Config + +} // namespace Parafoil \ No newline at end of file diff --git a/src/Parafoil/Sensors/SensorData.h b/src/Parafoil/Sensors/SensorData.h new file mode 100644 index 0000000000000000000000000000000000000000..cf7a88d15f2c3738e030ce07887597c7e903e953 --- /dev/null +++ b/src/Parafoil/Sensors/SensorData.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: 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 <sensors/SensorData.h> + +namespace Parafoil +{ + +struct SensorCalibrationData +{ + uint64_t timestamp = 0; + float magBiasX = 0.0f; + float magBiasY = 0.0f; + float magBiasZ = 0.0f; + float magScaleX = 0.0f; + float magScaleY = 0.0f; + float magScaleZ = 0.0f; + + static std::string header() + { + return "timestamp,magBiasX,magBiasY,magBiasZ,magScaleX,magScaleY," + "magScaleZ\n"; + } + + void print(std::ostream& os) const + { + os << timestamp << "," << magBiasX << "," << magBiasY << "," << magBiasZ + << "," << magScaleX << "," << magScaleY << "," << magScaleZ << "\n"; + } +}; + +} // namespace Parafoil \ No newline at end of file diff --git a/src/Parafoil/Sensors/Sensors.cpp b/src/Parafoil/Sensors/Sensors.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a52727c36fd0a43d7cd5c6984e236e168ec7a984 --- /dev/null +++ b/src/Parafoil/Sensors/Sensors.cpp @@ -0,0 +1,511 @@ +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: 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 "Sensors.h" + +#include <Parafoil/BoardScheduler.h> +#include <Parafoil/Buses.h> + +#include <chrono> + +using namespace Boardcore; +namespace config = Parafoil::Config::Sensors; +namespace hwmap = miosix::sensors; + +namespace Parafoil +{ + +bool Sensors::start() +{ + if (config::BMX160::ENABLED) + { + bmx160Init(); + bmx160WithCorrectionInit(); + } + + if (config::LIS3MDL::ENABLED) + { + lis3mdlInit(); + } + + if (config::H3LIS331DL::ENABLED) + { + h3lisInit(); + } + + if (config::LPS22DF::ENABLED) + { + lps22Init(); + } + + if (config::UBXGPS::ENABLED) + { + ubxGpsInit(); + } + + if (config::ADS131M08::ENABLED) + { + ads131Init(); + } + + if (config::InternalADC::ENABLED) + { + internalADCInit(); + } + + if (!postSensorCreationHook()) + { + LOG_ERR(logger, "Sensors post-creation hook failed"); + return false; + } + + if (!sensorManagerInit()) + { + LOG_ERR(logger, "Failed to initialize sensor manager"); + return false; + } + + auto& scheduler = getModule<BoardScheduler>()->sensors(); + + magCalibrationTaskId = scheduler.addTask( + [this]() + { + auto mag = getLIS3MDLLastSample(); + + miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex}; + magCalibrator.feed(mag); + }, + config::MagCalibration::SAMPLING_RATE); + + if (magCalibrationTaskId == 0) + { + LOG_ERR(logger, "Failed to add magnetometer calibration task"); + return false; + } + + // Immediately disable the task, so that is enabled only when needed + // from FlightModeManager. It is calibrated during the pre-flight + // initialization phase. + scheduler.disableTask(magCalibrationTaskId); + + started = true; + return true; +} + +bool Sensors::isStarted() { return started; } + +void Sensors::calibrate() +{ + bmx160WithCorrection->startCalibration(); + + miosix::Thread::sleep( + std::chrono::milliseconds{config::BMX160::CALIBRATION_DURATION} + .count()); + + bmx160WithCorrection->stopCalibration(); +} + +SensorCalibrationData Sensors::getCalibrationData() +{ + miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex}; + + return SensorCalibrationData{ + .timestamp = TimestampTimer::getTimestamp(), + .magBiasX = magCalibration.getb().x(), + .magBiasY = magCalibration.getb().y(), + .magBiasZ = magCalibration.getb().z(), + .magScaleX = magCalibration.getA().x(), + .magScaleY = magCalibration.getA().y(), + .magScaleZ = magCalibration.getA().z(), + }; +} + +void Sensors::resetMagCalibrator() +{ + miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex}; + magCalibrator = SoftAndHardIronCalibration{}; +} + +void Sensors::enableMagCalibrator() +{ + getModule<BoardScheduler>()->sensors().enableTask(magCalibrationTaskId); +} + +void Sensors::disableMagCalibrator() +{ + getModule<BoardScheduler>()->sensors().disableTask(magCalibrationTaskId); +} + +bool Sensors::saveMagCalibration() +{ + miosix::Lock<miosix::FastMutex> lock{magCalibrationMutex}; + + SixParametersCorrector calibration = magCalibrator.computeResult(); + + // Check if the calibration is valid + if (!std::isnan(calibration.getb()[0]) && + !std::isnan(calibration.getb()[1]) && + !std::isnan(calibration.getb()[2]) && + !std::isnan(calibration.getA()[0]) && + !std::isnan(calibration.getA()[1]) && + !std::isnan(calibration.getA()[2])) + { + // Its valid, save it and apply it + magCalibration = calibration; + return magCalibration.toFile(config::MagCalibration::CALIBRATION_PATH); + } + else + { + return false; + } +} + +BMX160Data Sensors::getBMX160LastSample() +{ + return bmx160 ? bmx160->getLastSample() : BMX160Data{}; +} + +BMX160WithCorrectionData Sensors::getBMX160WithCorrectionLastSample() +{ + return bmx160WithCorrection ? bmx160WithCorrection->getLastSample() + : BMX160WithCorrectionData{}; +} + +LIS3MDLData Sensors::getLIS3MDLLastSample() +{ + return lis3mdl ? lis3mdl->getLastSample() : LIS3MDLData{}; +} + +H3LIS331DLData Sensors::getH3LISLastSample() +{ + return h3lis331dl ? h3lis331dl->getLastSample() : H3LIS331DLData{}; +} + +LPS22DFData Sensors::getLPS22DFLastSample() +{ + return lps22df ? lps22df->getLastSample() : LPS22DFData{}; +} + +UBXGPSData Sensors::getUBXGPSLastSample() +{ + return ubxgps ? ubxgps->getLastSample() : UBXGPSData{}; +} + +ADS131M08Data Sensors::getADS131LastSample() +{ + return ads131m08 ? ads131m08->getLastSample() : ADS131M08Data{}; +} + +InternalADCData Sensors::getInternalADCLastSample() +{ + return internalAdc ? internalAdc->getLastSample() : InternalADCData{}; +} + +BatteryVoltageSensorData Sensors::getBatteryVoltage() +{ + auto sample = getInternalADCLastSample(); + + BatteryVoltageSensorData data; + data.voltageTimestamp = sample.timestamp; + data.channelId = static_cast<uint8_t>(config::InternalADC::VBAT_CH); + data.voltage = sample.voltage[config::InternalADC::VBAT_CH]; + data.batVoltage = sample.voltage[config::InternalADC::VBAT_CH] * + config::InternalADC::VBAT_COEFF; + + return data; +} + +std::vector<SensorInfo> Sensors::getSensorInfo() +{ + if (manager) + { + std::vector<SensorInfo> infos{}; + +#define PUSH_SENSOR_INFO(instance, name) \ + if (instance) \ + infos.push_back(manager->getSensorInfo(instance.get())); \ + else \ + infos.push_back( \ + SensorInfo { #name, config::name::SAMPLING_RATE, nullptr, false }) + + PUSH_SENSOR_INFO(bmx160, BMX160); + PUSH_SENSOR_INFO(bmx160WithCorrection, BMX160); + PUSH_SENSOR_INFO(h3lis331dl, H3LIS331DL); + PUSH_SENSOR_INFO(lis3mdl, LIS3MDL); + PUSH_SENSOR_INFO(lps22df, LPS22DF); + PUSH_SENSOR_INFO(ubxgps, UBXGPS); + PUSH_SENSOR_INFO(ads131m08, ADS131M08); + PUSH_SENSOR_INFO(internalAdc, InternalADC); + +#undef PUSH_SENSOR_INFO + + return infos; + } + else + { + return {}; + } +} + +void Sensors::bmx160Init() +{ + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_4; + + BMX160Config config; + config.fifoMode = BMX160Config::FifoMode::DISABLED; + config.fifoWatermark = config::BMX160::FIFO_WATERMARK; + config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; + + config.temperatureDivider = config::BMX160::TEMP_DIVIDER; + + config.accelerometerRange = config::BMX160::ACC_FSR; + config.gyroscopeRange = config::BMX160::GYRO_FSR; + + config.accelerometerDataRate = config::BMX160::ACC_GYRO_ODR; + config.gyroscopeDataRate = config::BMX160::ACC_GYRO_ODR; + config.magnetometerRate = config::BMX160::MAG_ODR; + + config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD; + + bmx160 = std::make_unique<BMX160>(getModule<Buses>()->spi1, + hwmap::bmx160::cs::getPin(), spiConfig, + config); + + LOG_INFO(logger, "BMX160 initialized!"); +} + +void Sensors::bmx160WithCorrectionInit() +{ + bmx160WithCorrection = std::make_unique<BMX160WithCorrection>( + bmx160.get(), config::BMX160::AXIS_ROTATION); + + LOG_INFO(logger, "BMX160WithCorrection initialized!"); +} + +void Sensors::h3lisInit() +{ + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_16; + + h3lis331dl = std::make_unique<H3LIS331DL>( + getModule<Buses>()->spi1, hwmap::h3lis331dl::cs::getPin(), spiConfig, + config::H3LIS331DL::ODR, config::H3LIS331DL::BDU, + config::H3LIS331DL::FSR); + + LOG_INFO(logger, "H3LIS331DL initialized!"); +} + +void Sensors::lis3mdlInit() +{ + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_32; + + LIS3MDL::Config config; + config.odr = config::LIS3MDL::ODR; + config.scale = config::LIS3MDL::FSR; + config.temperatureDivider = 1; + + lis3mdl = std::make_unique<LIS3MDL>(getModule<Buses>()->spi1, + hwmap::lis3mdl::cs::getPin(), spiConfig, + config); + + LOG_INFO(logger, "LIS3MDL initialized!"); +} + +void Sensors::lps22Init() +{ + auto spiConfig = LPS22DF::getDefaultSPIConfig(); + spiConfig.clockDivider = SPI::ClockDivider::DIV_16; + + LPS22DF::Config config; + config.avg = config::LPS22DF::AVG; + config.odr = config::LPS22DF::ODR; + + lps22df = std::make_unique<LPS22DF>(getModule<Buses>()->spi1, + hwmap::lps22df::cs::getPin(), spiConfig, + config); + + LOG_INFO(logger, "LPS22DF initialized!"); +} + +void Sensors::ubxGpsInit() +{ + auto spiConfig = UBXGPSSpi::getDefaultSPIConfig(); + spiConfig.clockDivider = SPI::ClockDivider::DIV_64; + + ubxgps = std::make_unique<UBXGPSSpi>(getModule<Buses>()->spi1, + hwmap::ubxgps::cs::getPin(), spiConfig, + config::UBXGPS::SAMPLING_RATE); + + LOG_INFO(logger, "UBXGPS initialized!"); +} + +void Sensors::ads131Init() +{ + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_32; + + ADS131M08::Config config; + config.oversamplingRatio = config::ADS131M08::OVERSAMPLING_RATIO; + config.globalChopModeEnabled = config::ADS131M08::GLOBAL_CHOP_MODE; + + ads131m08 = std::make_unique<ADS131M08>(getModule<Buses>()->spi1, + hwmap::ads131m08::cs::getPin(), + spiConfig, config); + + LOG_INFO(logger, "ADS131M08 initialized!"); +} + +void Sensors::internalADCInit() +{ + internalAdc = std::make_unique<InternalADC>(ADC3); + internalAdc->enableChannel(config::InternalADC::VBAT_CH); + internalAdc->enableTemperature(); + internalAdc->enableVbat(); + + LOG_INFO(logger, "InternalADC initialized!"); +} + +void Sensors::bmx160Callback() +{ + auto fifoSize = bmx160->getLastFifoSize(); + auto& fifo = bmx160->getLastFifo(); + + Logger::getInstance().log(bmx160->getTemperature()); + + for (auto i = 0; i < fifoSize; i++) + { + Logger::getInstance().log(fifo.at(i)); + } + + Logger::getInstance().log(bmx160->getFifoStats()); +} + +void Sensors::bmx160WithCorrectionCallback() +{ + BMX160WithCorrectionData lastSample = bmx160WithCorrection->getLastSample(); + Logger::getInstance().log(lastSample); +} + +void Sensors::h3lisCallback() +{ + H3LIS331DLData lastSample = h3lis331dl->getLastSample(); + Logger::getInstance().log(lastSample); +} + +void Sensors::lis3mdlCallback() +{ + LIS3MDLData lastSample = lis3mdl->getLastSample(); + Logger::getInstance().log(lastSample); +} + +void Sensors::lps22Callback() +{ + LPS22DFData lastSample = lps22df->getLastSample(); + Logger::getInstance().log(lastSample); +} + +void Sensors::ubxGpsCallback() +{ + UBXGPSData lastSample = ubxgps->getLastSample(); + Logger::getInstance().log(lastSample); +} + +void Sensors::ads131Callback() +{ + ADS131M08Data lastSample = ads131m08->getLastSample(); + Logger::getInstance().log(lastSample); +} + +void Sensors::internalADCCallback() +{ + InternalADCData lastSample = internalAdc->getLastSample(); + Logger::getInstance().log(lastSample); +} + +bool Sensors::sensorManagerInit() +{ + SensorManager::SensorMap_t map; + + if (bmx160) + { + SensorInfo info{"BMX160", config::BMX160::SAMPLING_RATE, + [this]() { bmx160Callback(); }}; + map.emplace(bmx160.get(), info); + } + + if (bmx160WithCorrection) + { + SensorInfo info{"BMX160WithCorrection", config::BMX160::SAMPLING_RATE, + [this]() { bmx160WithCorrectionCallback(); }}; + map.emplace(bmx160WithCorrection.get(), info); + } + + if (h3lis331dl) + { + SensorInfo info{"H3LIS331DL", config::H3LIS331DL::SAMPLING_RATE, + [this]() { h3lisCallback(); }}; + map.emplace(h3lis331dl.get(), info); + } + + if (lis3mdl) + { + SensorInfo info{"LIS3MDL", config::LIS3MDL::SAMPLING_RATE, + [this]() { lis3mdlCallback(); }}; + map.emplace(lis3mdl.get(), info); + } + + if (lps22df) + { + SensorInfo info{"LPS22DF", config::LPS22DF::SAMPLING_RATE, + [this]() { lps22Callback(); }}; + map.emplace(lps22df.get(), info); + } + + if (ubxgps) + { + SensorInfo info{"UBXGPS", config::UBXGPS::SAMPLING_RATE, + [this]() { ubxGpsCallback(); }}; + map.emplace(ubxgps.get(), info); + } + + if (ads131m08) + { + SensorInfo info{"ADS131M08", config::ADS131M08::SAMPLING_RATE, + [this]() { ads131Callback(); }}; + map.emplace(ads131m08.get(), info); + } + + if (internalAdc) + { + SensorInfo info{"InternalADC", config::InternalADC::SAMPLING_RATE, + [this]() { internalADCCallback(); }}; + map.emplace(internalAdc.get(), info); + } + + auto& scheduler = getModule<BoardScheduler>()->sensors(); + manager = std::make_unique<SensorManager>(map, &scheduler); + return manager->start(); +} + +} // namespace Parafoil \ No newline at end of file diff --git a/src/Parafoil/Sensors/Sensors.h b/src/Parafoil/Sensors/Sensors.h index 39c70a6d8ed71ad5a42f6925a577124b65811db2..46f40b8cbdb170c5f200c98d580dcbe95df4336a 100644 --- a/src/Parafoil/Sensors/Sensors.h +++ b/src/Parafoil/Sensors/Sensors.h @@ -22,6 +22,7 @@ #pragma once +#include <Parafoil/Configs/SensorsConfig.h> #include <drivers/adc/InternalADC.h> #include <sensors/ADS131M08/ADS131M08.h> #include <sensors/BMX160/BMX160.h> @@ -32,8 +33,11 @@ #include <sensors/SensorManager.h> #include <sensors/UBXGPS/UBXGPSSpi.h> #include <sensors/analog/BatteryVoltageSensor.h> +#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> #include <utils/DependencyManager/DependencyManager.h> +#include "SensorData.h" + namespace Parafoil { class BoardScheduler; @@ -60,22 +64,45 @@ public: void calibrate(); /** - * @brief Takes the result of the live magnetometer calibration and applies - * it to the current calibration + writes it in the csv file + * @brief Returns the current sensors calibration parameters. * - * @return true if the write was successful + * @internal Ensure mutexes are unlocked before calling this function. + */ + SensorCalibrationData getCalibrationData(); + + /** + * @brief Resets the magnetometer calibration. + */ + void resetMagCalibrator(); + + /** + * @brief Enables the magnetometer calibration task. + */ + void enableMagCalibrator(); + + /** + * @brief Disables the magnetometer calibration task. */ - bool writeMagCalibration(); + void disableMagCalibrator(); + + /** + * @brief Saves the current magnetometer calibration to file, overwriting + * the previous one, and sets it as the active calibration. + * + * @return Whether the calibration was saved successfully. + */ + bool saveMagCalibration(); Boardcore::BMX160Data getBMX160LastSample(); Boardcore::BMX160WithCorrectionData getBMX160WithCorrectionLastSample(); Boardcore::H3LIS331DLData getH3LISLastSample(); Boardcore::LIS3MDLData getLIS3MDLLastSample(); - Boardcore::LPS22DFData getLPS22LastSample(); - Boardcore::UBXGPSData getUbxGpsLastSample(); + Boardcore::LPS22DFData getLPS22DFLastSample(); + Boardcore::UBXGPSData getUBXGPSLastSample(); Boardcore::ADS131M08Data getADS131LastSample(); Boardcore::InternalADCData getInternalADCLastSample(); - Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample(); + + Boardcore::BatteryVoltageSensorData getBatteryVoltage(); /** * @brief Returns information about all sensors managed by this class @@ -97,7 +124,6 @@ protected: virtual bool postSensorCreationHook() { return true; } std::unique_ptr<Boardcore::BMX160> bmx160; - std::unique_ptr<Boardcore::BMX160WithCorrection> bmx160WithCorrection; std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl; std::unique_ptr<Boardcore::LIS3MDL> lis3mdl; std::unique_ptr<Boardcore::LPS22DF> lps22df; @@ -105,6 +131,10 @@ protected: std::unique_ptr<Boardcore::ADS131M08> ads131m08; std::unique_ptr<Boardcore::InternalADC> internalAdc; + std::unique_ptr<Boardcore::BMX160WithCorrection> bmx160WithCorrection; + + std::unique_ptr<Boardcore::SensorManager> manager; + private: /** * Sensors initialization and callback functions. @@ -134,10 +164,17 @@ private: void internalADCInit(); void internalADCCallback(); - void batteryVoltageInit(); - void batteryVoltageCallback(); - bool sensorManagerInit(); + + // Live calibration of the magnetomer + miosix::FastMutex magCalibrationMutex; + Boardcore::SoftAndHardIronCalibration magCalibrator; + Boardcore::SixParametersCorrector magCalibration; + size_t magCalibrationTaskId = 0; + + std::atomic<bool> started{false}; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors"); }; } // namespace Parafoil \ No newline at end of file diff --git a/src/Parafoil/parafoil-entry.cpp b/src/Parafoil/parafoil-entry.cpp index 8c07feaae339f268793159054e792d909dae5144..a38d1413b9d4f44a454c0b8fa66624fc610abf64 100644 --- a/src/Parafoil/parafoil-entry.cpp +++ b/src/Parafoil/parafoil-entry.cpp @@ -39,8 +39,7 @@ using namespace Parafoil; int main() { - std::cout << "Parafoil Entrypoint " - << "(" << BUILD_TYPE << ")" + std::cout << "Parafoil Entrypoint " << "(" << BUILD_TYPE << ")" << " by Skyward Experimental Rocketry" << std::endl; auto logger = Logging::getLogger("Mockup"); diff --git a/src/Payload/payload-entry.cpp b/src/Payload/payload-entry.cpp index a2160a7abf826f046417bfad198ea0429229a741..81fd9cb4d77cea0ef6c7391d230fbde98d60228c 100644 --- a/src/Payload/payload-entry.cpp +++ b/src/Payload/payload-entry.cpp @@ -106,9 +106,8 @@ using namespace Common; int main() { miosix::ledOff(); - std::cout << "Payload " << FLAVOR << " Entrypoint " - << "(" << BUILD_TYPE << ")" - << " by Skyward Experimental Rocketry" << std::endl; + std::cout << "Payload " << FLAVOR << " Entrypoint " << "(" << BUILD_TYPE + << ")" << " by Skyward Experimental Rocketry" << std::endl; // Unused but needed to set the log level properly auto logger = Logging::getLogger("Payload");