diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b31cb98642320cf129d5f48817efa4a2bd20cd5..423224651c7ec72141228b7ad0bdfad1bc2917c3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,17 +36,7 @@ project(OnBoardSoftware) add_executable(main-entry src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER}) target_include_directories(main-entry PRIVATE ${OBSW_INCLUDE_DIRS}) -sbs_target(main-entry stm32f767zi_death_stack_v4) - -add_executable(main-entry-roccaraso src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER}) -target_include_directories(main-entry-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS}) -target_compile_definitions(main-entry-roccaraso PRIVATE ROCCARASO) -sbs_target(main-entry-roccaraso stm32f767zi_death_stack_v4) - -add_executable(main-entry-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER}) -target_include_directories(main-entry-euroc PRIVATE ${OBSW_INCLUDE_DIRS}) -target_compile_definitions(main-entry-euroc PRIVATE EUROC) -sbs_target(main-entry-euroc stm32f767zi_death_stack_v4) +sbs_target(main-entry stm32f767zi_bikeshed) add_executable(payload-entry-roccaraso src/entrypoints/Payload/payload-entry.cpp ${PAYLOAD_COMPUTER}) target_include_directories(payload-entry-roccaraso PRIVATE ${OBSW_INCLUDE_DIRS}) diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index dd49c5b21842e30d343d79cc3862abde119b812a..2d154e5a53395872276d03a50330545881e1850d 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -25,22 +25,22 @@ set(OBSW_INCLUDE_DIRS ) set(MAIN_COMPUTER - src/boards/Main/BoardScheduler.cpp + # src/boards/Main/BoardScheduler.cpp src/boards/Main/Sensors/Sensors.cpp - src/boards/Main/StateMachines/NASController/NASController.cpp + # src/boards/Main/StateMachines/NASController/NASController.cpp src/boards/Main/Radio/Radio.cpp - src/boards/Main/TMRepository/TMRepository.cpp - src/boards/Main/CanHandler/CanHandler.cpp - src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp - src/boards/Main/Actuators/Actuators.cpp - src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp - src/boards/Main/StateMachines/ADAController/ADAController.cpp - src/boards/Main/PinHandler/PinHandler.cpp - src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp - src/boards/Main/StateMachines/ABKController/ABKController.cpp - src/boards/Main/StateMachines/MEAController/MEAController.cpp - src/boards/Main/StateMachines/Deployment/Deployment.cpp - src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp + # src/boards/Main/TMRepository/TMRepository.cpp + # src/boards/Main/CanHandler/CanHandler.cpp + # src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp + # src/boards/Main/Actuators/Actuators.cpp + # src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp + # src/boards/Main/StateMachines/ADAController/ADAController.cpp + # src/boards/Main/PinHandler/PinHandler.cpp + # src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp + # src/boards/Main/StateMachines/ABKController/ABKController.cpp + # src/boards/Main/StateMachines/MEAController/MEAController.cpp + # src/boards/Main/StateMachines/Deployment/Deployment.cpp + # src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp ) set(MOTOR_SOURCES diff --git a/src/boards/Main/Buses.h b/src/boards/Main/Buses.h index 13552ec6be401c1d3be9ebd4456e4da4e94d3b9a..b48775aafa8c60c4259b53edb46199daf1eba25d 100644 --- a/src/boards/Main/Buses.h +++ b/src/boards/Main/Buses.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Mor * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -27,29 +27,39 @@ #include <drivers/usart/USART.h> #include <interfaces-impl/hwmapping.h> +#include <unordered_map> #include <utils/ModuleManager/ModuleManager.hpp> + namespace Main { + class Buses : public Boardcore::Module { public: - Boardcore::SPIBus spi1; - Boardcore::SPIBus spi3; - Boardcore::SPIBus spi4; - Boardcore::SPIBus spi6; - - Boardcore::I2C i2c1; - - Boardcore::USART usart1; - Boardcore::USART usart2; - Boardcore::USART uart4; - - Buses() - : spi1(SPI1), spi3(SPI3), spi4(SPI4), spi6(SPI6), - i2c1(I2C1, miosix::interfaces::i2c1::scl::getPin(), - miosix::interfaces::i2c1::sda::getPin()), - usart1(USART1, 115200), usart2(USART2, 115200), uart4(UART4, 115200) - { - } + Buses() {} + + Boardcore::SPIBus &getH3LIS331DL() { return spi1; } + Boardcore::SPIBus &getLPS22DF() { return spi1; } + Boardcore::SPIBus &getLIS2MDL() { return spi3; } + Boardcore::SPIBus &getLSM6DSRX() { return spi3; } + Boardcore::SPIBus &getUBXGps() { return spi3; } + Boardcore::SPIBus &getADS131M08() { return spi4; } + Boardcore::SPIBus &getRadio() { return spi6; } + + Boardcore::USART &getHILUart() { return usart4; } + + Boardcore::I2C &getLPS28DFW() { return i2c1; } + +private: + Boardcore::SPIBus spi1{SPI1}; + Boardcore::SPIBus spi3{SPI3}; + Boardcore::SPIBus spi4{SPI4}; + Boardcore::SPIBus spi6{SPI6}; + + Boardcore::USART usart4{UART4, 256000, 1024}; + + Boardcore::I2C i2c1{I2C1, miosix::interfaces::i2c1::scl::getPin(), + miosix::interfaces::i2c1::sda::getPin()}; }; + } // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Configs/RadioConfig.h b/src/boards/Main/Configs/RadioConfig.h index 5726888cfa7d5001c477f4bdacca5009748092ef..f63b800e9df95a70d9ea78914578d7efc334ca8c 100644 --- a/src/boards/Main/Configs/RadioConfig.h +++ b/src/boards/Main/Configs/RadioConfig.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Mor * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -19,28 +19,31 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #pragma once #include <common/MavlinkGemini.h> namespace Main { -namespace RadioConfig + +namespace Config { -// Mavlink driver template parameters -constexpr uint32_t RADIO_PKT_LENGTH = 255; -constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 20; -constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE; -constexpr uint32_t RADIO_PERIODIC_TELEMETRY_PERIOD = 250; +namespace Radio +{ + +static constexpr unsigned int MAV_OUT_QUEUE_SIZE = 20; +static constexpr unsigned int MAV_MAX_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE; + +static constexpr uint16_t MAV_SLEEP_AFTER_SEND = 0; +static constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 10; -constexpr uint16_t RADIO_SLEEP_AFTER_SEND = 50; -constexpr size_t RADIO_OUT_BUFFER_MAX_AGE = 10; +static constexpr uint8_t MAV_SYSTEM_ID = 171; +static constexpr uint8_t MAV_COMPONENT_ID = 96; -constexpr uint8_t MAV_SYSTEM_ID = 171; -constexpr uint8_t MAV_COMP_ID = 96; +} -constexpr uint32_t MAVLINK_QUEUE_SIZE = 10; +} -} // namespace RadioConfig -} // namespace Main \ No newline at end of file +} \ No newline at end of file diff --git a/src/boards/Main/Configs/SensorsConfig.h b/src/boards/Main/Configs/SensorsConfig.h index 0873209db35bdcdb5aa8b11ddad26dcceb0ba4c4..31cd704a45409433613835ea068b1078828293aa 100644 --- a/src/boards/Main/Configs/SensorsConfig.h +++ b/src/boards/Main/Configs/SensorsConfig.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Mor * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -19,6 +19,7 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #pragma once #include <sensors/ADS131M08/ADS131M08.h> @@ -27,95 +28,87 @@ #include <sensors/LPS22DF/LPS22DF.h> #include <sensors/LPS28DFW/LPS28DFW.h> #include <sensors/LSM6DSRX/LSM6DSRX.h> -#include <sensors/UBXGPS/UBXGPSSpi.h> namespace Main { -namespace SensorsConfig + +namespace Config +{ + +namespace Sensors +{ + +namespace LPS22DF { -constexpr Boardcore::LPS22DF::AVG LPS22DF_AVG = Boardcore::LPS22DF::AVG_4; -constexpr Boardcore::LPS22DF::ODR LPS22DF_ODR = Boardcore::LPS22DF::ODR_100; +static constexpr Boardcore::LPS22DF::AVG AVG = Boardcore::LPS22DF::AVG_4; +static constexpr Boardcore::LPS22DF::ODR ODR = Boardcore::LPS22DF::ODR_100; +static constexpr uint32_t PERIOD = 20; // [ms] 50Hz +} // namespace LPS22DF -constexpr Boardcore::LPS28DFW::AVG LPS28DFW_AVG = Boardcore::LPS28DFW::AVG_4; -constexpr Boardcore::LPS28DFW::ODR LPS28DFW_ODR = Boardcore::LPS28DFW::ODR_100; -constexpr Boardcore::LPS28DFW::FullScaleRange LPS28DFW_FSR = +namespace LPS28DFW +{ +static constexpr Boardcore::LPS28DFW::FullScaleRange FS = Boardcore::LPS28DFW::FS_1260; +static constexpr Boardcore::LPS28DFW::AVG AVG = Boardcore::LPS28DFW::AVG_4; +static constexpr Boardcore::LPS28DFW::ODR ODR = Boardcore::LPS28DFW::ODR_100; +static constexpr uint32_t PERIOD = 20; // [ms] 50Hz +} // namespace LPS28DFW -constexpr Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS331DL_ODR = +namespace H3LIS331DL +{ +static constexpr Boardcore::H3LIS331DLDefs::OutputDataRate ODR = Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400; -constexpr Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS331DL_BDU = - Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE; -constexpr Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS331DL_FSR = +static constexpr Boardcore::H3LIS331DLDefs::FullScaleRange FS = Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100; +static constexpr uint32_t PERIOD = 10; // [ms] 100Hz +} // namespace H3LIS331DL -constexpr Boardcore::LIS2MDL::OperativeMode LIS2MDL_OPERATIVE_MODE = - Boardcore::LIS2MDL::MD_CONTINUOUS; -constexpr Boardcore::LIS2MDL::ODR LIS2MDL_ODR = Boardcore::LIS2MDL::ODR_100_HZ; -constexpr unsigned int LIS2MDL_TEMPERATURE_DIVIDER = 5; - -constexpr uint8_t UBXGPS_SAMPLE_RATE = 5; +namespace LIS2MDL +{ +static constexpr Boardcore::LIS2MDL::ODR ODR = Boardcore::LIS2MDL::ODR_100_HZ; +static constexpr unsigned int TEMP_DIVIDER = 5; +static constexpr uint32_t PERIOD = 10; // [ms] 100Hz +} // namespace LIS2MDL -constexpr Boardcore::LSM6DSRXConfig::BDU LSM6DSRX_BDU = - Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE; +namespace UBXGPS +{ +static constexpr uint32_t PERIOD = 200; // [ms] 5Hz +} -constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE LSM6DSRX_ACC_FS = +namespace LSM6DSRX +{ +static constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE ACC_FS = Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16; -constexpr Boardcore::LSM6DSRXConfig::ACC_ODR LSM6DSRX_ACC_ODR = +static constexpr Boardcore::LSM6DSRXConfig::ACC_ODR ACC_ODR = Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_416; -constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE LSM6DSRX_OPERATING_MODE = - Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL; +static constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE ACC_OP_MODE = + Boardcore::LSM6DSRXConfig::OPERATING_MODE::HIGH_PERFORMANCE; -constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE LSM6DSRX_GYR_FS = +static constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE GYR_FS = Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000; -constexpr Boardcore::LSM6DSRXConfig::GYR_ODR LSM6DSRX_GYR_ODR = +static constexpr Boardcore::LSM6DSRXConfig::GYR_ODR GYR_ODR = Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_416; +static constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE GYR_OP_MODE = + Boardcore::LSM6DSRXConfig::OPERATING_MODE::HIGH_PERFORMANCE; + +static constexpr uint32_t PERIOD = 20; // [ms] 50Hz +} // namespace LSM6DSRX + +namespace ADS131M08 +{ +static constexpr Boardcore::ADS131M08Defs::OversamplingRatio OSR = + Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192; +static constexpr bool GLOBAL_CHOP_MODE_EN = true; +static constexpr uint32_t PERIOD = 10; // [ms] 100Hz +} // namespace ADS131M08 + +namespace InternalADC +{ +static constexpr uint32_t PERIOD = 100; // [ms] 10Hz +} + +} // namespace Sensors + +} // namespace Config -constexpr Boardcore::LSM6DSRXConfig::FIFO_MODE LSM6DSRX_FIFO_MODE = - Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS; -constexpr Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION - LSM6DSRX_FIFO_TIMESTAMP_DECIMATION = - Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1; -constexpr Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR - LSM6DSRX_FIFO_TEMPERATURE_BDR = - Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED; - -constexpr Boardcore::ADS131M08Defs::OversamplingRatio - ADS131M08_OVERSAMPLING_RATIO = - Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192; -constexpr bool ADS131M08_GLOBAL_CHOP_MODE = true; - -constexpr uint32_t LPS22DF_PERIOD = 20; // [ms] 50Hz -constexpr uint32_t LPS28DFW_PERIOD = 20; // [ms] 50Hz -constexpr uint32_t H3LIS331DL_PERIOD = 10; // [ms] 100Hz -constexpr uint32_t LIS2MDL_PERIOD = 10; // [ms] 100Hz -constexpr uint32_t UBXGPS_PERIOD = 1000 / UBXGPS_SAMPLE_RATE; // [ms] -constexpr uint32_t LSM6DSRX_PERIOD = 20; // [ms] 50Hz -constexpr uint32_t ADS131M08_PERIOD = 10; // [ms] 100Hz -constexpr uint32_t IMU_PERIOD = 20; // [ms] Fake processed IMU 50Hz -constexpr uint32_t MAG_CALIBRATION_PERIOD = 20; // [ms] 50Hz - -// ADC sensors configs -constexpr float ADC_VOLTAGE_RANGE = 1.2f; // [V] Voltage reading range -constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_DPL = - Boardcore::ADS131M08Defs::Channel::CHANNEL_3; -constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_1 = - Boardcore::ADS131M08Defs::Channel::CHANNEL_4; -constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_2 = - Boardcore::ADS131M08Defs::Channel::CHANNEL_2; - -// ADC Voltage divider -constexpr float BATTERY_VOLTAGE_CONVERSION_FACTOR = - (20.f / 2.4f) + - 1; // 20 kOhm resistor and 2.4 kOhm resistor for voltage divider -constexpr float CURRENT_CONVERSION_FACTOR = - (20.f / 4.f) / (12.f / (12.f + 33.f)); -constexpr float CURRENT_OFFSET = 0.133333333f; // V in ADC - -// Calibration samples -constexpr unsigned int CALIBRATION_SAMPLES = 20; -constexpr unsigned int CALIBRATION_PERIOD = 100; - -constexpr unsigned int NUMBER_OF_SENSORS = 8; - -} // namespace SensorsConfig } // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Radio/Radio.cpp b/src/boards/Main/Radio/Radio.cpp index f3c6b40f4bd6076d4cd71c8670b49f5cf9824282..4a6d45fc413a6dfdc8f06e4ddf9d395a3c3d819f 100644 --- a/src/boards/Main/Radio/Radio.cpp +++ b/src/boards/Main/Radio/Radio.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Mor * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -19,479 +19,110 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -#include <Main/Actuators/Actuators.h> -#include <Main/AltitudeTrigger/AltitudeTrigger.h> + +#include "Radio.h" + #include <Main/Buses.h> -#include <Main/PinHandler/PinHandler.h> -#include <Main/Radio/Radio.h> -#include <Main/Sensors/Sensors.h> -#include <Main/StateMachines/ADAController/ADAController.h> -#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> -#include <Main/StateMachines/NASController/NASController.h> -#include <Main/TMRepository/TMRepository.h> -#include <common/Events.h> -#include <common/Topics.h> -#include <drivers/interrupt/external_interrupts.h> -#include <events/EventBroker.h> +#include <common/Radio.h> #include <radio/SX1278/SX1278Frontends.h> -#include <Eigen/Core> - +using namespace Main; using namespace Boardcore; +using namespace miosix; using namespace Common; -using namespace Eigen; -#define SX1278_IRQ_DIO0 EXTI3_IRQHandlerImpl -#define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl -#define SX1278_IRQ_DIO3 EXTI5_IRQHandlerImpl +SX1278Fsk* gRadio{nullptr}; -void __attribute__((used)) SX1278_IRQ_DIO0() +void handleDioIRQ() { - ModuleManager& modules = ModuleManager::getInstance(); - if (modules.get<Main::Radio>()->transceiver) + SX1278Fsk* instance = gRadio; + if (instance) { - modules.get<Main::Radio>()->transceiver->handleDioIRQ(); + instance->handleDioIRQ(); } } -void __attribute__((used)) SX1278_IRQ_DIO1() +void setIRQRadio(SX1278Fsk* radio) { - ModuleManager& modules = ModuleManager::getInstance(); - if (modules.get<Main::Radio>()->transceiver) - { - modules.get<Main::Radio>()->transceiver->handleDioIRQ(); - } + FastInterruptDisableLock dl; + gRadio = radio; } -void __attribute__((used)) SX1278_IRQ_DIO3() -{ - ModuleManager& modules = ModuleManager::getInstance(); - if (modules.get<Main::Radio>()->transceiver) - { - modules.get<Main::Radio>()->transceiver->handleDioIRQ(); - } -} -namespace Main -{ +void __attribute__((used)) MIOSIX_RADIO_DIO0_IRQ() { handleDioIRQ(); } +void __attribute__((used)) MIOSIX_RADIO_DIO1_IRQ() { handleDioIRQ(); } +void __attribute__((used)) MIOSIX_RADIO_DIO3_IRQ() { handleDioIRQ(); } -Radio::Radio(TaskScheduler* sched) : scheduler(sched) {} +bool Radio::isStarted() { return started; } bool Radio::start() { ModuleManager& modules = ModuleManager::getInstance(); - // Config the transceiver - SX1278Fsk::Config config; - config.freq_rf = 419000000; - config.freq_dev = 50000; - config.bitrate = 48000; - config.rx_bw = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000; - config.afc_bw = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000; - config.ocp = 120; - config.power = 13; - config.shaping = Boardcore::SX1278Fsk::Config::Shaping::GAUSSIAN_BT_1_0; - config.dc_free = Boardcore::SX1278Fsk::Config::DcFree::WHITENING; - config.enable_crc = false; - + // Setup the frontend std::unique_ptr<SX1278::ISX1278Frontend> frontend = std::make_unique<Skyward433Frontend>(); - transceiver = new SX1278Fsk( - modules.get<Buses>()->spi6, miosix::radio::cs::getPin(), - miosix::radio::dio0::getPin(), miosix::radio::dio1::getPin(), - miosix::radio::dio3::getPin(), SPI::ClockDivider::DIV_64, - std::move(frontend)); + // Setup transceiver + radio = std::make_unique<SX1278Fsk>( + modules.get<Buses>()->getRadio(), radio::cs::getPin(), + radio::dio0::getPin(), radio::dio1::getPin(), radio::dio3::getPin(), + SPI::ClockDivider::DIV_64, std::move(frontend)); - // Config the radio - SX1278Fsk::Error error = transceiver->init(config); + // Store the global radio instance + setIRQRadio(radio.get()); - // Add periodic telemetry send task - uint8_t result1 = - scheduler->addTask([&]() { this->sendPeriodicMessage(); }, - RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD, - TaskScheduler::Policy::RECOVER); - uint8_t result2 = scheduler->addTask( - [&]() - { - this->enqueueMsg( - modules.get<TMRepository>()->packSystemTm(MAV_STATS_ID, 0, 0)); - }, - RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD * 2, - TaskScheduler::Policy::RECOVER); + // Initialize radio + auto result = radio->init(MAIN_RADIO_CONFIG); + if (result != SX1278Fsk::Error::NONE) + { + LOG_ERR(logger, "Failed to initialize Main radio"); + return false; + } - // Config mavDriver - mavDriver = new MavDriver( - transceiver, - [=](MavDriver*, const mavlink_message_t& msg) - { this->handleMavlinkMessage(msg); }, - RadioConfig::RADIO_SLEEP_AFTER_SEND, - RadioConfig::RADIO_OUT_BUFFER_MAX_AGE); + // Initialize mavdriver + mavDriver = std::make_unique<MavDriver>( + radio.get(), + [this](MavDriver*, const mavlink_message_t& msg) + { handleMessage(msg); }, + Config::Radio::MAV_SLEEP_AFTER_SEND, + Config::Radio::MAV_OUT_BUFFER_MAX_AGE); - // Check radio failure - if (error != SX1278Fsk::Error::NONE) + if (!mavDriver->start()) { + LOG_ERR(logger, "Failed to initialize Main mav driver"); return false; } - // Start the mavlink driver thread - return mavDriver->start() && result1 != 0 && result2 != 0; + started = true; + return true; +} + +Boardcore::MavlinkStatus Radio::getMavStatus() +{ + return mavDriver->getStatus(); } void Radio::sendAck(const mavlink_message_t& msg) { mavlink_message_t ackMsg; - mavlink_msg_ack_tm_pack(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &ackMsg, msg.msgid, + mavlink_msg_ack_tm_pack(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &ackMsg, msg.msgid, msg.seq); - enqueueMsg(ackMsg); + mavDriver->enqueueMsg(ackMsg); } void Radio::sendNack(const mavlink_message_t& msg) { mavlink_message_t nackMsg; - mavlink_msg_nack_tm_pack(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &nackMsg, msg.msgid, - msg.seq); - enqueueMsg(nackMsg); + mavlink_msg_nack_tm_pack(Config::Radio::MAV_SYSTEM_ID, + Config::Radio::MAV_COMPONENT_ID, &nackMsg, + msg.msgid, msg.seq); + mavDriver->enqueueMsg(nackMsg); } -void Radio::logStatus() {} - -bool Radio::isStarted() +void Radio::handleMessage(const mavlink_message_t& msg) { - return mavDriver->isStarted() && scheduler->isRunning(); -} - -void Radio::handleMavlinkMessage(const mavlink_message_t& msg) -{ - ModuleManager& modules = ModuleManager::getInstance(); - - switch (msg.msgid) - { - case MAVLINK_MSG_ID_PING_TC: - { - // Do nothing, just add the ack to the queue - break; - } - case MAVLINK_MSG_ID_COMMAND_TC: - { - // Let the handle command reply to the message - return handleCommand(msg); - } - case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC: - { - SystemTMList tmId = static_cast<SystemTMList>( - mavlink_msg_system_tm_request_tc_get_tm_id(&msg)); - - if (tmId == MAV_PIN_OBS_ID) - { - for (uint8_t i = 0; i < PinHandler::PIN_END; i++) - { - mavlink_message_t msg; - mavlink_pin_tm_t tm; - - PinData data = modules.get<PinHandler>()->getPinData( - static_cast<PinHandler::PinList>(i)); - - tm.changes_counter = data.changesCount; - tm.current_state = tm.current_state; - tm.last_change_timestamp = tm.last_change_timestamp; - tm.pin_id = tm.pin_id; - tm.timestamp = TimestampTimer::getTimestamp(); - - mavlink_msg_pin_tm_encode(RadioConfig::MAV_SYSTEM_ID, - RadioConfig::MAV_COMP_ID, &msg, - &tm); - enqueueMsg(msg); - } - } - else if (tmId == MAV_SENSORS_STATE_ID) - { - auto sensorsState = modules.get<Sensors>()->getSensorInfo(); - - for (uint8_t i = 0; i < sensorsState.size(); i++) - { - mavlink_message_t msg; - mavlink_sensor_state_tm_t tm; - - strcpy(tm.sensor_name, sensorsState.at(i).id.c_str()); - tm.state = sensorsState.at(i).isInitialized; - - mavlink_msg_sensor_state_tm_encode( - RadioConfig::MAV_SYSTEM_ID, RadioConfig::MAV_COMP_ID, - &msg, &tm); - enqueueMsg(msg); - } - } - else - { - // Add to the queue the respose - mavlink_message_t response = - modules.get<TMRepository>()->packSystemTm(tmId, msg.msgid, - msg.seq); - // Add the response to the queue - enqueueMsg(response); - - // Check if the TM repo answered with a NACK. If so the function - // must return to avoid sending a default ack - if (response.msgid == MAVLINK_MSG_ID_NACK_TM) - { - return; - } - } - - break; - } - case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC: - { - SensorsTMList sensorId = static_cast<SensorsTMList>( - mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg)); - mavlink_message_t response = - modules.get<TMRepository>()->packSensorsTm(sensorId, msg.msgid, - msg.seq); - enqueueMsg(response); - - // If the response is a nack the method returns - if (response.msgid == MAVLINK_MSG_ID_NACK_TM) - { - return; - } - break; - } - case MAVLINK_MSG_ID_SERVO_TM_REQUEST_TC: - { - ServosList servo = static_cast<ServosList>( - mavlink_msg_servo_tm_request_tc_get_servo_id(&msg)); - mavlink_message_t response = - modules.get<TMRepository>()->packServoTm(servo, msg.msgid, - msg.seq); - enqueueMsg(response); - - // If the response is a nack the method returns - if (response.msgid == MAVLINK_MSG_ID_NACK_TM) - { - return; - } - break; - } - case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC: - { - ServosList servoId = static_cast<ServosList>( - mavlink_msg_set_servo_angle_tc_get_servo_id(&msg)); - float position = mavlink_msg_set_servo_angle_tc_get_angle(&msg); - - // Send nack if the FMM is not in test mode - if (!modules.get<FlightModeManager>()->testState( - &FlightModeManager::state_test_mode)) - { - return sendNack(msg); - } - - // If the state is test mode, the servo is set to the correct angle - modules.get<Actuators>()->setServoPosition(servoId, position); - break; - } - case MAVLINK_MSG_ID_WIGGLE_SERVO_TC: - { - ServosList servoId = static_cast<ServosList>( - mavlink_msg_wiggle_servo_tc_get_servo_id(&msg)); - - // Send nack if the FMM is not in test mode - if (!modules.get<FlightModeManager>()->testState( - &FlightModeManager::state_test_mode)) - { - return sendNack(msg); - } - - // If the state is test mode, the wiggle is done - modules.get<Actuators>()->wiggleServo(servoId); - - break; - } - case MAVLINK_MSG_ID_RESET_SERVO_TC: - { - ServosList servoId = static_cast<ServosList>( - mavlink_msg_reset_servo_tc_get_servo_id(&msg)); + LOG_INFO(logger, "Radio received data"); - // Send nack if the FMM is not in test mode - if (!modules.get<FlightModeManager>()->testState( - &FlightModeManager::state_test_mode)) - { - return sendNack(msg); - } - - // Set the servo position to 0 - modules.get<Actuators>()->setServoPosition(servoId, 0); - - break; - } - case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC: - { - float altitude = - mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg); - - modules.get<AltitudeTrigger>()->setDeploymentAltitude(altitude); - break; - } - case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC: - { - float altitude = - mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg); - - modules.get<ADAController>()->setReferenceAltitude(altitude); - modules.get<NASController>()->setReferenceAltitude(altitude); - - break; - } - case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC: - { - float temperature = - mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg); - - modules.get<ADAController>()->setReferenceTemperature(temperature); - modules.get<NASController>()->setReferenceTemperature(temperature); - - break; - } - case MAVLINK_MSG_ID_SET_COORDINATES_TC: - { - float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg); - float longitude = - mavlink_msg_set_coordinates_tc_get_longitude(&msg); - - Vector2f coordinates{latitude, longitude}; - - modules.get<NASController>()->setCoordinates(coordinates); - break; - } - case MAVLINK_MSG_ID_SET_ORIENTATION_TC: - { - float yaw = mavlink_msg_set_orientation_tc_get_yaw(&msg); - float pitch = mavlink_msg_set_orientation_tc_get_pitch(&msg); - float roll = mavlink_msg_set_orientation_tc_get_roll(&msg); - - modules.get<NASController>()->setOrientation(yaw, pitch, roll); - break; - } - default: - { - LOG_DEBUG(logger, "Received message is not of a known type"); - return sendNack(msg); - } - } - - // At the end send the ack message - sendAck(msg); -} - -void Radio::handleCommand(const mavlink_message_t& msg) -{ - MavCommandList commandId = static_cast<MavCommandList>( - mavlink_msg_command_tc_get_command_id(&msg)); - ModuleManager& modules = ModuleManager::getInstance(); - - // Create the map between the commands and the corresponding events - static const std::map<MavCommandList, Events> commandToEvent{ - {MAV_CMD_ARM, TMTC_ARM}, - {MAV_CMD_DISARM, TMTC_DISARM}, - {MAV_CMD_CALIBRATE, TMTC_CALIBRATE}, - {MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT}, - {MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH}, - {MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING}, - {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE}, - {MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION}, - {MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT}, - {MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD}, - {MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE}, - {MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE}, - {MAV_CMD_START_RECORDING, TMTC_START_RECORDING}, - {MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING}, - }; - - switch (commandId) - { - case MAV_CMD_SAVE_CALIBRATION: - { - // Save the sensor calibration and adopt it - bool result = modules.get<Sensors>()->writeMagCalibration(); - - if (!result) - { - return sendNack(msg); - } - - break; - } - case MAV_CMD_START_LOGGING: - { - bool result = Logger::getInstance().start(); - - // In case the logger is not started send to GS the result - if (!result) - { - return sendNack(msg); - } - break; - } - case MAV_CMD_STOP_LOGGING: - { - Logger::getInstance().stop(); - break; - } - default: - { - // If the command is not a particular one, look for it inside the - // map - auto it = commandToEvent.find(commandId); - - if (it != commandToEvent.end()) - { - EventBroker::getInstance().post(it->second, TOPIC_TMTC); - } - else - { - return sendNack(msg); - } - } - } - // Acknowledge the message sendAck(msg); -} - -void Radio::sendPeriodicMessage() -{ - ModuleManager& modules = ModuleManager::getInstance(); - - // Send all the queue messages - { - Lock<FastMutex> lock(queueMutex); - - for (uint8_t i = 0; i < messageQueueIndex; i++) - { - mavDriver->enqueueMsg(messageQueue[i]); - } - - // Reset the index - messageQueueIndex = 0; - } - - mavDriver->enqueueMsg( - modules.get<TMRepository>()->packSystemTm(MAV_MOTOR_ID, 0, 0)); - mavDriver->enqueueMsg( - modules.get<TMRepository>()->packSystemTm(MAV_FLIGHT_ID, 0, 0)); -} - -void Radio::enqueueMsg(const mavlink_message_t& msg) -{ - { - Lock<FastMutex> lock(queueMutex); - - // Insert the message inside the queue only if there is enough space - if (messageQueueIndex < RadioConfig::MAVLINK_QUEUE_SIZE) - { - messageQueue[messageQueueIndex] = msg; - messageQueueIndex++; - } - } -} -} // namespace Main \ No newline at end of file +} \ No newline at end of file diff --git a/src/boards/Main/Radio/Radio.h b/src/boards/Main/Radio/Radio.h index 9d7a5e80eb1332e9a8e3c6d091d22ed34f9c1791..457e84a7c7c74ecfc4d03f48c3c2610833218fd9 100644 --- a/src/boards/Main/Radio/Radio.h +++ b/src/boards/Main/Radio/Radio.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Mor * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -19,84 +19,45 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #pragma once #include <Main/Configs/RadioConfig.h> -#include <common/MavlinkGemini.h> +#include <common/Mavlink.h> #include <radio/MavlinkDriver/MavlinkDriver.h> #include <radio/SX1278/SX1278Fsk.h> -#include <scheduler/TaskScheduler.h> #include <utils/ModuleManager/ModuleManager.hpp> namespace Main { + using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU, - RadioConfig::RADIO_OUT_QUEUE_SIZE, - RadioConfig::RADIO_MAV_MSG_LENGTH>; + Config::Radio::MAV_OUT_QUEUE_SIZE, + Config::Radio::MAV_MAX_LENGTH>; class Radio : public Boardcore::Module { public: - Radio(Boardcore::TaskScheduler* sched); - - /** - * @brief Starts the MavlinkDriver - */ - [[nodiscard]] bool start(); - - /** - * @brief Sends via radio an acknowledge message about the parameter passed - * message - */ - void sendAck(const mavlink_message_t& msg); - - /** - * @brief Sends via radio an non-acknowledge message about the parameter - * passed message - */ - void sendNack(const mavlink_message_t& msg); - - /** - * @brief Saves the MavlinkDriver and transceiver status - */ - void logStatus(); + Radio() {} - /** - * @brief Returns if the radio module is correctly started - */ bool isStarted(); - Boardcore::SX1278Fsk* transceiver = nullptr; - MavDriver* mavDriver = nullptr; - -private: - /** - * @brief Called by the MavlinkDriver when a message is received - */ - void handleMavlinkMessage(const mavlink_message_t& msg); - - /** - * @brief Called by the handleMavlinkMessage to handle a command message - */ - void handleCommand(const mavlink_message_t& msg); + [[nodiscard]] bool start(); - /** - * @brief Sends the periodic telemetry - */ - void sendPeriodicMessage(); + Boardcore::MavlinkStatus getMavStatus(); - /** - * @brief Inserts the mavlink message into the queue - */ - void enqueueMsg(const mavlink_message_t& msg); +private: + void sendAck(const mavlink_message_t& msg); + void sendNack(const mavlink_message_t& msg); - // Messages queue - mavlink_message_t messageQueue[RadioConfig::MAVLINK_QUEUE_SIZE]; - uint32_t messageQueueIndex = 0; - miosix::FastMutex queueMutex; + void handleMessage(const mavlink_message_t& msg); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio"); - Boardcore::TaskScheduler* scheduler = nullptr; + + std::atomic<bool> started{false}; + std::unique_ptr<Boardcore::SX1278Fsk> radio; + std::unique_ptr<MavDriver> mavDriver; }; + } // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Sensors/Sensors.cpp b/src/boards/Main/Sensors/Sensors.cpp index 2333ee4e1a3748105e91febe1245ce29d9425bfd..c1d01a993f2ad91afacfe49f7c8fe47e9b4688bb 100644 --- a/src/boards/Main/Sensors/Sensors.cpp +++ b/src/boards/Main/Sensors/Sensors.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Mor * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -26,710 +26,240 @@ #include <Main/Configs/SensorsConfig.h> #include <interfaces-impl/hwmapping.h> +#include <utils/ModuleManager/ModuleManager.hpp> + +using namespace Main; using namespace Boardcore; -using namespace std; -using namespace Main::SensorsConfig; -namespace Main -{ -LPS22DFData Sensors::getLPS22DFLastSample() -{ - miosix::PauseKernelLock lock; - return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{}; -} -LPS28DFWData Sensors::getLPS28DFW_1LastSample() -{ - miosix::PauseKernelLock lock; - return lps28dfw_1 != nullptr ? lps28dfw_1->getLastSample() : LPS28DFWData{}; -} -LPS28DFWData Sensors::getLPS28DFW_2LastSample() -{ - miosix::PauseKernelLock lock; - return lps28dfw_2 != nullptr ? lps28dfw_2->getLastSample() : LPS28DFWData{}; -} -H3LIS331DLData Sensors::getH3LIS331DLLastSample() -{ - miosix::PauseKernelLock lock; - return h3lis331dl != nullptr ? h3lis331dl->getLastSample() - : H3LIS331DLData{}; -} -LIS2MDLData Sensors::getLIS2MDLLastSample() -{ - miosix::PauseKernelLock lock; - return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{}; -} -UBXGPSData Sensors::getGPSLastSample() -{ - miosix::PauseKernelLock lock; - return ubxgps != nullptr ? ubxgps->getLastSample() : UBXGPSData{}; -} -LSM6DSRXData Sensors::getLSM6DSRXLastSample() -{ - miosix::PauseKernelLock lock; - return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{}; -} -ADS131M08Data Sensors::getADS131M08LastSample() -{ - miosix::PauseKernelLock lock; - return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{}; -} +using namespace miosix; -PitotData Sensors::getPitotLastSample() -{ - miosix::PauseKernelLock lock; - return canPitot; -} -PressureData Sensors::getCCPressureLastSample() -{ - miosix::PauseKernelLock lock; - return canCCPressure; -} -PressureData Sensors::getBottomTankPressureLastSample() -{ - miosix::PauseKernelLock lock; - return canBottomTankPressure; -} -PressureData Sensors::getTopTankPressureLastSample() -{ - miosix::PauseKernelLock lock; - return canTopTankPressure; -} -TemperatureData Sensors::getTankTemperatureLastSample() -{ - miosix::PauseKernelLock lock; - return canTankTemperature; -} -BatteryVoltageSensorData Sensors::getMotorBatteryVoltage() -{ - miosix::PauseKernelLock lock; - return canMotorBatteryVoltage; -} -CurrentData Sensors::getMotorCurrent() -{ - miosix::PauseKernelLock lock; - return canMotorCurrent; -} +bool Sensors::isStarted() { return started; } -// Processed Getters -BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample() +bool Sensors::start() { - // Do not need to pause the kernel, the last sample getter is already - // protected - ADS131M08Data sample = getADS131M08LastSample(); - BatteryVoltageSensorData data; - - // Populate the data - data.voltageTimestamp = sample.timestamp; - data.channelId = 1; - data.voltage = sample.voltage[1]; - data.batVoltage = sample.voltage[1] * BATTERY_VOLTAGE_CONVERSION_FACTOR; - return data; -} + SensorManager::SensorMap_t map; + lps22dfInit(map); + lps28dfwInit(map); + h3lis331dlInit(map); + lis2mdlInit(map); + ubxgpsInit(map); + lsm6dsrxInit(map); + ads131m08Init(map); + internalAdcInit(map); + + manager = std::make_unique<SensorManager>(map, &scheduler); + if (!manager->start()) + { + return false; + } -BatteryVoltageSensorData Sensors::getCamBatteryVoltageLastSample() -{ - // Do not need to pause the kernel, the last sample getter is already - // protected - ADS131M08Data sample = getADS131M08LastSample(); - BatteryVoltageSensorData data; - - // Populate the data - data.voltageTimestamp = sample.timestamp; - data.channelId = 7; - data.voltage = sample.voltage[7]; - data.batVoltage = sample.voltage[7] * BATTERY_VOLTAGE_CONVERSION_FACTOR; - return data; + started = true; + return true; } -CurrentData Sensors::getCurrentLastSample() +std::vector<Boardcore::SensorInfo> Sensors::getSensorInfo() { - // Do not need to pause the kernel, the last sample getter is already - // protected - ADS131M08Data sample = getADS131M08LastSample(); - CurrentData data; - - // Populate the data - data.currentTimestamp = sample.timestamp; - data.current = - (sample.voltage[5] - CURRENT_OFFSET) * CURRENT_CONVERSION_FACTOR; - return data; + return {manager->getSensorInfo(lps22df.get()), + manager->getSensorInfo(lps28dfw.get()), + manager->getSensorInfo(h3lis331dl.get()), + manager->getSensorInfo(lis2mdl.get()), + manager->getSensorInfo(ubxgps.get()), + manager->getSensorInfo(lsm6dsrx.get()), + manager->getSensorInfo(ads131m08.get()), + manager->getSensorInfo(internalAdc.get())}; } -MPXH6400AData Sensors::getDeploymentPressureLastSample() +void Sensors::lps22dfInit(SensorManager::SensorMap_t &map) { - miosix::PauseKernelLock lock; - return mpxh6400a != nullptr ? mpxh6400a->getLastSample() : MPXH6400AData{}; -} + ModuleManager &modules = ModuleManager::getInstance(); -HSCMRNN015PAData Sensors::getStaticPressure1LastSample() -{ - miosix::PauseKernelLock lock; - return hscmrnn015pa_1 != nullptr ? hscmrnn015pa_1->getLastSample() - : HSCMRNN015PAData{}; -} + SPIBusConfig spiConfig = LPS22DF::getDefaultSPIConfig(); + spiConfig.clockDivider = SPI::ClockDivider::DIV_16; -HSCMRNN015PAData Sensors::getStaticPressure2LastSample() -{ - miosix::PauseKernelLock lock; - return hscmrnn015pa_2 != nullptr ? hscmrnn015pa_2->getLastSample() - : HSCMRNN015PAData{}; -} + LPS22DF::Config config; + config.avg = Config::Sensors::LPS22DF::AVG; + config.odr = Config::Sensors::LPS22DF::ODR; -RotatedIMUData Sensors::getIMULastSample() -{ - miosix::PauseKernelLock lock; - return imu != nullptr ? imu->getLastSample() : RotatedIMUData{}; + lps22df = std::make_unique<LPS22DF>(modules.get<Buses>()->getLPS22DF(), + sensors::LPS22DF::cs::getPin(), + spiConfig, config); + + SensorInfo info{"LPS22DF", Config::Sensors::LPS22DF::PERIOD, + [this]() { lps22dfCallback(); }}; + map.emplace(lps22df.get(), info); } -MagnetometerData Sensors::getCalibratedMagnetometerLastSample() +void Sensors::lps22dfCallback() { - // Do not need to pause the kernel, the last sample getter is already - // protected - MagnetometerData lastSample = getLIS2MDLLastSample(); - MagnetometerData result; - - // Correct the result and copy the timestamp - { - miosix::Lock<FastMutex> l(calibrationMutex); - result = - static_cast<MagnetometerData>(magCalibration.correct(lastSample)); - } + auto sample = lps22df->getLastSample(); - result.magneticFieldTimestamp = lastSample.magneticFieldTimestamp; - return result; + LOG_INFO(logger, "LPS22DF {}", sample.pressure); } -void Sensors::setPitot(PitotData data) -{ - miosix::PauseKernelLock lock; - canPitot = data; - canPitot.timestamp = TimestampTimer::getTimestamp(); -} -void Sensors::setCCPressure(PressureData data) +void Sensors::lps28dfwInit(SensorManager::SensorMap_t &map) { - miosix::PauseKernelLock lock; - canCCPressure = data; - canCCPressure.pressureTimestamp = TimestampTimer::getTimestamp(); -} -void Sensors::setBottomTankPressure(PressureData data) -{ - miosix::PauseKernelLock lock; - canBottomTankPressure = data; - canBottomTankPressure.pressureTimestamp = TimestampTimer::getTimestamp(); -} -void Sensors::setTopTankPressure(PressureData data) -{ - miosix::PauseKernelLock lock; - canTopTankPressure = data; - canTopTankPressure.pressureTimestamp = TimestampTimer::getTimestamp(); -} -void Sensors::setTankTemperature(TemperatureData data) -{ - miosix::PauseKernelLock lock; - canTankTemperature = data; - canTankTemperature.temperatureTimestamp = TimestampTimer::getTimestamp(); -} -void Sensors::setMotorBatteryVoltage(BatteryVoltageSensorData data) -{ - miosix::PauseKernelLock lock; - canMotorBatteryVoltage.batVoltage = data.batVoltage; - canMotorBatteryVoltage.voltageTimestamp = TimestampTimer::getTimestamp(); -} -void Sensors::setMotorCurrent(CurrentData data) -{ - miosix::PauseKernelLock lock; - canMotorCurrent.current = data.current; - canMotorCurrent.currentTimestamp = TimestampTimer::getTimestamp(); -} - -Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {} + ModuleManager &modules = ModuleManager::getInstance(); -bool Sensors::start() -{ - // Read the magnetometer calibration from predefined file - magCalibration.fromFile("/sd/magCalibration.csv"); - - // Init all the sensors - lps22dfInit(); - lps28dfw_1Init(); - // lps28dfw_2Init(); - h3lis331dlInit(); - lis2mdlInit(); - ubxgpsInit(); - lsm6dsrxInit(); - ads131m08Init(); - deploymentPressureInit(); - staticPressure1Init(); - staticPressure2Init(); - imuInit(); - - // Add the magnetometer calibration to the scheduler - size_t result = scheduler->addTask( - [&]() - { - // Gather the last sample data - MagnetometerData lastSample = getLIS2MDLLastSample(); - - // Feed the data to the calibrator inside a protected area. - // Contention is not high and the use of a mutex is suitable to - // avoid pausing the kernel for this calibration operation - { - miosix::Lock<FastMutex> l(calibrationMutex); - magCalibrator.feed(lastSample); - } - }, - MAG_CALIBRATION_PERIOD); - - // Create sensor manager with populated map and configured scheduler - manager = new SensorManager(sensorMap, scheduler); - return manager->start() && result != 0; -} + LPS28DFW::SensorConfig config; + config.sa0 = true; + config.fsr = Config::Sensors::LPS28DFW::FS; + config.avg = Config::Sensors::LPS28DFW::AVG; + config.odr = Config::Sensors::LPS28DFW::ODR; + config.drdy = false; -void Sensors::stop() { manager->stop(); } + lps28dfw = + std::make_unique<LPS28DFW>(modules.get<Buses>()->getLPS28DFW(), config); -bool Sensors::isStarted() -{ - return manager->areAllSensorsInitialized() && scheduler->isRunning(); + SensorInfo info{"LPS28DFW", Config::Sensors::LPS28DFW::PERIOD, + [this]() { lps28dfwCallback(); }}; + map.emplace(lps28dfw.get(), info); } -void Sensors::calibrate() +void Sensors::lps28dfwCallback() { - // Create the stats to calibrate the barometers - Stats lps28dfw1Stats; - // Stats lps28dfw2Stats; - Stats staticPressure1Stats; - Stats staticPressure2Stats; - Stats deploymentPressureStats; - - // Add N samples to the stats - for (unsigned int i = 0; i < SensorsConfig::CALIBRATION_SAMPLES; i++) - { - lps28dfw1Stats.add(getLPS28DFW_1LastSample().pressure); - // lps28dfw2Stats.add(getLPS28DFW_2LastSample().pressure); - staticPressure1Stats.add(getStaticPressure1LastSample().pressure); - staticPressure2Stats.add(getStaticPressure2LastSample().pressure); - deploymentPressureStats.add(getDeploymentPressureLastSample().pressure); - - // Delay for the expected period - miosix::Thread::sleep(SensorsConfig::CALIBRATION_PERIOD); - } + auto sample = lps28dfw->getLastSample(); - // Compute the difference between the mean value from LPS28DFW - float reference = lps28dfw1Stats.getStats().mean; - - hscmrnn015pa_1->updateOffset(staticPressure1Stats.getStats().mean - - reference); - hscmrnn015pa_2->updateOffset(staticPressure2Stats.getStats().mean - - reference); - mpxh6400a->updateOffset(deploymentPressureStats.getStats().mean - - reference); - - // Log the offsets - SensorsCalibrationParameter cal{}; - cal.timestamp = TimestampTimer::getTimestamp(); - cal.offsetStatic1 = staticPressure1Stats.getStats().mean - reference; - cal.offsetStatic2 = staticPressure2Stats.getStats().mean - reference; - cal.offsetDeployment = deploymentPressureStats.getStats().mean - reference; - cal.referencePressure = reference; - - Logger::getInstance().log(cal); + LOG_INFO(logger, "LPS28DFW {}", sample.pressure); } -bool Sensors::writeMagCalibration() +void Sensors::h3lis331dlInit(SensorManager::SensorMap_t &map) { - // Compute the calibration result in protected area - { - miosix::Lock<FastMutex> l(calibrationMutex); - SixParametersCorrector cal = magCalibrator.computeResult(); - - // Check result validity - if (!isnan(cal.getb()[0]) && !isnan(cal.getb()[1]) && - !isnan(cal.getb()[2]) && !isnan(cal.getA()[0]) && - !isnan(cal.getA()[1]) && !isnan(cal.getA()[2])) - { - magCalibration = cal; - - // Save the calibration to the calibration file - return magCalibration.toFile("/sd/magCalibration.csv"); - } - return false; - } -} + ModuleManager &modules = ModuleManager::getInstance(); -std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> -Sensors::getSensorInfo() -{ - std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> sensorState; - for (size_t i = 0; i < sensorsInit.size(); i++) - { - // Check wether the lambda is existent - if (sensorsInit[i]) - { - sensorState[i] = sensorsInit[i](); - } - } - return sensorState; -} - -void Sensors::lps22dfInit() -{ - ModuleManager& modules = ModuleManager::getInstance(); - - // Get the correct SPI configuration - SPIBusConfig config = LPS22DF::getDefaultSPIConfig(); - config.clockDivider = SPI::ClockDivider::DIV_16; - - // Configure the device - LPS22DF::Config sensorConfig; - sensorConfig.avg = LPS22DF_AVG; - sensorConfig.odr = LPS22DF_ODR; - - // Create sensor instance with configured parameters - lps22df = new LPS22DF(modules.get<Buses>()->spi3, - miosix::sensors::LPS22DF::cs::getPin(), config, - sensorConfig); - - // Emplace the sensor inside the map - SensorInfo info("LPS22DF", LPS22DF_PERIOD, - bind(&Sensors::lps22dfCallback, this)); - sensorMap.emplace(make_pair(lps22df, info)); - - // Add the sensor info getter to the array - sensorsInit[sensorsId++] = [&]() -> SensorInfo - { return manager->getSensorInfo(lps22df); }; -} -void Sensors::lps28dfw_1Init() -{ - ModuleManager& modules = ModuleManager::getInstance(); + SPIBusConfig spiConfig = H3LIS331DL::getDefaultSPIConfig(); + spiConfig.clockDivider = SPI::ClockDivider::DIV_16; - // Configure the sensor - LPS28DFW::SensorConfig config{false, LPS28DFW_FSR, LPS28DFW_AVG, - LPS28DFW_ODR, false}; + h3lis331dl = std::make_unique<H3LIS331DL>( + modules.get<Buses>()->getH3LIS331DL(), + sensors::H3LIS331DL::cs::getPin(), Config::Sensors::H3LIS331DL::ODR, + H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE, + Config::Sensors::H3LIS331DL::FS); - // Create sensor instance with configured parameters - lps28dfw_1 = new LPS28DFW(modules.get<Buses>()->i2c1, config); + SensorInfo info{"H3LIS331DL", Config::Sensors::H3LIS331DL::PERIOD, + [this]() { h3lis331dlCallback(); }}; + map.emplace(h3lis331dl.get(), info); +} - // Emplace the sensor inside the map - SensorInfo info("LPS28DFW_1", LPS28DFW_PERIOD, - bind(&Sensors::lps28dfw_1Callback, this)); - sensorMap.emplace(make_pair(lps28dfw_1, info)); +void Sensors::h3lis331dlCallback() {} - // Add the sensor info getter to the array - sensorsInit[sensorsId++] = [&]() -> SensorInfo - { return manager->getSensorInfo(lps28dfw_1); }; -} -void Sensors::lps28dfw_2Init() +void Sensors::lis2mdlInit(SensorManager::SensorMap_t &map) { - ModuleManager& modules = ModuleManager::getInstance(); + ModuleManager &modules = ModuleManager::getInstance(); - // Configure the sensor - LPS28DFW::SensorConfig config{true, LPS28DFW_FSR, LPS28DFW_AVG, - LPS28DFW_ODR, false}; + SPIBusConfig spiConfig = H3LIS331DL::getDefaultSPIConfig(); + spiConfig.clockDivider = SPI::ClockDivider::DIV_16; - // Create sensor instance with configured parameters - lps28dfw_2 = new LPS28DFW(modules.get<Buses>()->i2c1, config); + LIS2MDL::Config config; + config.deviceMode = LIS2MDL::MD_CONTINUOUS; + config.odr = Config::Sensors::LIS2MDL::ODR; + config.temperatureDivider = Config::Sensors::LIS2MDL::TEMP_DIVIDER; - // Emplace the sensor inside the map - SensorInfo info("LPS28DFW_2", LPS28DFW_PERIOD, - bind(&Sensors::lps28dfw_2Callback, this)); - sensorMap.emplace(make_pair(lps28dfw_2, info)); + lis2mdl = std::make_unique<LIS2MDL>(modules.get<Buses>()->getLIS2MDL(), + sensors::LIS2MDL::cs::getPin(), + spiConfig, config); - // Add the sensor info getter to the array - sensorsInit[sensorsId++] = [&]() -> SensorInfo - { return manager->getSensorInfo(lps28dfw_2); }; + SensorInfo info{"LIS2MDL", Config::Sensors::LIS2MDL::PERIOD, + [this]() { lis2mdlCallback(); }}; + map.emplace(lis2mdl.get(), info); } -void Sensors::h3lis331dlInit() -{ - ModuleManager& modules = ModuleManager::getInstance(); - // Get the correct SPI configuration - SPIBusConfig config = H3LIS331DL::getDefaultSPIConfig(); - config.clockDivider = SPI::ClockDivider::DIV_16; +void Sensors::lis2mdlCallback() {} - // Create sensor instance with configured parameters - h3lis331dl = new H3LIS331DL( - modules.get<Buses>()->spi3, miosix::sensors::H3LIS331DL::cs::getPin(), - config, H3LIS331DL_ODR, H3LIS331DL_BDU, H3LIS331DL_FSR); +void Sensors::ubxgpsInit(SensorManager::SensorMap_t &map) +{ + ModuleManager &modules = ModuleManager::getInstance(); + + SPIBusConfig spiConfig = UBXGPSSpi::getDefaultSPIConfig(); + spiConfig.clockDivider = SPI::ClockDivider::DIV_64; - // Emplace the sensor inside the map - SensorInfo info("H3LIS331DL", H3LIS331DL_PERIOD, - bind(&Sensors::h3lis331dlCallback, this)); - sensorMap.emplace(make_pair(h3lis331dl, info)); + ubxgps = std::make_unique<UBXGPSSpi>(modules.get<Buses>()->getUBXGps(), + sensors::UBXGps::cs::getPin(), + spiConfig, 5); - // Add the sensor info getter to the array - sensorsInit[sensorsId++] = [&]() -> SensorInfo - { return manager->getSensorInfo(h3lis331dl); }; + SensorInfo info{"UBXGPS", Config::Sensors::UBXGPS::PERIOD, + [this]() { ubxgpsCallback(); }}; + map.emplace(ubxgps.get(), info); } -void Sensors::lis2mdlInit() + +void Sensors::ubxgpsCallback() { - ModuleManager& modules = ModuleManager::getInstance(); - - // Get the correct SPI configuration - SPIBusConfig config = LIS2MDL::getDefaultSPIConfig(); - config.clockDivider = SPI::ClockDivider::DIV_16; - - // Configure the sensor - LIS2MDL::Config sensorConfig; - sensorConfig.deviceMode = LIS2MDL_OPERATIVE_MODE; - sensorConfig.odr = LIS2MDL_ODR; - sensorConfig.temperatureDivider = LIS2MDL_TEMPERATURE_DIVIDER; - - // Create sensor instance with configured parameters - lis2mdl = new LIS2MDL(modules.get<Buses>()->spi3, - miosix::sensors::LIS2MDL::cs::getPin(), config, - sensorConfig); - - // Emplace the sensor inside the map - SensorInfo info("LIS2MDL", LIS2MDL_PERIOD, - bind(&Sensors::lis2mdlCallback, this)); - sensorMap.emplace(make_pair(lis2mdl, info)); - - // Add the sensor info getter to the array - sensorsInit[sensorsId++] = [&]() -> SensorInfo - { return manager->getSensorInfo(lis2mdl); }; + auto sample = ubxgps->getLastSample(); + + LOG_INFO(logger, "UBXGPS {} {} {} {}", sample.fix, sample.satellites, + sample.latitude, sample.longitude); } -void Sensors::ubxgpsInit() +void Sensors::lsm6dsrxInit(SensorManager::SensorMap_t &map) { - ModuleManager& modules = ModuleManager::getInstance(); + ModuleManager &modules = ModuleManager::getInstance(); - // Get the correct SPI configuration - SPIBusConfig config = UBXGPSSpi::getDefaultSPIConfig(); - config.clockDivider = SPI::ClockDivider::DIV_64; + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_32; + spiConfig.mode = SPI::Mode::MODE_0; - // Create sensor instance with configured parameters - ubxgps = new UBXGPSSpi(modules.get<Buses>()->spi4, - miosix::sensors::GPS::cs::getPin(), config, 5); + LSM6DSRXConfig config; + config.bdu = LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE; - // Emplace the sensor inside the map - SensorInfo info("UBXGPS", UBXGPS_PERIOD, - bind(&Sensors::ubxgpsCallback, this)); - sensorMap.emplace(make_pair(ubxgps, info)); + config.fsAcc = Config::Sensors::LSM6DSRX::ACC_FS; + config.odrAcc = Config::Sensors::LSM6DSRX::ACC_ODR; + config.opModeAcc = Config::Sensors::LSM6DSRX::ACC_OP_MODE; - // Add the sensor info getter to the array - sensorsInit[sensorsId++] = [&]() -> SensorInfo - { return manager->getSensorInfo(ubxgps); }; -} + config.fsGyr = Config::Sensors::LSM6DSRX::GYR_FS; + config.odrGyr = Config::Sensors::LSM6DSRX::GYR_ODR; + config.opModeGyr = Config::Sensors::LSM6DSRX::GYR_OP_MODE; -void Sensors::lsm6dsrxInit() -{ - ModuleManager& modules = ModuleManager::getInstance(); - - // Configure the SPI - SPIBusConfig config; - config.clockDivider = SPI::ClockDivider::DIV_32; - config.mode = SPI::Mode::MODE_0; - - // Configure the sensor - LSM6DSRXConfig sensorConfig; - sensorConfig.bdu = LSM6DSRX_BDU; - - // Accelerometer - sensorConfig.fsAcc = LSM6DSRX_ACC_FS; - sensorConfig.odrAcc = LSM6DSRX_ACC_ODR; - sensorConfig.opModeAcc = LSM6DSRX_OPERATING_MODE; - - // Gyroscope - sensorConfig.fsGyr = LSM6DSRX_GYR_FS; - sensorConfig.odrGyr = LSM6DSRX_GYR_ODR; - sensorConfig.opModeGyr = LSM6DSRX_OPERATING_MODE; - - // Fifo - sensorConfig.fifoMode = LSM6DSRX_FIFO_MODE; - sensorConfig.fifoTimestampDecimation = LSM6DSRX_FIFO_TIMESTAMP_DECIMATION; - sensorConfig.fifoTemperatureBdr = LSM6DSRX_FIFO_TEMPERATURE_BDR; - - // Create sensor instance with configured parameters - lsm6dsrx = new LSM6DSRX(modules.get<Buses>()->spi1, - miosix::sensors::LSM6DSRX::cs::getPin(), config, - sensorConfig); - - // Emplace the sensor inside the map - SensorInfo info("LSM6DSRX", LSM6DSRX_PERIOD, - bind(&Sensors::lsm6dsrxCallback, this)); - sensorMap.emplace(make_pair(lsm6dsrx, info)); - - // Add the sensor info getter to the array - sensorsInit[sensorsId++] = [&]() -> SensorInfo - { return manager->getSensorInfo(lsm6dsrx); }; -} + config.fifoMode = LSM6DSRXConfig::FIFO_MODE::CONTINUOUS; + config.fifoTimestampDecimation = + LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1; + config.fifoTemperatureBdr = LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED; -void Sensors::ads131m08Init() -{ - ModuleManager& modules = ModuleManager::getInstance(); - - // Configure the SPI - SPIBusConfig config; - config.clockDivider = SPI::ClockDivider::DIV_32; - - // Configure the device - ADS131M08::Config sensorConfig; - sensorConfig.oversamplingRatio = ADS131M08_OVERSAMPLING_RATIO; - sensorConfig.globalChopModeEnabled = ADS131M08_GLOBAL_CHOP_MODE; - - // Create the sensor instance with configured parameters - ads131m08 = new ADS131M08(modules.get<Buses>()->spi4, - miosix::sensors::ADS131::cs::getPin(), config, - sensorConfig); - - // Emplace the sensor inside the map - SensorInfo info("ADS131M08", ADS131M08_PERIOD, - bind(&Sensors::ads131m08Callback, this)); - sensorMap.emplace(make_pair(ads131m08, info)); - - // Add the sensor info getter to the array - sensorsInit[sensorsId++] = [&]() -> SensorInfo - { return manager->getSensorInfo(ads131m08); }; -} + lsm6dsrx = std::make_unique<LSM6DSRX>(modules.get<Buses>()->getLSM6DSRX(), + sensors::LSM6DSRX::cs::getPin(), + spiConfig, config); -void Sensors::deploymentPressureInit() -{ - // Create the lambda function to get the voltage - function<ADCData()> getVoltage = [&]() - { - // No need to synchronize, the sampling thread is the same - ADS131M08Data sample = ads131m08->getLastSample(); - return sample.getVoltage(ADC_CH_DPL); - }; - - // Create the sensor instance with created function - mpxh6400a = new MPXH6400A(getVoltage, ADC_VOLTAGE_RANGE); - - // Emplace the sensor inside the map - SensorInfo info("MPXH6400A", ADS131M08_PERIOD, - bind(&Sensors::deploymentPressureCallback, this)); - sensorMap.emplace(make_pair(mpxh6400a, info)); + SensorInfo info{"LSM6DSRX", Config::Sensors::LSM6DSRX::PERIOD, + [this]() { lsm6dsrxCallback(); }}; + map.emplace(lsm6dsrx.get(), info); } -void Sensors::staticPressure1Init() -{ - // Create the lambda function to get the voltage - function<ADCData()> getVoltage = [&]() - { - // No need to synchronize, the sampling thread is the same - ADS131M08Data sample = ads131m08->getLastSample(); - return sample.getVoltage(ADC_CH_STATIC_1); - }; - - // Create the sensor instance with created function - hscmrnn015pa_1 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE); - - // Emplace the sensor inside the map - SensorInfo info("HSCMRNN015PA_1", ADS131M08_PERIOD, - bind(&Sensors::staticPressure1Callback, this)); - sensorMap.emplace(make_pair(hscmrnn015pa_1, info)); -} +void Sensors::lsm6dsrxCallback() {} -void Sensors::staticPressure2Init() +void Sensors::ads131m08Init(SensorManager::SensorMap_t &map) { - // Create the lambda function to get the voltage - function<ADCData()> getVoltage = [&]() - { - // No need to synchronize, the sampling thread is the same - ADS131M08Data sample = ads131m08->getLastSample(); - return sample.getVoltage(ADC_CH_STATIC_2); - }; - - // Create the sensor instance with created function - hscmrnn015pa_2 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE); - - // Emplace the sensor inside the map - SensorInfo info("HSCMRNN015PA_2", ADS131M08_PERIOD, - bind(&Sensors::staticPressure2Callback, this)); - sensorMap.emplace(make_pair(hscmrnn015pa_2, info)); -} + ModuleManager &modules = ModuleManager::getInstance(); -void Sensors::imuInit() -{ - // Register the IMU as the fake sensor, passing as parameters the methods to - // retrieve real data. The sensor is not synchronized, but the sampling - // thread is always the same. - imu = new RotatedIMU( - bind(&LSM6DSRX::getLastSample, lsm6dsrx), - bind(&Sensors::getCalibratedMagnetometerLastSample, this), - bind(&LSM6DSRX::getLastSample, lsm6dsrx)); - - // Invert the Y axis on the magnetometer - Eigen::Matrix3f m{{1, 0, 0}, {0, -1, 0}, {0, 0, 1}}; - imu->addMagTransformation(m); - imu->addAccTransformation(imu->rotateAroundX(45)); - imu->addGyroTransformation(imu->rotateAroundX(45)); - imu->addMagTransformation(imu->rotateAroundX(45)); - - // Emplace the sensor inside the map - SensorInfo info("RotatedIMU", IMU_PERIOD, - bind(&Sensors::imuCallback, this)); - sensorMap.emplace(make_pair(imu, info)); -} + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_32; -void Sensors::lps22dfCallback() -{ - LPS22DFData lastSample = lps22df->getLastSample(); - Logger::getInstance().log(lastSample); -} -void Sensors::lps28dfw_1Callback() -{ - LPS28DFW_1Data lastSample = - static_cast<LPS28DFW_1Data>(lps28dfw_1->getLastSample()); - Logger::getInstance().log(lastSample); -} -void Sensors::lps28dfw_2Callback() -{ - LPS28DFW_2Data lastSample = - static_cast<LPS28DFW_2Data>(lps28dfw_2->getLastSample()); - Logger::getInstance().log(lastSample); -} -void Sensors::h3lis331dlCallback() -{ - H3LIS331DLData lastSample = h3lis331dl->getLastSample(); - Logger::getInstance().log(lastSample); -} -void Sensors::lis2mdlCallback() -{ - LIS2MDLData lastSample = lis2mdl->getLastSample(); - Logger::getInstance().log(lastSample); -} -void Sensors::ubxgpsCallback() -{ - UBXGPSData lastSample = ubxgps->getLastSample(); - Logger::getInstance().log(lastSample); -} -void Sensors::lsm6dsrxCallback() -{ - auto& fifo = lsm6dsrx->getLastFifo(); - uint16_t fifoSize = lsm6dsrx->getLastFifoSize(); + ADS131M08::Config config; + config.oversamplingRatio = Config::Sensors::ADS131M08::OSR; + config.globalChopModeEnabled = + Config::Sensors::ADS131M08::GLOBAL_CHOP_MODE_EN; - // For every instance inside the fifo log the sample - for (uint16_t i = 0; i < fifoSize; i++) - { - Logger::getInstance().log(fifo.at(i)); - } -} -void Sensors::ads131m08Callback() -{ - ADS131M08Data lastSample = ads131m08->getLastSample(); - Logger::getInstance().log(lastSample); -} -void Sensors::deploymentPressureCallback() -{ - MPXH6400AData lastSample = mpxh6400a->getLastSample(); - Logger::getInstance().log(lastSample); -} -void Sensors::staticPressure1Callback() -{ - HSCMRNN015PA_1Data lastSample = - static_cast<HSCMRNN015PA_1Data>(hscmrnn015pa_1->getLastSample()); - Logger::getInstance().log(lastSample); -} -void Sensors::staticPressure2Callback() -{ - HSCMRNN015PA_2Data lastSample = - static_cast<HSCMRNN015PA_2Data>(hscmrnn015pa_2->getLastSample()); - Logger::getInstance().log(lastSample); + ads131m08 = std::make_unique<ADS131M08>( + modules.get<Buses>()->getADS131M08(), sensors::ADS131M08::cs::getPin(), + spiConfig, config); + + SensorInfo info{"ADS131M08", Config::Sensors::ADS131M08::PERIOD, + [this]() { ads131m08Callback(); }}; + map.emplace(ads131m08.get(), info); } -void Sensors::imuCallback() + +void Sensors::ads131m08Callback() {} + +void Sensors::internalAdcInit(Boardcore::SensorManager::SensorMap_t &map) { - RotatedIMUData lastSample = imu->getLastSample(); - Logger::getInstance().log(lastSample); + ModuleManager &modules = ModuleManager::getInstance(); + + internalAdc = std::make_unique<InternalADC>(ADC2); + internalAdc->enableChannel(InternalADC::CH8); + internalAdc->enableChannel(InternalADC::CH9); + internalAdc->enableChannel(InternalADC::CH11); + internalAdc->enableTemperature(); + internalAdc->enableVbat(); + + SensorInfo info{"InternalADC", Config::Sensors::InternalADC::PERIOD, + [this]() { internalAdcCallback(); }}; + map.emplace(internalAdc.get(), info); } -} // namespace Main \ No newline at end of file +void Sensors::internalAdcCallback() {} diff --git a/src/boards/Main/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h index 44ad52749f84cc0a750388f1581969611e76cdf2..ad6719ac9eacbd25901b7d4e7ad1a8608795ce5b 100644 --- a/src/boards/Main/Sensors/Sensors.h +++ b/src/boards/Main/Sensors/Sensors.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Mor * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -19,184 +19,82 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #pragma once -#include <Main/Configs/SensorsConfig.h> -#include <Main/Sensors/RotatedIMU/RotatedIMU.h> -#include <Main/Sensors/SensorsData.h> +#include <diagnostic/PrintLogger.h> +#include <scheduler/TaskScheduler.h> #include <sensors/ADS131M08/ADS131M08.h> #include <sensors/H3LIS331DL/H3LIS331DL.h> #include <sensors/LIS2MDL/LIS2MDL.h> #include <sensors/LPS22DF/LPS22DF.h> #include <sensors/LPS28DFW/LPS28DFW.h> #include <sensors/LSM6DSRX/LSM6DSRX.h> -#include <sensors/SensorData.h> #include <sensors/SensorManager.h> #include <sensors/UBXGPS/UBXGPSSpi.h> -#include <sensors/analog/BatteryVoltageSensorData.h> -#include <sensors/analog/Pitot/PitotData.h> -#include <sensors/analog/pressure/honeywell/HSCMRNN015PA.h> -#include <sensors/analog/pressure/nxp/MPXH6400A.h> -#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> -#include <stdint.h> +#include <drivers/adc/InternalADC.h> +#include <memory> #include <utils/ModuleManager/ModuleManager.hpp> +#include <vector> namespace Main { + class Sensors : public Boardcore::Module { public: - explicit Sensors(Boardcore::TaskScheduler* sched); - - [[nodiscard]] bool start(); + explicit Sensors(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler} + { + } - /** - * @brief Stops the sensor manager - * @warning Stops the passed scheduler - */ - void stop(); - - /** - * @brief Returns if all the sensors are started successfully - */ bool isStarted(); - /** - * @brief Calibrates the sensors with an offset - */ - void calibrate(); - - /** - * @brief Takes the result of the live magnetometer calibration and applies - * it to the current calibration + writes it in the csv file - */ - bool writeMagCalibration(); - - // Sensor getters - Boardcore::LPS22DFData getLPS22DFLastSample(); - Boardcore::LPS28DFWData getLPS28DFW_1LastSample(); - Boardcore::LPS28DFWData getLPS28DFW_2LastSample(); - Boardcore::H3LIS331DLData getH3LIS331DLLastSample(); - Boardcore::LIS2MDLData getLIS2MDLLastSample(); - Boardcore::UBXGPSData getGPSLastSample(); - Boardcore::LSM6DSRXData getLSM6DSRXLastSample(); - Boardcore::ADS131M08Data getADS131M08LastSample(); - - // Processed getters - Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample(); - Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample(); - Boardcore::CurrentData getCurrentLastSample(); - Boardcore::MPXH6400AData getDeploymentPressureLastSample(); - Boardcore::HSCMRNN015PAData getStaticPressure1LastSample(); - Boardcore::HSCMRNN015PAData getStaticPressure2LastSample(); - RotatedIMUData getIMULastSample(); - Boardcore::MagnetometerData getCalibratedMagnetometerLastSample(); - - // CAN fake sensors setters - void setPitot(Boardcore::PitotData data); - void setCCPressure(Boardcore::PressureData data); - void setBottomTankPressure(Boardcore::PressureData data); - void setTopTankPressure(Boardcore::PressureData data); - void setTankTemperature(Boardcore::TemperatureData data); - void setMotorBatteryVoltage(Boardcore::BatteryVoltageSensorData data); - void setMotorCurrent(Boardcore::CurrentData data); - - // CAN fake sensors getters - Boardcore::PitotData getPitotLastSample(); - Boardcore::PressureData getCCPressureLastSample(); - Boardcore::PressureData getBottomTankPressureLastSample(); - Boardcore::PressureData getTopTankPressureLastSample(); - Boardcore::TemperatureData getTankTemperatureLastSample(); - Boardcore::BatteryVoltageSensorData getMotorBatteryVoltage(); - Boardcore::CurrentData getMotorCurrent(); - - // Returns the sensors statuses - std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> - getSensorInfo(); + [[nodiscard]] bool start(); + + std::vector<Boardcore::SensorInfo> getSensorInfo(); private: - // Init and callbacks methods - void lps22dfInit(); + void lps22dfInit(Boardcore::SensorManager::SensorMap_t &map); void lps22dfCallback(); - void lps28dfw_1Init(); - void lps28dfw_1Callback(); + void lps28dfwInit(Boardcore::SensorManager::SensorMap_t &map); + void lps28dfwCallback(); - void lps28dfw_2Init(); - void lps28dfw_2Callback(); - - void h3lis331dlInit(); + void h3lis331dlInit(Boardcore::SensorManager::SensorMap_t &map); void h3lis331dlCallback(); - void lis2mdlInit(); + void lis2mdlInit(Boardcore::SensorManager::SensorMap_t &map); void lis2mdlCallback(); - void ubxgpsInit(); + void ubxgpsInit(Boardcore::SensorManager::SensorMap_t &map); void ubxgpsCallback(); - void lsm6dsrxInit(); + void lsm6dsrxInit(Boardcore::SensorManager::SensorMap_t &map); void lsm6dsrxCallback(); - void ads131m08Init(); + void ads131m08Init(Boardcore::SensorManager::SensorMap_t &map); void ads131m08Callback(); - void deploymentPressureInit(); - void deploymentPressureCallback(); - - void staticPressure1Init(); - void staticPressure1Callback(); - - void staticPressure2Init(); - void staticPressure2Callback(); - - void imuInit(); - void imuCallback(); - - // Sensors instances - Boardcore::LPS22DF* lps22df = nullptr; - Boardcore::LPS28DFW* lps28dfw_1 = nullptr; - Boardcore::LPS28DFW* lps28dfw_2 = nullptr; - Boardcore::H3LIS331DL* h3lis331dl = nullptr; - Boardcore::LIS2MDL* lis2mdl = nullptr; - Boardcore::UBXGPSSpi* ubxgps = nullptr; - Boardcore::LSM6DSRX* lsm6dsrx = nullptr; - Boardcore::ADS131M08* ads131m08 = nullptr; - - // Can sensors - Boardcore::PitotData canPitot{0, 0, 0}; - Boardcore::PressureData canCCPressure{0, 0}; - Boardcore::PressureData canBottomTankPressure{0, 0}; - Boardcore::PressureData canTopTankPressure{0, 0}; - Boardcore::TemperatureData canTankTemperature{0, 0}; - Boardcore::BatteryVoltageSensorData canMotorBatteryVoltage{}; - Boardcore::CurrentData canMotorCurrent{}; - - // Fake processed sensors - RotatedIMU* imu = nullptr; - Boardcore::MPXH6400A* mpxh6400a = nullptr; - Boardcore::HSCMRNN015PA* hscmrnn015pa_1 = nullptr; - Boardcore::HSCMRNN015PA* hscmrnn015pa_2 = nullptr; - - // Magnetometer live calibration - Boardcore::SoftAndHardIronCalibration magCalibrator; - Boardcore::SixParametersCorrector magCalibration; - miosix::FastMutex calibrationMutex; - - // Sensor manager - Boardcore::SensorManager* manager = nullptr; - Boardcore::SensorManager::SensorMap_t sensorMap; - Boardcore::TaskScheduler* scheduler = nullptr; - - // Collection of lambdas to get the sensor init statuses - std::array<std::function<Boardcore::SensorInfo()>, - SensorsConfig::NUMBER_OF_SENSORS> - sensorsInit; - uint8_t sensorsId = 0; - - // SD logger - Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance(); + void internalAdcInit(Boardcore::SensorManager::SensorMap_t &map); + void internalAdcCallback(); Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors"); + + std::atomic<bool> started{false}; + + std::unique_ptr<Boardcore::LPS22DF> lps22df; + std::unique_ptr<Boardcore::LPS28DFW> lps28dfw; + std::unique_ptr<Boardcore::H3LIS331DL> h3lis331dl; + std::unique_ptr<Boardcore::LIS2MDL> lis2mdl; + std::unique_ptr<Boardcore::UBXGPSSpi> ubxgps; + std::unique_ptr<Boardcore::LSM6DSRX> lsm6dsrx; + std::unique_ptr<Boardcore::ADS131M08> ads131m08; + std::unique_ptr<Boardcore::InternalADC> internalAdc; + + std::unique_ptr<Boardcore::SensorManager> manager; + + Boardcore::TaskScheduler &scheduler; }; + } // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Actuators/Actuators.cpp b/src/boards/Main_old/Actuators/Actuators.cpp similarity index 100% rename from src/boards/Main/Actuators/Actuators.cpp rename to src/boards/Main_old/Actuators/Actuators.cpp diff --git a/src/boards/Main/Actuators/Actuators.h b/src/boards/Main_old/Actuators/Actuators.h similarity index 100% rename from src/boards/Main/Actuators/Actuators.h rename to src/boards/Main_old/Actuators/Actuators.h diff --git a/src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp b/src/boards/Main_old/AltitudeTrigger/AltitudeTrigger.cpp similarity index 100% rename from src/boards/Main/AltitudeTrigger/AltitudeTrigger.cpp rename to src/boards/Main_old/AltitudeTrigger/AltitudeTrigger.cpp diff --git a/src/boards/Main/AltitudeTrigger/AltitudeTrigger.h b/src/boards/Main_old/AltitudeTrigger/AltitudeTrigger.h similarity index 100% rename from src/boards/Main/AltitudeTrigger/AltitudeTrigger.h rename to src/boards/Main_old/AltitudeTrigger/AltitudeTrigger.h diff --git a/src/boards/Main/BoardScheduler.cpp b/src/boards/Main_old/BoardScheduler.cpp similarity index 100% rename from src/boards/Main/BoardScheduler.cpp rename to src/boards/Main_old/BoardScheduler.cpp diff --git a/src/boards/Main/BoardScheduler.h b/src/boards/Main_old/BoardScheduler.h similarity index 100% rename from src/boards/Main/BoardScheduler.h rename to src/boards/Main_old/BoardScheduler.h diff --git a/src/boards/Main_old/Buses.h b/src/boards/Main_old/Buses.h new file mode 100644 index 0000000000000000000000000000000000000000..13552ec6be401c1d3be9ebd4456e4da4e94d3b9a --- /dev/null +++ b/src/boards/Main_old/Buses.h @@ -0,0 +1,55 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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/i2c/I2C.h> +#include <drivers/spi/SPIBus.h> +#include <drivers/usart/USART.h> +#include <interfaces-impl/hwmapping.h> + +#include <utils/ModuleManager/ModuleManager.hpp> +namespace Main +{ +class Buses : public Boardcore::Module +{ +public: + Boardcore::SPIBus spi1; + Boardcore::SPIBus spi3; + Boardcore::SPIBus spi4; + Boardcore::SPIBus spi6; + + Boardcore::I2C i2c1; + + Boardcore::USART usart1; + Boardcore::USART usart2; + Boardcore::USART uart4; + + Buses() + : spi1(SPI1), spi3(SPI3), spi4(SPI4), spi6(SPI6), + i2c1(I2C1, miosix::interfaces::i2c1::scl::getPin(), + miosix::interfaces::i2c1::sda::getPin()), + usart1(USART1, 115200), usart2(USART2, 115200), uart4(UART4, 115200) + { + } +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/CanHandler/CanHandler.cpp b/src/boards/Main_old/CanHandler/CanHandler.cpp similarity index 100% rename from src/boards/Main/CanHandler/CanHandler.cpp rename to src/boards/Main_old/CanHandler/CanHandler.cpp diff --git a/src/boards/Main/CanHandler/CanHandler.h b/src/boards/Main_old/CanHandler/CanHandler.h similarity index 100% rename from src/boards/Main/CanHandler/CanHandler.h rename to src/boards/Main_old/CanHandler/CanHandler.h diff --git a/src/boards/Main/CanHandler/CanHandlerData.h b/src/boards/Main_old/CanHandler/CanHandlerData.h similarity index 100% rename from src/boards/Main/CanHandler/CanHandlerData.h rename to src/boards/Main_old/CanHandler/CanHandlerData.h diff --git a/src/boards/Main/Configs/ABKConfig.h b/src/boards/Main_old/Configs/ABKConfig.h similarity index 100% rename from src/boards/Main/Configs/ABKConfig.h rename to src/boards/Main_old/Configs/ABKConfig.h diff --git a/src/boards/Main/Configs/ADAConfig.h b/src/boards/Main_old/Configs/ADAConfig.h similarity index 100% rename from src/boards/Main/Configs/ADAConfig.h rename to src/boards/Main_old/Configs/ADAConfig.h diff --git a/src/boards/Main/Configs/ActuatorsConfig.h b/src/boards/Main_old/Configs/ActuatorsConfig.h similarity index 100% rename from src/boards/Main/Configs/ActuatorsConfig.h rename to src/boards/Main_old/Configs/ActuatorsConfig.h diff --git a/src/boards/Main/Configs/AltitudeTriggerConfig.h b/src/boards/Main_old/Configs/AltitudeTriggerConfig.h similarity index 100% rename from src/boards/Main/Configs/AltitudeTriggerConfig.h rename to src/boards/Main_old/Configs/AltitudeTriggerConfig.h diff --git a/src/boards/Main/Configs/CanHandlerConfig.h b/src/boards/Main_old/Configs/CanHandlerConfig.h similarity index 100% rename from src/boards/Main/Configs/CanHandlerConfig.h rename to src/boards/Main_old/Configs/CanHandlerConfig.h diff --git a/src/boards/Main/Configs/DeploymentConfig.h b/src/boards/Main_old/Configs/DeploymentConfig.h similarity index 100% rename from src/boards/Main/Configs/DeploymentConfig.h rename to src/boards/Main_old/Configs/DeploymentConfig.h diff --git a/src/boards/Main/Configs/FlightModeManagerConfig.h b/src/boards/Main_old/Configs/FlightModeManagerConfig.h similarity index 100% rename from src/boards/Main/Configs/FlightModeManagerConfig.h rename to src/boards/Main_old/Configs/FlightModeManagerConfig.h diff --git a/src/boards/Main/Configs/FlightStatsRecorderConfig.h b/src/boards/Main_old/Configs/FlightStatsRecorderConfig.h similarity index 100% rename from src/boards/Main/Configs/FlightStatsRecorderConfig.h rename to src/boards/Main_old/Configs/FlightStatsRecorderConfig.h diff --git a/src/boards/Main/Configs/MEAConfig.h b/src/boards/Main_old/Configs/MEAConfig.h similarity index 100% rename from src/boards/Main/Configs/MEAConfig.h rename to src/boards/Main_old/Configs/MEAConfig.h diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main_old/Configs/NASConfig.h similarity index 100% rename from src/boards/Main/Configs/NASConfig.h rename to src/boards/Main_old/Configs/NASConfig.h diff --git a/src/boards/Main/Configs/PinHandlerConfig.h b/src/boards/Main_old/Configs/PinHandlerConfig.h similarity index 100% rename from src/boards/Main/Configs/PinHandlerConfig.h rename to src/boards/Main_old/Configs/PinHandlerConfig.h diff --git a/src/boards/Main_old/Configs/RadioConfig.h b/src/boards/Main_old/Configs/RadioConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..42354f01172a5f62b245cef278681457b5e1681f --- /dev/null +++ b/src/boards/Main_old/Configs/RadioConfig.h @@ -0,0 +1,46 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <common/Mavlink.h> + +namespace Main +{ +namespace RadioConfig +{ +// Mavlink driver template parameters +constexpr uint32_t RADIO_PKT_LENGTH = 255; +constexpr uint32_t RADIO_OUT_QUEUE_SIZE = 20; +constexpr uint32_t RADIO_MAV_MSG_LENGTH = MAVLINK_MAX_DIALECT_PAYLOAD_SIZE; + +constexpr uint32_t RADIO_PERIODIC_TELEMETRY_PERIOD = 250; + +constexpr uint16_t RADIO_SLEEP_AFTER_SEND = 50; +constexpr size_t RADIO_OUT_BUFFER_MAX_AGE = 10; + +constexpr uint8_t MAV_SYSTEM_ID = 171; +constexpr uint8_t MAV_COMP_ID = 96; + +constexpr uint32_t MAVLINK_QUEUE_SIZE = 10; + +} // namespace RadioConfig +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main_old/Configs/SensorsConfig.h b/src/boards/Main_old/Configs/SensorsConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..0873209db35bdcdb5aa8b11ddad26dcceb0ba4c4 --- /dev/null +++ b/src/boards/Main_old/Configs/SensorsConfig.h @@ -0,0 +1,121 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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/ADS131M08/ADS131M08.h> +#include <sensors/H3LIS331DL/H3LIS331DL.h> +#include <sensors/LIS2MDL/LIS2MDL.h> +#include <sensors/LPS22DF/LPS22DF.h> +#include <sensors/LPS28DFW/LPS28DFW.h> +#include <sensors/LSM6DSRX/LSM6DSRX.h> +#include <sensors/UBXGPS/UBXGPSSpi.h> + +namespace Main +{ +namespace SensorsConfig +{ +constexpr Boardcore::LPS22DF::AVG LPS22DF_AVG = Boardcore::LPS22DF::AVG_4; +constexpr Boardcore::LPS22DF::ODR LPS22DF_ODR = Boardcore::LPS22DF::ODR_100; + +constexpr Boardcore::LPS28DFW::AVG LPS28DFW_AVG = Boardcore::LPS28DFW::AVG_4; +constexpr Boardcore::LPS28DFW::ODR LPS28DFW_ODR = Boardcore::LPS28DFW::ODR_100; +constexpr Boardcore::LPS28DFW::FullScaleRange LPS28DFW_FSR = + Boardcore::LPS28DFW::FS_1260; + +constexpr Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS331DL_ODR = + Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400; +constexpr Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS331DL_BDU = + Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE; +constexpr Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS331DL_FSR = + Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100; + +constexpr Boardcore::LIS2MDL::OperativeMode LIS2MDL_OPERATIVE_MODE = + Boardcore::LIS2MDL::MD_CONTINUOUS; +constexpr Boardcore::LIS2MDL::ODR LIS2MDL_ODR = Boardcore::LIS2MDL::ODR_100_HZ; +constexpr unsigned int LIS2MDL_TEMPERATURE_DIVIDER = 5; + +constexpr uint8_t UBXGPS_SAMPLE_RATE = 5; + +constexpr Boardcore::LSM6DSRXConfig::BDU LSM6DSRX_BDU = + Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE; + +constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE LSM6DSRX_ACC_FS = + Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16; +constexpr Boardcore::LSM6DSRXConfig::ACC_ODR LSM6DSRX_ACC_ODR = + Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_416; +constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE LSM6DSRX_OPERATING_MODE = + Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL; + +constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE LSM6DSRX_GYR_FS = + Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000; +constexpr Boardcore::LSM6DSRXConfig::GYR_ODR LSM6DSRX_GYR_ODR = + Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_416; + +constexpr Boardcore::LSM6DSRXConfig::FIFO_MODE LSM6DSRX_FIFO_MODE = + Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS; +constexpr Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION + LSM6DSRX_FIFO_TIMESTAMP_DECIMATION = + Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1; +constexpr Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR + LSM6DSRX_FIFO_TEMPERATURE_BDR = + Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED; + +constexpr Boardcore::ADS131M08Defs::OversamplingRatio + ADS131M08_OVERSAMPLING_RATIO = + Boardcore::ADS131M08Defs::OversamplingRatio::OSR_8192; +constexpr bool ADS131M08_GLOBAL_CHOP_MODE = true; + +constexpr uint32_t LPS22DF_PERIOD = 20; // [ms] 50Hz +constexpr uint32_t LPS28DFW_PERIOD = 20; // [ms] 50Hz +constexpr uint32_t H3LIS331DL_PERIOD = 10; // [ms] 100Hz +constexpr uint32_t LIS2MDL_PERIOD = 10; // [ms] 100Hz +constexpr uint32_t UBXGPS_PERIOD = 1000 / UBXGPS_SAMPLE_RATE; // [ms] +constexpr uint32_t LSM6DSRX_PERIOD = 20; // [ms] 50Hz +constexpr uint32_t ADS131M08_PERIOD = 10; // [ms] 100Hz +constexpr uint32_t IMU_PERIOD = 20; // [ms] Fake processed IMU 50Hz +constexpr uint32_t MAG_CALIBRATION_PERIOD = 20; // [ms] 50Hz + +// ADC sensors configs +constexpr float ADC_VOLTAGE_RANGE = 1.2f; // [V] Voltage reading range +constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_DPL = + Boardcore::ADS131M08Defs::Channel::CHANNEL_3; +constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_1 = + Boardcore::ADS131M08Defs::Channel::CHANNEL_4; +constexpr Boardcore::ADS131M08Defs::Channel ADC_CH_STATIC_2 = + Boardcore::ADS131M08Defs::Channel::CHANNEL_2; + +// ADC Voltage divider +constexpr float BATTERY_VOLTAGE_CONVERSION_FACTOR = + (20.f / 2.4f) + + 1; // 20 kOhm resistor and 2.4 kOhm resistor for voltage divider +constexpr float CURRENT_CONVERSION_FACTOR = + (20.f / 4.f) / (12.f / (12.f + 33.f)); +constexpr float CURRENT_OFFSET = 0.133333333f; // V in ADC + +// Calibration samples +constexpr unsigned int CALIBRATION_SAMPLES = 20; +constexpr unsigned int CALIBRATION_PERIOD = 100; + +constexpr unsigned int NUMBER_OF_SENSORS = 8; + +} // namespace SensorsConfig +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Main_old/FlightStatsRecorder/FlightStatsRecorder.cpp similarity index 100% rename from src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.cpp rename to src/boards/Main_old/FlightStatsRecorder/FlightStatsRecorder.cpp diff --git a/src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.h b/src/boards/Main_old/FlightStatsRecorder/FlightStatsRecorder.h similarity index 100% rename from src/boards/Main/FlightStatsRecorder/FlightStatsRecorder.h rename to src/boards/Main_old/FlightStatsRecorder/FlightStatsRecorder.h diff --git a/src/boards/Main/PinHandler/PinData.h b/src/boards/Main_old/PinHandler/PinData.h similarity index 100% rename from src/boards/Main/PinHandler/PinData.h rename to src/boards/Main_old/PinHandler/PinData.h diff --git a/src/boards/Main/PinHandler/PinHandler.cpp b/src/boards/Main_old/PinHandler/PinHandler.cpp similarity index 100% rename from src/boards/Main/PinHandler/PinHandler.cpp rename to src/boards/Main_old/PinHandler/PinHandler.cpp diff --git a/src/boards/Main/PinHandler/PinHandler.h b/src/boards/Main_old/PinHandler/PinHandler.h similarity index 100% rename from src/boards/Main/PinHandler/PinHandler.h rename to src/boards/Main_old/PinHandler/PinHandler.h diff --git a/src/boards/Main_old/Radio/Radio.cpp b/src/boards/Main_old/Radio/Radio.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f3c6b40f4bd6076d4cd71c8670b49f5cf9824282 --- /dev/null +++ b/src/boards/Main_old/Radio/Radio.cpp @@ -0,0 +1,497 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <Main/Actuators/Actuators.h> +#include <Main/AltitudeTrigger/AltitudeTrigger.h> +#include <Main/Buses.h> +#include <Main/PinHandler/PinHandler.h> +#include <Main/Radio/Radio.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/ADAController/ADAController.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Main/StateMachines/NASController/NASController.h> +#include <Main/TMRepository/TMRepository.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <drivers/interrupt/external_interrupts.h> +#include <events/EventBroker.h> +#include <radio/SX1278/SX1278Frontends.h> + +#include <Eigen/Core> + +using namespace Boardcore; +using namespace Common; +using namespace Eigen; + +#define SX1278_IRQ_DIO0 EXTI3_IRQHandlerImpl +#define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl +#define SX1278_IRQ_DIO3 EXTI5_IRQHandlerImpl + +void __attribute__((used)) SX1278_IRQ_DIO0() +{ + ModuleManager& modules = ModuleManager::getInstance(); + if (modules.get<Main::Radio>()->transceiver) + { + modules.get<Main::Radio>()->transceiver->handleDioIRQ(); + } +} + +void __attribute__((used)) SX1278_IRQ_DIO1() +{ + ModuleManager& modules = ModuleManager::getInstance(); + if (modules.get<Main::Radio>()->transceiver) + { + modules.get<Main::Radio>()->transceiver->handleDioIRQ(); + } +} + +void __attribute__((used)) SX1278_IRQ_DIO3() +{ + ModuleManager& modules = ModuleManager::getInstance(); + if (modules.get<Main::Radio>()->transceiver) + { + modules.get<Main::Radio>()->transceiver->handleDioIRQ(); + } +} +namespace Main +{ + +Radio::Radio(TaskScheduler* sched) : scheduler(sched) {} + +bool Radio::start() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Config the transceiver + SX1278Fsk::Config config; + config.freq_rf = 419000000; + config.freq_dev = 50000; + config.bitrate = 48000; + config.rx_bw = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000; + config.afc_bw = Boardcore::SX1278Fsk::Config::RxBw::HZ_125000; + config.ocp = 120; + config.power = 13; + config.shaping = Boardcore::SX1278Fsk::Config::Shaping::GAUSSIAN_BT_1_0; + config.dc_free = Boardcore::SX1278Fsk::Config::DcFree::WHITENING; + config.enable_crc = false; + + std::unique_ptr<SX1278::ISX1278Frontend> frontend = + std::make_unique<Skyward433Frontend>(); + + transceiver = new SX1278Fsk( + modules.get<Buses>()->spi6, miosix::radio::cs::getPin(), + miosix::radio::dio0::getPin(), miosix::radio::dio1::getPin(), + miosix::radio::dio3::getPin(), SPI::ClockDivider::DIV_64, + std::move(frontend)); + + // Config the radio + SX1278Fsk::Error error = transceiver->init(config); + + // Add periodic telemetry send task + uint8_t result1 = + scheduler->addTask([&]() { this->sendPeriodicMessage(); }, + RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD, + TaskScheduler::Policy::RECOVER); + uint8_t result2 = scheduler->addTask( + [&]() + { + this->enqueueMsg( + modules.get<TMRepository>()->packSystemTm(MAV_STATS_ID, 0, 0)); + }, + RadioConfig::RADIO_PERIODIC_TELEMETRY_PERIOD * 2, + TaskScheduler::Policy::RECOVER); + + // Config mavDriver + mavDriver = new MavDriver( + transceiver, + [=](MavDriver*, const mavlink_message_t& msg) + { this->handleMavlinkMessage(msg); }, + RadioConfig::RADIO_SLEEP_AFTER_SEND, + RadioConfig::RADIO_OUT_BUFFER_MAX_AGE); + + // Check radio failure + if (error != SX1278Fsk::Error::NONE) + { + return false; + } + + // Start the mavlink driver thread + return mavDriver->start() && result1 != 0 && result2 != 0; +} + +void Radio::sendAck(const mavlink_message_t& msg) +{ + mavlink_message_t ackMsg; + mavlink_msg_ack_tm_pack(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &ackMsg, msg.msgid, + msg.seq); + enqueueMsg(ackMsg); +} + +void Radio::sendNack(const mavlink_message_t& msg) +{ + mavlink_message_t nackMsg; + mavlink_msg_nack_tm_pack(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &nackMsg, msg.msgid, + msg.seq); + enqueueMsg(nackMsg); +} + +void Radio::logStatus() {} + +bool Radio::isStarted() +{ + return mavDriver->isStarted() && scheduler->isRunning(); +} + +void Radio::handleMavlinkMessage(const mavlink_message_t& msg) +{ + ModuleManager& modules = ModuleManager::getInstance(); + + switch (msg.msgid) + { + case MAVLINK_MSG_ID_PING_TC: + { + // Do nothing, just add the ack to the queue + break; + } + case MAVLINK_MSG_ID_COMMAND_TC: + { + // Let the handle command reply to the message + return handleCommand(msg); + } + case MAVLINK_MSG_ID_SYSTEM_TM_REQUEST_TC: + { + SystemTMList tmId = static_cast<SystemTMList>( + mavlink_msg_system_tm_request_tc_get_tm_id(&msg)); + + if (tmId == MAV_PIN_OBS_ID) + { + for (uint8_t i = 0; i < PinHandler::PIN_END; i++) + { + mavlink_message_t msg; + mavlink_pin_tm_t tm; + + PinData data = modules.get<PinHandler>()->getPinData( + static_cast<PinHandler::PinList>(i)); + + tm.changes_counter = data.changesCount; + tm.current_state = tm.current_state; + tm.last_change_timestamp = tm.last_change_timestamp; + tm.pin_id = tm.pin_id; + tm.timestamp = TimestampTimer::getTimestamp(); + + mavlink_msg_pin_tm_encode(RadioConfig::MAV_SYSTEM_ID, + RadioConfig::MAV_COMP_ID, &msg, + &tm); + enqueueMsg(msg); + } + } + else if (tmId == MAV_SENSORS_STATE_ID) + { + auto sensorsState = modules.get<Sensors>()->getSensorInfo(); + + for (uint8_t i = 0; i < sensorsState.size(); i++) + { + mavlink_message_t msg; + mavlink_sensor_state_tm_t tm; + + strcpy(tm.sensor_name, sensorsState.at(i).id.c_str()); + tm.state = sensorsState.at(i).isInitialized; + + mavlink_msg_sensor_state_tm_encode( + RadioConfig::MAV_SYSTEM_ID, RadioConfig::MAV_COMP_ID, + &msg, &tm); + enqueueMsg(msg); + } + } + else + { + // Add to the queue the respose + mavlink_message_t response = + modules.get<TMRepository>()->packSystemTm(tmId, msg.msgid, + msg.seq); + // Add the response to the queue + enqueueMsg(response); + + // Check if the TM repo answered with a NACK. If so the function + // must return to avoid sending a default ack + if (response.msgid == MAVLINK_MSG_ID_NACK_TM) + { + return; + } + } + + break; + } + case MAVLINK_MSG_ID_SENSOR_TM_REQUEST_TC: + { + SensorsTMList sensorId = static_cast<SensorsTMList>( + mavlink_msg_sensor_tm_request_tc_get_sensor_name(&msg)); + mavlink_message_t response = + modules.get<TMRepository>()->packSensorsTm(sensorId, msg.msgid, + msg.seq); + enqueueMsg(response); + + // If the response is a nack the method returns + if (response.msgid == MAVLINK_MSG_ID_NACK_TM) + { + return; + } + break; + } + case MAVLINK_MSG_ID_SERVO_TM_REQUEST_TC: + { + ServosList servo = static_cast<ServosList>( + mavlink_msg_servo_tm_request_tc_get_servo_id(&msg)); + mavlink_message_t response = + modules.get<TMRepository>()->packServoTm(servo, msg.msgid, + msg.seq); + enqueueMsg(response); + + // If the response is a nack the method returns + if (response.msgid == MAVLINK_MSG_ID_NACK_TM) + { + return; + } + break; + } + case MAVLINK_MSG_ID_SET_SERVO_ANGLE_TC: + { + ServosList servoId = static_cast<ServosList>( + mavlink_msg_set_servo_angle_tc_get_servo_id(&msg)); + float position = mavlink_msg_set_servo_angle_tc_get_angle(&msg); + + // Send nack if the FMM is not in test mode + if (!modules.get<FlightModeManager>()->testState( + &FlightModeManager::state_test_mode)) + { + return sendNack(msg); + } + + // If the state is test mode, the servo is set to the correct angle + modules.get<Actuators>()->setServoPosition(servoId, position); + break; + } + case MAVLINK_MSG_ID_WIGGLE_SERVO_TC: + { + ServosList servoId = static_cast<ServosList>( + mavlink_msg_wiggle_servo_tc_get_servo_id(&msg)); + + // Send nack if the FMM is not in test mode + if (!modules.get<FlightModeManager>()->testState( + &FlightModeManager::state_test_mode)) + { + return sendNack(msg); + } + + // If the state is test mode, the wiggle is done + modules.get<Actuators>()->wiggleServo(servoId); + + break; + } + case MAVLINK_MSG_ID_RESET_SERVO_TC: + { + ServosList servoId = static_cast<ServosList>( + mavlink_msg_reset_servo_tc_get_servo_id(&msg)); + + // Send nack if the FMM is not in test mode + if (!modules.get<FlightModeManager>()->testState( + &FlightModeManager::state_test_mode)) + { + return sendNack(msg); + } + + // Set the servo position to 0 + modules.get<Actuators>()->setServoPosition(servoId, 0); + + break; + } + case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC: + { + float altitude = + mavlink_msg_set_deployment_altitude_tc_get_dpl_altitude(&msg); + + modules.get<AltitudeTrigger>()->setDeploymentAltitude(altitude); + break; + } + case MAVLINK_MSG_ID_SET_REFERENCE_ALTITUDE_TC: + { + float altitude = + mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg); + + modules.get<ADAController>()->setReferenceAltitude(altitude); + modules.get<NASController>()->setReferenceAltitude(altitude); + + break; + } + case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC: + { + float temperature = + mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg); + + modules.get<ADAController>()->setReferenceTemperature(temperature); + modules.get<NASController>()->setReferenceTemperature(temperature); + + break; + } + case MAVLINK_MSG_ID_SET_COORDINATES_TC: + { + float latitude = mavlink_msg_set_coordinates_tc_get_latitude(&msg); + float longitude = + mavlink_msg_set_coordinates_tc_get_longitude(&msg); + + Vector2f coordinates{latitude, longitude}; + + modules.get<NASController>()->setCoordinates(coordinates); + break; + } + case MAVLINK_MSG_ID_SET_ORIENTATION_TC: + { + float yaw = mavlink_msg_set_orientation_tc_get_yaw(&msg); + float pitch = mavlink_msg_set_orientation_tc_get_pitch(&msg); + float roll = mavlink_msg_set_orientation_tc_get_roll(&msg); + + modules.get<NASController>()->setOrientation(yaw, pitch, roll); + break; + } + default: + { + LOG_DEBUG(logger, "Received message is not of a known type"); + return sendNack(msg); + } + } + + // At the end send the ack message + sendAck(msg); +} + +void Radio::handleCommand(const mavlink_message_t& msg) +{ + MavCommandList commandId = static_cast<MavCommandList>( + mavlink_msg_command_tc_get_command_id(&msg)); + ModuleManager& modules = ModuleManager::getInstance(); + + // Create the map between the commands and the corresponding events + static const std::map<MavCommandList, Events> commandToEvent{ + {MAV_CMD_ARM, TMTC_ARM}, + {MAV_CMD_DISARM, TMTC_DISARM}, + {MAV_CMD_CALIBRATE, TMTC_CALIBRATE}, + {MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT}, + {MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH}, + {MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING}, + {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE}, + {MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION}, + {MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT}, + {MAV_CMD_FORCE_REBOOT, TMTC_RESET_BOARD}, + {MAV_CMD_ENTER_TEST_MODE, TMTC_ENTER_TEST_MODE}, + {MAV_CMD_EXIT_TEST_MODE, TMTC_EXIT_TEST_MODE}, + {MAV_CMD_START_RECORDING, TMTC_START_RECORDING}, + {MAV_CMD_STOP_RECORDING, TMTC_STOP_RECORDING}, + }; + + switch (commandId) + { + case MAV_CMD_SAVE_CALIBRATION: + { + // Save the sensor calibration and adopt it + bool result = modules.get<Sensors>()->writeMagCalibration(); + + if (!result) + { + return sendNack(msg); + } + + break; + } + case MAV_CMD_START_LOGGING: + { + bool result = Logger::getInstance().start(); + + // In case the logger is not started send to GS the result + if (!result) + { + return sendNack(msg); + } + break; + } + case MAV_CMD_STOP_LOGGING: + { + Logger::getInstance().stop(); + break; + } + default: + { + // If the command is not a particular one, look for it inside the + // map + auto it = commandToEvent.find(commandId); + + if (it != commandToEvent.end()) + { + EventBroker::getInstance().post(it->second, TOPIC_TMTC); + } + else + { + return sendNack(msg); + } + } + } + // Acknowledge the message + sendAck(msg); +} + +void Radio::sendPeriodicMessage() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Send all the queue messages + { + Lock<FastMutex> lock(queueMutex); + + for (uint8_t i = 0; i < messageQueueIndex; i++) + { + mavDriver->enqueueMsg(messageQueue[i]); + } + + // Reset the index + messageQueueIndex = 0; + } + + mavDriver->enqueueMsg( + modules.get<TMRepository>()->packSystemTm(MAV_MOTOR_ID, 0, 0)); + mavDriver->enqueueMsg( + modules.get<TMRepository>()->packSystemTm(MAV_FLIGHT_ID, 0, 0)); +} + +void Radio::enqueueMsg(const mavlink_message_t& msg) +{ + { + Lock<FastMutex> lock(queueMutex); + + // Insert the message inside the queue only if there is enough space + if (messageQueueIndex < RadioConfig::MAVLINK_QUEUE_SIZE) + { + messageQueue[messageQueueIndex] = msg; + messageQueueIndex++; + } + } +} +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main_old/Radio/Radio.h b/src/boards/Main_old/Radio/Radio.h new file mode 100644 index 0000000000000000000000000000000000000000..8c1fd4cde31fcbde4b190b31c0e3c56291b728e6 --- /dev/null +++ b/src/boards/Main_old/Radio/Radio.h @@ -0,0 +1,102 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <Main/Configs/RadioConfig.h> +#include <common/Mavlink.h> +#include <radio/MavlinkDriver/MavlinkDriver.h> +#include <radio/SX1278/SX1278Fsk.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +using MavDriver = Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU, + RadioConfig::RADIO_OUT_QUEUE_SIZE, + RadioConfig::RADIO_MAV_MSG_LENGTH>; + +class Radio : public Boardcore::Module +{ +public: + Radio(Boardcore::TaskScheduler* sched); + + /** + * @brief Starts the MavlinkDriver + */ + [[nodiscard]] bool start(); + + /** + * @brief Sends via radio an acknowledge message about the parameter passed + * message + */ + void sendAck(const mavlink_message_t& msg); + + /** + * @brief Sends via radio an non-acknowledge message about the parameter + * passed message + */ + void sendNack(const mavlink_message_t& msg); + + /** + * @brief Saves the MavlinkDriver and transceiver status + */ + void logStatus(); + + /** + * @brief Returns if the radio module is correctly started + */ + bool isStarted(); + + Boardcore::SX1278Fsk* transceiver = nullptr; + MavDriver* mavDriver = nullptr; + +private: + /** + * @brief Called by the MavlinkDriver when a message is received + */ + void handleMavlinkMessage(const mavlink_message_t& msg); + + /** + * @brief Called by the handleMavlinkMessage to handle a command message + */ + void handleCommand(const mavlink_message_t& msg); + + /** + * @brief Sends the periodic telemetry + */ + void sendPeriodicMessage(); + + /** + * @brief Inserts the mavlink message into the queue + */ + void enqueueMsg(const mavlink_message_t& msg); + + // Messages queue + mavlink_message_t messageQueue[RadioConfig::MAVLINK_QUEUE_SIZE]; + uint32_t messageQueueIndex = 0; + miosix::FastMutex queueMutex; + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio"); + Boardcore::TaskScheduler* scheduler = nullptr; +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp b/src/boards/Main_old/Sensors/RotatedIMU/RotatedIMU.cpp similarity index 100% rename from src/boards/Main/Sensors/RotatedIMU/RotatedIMU.cpp rename to src/boards/Main_old/Sensors/RotatedIMU/RotatedIMU.cpp diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMU.h b/src/boards/Main_old/Sensors/RotatedIMU/RotatedIMU.h similarity index 100% rename from src/boards/Main/Sensors/RotatedIMU/RotatedIMU.h rename to src/boards/Main_old/Sensors/RotatedIMU/RotatedIMU.h diff --git a/src/boards/Main/Sensors/RotatedIMU/RotatedIMUData.h b/src/boards/Main_old/Sensors/RotatedIMU/RotatedIMUData.h similarity index 100% rename from src/boards/Main/Sensors/RotatedIMU/RotatedIMUData.h rename to src/boards/Main_old/Sensors/RotatedIMU/RotatedIMUData.h diff --git a/src/boards/Main_old/Sensors/Sensors.cpp b/src/boards/Main_old/Sensors/Sensors.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2333ee4e1a3748105e91febe1245ce29d9425bfd --- /dev/null +++ b/src/boards/Main_old/Sensors/Sensors.cpp @@ -0,0 +1,735 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <Main/Buses.h> +#include <Main/Configs/SensorsConfig.h> +#include <interfaces-impl/hwmapping.h> + +using namespace Boardcore; +using namespace std; +using namespace Main::SensorsConfig; +namespace Main +{ +LPS22DFData Sensors::getLPS22DFLastSample() +{ + miosix::PauseKernelLock lock; + return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{}; +} +LPS28DFWData Sensors::getLPS28DFW_1LastSample() +{ + miosix::PauseKernelLock lock; + return lps28dfw_1 != nullptr ? lps28dfw_1->getLastSample() : LPS28DFWData{}; +} +LPS28DFWData Sensors::getLPS28DFW_2LastSample() +{ + miosix::PauseKernelLock lock; + return lps28dfw_2 != nullptr ? lps28dfw_2->getLastSample() : LPS28DFWData{}; +} +H3LIS331DLData Sensors::getH3LIS331DLLastSample() +{ + miosix::PauseKernelLock lock; + return h3lis331dl != nullptr ? h3lis331dl->getLastSample() + : H3LIS331DLData{}; +} +LIS2MDLData Sensors::getLIS2MDLLastSample() +{ + miosix::PauseKernelLock lock; + return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{}; +} +UBXGPSData Sensors::getGPSLastSample() +{ + miosix::PauseKernelLock lock; + return ubxgps != nullptr ? ubxgps->getLastSample() : UBXGPSData{}; +} +LSM6DSRXData Sensors::getLSM6DSRXLastSample() +{ + miosix::PauseKernelLock lock; + return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{}; +} +ADS131M08Data Sensors::getADS131M08LastSample() +{ + miosix::PauseKernelLock lock; + return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{}; +} + +PitotData Sensors::getPitotLastSample() +{ + miosix::PauseKernelLock lock; + return canPitot; +} +PressureData Sensors::getCCPressureLastSample() +{ + miosix::PauseKernelLock lock; + return canCCPressure; +} +PressureData Sensors::getBottomTankPressureLastSample() +{ + miosix::PauseKernelLock lock; + return canBottomTankPressure; +} +PressureData Sensors::getTopTankPressureLastSample() +{ + miosix::PauseKernelLock lock; + return canTopTankPressure; +} +TemperatureData Sensors::getTankTemperatureLastSample() +{ + miosix::PauseKernelLock lock; + return canTankTemperature; +} +BatteryVoltageSensorData Sensors::getMotorBatteryVoltage() +{ + miosix::PauseKernelLock lock; + return canMotorBatteryVoltage; +} +CurrentData Sensors::getMotorCurrent() +{ + miosix::PauseKernelLock lock; + return canMotorCurrent; +} + +// Processed Getters +BatteryVoltageSensorData Sensors::getBatteryVoltageLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + ADS131M08Data sample = getADS131M08LastSample(); + BatteryVoltageSensorData data; + + // Populate the data + data.voltageTimestamp = sample.timestamp; + data.channelId = 1; + data.voltage = sample.voltage[1]; + data.batVoltage = sample.voltage[1] * BATTERY_VOLTAGE_CONVERSION_FACTOR; + return data; +} + +BatteryVoltageSensorData Sensors::getCamBatteryVoltageLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + ADS131M08Data sample = getADS131M08LastSample(); + BatteryVoltageSensorData data; + + // Populate the data + data.voltageTimestamp = sample.timestamp; + data.channelId = 7; + data.voltage = sample.voltage[7]; + data.batVoltage = sample.voltage[7] * BATTERY_VOLTAGE_CONVERSION_FACTOR; + return data; +} + +CurrentData Sensors::getCurrentLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + ADS131M08Data sample = getADS131M08LastSample(); + CurrentData data; + + // Populate the data + data.currentTimestamp = sample.timestamp; + data.current = + (sample.voltage[5] - CURRENT_OFFSET) * CURRENT_CONVERSION_FACTOR; + return data; +} + +MPXH6400AData Sensors::getDeploymentPressureLastSample() +{ + miosix::PauseKernelLock lock; + return mpxh6400a != nullptr ? mpxh6400a->getLastSample() : MPXH6400AData{}; +} + +HSCMRNN015PAData Sensors::getStaticPressure1LastSample() +{ + miosix::PauseKernelLock lock; + return hscmrnn015pa_1 != nullptr ? hscmrnn015pa_1->getLastSample() + : HSCMRNN015PAData{}; +} + +HSCMRNN015PAData Sensors::getStaticPressure2LastSample() +{ + miosix::PauseKernelLock lock; + return hscmrnn015pa_2 != nullptr ? hscmrnn015pa_2->getLastSample() + : HSCMRNN015PAData{}; +} + +RotatedIMUData Sensors::getIMULastSample() +{ + miosix::PauseKernelLock lock; + return imu != nullptr ? imu->getLastSample() : RotatedIMUData{}; +} + +MagnetometerData Sensors::getCalibratedMagnetometerLastSample() +{ + // Do not need to pause the kernel, the last sample getter is already + // protected + MagnetometerData lastSample = getLIS2MDLLastSample(); + MagnetometerData result; + + // Correct the result and copy the timestamp + { + miosix::Lock<FastMutex> l(calibrationMutex); + result = + static_cast<MagnetometerData>(magCalibration.correct(lastSample)); + } + + result.magneticFieldTimestamp = lastSample.magneticFieldTimestamp; + return result; +} + +void Sensors::setPitot(PitotData data) +{ + miosix::PauseKernelLock lock; + canPitot = data; + canPitot.timestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setCCPressure(PressureData data) +{ + miosix::PauseKernelLock lock; + canCCPressure = data; + canCCPressure.pressureTimestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setBottomTankPressure(PressureData data) +{ + miosix::PauseKernelLock lock; + canBottomTankPressure = data; + canBottomTankPressure.pressureTimestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setTopTankPressure(PressureData data) +{ + miosix::PauseKernelLock lock; + canTopTankPressure = data; + canTopTankPressure.pressureTimestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setTankTemperature(TemperatureData data) +{ + miosix::PauseKernelLock lock; + canTankTemperature = data; + canTankTemperature.temperatureTimestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setMotorBatteryVoltage(BatteryVoltageSensorData data) +{ + miosix::PauseKernelLock lock; + canMotorBatteryVoltage.batVoltage = data.batVoltage; + canMotorBatteryVoltage.voltageTimestamp = TimestampTimer::getTimestamp(); +} +void Sensors::setMotorCurrent(CurrentData data) +{ + miosix::PauseKernelLock lock; + canMotorCurrent.current = data.current; + canMotorCurrent.currentTimestamp = TimestampTimer::getTimestamp(); +} + +Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {} + +bool Sensors::start() +{ + // Read the magnetometer calibration from predefined file + magCalibration.fromFile("/sd/magCalibration.csv"); + + // Init all the sensors + lps22dfInit(); + lps28dfw_1Init(); + // lps28dfw_2Init(); + h3lis331dlInit(); + lis2mdlInit(); + ubxgpsInit(); + lsm6dsrxInit(); + ads131m08Init(); + deploymentPressureInit(); + staticPressure1Init(); + staticPressure2Init(); + imuInit(); + + // Add the magnetometer calibration to the scheduler + size_t result = scheduler->addTask( + [&]() + { + // Gather the last sample data + MagnetometerData lastSample = getLIS2MDLLastSample(); + + // Feed the data to the calibrator inside a protected area. + // Contention is not high and the use of a mutex is suitable to + // avoid pausing the kernel for this calibration operation + { + miosix::Lock<FastMutex> l(calibrationMutex); + magCalibrator.feed(lastSample); + } + }, + MAG_CALIBRATION_PERIOD); + + // Create sensor manager with populated map and configured scheduler + manager = new SensorManager(sensorMap, scheduler); + return manager->start() && result != 0; +} + +void Sensors::stop() { manager->stop(); } + +bool Sensors::isStarted() +{ + return manager->areAllSensorsInitialized() && scheduler->isRunning(); +} + +void Sensors::calibrate() +{ + // Create the stats to calibrate the barometers + Stats lps28dfw1Stats; + // Stats lps28dfw2Stats; + Stats staticPressure1Stats; + Stats staticPressure2Stats; + Stats deploymentPressureStats; + + // Add N samples to the stats + for (unsigned int i = 0; i < SensorsConfig::CALIBRATION_SAMPLES; i++) + { + lps28dfw1Stats.add(getLPS28DFW_1LastSample().pressure); + // lps28dfw2Stats.add(getLPS28DFW_2LastSample().pressure); + staticPressure1Stats.add(getStaticPressure1LastSample().pressure); + staticPressure2Stats.add(getStaticPressure2LastSample().pressure); + deploymentPressureStats.add(getDeploymentPressureLastSample().pressure); + + // Delay for the expected period + miosix::Thread::sleep(SensorsConfig::CALIBRATION_PERIOD); + } + + // Compute the difference between the mean value from LPS28DFW + float reference = lps28dfw1Stats.getStats().mean; + + hscmrnn015pa_1->updateOffset(staticPressure1Stats.getStats().mean - + reference); + hscmrnn015pa_2->updateOffset(staticPressure2Stats.getStats().mean - + reference); + mpxh6400a->updateOffset(deploymentPressureStats.getStats().mean - + reference); + + // Log the offsets + SensorsCalibrationParameter cal{}; + cal.timestamp = TimestampTimer::getTimestamp(); + cal.offsetStatic1 = staticPressure1Stats.getStats().mean - reference; + cal.offsetStatic2 = staticPressure2Stats.getStats().mean - reference; + cal.offsetDeployment = deploymentPressureStats.getStats().mean - reference; + cal.referencePressure = reference; + + Logger::getInstance().log(cal); +} + +bool Sensors::writeMagCalibration() +{ + // Compute the calibration result in protected area + { + miosix::Lock<FastMutex> l(calibrationMutex); + SixParametersCorrector cal = magCalibrator.computeResult(); + + // Check result validity + if (!isnan(cal.getb()[0]) && !isnan(cal.getb()[1]) && + !isnan(cal.getb()[2]) && !isnan(cal.getA()[0]) && + !isnan(cal.getA()[1]) && !isnan(cal.getA()[2])) + { + magCalibration = cal; + + // Save the calibration to the calibration file + return magCalibration.toFile("/sd/magCalibration.csv"); + } + return false; + } +} + +std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> +Sensors::getSensorInfo() +{ + std::array<SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> sensorState; + for (size_t i = 0; i < sensorsInit.size(); i++) + { + // Check wether the lambda is existent + if (sensorsInit[i]) + { + sensorState[i] = sensorsInit[i](); + } + } + return sensorState; +} + +void Sensors::lps22dfInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Get the correct SPI configuration + SPIBusConfig config = LPS22DF::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_16; + + // Configure the device + LPS22DF::Config sensorConfig; + sensorConfig.avg = LPS22DF_AVG; + sensorConfig.odr = LPS22DF_ODR; + + // Create sensor instance with configured parameters + lps22df = new LPS22DF(modules.get<Buses>()->spi3, + miosix::sensors::LPS22DF::cs::getPin(), config, + sensorConfig); + + // Emplace the sensor inside the map + SensorInfo info("LPS22DF", LPS22DF_PERIOD, + bind(&Sensors::lps22dfCallback, this)); + sensorMap.emplace(make_pair(lps22df, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(lps22df); }; +} +void Sensors::lps28dfw_1Init() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Configure the sensor + LPS28DFW::SensorConfig config{false, LPS28DFW_FSR, LPS28DFW_AVG, + LPS28DFW_ODR, false}; + + // Create sensor instance with configured parameters + lps28dfw_1 = new LPS28DFW(modules.get<Buses>()->i2c1, config); + + // Emplace the sensor inside the map + SensorInfo info("LPS28DFW_1", LPS28DFW_PERIOD, + bind(&Sensors::lps28dfw_1Callback, this)); + sensorMap.emplace(make_pair(lps28dfw_1, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(lps28dfw_1); }; +} +void Sensors::lps28dfw_2Init() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Configure the sensor + LPS28DFW::SensorConfig config{true, LPS28DFW_FSR, LPS28DFW_AVG, + LPS28DFW_ODR, false}; + + // Create sensor instance with configured parameters + lps28dfw_2 = new LPS28DFW(modules.get<Buses>()->i2c1, config); + + // Emplace the sensor inside the map + SensorInfo info("LPS28DFW_2", LPS28DFW_PERIOD, + bind(&Sensors::lps28dfw_2Callback, this)); + sensorMap.emplace(make_pair(lps28dfw_2, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(lps28dfw_2); }; +} +void Sensors::h3lis331dlInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Get the correct SPI configuration + SPIBusConfig config = H3LIS331DL::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_16; + + // Create sensor instance with configured parameters + h3lis331dl = new H3LIS331DL( + modules.get<Buses>()->spi3, miosix::sensors::H3LIS331DL::cs::getPin(), + config, H3LIS331DL_ODR, H3LIS331DL_BDU, H3LIS331DL_FSR); + + // Emplace the sensor inside the map + SensorInfo info("H3LIS331DL", H3LIS331DL_PERIOD, + bind(&Sensors::h3lis331dlCallback, this)); + sensorMap.emplace(make_pair(h3lis331dl, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(h3lis331dl); }; +} +void Sensors::lis2mdlInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Get the correct SPI configuration + SPIBusConfig config = LIS2MDL::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_16; + + // Configure the sensor + LIS2MDL::Config sensorConfig; + sensorConfig.deviceMode = LIS2MDL_OPERATIVE_MODE; + sensorConfig.odr = LIS2MDL_ODR; + sensorConfig.temperatureDivider = LIS2MDL_TEMPERATURE_DIVIDER; + + // Create sensor instance with configured parameters + lis2mdl = new LIS2MDL(modules.get<Buses>()->spi3, + miosix::sensors::LIS2MDL::cs::getPin(), config, + sensorConfig); + + // Emplace the sensor inside the map + SensorInfo info("LIS2MDL", LIS2MDL_PERIOD, + bind(&Sensors::lis2mdlCallback, this)); + sensorMap.emplace(make_pair(lis2mdl, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(lis2mdl); }; +} + +void Sensors::ubxgpsInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Get the correct SPI configuration + SPIBusConfig config = UBXGPSSpi::getDefaultSPIConfig(); + config.clockDivider = SPI::ClockDivider::DIV_64; + + // Create sensor instance with configured parameters + ubxgps = new UBXGPSSpi(modules.get<Buses>()->spi4, + miosix::sensors::GPS::cs::getPin(), config, 5); + + // Emplace the sensor inside the map + SensorInfo info("UBXGPS", UBXGPS_PERIOD, + bind(&Sensors::ubxgpsCallback, this)); + sensorMap.emplace(make_pair(ubxgps, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(ubxgps); }; +} + +void Sensors::lsm6dsrxInit() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Configure the SPI + SPIBusConfig config; + config.clockDivider = SPI::ClockDivider::DIV_32; + config.mode = SPI::Mode::MODE_0; + + // Configure the sensor + LSM6DSRXConfig sensorConfig; + sensorConfig.bdu = LSM6DSRX_BDU; + + // Accelerometer + sensorConfig.fsAcc = LSM6DSRX_ACC_FS; + sensorConfig.odrAcc = LSM6DSRX_ACC_ODR; + sensorConfig.opModeAcc = LSM6DSRX_OPERATING_MODE; + + // Gyroscope + sensorConfig.fsGyr = LSM6DSRX_GYR_FS; + sensorConfig.odrGyr = LSM6DSRX_GYR_ODR; + sensorConfig.opModeGyr = LSM6DSRX_OPERATING_MODE; + + // Fifo + sensorConfig.fifoMode = LSM6DSRX_FIFO_MODE; + sensorConfig.fifoTimestampDecimation = LSM6DSRX_FIFO_TIMESTAMP_DECIMATION; + sensorConfig.fifoTemperatureBdr = LSM6DSRX_FIFO_TEMPERATURE_BDR; + + // Create sensor instance with configured parameters + lsm6dsrx = new LSM6DSRX(modules.get<Buses>()->spi1, + miosix::sensors::LSM6DSRX::cs::getPin(), config, + sensorConfig); + + // Emplace the sensor inside the map + SensorInfo info("LSM6DSRX", LSM6DSRX_PERIOD, + bind(&Sensors::lsm6dsrxCallback, this)); + sensorMap.emplace(make_pair(lsm6dsrx, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(lsm6dsrx); }; +} + +void Sensors::ads131m08Init() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Configure the SPI + SPIBusConfig config; + config.clockDivider = SPI::ClockDivider::DIV_32; + + // Configure the device + ADS131M08::Config sensorConfig; + sensorConfig.oversamplingRatio = ADS131M08_OVERSAMPLING_RATIO; + sensorConfig.globalChopModeEnabled = ADS131M08_GLOBAL_CHOP_MODE; + + // Create the sensor instance with configured parameters + ads131m08 = new ADS131M08(modules.get<Buses>()->spi4, + miosix::sensors::ADS131::cs::getPin(), config, + sensorConfig); + + // Emplace the sensor inside the map + SensorInfo info("ADS131M08", ADS131M08_PERIOD, + bind(&Sensors::ads131m08Callback, this)); + sensorMap.emplace(make_pair(ads131m08, info)); + + // Add the sensor info getter to the array + sensorsInit[sensorsId++] = [&]() -> SensorInfo + { return manager->getSensorInfo(ads131m08); }; +} + +void Sensors::deploymentPressureInit() +{ + // Create the lambda function to get the voltage + function<ADCData()> getVoltage = [&]() + { + // No need to synchronize, the sampling thread is the same + ADS131M08Data sample = ads131m08->getLastSample(); + return sample.getVoltage(ADC_CH_DPL); + }; + + // Create the sensor instance with created function + mpxh6400a = new MPXH6400A(getVoltage, ADC_VOLTAGE_RANGE); + + // Emplace the sensor inside the map + SensorInfo info("MPXH6400A", ADS131M08_PERIOD, + bind(&Sensors::deploymentPressureCallback, this)); + sensorMap.emplace(make_pair(mpxh6400a, info)); +} + +void Sensors::staticPressure1Init() +{ + // Create the lambda function to get the voltage + function<ADCData()> getVoltage = [&]() + { + // No need to synchronize, the sampling thread is the same + ADS131M08Data sample = ads131m08->getLastSample(); + return sample.getVoltage(ADC_CH_STATIC_1); + }; + + // Create the sensor instance with created function + hscmrnn015pa_1 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE); + + // Emplace the sensor inside the map + SensorInfo info("HSCMRNN015PA_1", ADS131M08_PERIOD, + bind(&Sensors::staticPressure1Callback, this)); + sensorMap.emplace(make_pair(hscmrnn015pa_1, info)); +} + +void Sensors::staticPressure2Init() +{ + // Create the lambda function to get the voltage + function<ADCData()> getVoltage = [&]() + { + // No need to synchronize, the sampling thread is the same + ADS131M08Data sample = ads131m08->getLastSample(); + return sample.getVoltage(ADC_CH_STATIC_2); + }; + + // Create the sensor instance with created function + hscmrnn015pa_2 = new HSCMRNN015PA(getVoltage, ADC_VOLTAGE_RANGE); + + // Emplace the sensor inside the map + SensorInfo info("HSCMRNN015PA_2", ADS131M08_PERIOD, + bind(&Sensors::staticPressure2Callback, this)); + sensorMap.emplace(make_pair(hscmrnn015pa_2, info)); +} + +void Sensors::imuInit() +{ + // Register the IMU as the fake sensor, passing as parameters the methods to + // retrieve real data. The sensor is not synchronized, but the sampling + // thread is always the same. + imu = new RotatedIMU( + bind(&LSM6DSRX::getLastSample, lsm6dsrx), + bind(&Sensors::getCalibratedMagnetometerLastSample, this), + bind(&LSM6DSRX::getLastSample, lsm6dsrx)); + + // Invert the Y axis on the magnetometer + Eigen::Matrix3f m{{1, 0, 0}, {0, -1, 0}, {0, 0, 1}}; + imu->addMagTransformation(m); + imu->addAccTransformation(imu->rotateAroundX(45)); + imu->addGyroTransformation(imu->rotateAroundX(45)); + imu->addMagTransformation(imu->rotateAroundX(45)); + + // Emplace the sensor inside the map + SensorInfo info("RotatedIMU", IMU_PERIOD, + bind(&Sensors::imuCallback, this)); + sensorMap.emplace(make_pair(imu, info)); +} + +void Sensors::lps22dfCallback() +{ + LPS22DFData lastSample = lps22df->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::lps28dfw_1Callback() +{ + LPS28DFW_1Data lastSample = + static_cast<LPS28DFW_1Data>(lps28dfw_1->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::lps28dfw_2Callback() +{ + LPS28DFW_2Data lastSample = + static_cast<LPS28DFW_2Data>(lps28dfw_2->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::h3lis331dlCallback() +{ + H3LIS331DLData lastSample = h3lis331dl->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::lis2mdlCallback() +{ + LIS2MDLData lastSample = lis2mdl->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::ubxgpsCallback() +{ + UBXGPSData lastSample = ubxgps->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::lsm6dsrxCallback() +{ + auto& fifo = lsm6dsrx->getLastFifo(); + uint16_t fifoSize = lsm6dsrx->getLastFifoSize(); + + // For every instance inside the fifo log the sample + for (uint16_t i = 0; i < fifoSize; i++) + { + Logger::getInstance().log(fifo.at(i)); + } +} +void Sensors::ads131m08Callback() +{ + ADS131M08Data lastSample = ads131m08->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::deploymentPressureCallback() +{ + MPXH6400AData lastSample = mpxh6400a->getLastSample(); + Logger::getInstance().log(lastSample); +} +void Sensors::staticPressure1Callback() +{ + HSCMRNN015PA_1Data lastSample = + static_cast<HSCMRNN015PA_1Data>(hscmrnn015pa_1->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::staticPressure2Callback() +{ + HSCMRNN015PA_2Data lastSample = + static_cast<HSCMRNN015PA_2Data>(hscmrnn015pa_2->getLastSample()); + Logger::getInstance().log(lastSample); +} +void Sensors::imuCallback() +{ + RotatedIMUData lastSample = imu->getLastSample(); + Logger::getInstance().log(lastSample); +} + +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main_old/Sensors/Sensors.h b/src/boards/Main_old/Sensors/Sensors.h new file mode 100644 index 0000000000000000000000000000000000000000..44ad52749f84cc0a750388f1581969611e76cdf2 --- /dev/null +++ b/src/boards/Main_old/Sensors/Sensors.h @@ -0,0 +1,202 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <Main/Configs/SensorsConfig.h> +#include <Main/Sensors/RotatedIMU/RotatedIMU.h> +#include <Main/Sensors/SensorsData.h> +#include <sensors/ADS131M08/ADS131M08.h> +#include <sensors/H3LIS331DL/H3LIS331DL.h> +#include <sensors/LIS2MDL/LIS2MDL.h> +#include <sensors/LPS22DF/LPS22DF.h> +#include <sensors/LPS28DFW/LPS28DFW.h> +#include <sensors/LSM6DSRX/LSM6DSRX.h> +#include <sensors/SensorData.h> +#include <sensors/SensorManager.h> +#include <sensors/UBXGPS/UBXGPSSpi.h> +#include <sensors/analog/BatteryVoltageSensorData.h> +#include <sensors/analog/Pitot/PitotData.h> +#include <sensors/analog/pressure/honeywell/HSCMRNN015PA.h> +#include <sensors/analog/pressure/nxp/MPXH6400A.h> +#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> +#include <stdint.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Main +{ +class Sensors : public Boardcore::Module +{ +public: + explicit Sensors(Boardcore::TaskScheduler* sched); + + [[nodiscard]] bool start(); + + /** + * @brief Stops the sensor manager + * @warning Stops the passed scheduler + */ + void stop(); + + /** + * @brief Returns if all the sensors are started successfully + */ + bool isStarted(); + + /** + * @brief Calibrates the sensors with an offset + */ + void calibrate(); + + /** + * @brief Takes the result of the live magnetometer calibration and applies + * it to the current calibration + writes it in the csv file + */ + bool writeMagCalibration(); + + // Sensor getters + Boardcore::LPS22DFData getLPS22DFLastSample(); + Boardcore::LPS28DFWData getLPS28DFW_1LastSample(); + Boardcore::LPS28DFWData getLPS28DFW_2LastSample(); + Boardcore::H3LIS331DLData getH3LIS331DLLastSample(); + Boardcore::LIS2MDLData getLIS2MDLLastSample(); + Boardcore::UBXGPSData getGPSLastSample(); + Boardcore::LSM6DSRXData getLSM6DSRXLastSample(); + Boardcore::ADS131M08Data getADS131M08LastSample(); + + // Processed getters + Boardcore::BatteryVoltageSensorData getBatteryVoltageLastSample(); + Boardcore::BatteryVoltageSensorData getCamBatteryVoltageLastSample(); + Boardcore::CurrentData getCurrentLastSample(); + Boardcore::MPXH6400AData getDeploymentPressureLastSample(); + Boardcore::HSCMRNN015PAData getStaticPressure1LastSample(); + Boardcore::HSCMRNN015PAData getStaticPressure2LastSample(); + RotatedIMUData getIMULastSample(); + Boardcore::MagnetometerData getCalibratedMagnetometerLastSample(); + + // CAN fake sensors setters + void setPitot(Boardcore::PitotData data); + void setCCPressure(Boardcore::PressureData data); + void setBottomTankPressure(Boardcore::PressureData data); + void setTopTankPressure(Boardcore::PressureData data); + void setTankTemperature(Boardcore::TemperatureData data); + void setMotorBatteryVoltage(Boardcore::BatteryVoltageSensorData data); + void setMotorCurrent(Boardcore::CurrentData data); + + // CAN fake sensors getters + Boardcore::PitotData getPitotLastSample(); + Boardcore::PressureData getCCPressureLastSample(); + Boardcore::PressureData getBottomTankPressureLastSample(); + Boardcore::PressureData getTopTankPressureLastSample(); + Boardcore::TemperatureData getTankTemperatureLastSample(); + Boardcore::BatteryVoltageSensorData getMotorBatteryVoltage(); + Boardcore::CurrentData getMotorCurrent(); + + // Returns the sensors statuses + std::array<Boardcore::SensorInfo, SensorsConfig::NUMBER_OF_SENSORS> + getSensorInfo(); + +private: + // Init and callbacks methods + void lps22dfInit(); + void lps22dfCallback(); + + void lps28dfw_1Init(); + void lps28dfw_1Callback(); + + void lps28dfw_2Init(); + void lps28dfw_2Callback(); + + void h3lis331dlInit(); + void h3lis331dlCallback(); + + void lis2mdlInit(); + void lis2mdlCallback(); + + void ubxgpsInit(); + void ubxgpsCallback(); + + void lsm6dsrxInit(); + void lsm6dsrxCallback(); + + void ads131m08Init(); + void ads131m08Callback(); + + void deploymentPressureInit(); + void deploymentPressureCallback(); + + void staticPressure1Init(); + void staticPressure1Callback(); + + void staticPressure2Init(); + void staticPressure2Callback(); + + void imuInit(); + void imuCallback(); + + // Sensors instances + Boardcore::LPS22DF* lps22df = nullptr; + Boardcore::LPS28DFW* lps28dfw_1 = nullptr; + Boardcore::LPS28DFW* lps28dfw_2 = nullptr; + Boardcore::H3LIS331DL* h3lis331dl = nullptr; + Boardcore::LIS2MDL* lis2mdl = nullptr; + Boardcore::UBXGPSSpi* ubxgps = nullptr; + Boardcore::LSM6DSRX* lsm6dsrx = nullptr; + Boardcore::ADS131M08* ads131m08 = nullptr; + + // Can sensors + Boardcore::PitotData canPitot{0, 0, 0}; + Boardcore::PressureData canCCPressure{0, 0}; + Boardcore::PressureData canBottomTankPressure{0, 0}; + Boardcore::PressureData canTopTankPressure{0, 0}; + Boardcore::TemperatureData canTankTemperature{0, 0}; + Boardcore::BatteryVoltageSensorData canMotorBatteryVoltage{}; + Boardcore::CurrentData canMotorCurrent{}; + + // Fake processed sensors + RotatedIMU* imu = nullptr; + Boardcore::MPXH6400A* mpxh6400a = nullptr; + Boardcore::HSCMRNN015PA* hscmrnn015pa_1 = nullptr; + Boardcore::HSCMRNN015PA* hscmrnn015pa_2 = nullptr; + + // Magnetometer live calibration + Boardcore::SoftAndHardIronCalibration magCalibrator; + Boardcore::SixParametersCorrector magCalibration; + miosix::FastMutex calibrationMutex; + + // Sensor manager + Boardcore::SensorManager* manager = nullptr; + Boardcore::SensorManager::SensorMap_t sensorMap; + Boardcore::TaskScheduler* scheduler = nullptr; + + // Collection of lambdas to get the sensor init statuses + std::array<std::function<Boardcore::SensorInfo()>, + SensorsConfig::NUMBER_OF_SENSORS> + sensorsInit; + uint8_t sensorsId = 0; + + // SD logger + Boardcore::Logger& SDlogger = Boardcore::Logger::getInstance(); + + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Sensors"); +}; +} // namespace Main \ No newline at end of file diff --git a/src/boards/Main/Sensors/SensorsData.h b/src/boards/Main_old/Sensors/SensorsData.h similarity index 100% rename from src/boards/Main/Sensors/SensorsData.h rename to src/boards/Main_old/Sensors/SensorsData.h diff --git a/src/boards/Main/StateMachines/ABKController/ABKController.cpp b/src/boards/Main_old/StateMachines/ABKController/ABKController.cpp similarity index 100% rename from src/boards/Main/StateMachines/ABKController/ABKController.cpp rename to src/boards/Main_old/StateMachines/ABKController/ABKController.cpp diff --git a/src/boards/Main/StateMachines/ABKController/ABKController.h b/src/boards/Main_old/StateMachines/ABKController/ABKController.h similarity index 100% rename from src/boards/Main/StateMachines/ABKController/ABKController.h rename to src/boards/Main_old/StateMachines/ABKController/ABKController.h diff --git a/src/boards/Main/StateMachines/ABKController/ABKControllerData.h b/src/boards/Main_old/StateMachines/ABKController/ABKControllerData.h similarity index 100% rename from src/boards/Main/StateMachines/ABKController/ABKControllerData.h rename to src/boards/Main_old/StateMachines/ABKController/ABKControllerData.h diff --git a/src/boards/Main/StateMachines/ABKController/TrajectorySet.h b/src/boards/Main_old/StateMachines/ABKController/TrajectorySet.h similarity index 100% rename from src/boards/Main/StateMachines/ABKController/TrajectorySet.h rename to src/boards/Main_old/StateMachines/ABKController/TrajectorySet.h diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main_old/StateMachines/ADAController/ADAController.cpp similarity index 100% rename from src/boards/Main/StateMachines/ADAController/ADAController.cpp rename to src/boards/Main_old/StateMachines/ADAController/ADAController.cpp diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.h b/src/boards/Main_old/StateMachines/ADAController/ADAController.h similarity index 100% rename from src/boards/Main/StateMachines/ADAController/ADAController.h rename to src/boards/Main_old/StateMachines/ADAController/ADAController.h diff --git a/src/boards/Main/StateMachines/ADAController/ADAControllerData.h b/src/boards/Main_old/StateMachines/ADAController/ADAControllerData.h similarity index 100% rename from src/boards/Main/StateMachines/ADAController/ADAControllerData.h rename to src/boards/Main_old/StateMachines/ADAController/ADAControllerData.h diff --git a/src/boards/Main/StateMachines/Deployment/Deployment.cpp b/src/boards/Main_old/StateMachines/Deployment/Deployment.cpp similarity index 100% rename from src/boards/Main/StateMachines/Deployment/Deployment.cpp rename to src/boards/Main_old/StateMachines/Deployment/Deployment.cpp diff --git a/src/boards/Main/StateMachines/Deployment/Deployment.h b/src/boards/Main_old/StateMachines/Deployment/Deployment.h similarity index 100% rename from src/boards/Main/StateMachines/Deployment/Deployment.h rename to src/boards/Main_old/StateMachines/Deployment/Deployment.h diff --git a/src/boards/Main/StateMachines/Deployment/DeploymentData.h b/src/boards/Main_old/StateMachines/Deployment/DeploymentData.h similarity index 100% rename from src/boards/Main/StateMachines/Deployment/DeploymentData.h rename to src/boards/Main_old/StateMachines/Deployment/DeploymentData.h diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManager.cpp similarity index 100% rename from src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.cpp rename to src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManager.cpp diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h b/src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManager.h similarity index 100% rename from src/boards/Main/StateMachines/FlightModeManager/FlightModeManager.h rename to src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManager.h diff --git a/src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h b/src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManagerData.h similarity index 100% rename from src/boards/Main/StateMachines/FlightModeManager/FlightModeManagerData.h rename to src/boards/Main_old/StateMachines/FlightModeManager/FlightModeManagerData.h diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.cpp b/src/boards/Main_old/StateMachines/MEAController/MEAController.cpp similarity index 100% rename from src/boards/Main/StateMachines/MEAController/MEAController.cpp rename to src/boards/Main_old/StateMachines/MEAController/MEAController.cpp diff --git a/src/boards/Main/StateMachines/MEAController/MEAController.h b/src/boards/Main_old/StateMachines/MEAController/MEAController.h similarity index 100% rename from src/boards/Main/StateMachines/MEAController/MEAController.h rename to src/boards/Main_old/StateMachines/MEAController/MEAController.h diff --git a/src/boards/Main/StateMachines/MEAController/MEAControllerData.h b/src/boards/Main_old/StateMachines/MEAController/MEAControllerData.h similarity index 100% rename from src/boards/Main/StateMachines/MEAController/MEAControllerData.h rename to src/boards/Main_old/StateMachines/MEAController/MEAControllerData.h diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main_old/StateMachines/NASController/NASController.cpp similarity index 100% rename from src/boards/Main/StateMachines/NASController/NASController.cpp rename to src/boards/Main_old/StateMachines/NASController/NASController.cpp diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main_old/StateMachines/NASController/NASController.h similarity index 100% rename from src/boards/Main/StateMachines/NASController/NASController.h rename to src/boards/Main_old/StateMachines/NASController/NASController.h diff --git a/src/boards/Main/StateMachines/NASController/NASControllerData.h b/src/boards/Main_old/StateMachines/NASController/NASControllerData.h similarity index 100% rename from src/boards/Main/StateMachines/NASController/NASControllerData.h rename to src/boards/Main_old/StateMachines/NASController/NASControllerData.h diff --git a/src/boards/Main/TMRepository/TMRepository.cpp b/src/boards/Main_old/TMRepository/TMRepository.cpp similarity index 100% rename from src/boards/Main/TMRepository/TMRepository.cpp rename to src/boards/Main_old/TMRepository/TMRepository.cpp diff --git a/src/boards/Main/TMRepository/TMRepository.h b/src/boards/Main_old/TMRepository/TMRepository.h similarity index 100% rename from src/boards/Main/TMRepository/TMRepository.h rename to src/boards/Main_old/TMRepository/TMRepository.h diff --git a/src/entrypoints/Main/main-entry.cpp b/src/entrypoints/Main/main-entry.cpp index f005365af313df44c300a8b482fbad0f30e62c06..980948471951d10580a821b4354ef38765f4daef 100644 --- a/src/entrypoints/Main/main-entry.cpp +++ b/src/entrypoints/Main/main-entry.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro +/* Copyright (c) 2024 Skyward Experimental Rocketry + * Author: Davide Mor * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -20,299 +20,47 @@ * THE SOFTWARE. */ -#include <Main/Actuators/Actuators.h> -#include <Main/AltitudeTrigger/AltitudeTrigger.h> -#include <Main/BoardScheduler.h> #include <Main/Buses.h> -#include <Main/CanHandler/CanHandler.h> -#include <Main/FlightStatsRecorder/FlightStatsRecorder.h> -#include <Main/PinHandler/PinHandler.h> -#include <Main/Radio/Radio.h> #include <Main/Sensors/Sensors.h> -#include <Main/StateMachines/ABKController/ABKController.h> -#include <Main/StateMachines/ADAController/ADAController.h> -#include <Main/StateMachines/Deployment/Deployment.h> -#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> -#include <Main/StateMachines/MEAController/MEAController.h> -#include <Main/StateMachines/NASController/NASController.h> -#include <Main/TMRepository/TMRepository.h> -#include <common/Events.h> -#include <common/Topics.h> -#include <diagnostic/CpuMeter/CpuMeter.h> -#include <diagnostic/PrintLogger.h> -#include <diagnostic/StackLogger.h> -#include <drivers/timer/TimestampTimer.h> -#include <events/EventBroker.h> -#include <events/EventData.h> -#include <events/utils/EventSniffer.h> +#include <Main/Radio/Radio.h> +#include <drivers/timer/PWM.h> +#include <miosix.h> #include <utils/ModuleManager/ModuleManager.hpp> +using namespace miosix; using namespace Boardcore; using namespace Main; -using namespace Common; int main() { - ModuleManager& modules = ModuleManager::getInstance(); - - // Overall status, if at some point it becomes false, there is a problem - // somewhere - bool initResult = true; - PrintLogger logger = Logging::getLogger("main"); - - // Create modules - BoardScheduler* scheduler = new BoardScheduler(); - Buses* buses = new Buses(); - Sensors* sensors = - new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1)); - NASController* nas = - new NASController(scheduler->getScheduler(miosix::PRIORITY_MAX)); - ADAController* ada = - new ADAController(scheduler->getScheduler(miosix::PRIORITY_MAX)); - Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2)); - TMRepository* tmRepo = new TMRepository(); - CanHandler* canHandler = - new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2)); - FlightModeManager* fmm = new FlightModeManager(); - Actuators* actuators = - new Actuators(scheduler->getScheduler(miosix::MAIN_PRIORITY)); - Deployment* dpl = new Deployment(); - PinHandler* pinHandler = new PinHandler(); - AltitudeTrigger* altitudeTrigger = - new AltitudeTrigger(scheduler->getScheduler(miosix::PRIORITY_MAX)); - MEAController* mea = - new MEAController(scheduler->getScheduler(miosix::PRIORITY_MAX)); - ABKController* abk = - new ABKController(scheduler->getScheduler(miosix::PRIORITY_MAX)); - FlightStatsRecorder* recorder = - new FlightStatsRecorder(scheduler->getScheduler(miosix::MAIN_PRIORITY)); - - // Insert modules - if (!modules.insert<BoardScheduler>(scheduler)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the Board Scheduler module"); - } - - if (!modules.insert<Buses>(buses)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the Buses module"); - } - - if (!modules.insert<Sensors>(sensors)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the Sensor module"); - } - - if (!modules.insert<Actuators>(actuators)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the Actuators module"); - } - - if (!modules.insert<Deployment>(dpl)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the DPL module"); - } - - if (!modules.insert<NASController>(nas)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the NAS module"); - } - - if (!modules.insert<ADAController>(ada)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the ADA module"); - } - - if (!modules.insert<MEAController>(mea)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the MEA module"); - } - - if (!modules.insert<ABKController>(abk)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the ABK controller module"); - } - - if (!modules.insert<Radio>(radio)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the Radio module"); - } - - if (!modules.insert<FlightModeManager>(fmm)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the FMM module"); - } - - if (!modules.insert<TMRepository>(tmRepo)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the TMRepository module"); - } - - if (!modules.insert<CanHandler>(canHandler)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the CanHandler module"); - } - - if (!modules.insert<PinHandler>(pinHandler)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the PinHandler module"); - } - - if (!modules.insert<AltitudeTrigger>(altitudeTrigger)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the Altitude Trigger module"); - } - - if (!modules.insert<FlightStatsRecorder>(recorder)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the Flight Stats Recorder module"); - } - - // Start modules - if (!Logger::getInstance().start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the Logger module"); - } - - if (!EventBroker::getInstance().start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the EventBroker module"); - } - - if (!modules.get<BoardScheduler>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the Board Scheduler module"); - } - - if (!modules.get<Actuators>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the Actuators module"); - } - - if (!modules.get<Deployment>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the Deployment module"); - } - - if (!modules.get<Sensors>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the Sensors module"); - } - - if (!modules.get<NASController>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the NAS module"); - } - - if (!modules.get<ADAController>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the ADA module"); - } - - if (!modules.get<MEAController>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the MEA module"); - } - - if (!modules.get<ABKController>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the ABK controller module"); - } + ModuleManager &modules = ModuleManager::getInstance(); - if (!modules.get<FlightModeManager>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the FMM module"); - } + TaskScheduler scheduler(2); - if (!modules.get<Radio>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the Radio module"); - } + Buses *buses = new Buses(); + Sensors *sensors = new Sensors(scheduler); + Radio *radio = new Radio(); - if (!modules.get<CanHandler>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the CanHandler module"); - } + if(!modules.insert<Buses>(buses)) { - if (!modules.get<PinHandler>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the PinHandler module"); } - if (!modules.get<AltitudeTrigger>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the Altitude Trigger module"); - } + if(!modules.insert<Sensors>(sensors)) { - if (!modules.get<FlightStatsRecorder>()->start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the Flight Stats Recorder module"); } - // Log all the events - EventSniffer sniffer( - EventBroker::getInstance(), TOPICS_LIST, - [](uint8_t event, uint8_t topic) - { - EventData ev{TimestampTimer::getTimestamp(), event, topic}; - Logger::getInstance().log(ev); - }); - - // Check the init result and launch an event - if (initResult) - { - // Post OK - EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM); + sensors->start(); + radio->start(); + scheduler.start(); - // Set the LED status - miosix::led1On(); - } - else - { - EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM); - LOG_ERR(logger, "Failed to initialize"); + for(auto &info : sensors->getSensorInfo()) { + printf("Sensor: %s %d\n", info.id.c_str(), info.isInitialized); } - // Periodic statistics while (true) { Thread::sleep(1000); - Logger::getInstance().log(CpuMeter::getCpuStats()); - CpuMeter::resetCpuStats(); - StackLogger::getInstance().log(); } return 0; diff --git a/src/entrypoints/Main_old/main-entry.cpp b/src/entrypoints/Main_old/main-entry.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f005365af313df44c300a8b482fbad0f30e62c06 --- /dev/null +++ b/src/entrypoints/Main_old/main-entry.cpp @@ -0,0 +1,319 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * 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 <Main/Actuators/Actuators.h> +#include <Main/AltitudeTrigger/AltitudeTrigger.h> +#include <Main/BoardScheduler.h> +#include <Main/Buses.h> +#include <Main/CanHandler/CanHandler.h> +#include <Main/FlightStatsRecorder/FlightStatsRecorder.h> +#include <Main/PinHandler/PinHandler.h> +#include <Main/Radio/Radio.h> +#include <Main/Sensors/Sensors.h> +#include <Main/StateMachines/ABKController/ABKController.h> +#include <Main/StateMachines/ADAController/ADAController.h> +#include <Main/StateMachines/Deployment/Deployment.h> +#include <Main/StateMachines/FlightModeManager/FlightModeManager.h> +#include <Main/StateMachines/MEAController/MEAController.h> +#include <Main/StateMachines/NASController/NASController.h> +#include <Main/TMRepository/TMRepository.h> +#include <common/Events.h> +#include <common/Topics.h> +#include <diagnostic/CpuMeter/CpuMeter.h> +#include <diagnostic/PrintLogger.h> +#include <diagnostic/StackLogger.h> +#include <drivers/timer/TimestampTimer.h> +#include <events/EventBroker.h> +#include <events/EventData.h> +#include <events/utils/EventSniffer.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +using namespace Boardcore; +using namespace Main; +using namespace Common; + +int main() +{ + ModuleManager& modules = ModuleManager::getInstance(); + + // Overall status, if at some point it becomes false, there is a problem + // somewhere + bool initResult = true; + PrintLogger logger = Logging::getLogger("main"); + + // Create modules + BoardScheduler* scheduler = new BoardScheduler(); + Buses* buses = new Buses(); + Sensors* sensors = + new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1)); + NASController* nas = + new NASController(scheduler->getScheduler(miosix::PRIORITY_MAX)); + ADAController* ada = + new ADAController(scheduler->getScheduler(miosix::PRIORITY_MAX)); + Radio* radio = new Radio(scheduler->getScheduler(miosix::PRIORITY_MAX - 2)); + TMRepository* tmRepo = new TMRepository(); + CanHandler* canHandler = + new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2)); + FlightModeManager* fmm = new FlightModeManager(); + Actuators* actuators = + new Actuators(scheduler->getScheduler(miosix::MAIN_PRIORITY)); + Deployment* dpl = new Deployment(); + PinHandler* pinHandler = new PinHandler(); + AltitudeTrigger* altitudeTrigger = + new AltitudeTrigger(scheduler->getScheduler(miosix::PRIORITY_MAX)); + MEAController* mea = + new MEAController(scheduler->getScheduler(miosix::PRIORITY_MAX)); + ABKController* abk = + new ABKController(scheduler->getScheduler(miosix::PRIORITY_MAX)); + FlightStatsRecorder* recorder = + new FlightStatsRecorder(scheduler->getScheduler(miosix::MAIN_PRIORITY)); + + // Insert modules + if (!modules.insert<BoardScheduler>(scheduler)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Board Scheduler module"); + } + + if (!modules.insert<Buses>(buses)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Buses module"); + } + + if (!modules.insert<Sensors>(sensors)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Sensor module"); + } + + if (!modules.insert<Actuators>(actuators)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Actuators module"); + } + + if (!modules.insert<Deployment>(dpl)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the DPL module"); + } + + if (!modules.insert<NASController>(nas)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the NAS module"); + } + + if (!modules.insert<ADAController>(ada)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the ADA module"); + } + + if (!modules.insert<MEAController>(mea)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the MEA module"); + } + + if (!modules.insert<ABKController>(abk)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the ABK controller module"); + } + + if (!modules.insert<Radio>(radio)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Radio module"); + } + + if (!modules.insert<FlightModeManager>(fmm)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the FMM module"); + } + + if (!modules.insert<TMRepository>(tmRepo)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the TMRepository module"); + } + + if (!modules.insert<CanHandler>(canHandler)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the CanHandler module"); + } + + if (!modules.insert<PinHandler>(pinHandler)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the PinHandler module"); + } + + if (!modules.insert<AltitudeTrigger>(altitudeTrigger)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Altitude Trigger module"); + } + + if (!modules.insert<FlightStatsRecorder>(recorder)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the Flight Stats Recorder module"); + } + + // Start modules + if (!Logger::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Logger module"); + } + + if (!EventBroker::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the EventBroker module"); + } + + if (!modules.get<BoardScheduler>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Board Scheduler module"); + } + + if (!modules.get<Actuators>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Actuators module"); + } + + if (!modules.get<Deployment>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Deployment module"); + } + + if (!modules.get<Sensors>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Sensors module"); + } + + if (!modules.get<NASController>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the NAS module"); + } + + if (!modules.get<ADAController>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the ADA module"); + } + + if (!modules.get<MEAController>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the MEA module"); + } + + if (!modules.get<ABKController>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the ABK controller module"); + } + + if (!modules.get<FlightModeManager>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the FMM module"); + } + + if (!modules.get<Radio>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Radio module"); + } + + if (!modules.get<CanHandler>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the CanHandler module"); + } + + if (!modules.get<PinHandler>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the PinHandler module"); + } + + if (!modules.get<AltitudeTrigger>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Altitude Trigger module"); + } + + if (!modules.get<FlightStatsRecorder>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the Flight Stats Recorder module"); + } + + // Log all the events + EventSniffer sniffer( + EventBroker::getInstance(), TOPICS_LIST, + [](uint8_t event, uint8_t topic) + { + EventData ev{TimestampTimer::getTimestamp(), event, topic}; + Logger::getInstance().log(ev); + }); + + // Check the init result and launch an event + if (initResult) + { + // Post OK + EventBroker::getInstance().post(FMM_INIT_OK, TOPIC_FMM); + + // Set the LED status + miosix::led1On(); + } + else + { + EventBroker::getInstance().post(FMM_INIT_ERROR, TOPIC_FMM); + LOG_ERR(logger, "Failed to initialize"); + } + + // Periodic statistics + while (true) + { + Thread::sleep(1000); + Logger::getInstance().log(CpuMeter::getCpuStats()); + CpuMeter::resetCpuStats(); + StackLogger::getInstance().log(); + } + + return 0; +} \ No newline at end of file