diff --git a/CMakeLists.txt b/CMakeLists.txt index af7fb520a42518fdf2ffd12661669be1836707ba..1b31cb98642320cf129d5f48817efa4a2bd20cd5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,7 +60,7 @@ sbs_target(payload-entry-euroc stm32f767zi_death_stack_v4) add_executable(motor-entry src/entrypoints/Motor/motor-entry.cpp ${MOTOR_SOURCES}) target_include_directories(motor-entry PRIVATE ${OBSW_INCLUDE_DIRS}) -sbs_target(motor-entry stm32f767zi_gemini_motor) +sbs_target(motor-entry stm32f767zi_lyra_motor) add_executable(rig-entry src/entrypoints/RIG/rig-entry.cpp ${RIG_COMPUTER}) target_include_directories(rig-entry PRIVATE ${OBSW_INCLUDE_DIRS}) diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index b6d1c4caa7ef76f225bac38751994a2274d82192..c18e37c5e5dd2662f12ed506911b1c99e09c75b4 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -44,9 +44,9 @@ set(MAIN_COMPUTER ) set(MOTOR_SOURCES - src/boards/Motor/Actuators/Actuators.cpp + # src/boards/Motor/Actuators/Actuators.cpp src/boards/Motor/Sensors/Sensors.cpp - src/boards/Motor/BoardScheduler.cpp + # src/boards/Motor/BoardScheduler.cpp src/boards/Motor/CanHandler/CanHandler.cpp ) diff --git a/src/boards/Motor/Buses.h b/src/boards/Motor/Buses.h index 899fde6bbc0cf07e07fff4fc0588414ab5aef839..41b06f7c2fff68f5ea1d1c0a438d3cf50f658559 100644 --- a/src/boards/Motor/Buses.h +++ b/src/boards/Motor/Buses.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Alberto Nidasio +/* 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 @@ -24,7 +24,6 @@ #include <drivers/spi/SPIBus.h> #include <drivers/usart/USART.h> -#include <miosix.h> #include <utils/ModuleManager/ModuleManager.hpp> @@ -34,13 +33,22 @@ namespace Motor class Buses : public Boardcore::Module { public: - Boardcore::SPIBus spi1; - Boardcore::SPIBus spi3; - Boardcore::SPIBus spi4; + Buses() {} - Boardcore::USART usart2; + Boardcore::SPIBus &getH3LIS331DL() { return spi1; } + Boardcore::SPIBus &getLPS22DF() { return spi1; } + Boardcore::SPIBus &getLIS2MDL() { return spi3; } + Boardcore::SPIBus &getLSM6DSRX() { return spi3; } + Boardcore::SPIBus &getADS131M08() { return spi4; } - Buses() : spi1(SPI1), spi3(SPI3), spi4(SPI4), usart2(USART2, 115200) {} + Boardcore::USART &getHILUart() { return usart4; } + +private: + Boardcore::SPIBus spi1{SPI1}; + Boardcore::SPIBus spi3{SPI3}; + Boardcore::SPIBus spi4{SPI4}; + + Boardcore::USART usart4{UART4, 460800, 1024}; }; -} // namespace Motor \ No newline at end of file +} \ No newline at end of file diff --git a/src/boards/Motor/CanHandler/CanHandler.cpp b/src/boards/Motor/CanHandler/CanHandler.cpp index 0ef05206248bcfc0996d8ac70e146d2d01a5a05a..0cda12469670620d2ac4ba7c5212974af552837c 100644 --- a/src/boards/Motor/CanHandler/CanHandler.cpp +++ b/src/boards/Motor/CanHandler/CanHandler.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Authors: Federico Mandelli, Alberto Nidasio, 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 @@ -22,243 +22,48 @@ #include "CanHandler.h" -#include <Motor/Actuators/Actuators.h> -#include <Motor/Configs/CanHandlerConfig.h> -#include <Motor/Sensors/Sensors.h> #include <common/CanConfig.h> -#include <common/Events.h> -#include <functional> - -using namespace std; -using namespace placeholders; +using namespace Motor; using namespace Boardcore; -using namespace Canbus; +using namespace Boardcore::Canbus; using namespace Common; -using namespace CanConfig; -using namespace Motor::CanHandlerConfig; - -namespace Motor -{ -CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched) +CanHandler::CanHandler() { - // Configure the CAN driver CanbusDriver::AutoBitTiming bitTiming; - bitTiming.baudRate = BAUD_RATE; - bitTiming.samplePoint = SAMPLE_POINT; - CanbusDriver::CanbusConfig config; - - driver = new CanbusDriver(CAN2, config, bitTiming); - - // Create the protocol with the defined driver - protocol = - new CanProtocol(driver, bind(&CanHandler::handleCanMessage, this, _1), - miosix::PRIORITY_MAX - 1); - - // Accept messages only from the main and RIG board - protocol->addFilter(static_cast<uint8_t>(Board::MAIN), - static_cast<uint8_t>(Board::BROADCAST)); - protocol->addFilter(static_cast<uint8_t>(Board::RIG), - static_cast<uint8_t>(Board::BROADCAST)); - driver->init(); -} - -bool CanHandler::start() -{ - // 0 if fail - auto result1 = scheduler->addTask( - [&]() - { - auto sensors = ModuleManager::getInstance().get<Sensors>(); - - protocol->enqueueData(static_cast<uint8_t>(Priority::HIGH), - static_cast<uint8_t>(PrimaryType::SENSORS), - static_cast<uint8_t>(Board::MOTOR), - static_cast<uint8_t>(Board::BROADCAST), - static_cast<uint8_t>(SensorId::CC_PRESSURE), - static_cast<PressureData>( - sensors->getChamberPressureSensorData())); - - protocol->enqueueData( - static_cast<uint8_t>(Priority::HIGH), - static_cast<uint8_t>(PrimaryType::SENSORS), - static_cast<uint8_t>(Board::MOTOR), - static_cast<uint8_t>(Board::BROADCAST), - static_cast<uint8_t>(SensorId::BOTTOM_TANK_PRESSURE), - static_cast<PressureData>( - sensors->getTankPressureSensor1Data())); - - protocol->enqueueData( - static_cast<uint8_t>(Priority::HIGH), - static_cast<uint8_t>(PrimaryType::SENSORS), - static_cast<uint8_t>(Board::MOTOR), - static_cast<uint8_t>(Board::BROADCAST), - static_cast<uint8_t>(SensorId::TOP_TANK_PRESSURE), - static_cast<PressureData>( - sensors->getTankPressureSensor2Data())); - }, - PRESSURES_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER); - - auto result2 = scheduler->addTask( - [&]() - { - auto sensors = ModuleManager::getInstance().get<Sensors>(); - - protocol->enqueueData( - static_cast<uint8_t>(Priority::MEDIUM), - static_cast<uint8_t>(PrimaryType::SENSORS), - static_cast<uint8_t>(Board::MOTOR), - static_cast<uint8_t>(Board::BROADCAST), - static_cast<uint8_t>(SensorId::TANK_TEMPERATURE), - static_cast<TemperatureData>(sensors->getMAX31856Data())); - - protocol->enqueueData( - static_cast<uint8_t>(Priority::MEDIUM), - static_cast<uint8_t>(PrimaryType::SENSORS), - static_cast<uint8_t>(Board::MOTOR), - static_cast<uint8_t>(Board::BROADCAST), - static_cast<uint8_t>(SensorId::MOTOR_ACTUATORS_CURRENT), - static_cast<CurrentData>(sensors->getServoCurrentData())); - - protocol->enqueueData( - static_cast<uint8_t>(Priority::MEDIUM), - static_cast<uint8_t>(PrimaryType::SENSORS), - static_cast<uint8_t>(Board::MOTOR), - static_cast<uint8_t>(Board::BROADCAST), - static_cast<uint8_t>(SensorId::MOTOR_BOARD_VOLTAGE), - static_cast<BatteryVoltageSensorData>( - sensors->getBatteryData())); - }, - TEMPERATURE_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER); + bitTiming.baudRate = CanConfig::BAUD_RATE; + bitTiming.samplePoint = CanConfig::SAMPLE_POINT; - auto result3 = scheduler->addTask( - [&]() - { - protocol->enqueueSimplePacket( - static_cast<uint8_t>(Priority::MEDIUM), - static_cast<uint8_t>(PrimaryType::STATUS), - static_cast<uint8_t>(Board::MOTOR), - static_cast<uint8_t>(Board::BROADCAST), initStatus, initStatus); - }, - STATUS_TRANSMISSION_PERIOD); + CanbusDriver::CanbusConfig config; - auto result4 = scheduler->addTask( - [&]() - { - ModuleManager &modules = ModuleManager::getInstance(); + driver = std::make_unique<CanbusDriver>(CAN1, config, bitTiming); - protocol->enqueueData( - static_cast<uint8_t>(Priority::HIGH), - static_cast<uint8_t>(PrimaryType::ACTUATORS), - static_cast<uint8_t>(Board::MOTOR), - static_cast<uint8_t>(Board::BROADCAST), - static_cast<uint8_t>(ServosList::MAIN_VALVE), - ServoData{TimestampTimer::getTimestamp(), 0, 0, - modules.get<Actuators>()->getServoPosition( - ServosList::MAIN_VALVE)}); + protocol = std::make_unique<CanProtocol>( + driver.get(), + [this](const Canbus::CanMessage &msg) { handleCanMessage(msg); }, 3); - protocol->enqueueData( - static_cast<uint8_t>(Priority::HIGH), - static_cast<uint8_t>(PrimaryType::ACTUATORS), - static_cast<uint8_t>(Board::MOTOR), - static_cast<uint8_t>(Board::BROADCAST), - static_cast<uint8_t>(ServosList::VENTING_VALVE), - ServoData{TimestampTimer::getTimestamp(), 0, 0, - modules.get<Actuators>()->getServoPosition( - ServosList::VENTING_VALVE)}); - }, - ACTUATORS_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER); + protocol->addFilter(static_cast<uint8_t>(CanConfig::Board::RIG), + static_cast<uint8_t>(CanConfig::Board::BROADCAST)); + protocol->addFilter(static_cast<uint8_t>(CanConfig::Board::MAIN), + static_cast<uint8_t>(CanConfig::Board::BROADCAST)); - return protocol->start() && result1 != 0 && result2 != 0 && result3 != 0 && - result4 != 0; + driver->init(); } -bool CanHandler::isStarted() -{ - return protocol->isStarted() && scheduler->isRunning(); -} +bool CanHandler::start() { return protocol->start(); } -void CanHandler::sendEvent(EventId event) +void CanHandler::sendEvent(Common::CanConfig::EventId event) { - protocol->enqueueEvent(static_cast<uint8_t>(Priority::CRITICAL), - static_cast<uint8_t>(PrimaryType::EVENTS), - static_cast<uint8_t>(Board::MOTOR), - static_cast<uint8_t>(Board::BROADCAST), + protocol->enqueueEvent(static_cast<uint8_t>(CanConfig::Priority::CRITICAL), + static_cast<uint8_t>(CanConfig::PrimaryType::EVENTS), + static_cast<uint8_t>(CanConfig::Board::MAIN), + static_cast<uint8_t>(CanConfig::Board::BROADCAST), static_cast<uint8_t>(event)); } -void CanHandler::setInitStatus(bool initResult) { initStatus = initResult; } - -void CanHandler::handleCanMessage(const CanMessage &msg) +void CanHandler::handleCanMessage(const Canbus::CanMessage &msg) { - PrimaryType msgType = static_cast<PrimaryType>(msg.getPrimaryType()); - - // Depending on the received message, call the handling method - switch (msgType) - { - case PrimaryType::EVENTS: - { - handleCanEvent(msg); - break; - } - case PrimaryType::COMMAND: - { - handleCanCommand(msg); - break; - } - default: - { - LOG_WARN(logger, "Received unsupported message type: type={}", - msgType); - break; - } - } + LOG_INFO(logger, "Received can message {}", msg.getPrimaryType(), + msg.getSecondaryType()); } - -void CanHandler::handleCanEvent(const CanMessage &msg) -{ - EventId eventId = static_cast<EventId>(msg.getSecondaryType()); - - switch (eventId) - { - case EventId::CALIBRATE: - { - auto sensors = ModuleManager::getInstance().get<Sensors>(); - sensors->calibrate(); - - break; - } - default: - { - LOG_WARN(logger, "Received unsupported event: id={}", eventId); - break; - } - } -} - -void CanHandler::handleCanCommand(const CanMessage &msg) -{ - uint64_t payload = msg.payload[0]; - ServosList servo = static_cast<ServosList>(msg.getSecondaryType()); - bool targetState = static_cast<uint8_t>(payload); - uint32_t delay = static_cast<uint32_t>(payload >> 8); - - auto actuators = ModuleManager::getInstance().get<Actuators>(); - - if (actuators != nullptr) - { - if (targetState) - { - // printf("Opening %d for %ld\n", msg.getSecondaryType(), delay); - actuators->openServoAtomic(servo, delay); - } - else - { - // printf("Closing %d for %ld\n", msg.getSecondaryType(), delay); - actuators->closeServo(servo); - } - } -} - -} // namespace Motor diff --git a/src/boards/Motor/CanHandler/CanHandler.h b/src/boards/Motor/CanHandler/CanHandler.h index 1ccc2b90a4c43e77250b634882491a866f71e2b5..309f169b818ac218e669b5a66280609f008f6e72 100644 --- a/src/boards/Motor/CanHandler/CanHandler.h +++ b/src/boards/Motor/CanHandler/CanHandler.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Authors: Federico Mandelli, Alberto Nidasio +/* 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 @@ -22,9 +22,8 @@ #pragma once -#include <common/CanConfig.h> #include <drivers/canbus/CanProtocol/CanProtocol.h> -#include <scheduler/TaskScheduler.h> +#include <common/CanConfig.h> #include <utils/ModuleManager/ModuleManager.hpp> @@ -33,52 +32,20 @@ namespace Motor class CanHandler : public Boardcore::Module { - public: - explicit CanHandler(Boardcore::TaskScheduler *sched); + CanHandler(); - /** - * @brief Adds the periodic task to the scheduler and starts the protocol - * threads - */ bool start(); - /** - * @brief Returns true if the protocol threads are started and the scheduler - * is running - */ - bool isStarted(); - - /** - * @brief Sends a CAN event on the bus - */ void sendEvent(Common::CanConfig::EventId event); - /** - * @brief Set the initialization flag to true - */ - void setInitStatus(bool initResult); - private: - /** - * @brief Handles a generic CAN message and dispatch the message to the - * correct handler - */ void handleCanMessage(const Boardcore::Canbus::CanMessage &msg); - // CAN message handlers - void handleCanEvent(const Boardcore::Canbus::CanMessage &msg); - void handleCanCommand(const Boardcore::Canbus::CanMessage &msg); - - // Init status - std::atomic<bool> initStatus{false}; - - // CAN interfaces - Boardcore::Canbus::CanbusDriver *driver; - Boardcore::Canbus::CanProtocol *protocol; + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("CanHandler"); - Boardcore::TaskScheduler *scheduler; - Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler"); + std::unique_ptr<Boardcore::Canbus::CanbusDriver> driver; + std::unique_ptr<Boardcore::Canbus::CanProtocol> protocol; }; -} // namespace Motor +} // namespace Main diff --git a/src/boards/Motor/Configs/SensorsConfig.h b/src/boards/Motor/Configs/SensorsConfig.h index 44b13b762ad92deb64f7870ab34d8a1fdac827d0..1166dc6612e4d47d84f544c37ab091b883d7a895 100644 --- a/src/boards/Motor/Configs/SensorsConfig.h +++ b/src/boards/Motor/Configs/SensorsConfig.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Authors: Alberto Nidasio +/* 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 @@ -22,178 +22,78 @@ #pragma once -#include <Motor/Sensors/Sensors.h> +#include <sensors/ADS131M08/ADS131M08.h> +#include <sensors/H3LIS331DL/H3LIS331DL.h> +#include <sensors/LIS2MDL/LIS2MDL.h> +#include <sensors/LPS22DF/LPS22DF.h> +#include <sensors/LSM6DSRX/LSM6DSRX.h> namespace Motor { -namespace SensorsConfig +namespace Config { -constexpr Boardcore::InternalADC::Channel ADC_BATTERY_VOLTAGE_CH = - Boardcore::InternalADC::Channel::CH15; -// The battery is connected to the stm32's adc with a voltage divider -constexpr float ADC_BATTERY_VOLTAGE_COEFF = (10 + 20) / 10; - -// Boardcore::LSM6DSRXConfig LSM6_SENSOR_CONFIG{ -// .bdu = Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE, -// .odrAcc = Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_1660, -// .opModeAcc = Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL, -// .fsAcc = Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16, -// .odrGyr = Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_1660, -// .opModeGyr = Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL, -// .fsGyr = Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_1000, -// .fifoMode = Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS, -// .fifoTimestampDecimation = -// Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1, -// .fifoTemperatureBdr = -// Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED, -// .int1InterruptSelection = Boardcore::LSM6DSRXConfig::INTERRUPT::NOTHING, -// .int2InterruptSelection = -// Boardcore::LSM6DSRXConfig::INTERRUPT::FIFO_THRESHOLD, -// .fifoWatermark = 170, -// }; -// Boardcore::SPIBusConfig LSM6_SPI_CONFIG{ -// Boardcore::SPI::ClockDivider::DIV_16, -// Boardcore::SPI::Mode::MODE_0, -// }; - -Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS_ODR = - Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_1000; -Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS_BDU = - Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE; -Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS_FSR = +namespace Sensors +{ + +namespace LPS22DF +{ +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 + +namespace H3LIS331DL +{ +static constexpr Boardcore::H3LIS331DLDefs::OutputDataRate ODR = + Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_400; +static constexpr Boardcore::H3LIS331DLDefs::FullScaleRange FS = Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100; -Boardcore::SPIBusConfig H3LIS_SPI_CONFIG{ - Boardcore::SPI::ClockDivider::DIV_16, -}; - -Boardcore::LIS2MDL::Config LIS2_SENSOR_CONFIG{ - .odr = Boardcore::LIS2MDL::ODR_100_HZ, - .deviceMode = Boardcore::LIS2MDL::MD_CONTINUOUS, - .temperatureDivider = 100, -}; -Boardcore::SPIBusConfig LIS2_SPI_CONFIG{ - Boardcore::SPI::ClockDivider::DIV_16, - Boardcore::SPI::Mode::MODE_3, - Boardcore::SPI::Order::MSB_FIRST, - Boardcore::SPI::Order::LSB_FIRST, -}; - -Boardcore::LPS22DF::Config LPS22_SENSOR_CONFIG{ - .odr = Boardcore::LPS22DF::ODR_100, - .avg = Boardcore::LPS22DF::AVG_512, -}; -Boardcore::SPIBusConfig LPS22_SPI_CONFIG{ - Boardcore::SPI::ClockDivider::DIV_16, - Boardcore::SPI::Mode::MODE_3, - Boardcore::SPI::Order::MSB_FIRST, - Boardcore::SPI::Order::LSB_FIRST, -}; - -Boardcore::ADS131M08::Config ADS131_SENSOR_CONFIG{ - .channelsConfig = - { - {.enabled = false}, - // For the servo motor's current sensor the current range is from 0A - // to 20A and the output voltage from 0.65V to 2.65V. The sensor is - // connected to the adc through a voltage divider with a gain - // of 4.166666 So at the ADC side we have maximum 0.636 and we have - // to set a gain of 1 on the ADC - { - .enabled = true, - .pga = Boardcore::ADS131M08Defs::PGA::PGA_1, - }, - {.enabled = false}, - {.enabled = false}, - {.enabled = false}, - // The analog pressure sensors are measured with a 30ohm shunt - // resistor. The current ranges from 4mAh to 20mAh. So we can have a - // voltage up to 0.6V We set a gain of 2 to maximize resolution - { - .enabled = true, - .pga = Boardcore::ADS131M08Defs::PGA::PGA_2, - }, - { - .enabled = true, - .pga = Boardcore::ADS131M08Defs::PGA::PGA_2, - }, - { - .enabled = true, - .pga = Boardcore::ADS131M08Defs::PGA::PGA_2, - }, - }, - .oversamplingRatio = Boardcore::ADS131M08Defs::OversamplingRatio::OSR_4096, - .globalChopModeEnabled = true, -}; -Boardcore::SPIBusConfig ADS131_SPI_CONFIG{ - Boardcore::SPI::ClockDivider::DIV_8, - Boardcore::SPI::Mode::MODE_1, -}; -Boardcore::ADS131M08Defs::Channel ADS131_CHAMBER_PRESSURE_CH = - Boardcore::ADS131M08Defs::Channel::CHANNEL_5; -Boardcore::ADS131M08Defs::Channel ADS131_TANK_PRESSURE_1_CH = - Boardcore::ADS131M08Defs::Channel::CHANNEL_6; -Boardcore::ADS131M08Defs::Channel ADS131_TANK_PRESSURE_2_CH = - Boardcore::ADS131M08Defs::Channel::CHANNEL_7; -Boardcore::ADS131M08Defs::Channel ADS131_SERVO_CURRENT_CH = - Boardcore::ADS131M08Defs::Channel::CHANNEL_1; - -// The chamber pressure sensor is a 0-40bar sensor with a 4-20mA output -// On the motor motherboard there a 30ohm shunt. The shunt then goes through a -// filter with 491Hz of bandwidth and unitary gain. -// We use only a gain coefficient because the offset is removed by the ADC -// offset calibration feature. -constexpr float CHAMBER_PRESSURE_MAX = 40; -constexpr float CHAMBER_PRESSURE_MIN = 0; -constexpr float CHAMBER_PRESSURE_SHUNT = 30; // [ohm] -constexpr float CHAMBER_PRESSURE_CURR_MIN = 0.004; // [A] -constexpr float CHAMBER_PRESSURE_CURR_MAX = 0.020; // [A] -constexpr float CHAMBER_PRESSURE_COEFF = - CHAMBER_PRESSURE_MAX / - (CHAMBER_PRESSURE_CURR_MAX - CHAMBER_PRESSURE_CURR_MIN); // [bar/A] - -// The tank pressure sensors are 0-100bar sensors with a 4-20mA output -// On the motor motherboard there a 30ohm shunt. The shunt then goes through a -// filter with 491Hz of bandwidth and unitary gain. -// We use only a gain coefficient because the offset is removed by the ADC -// offset calibration feature. -constexpr float TANK_PRESSURE_1_MAX = 100; -constexpr float TANK_PRESSURE_1_MIN = 0; -constexpr float TANK_PRESSURE_1_SHUNT = 30; // [ohm] -constexpr float TANK_PRESSURE_1_CURR_MIN = 0.004; // [A] -constexpr float TANK_PRESSURE_1_CURR_MAX = 0.020; // [A] -constexpr float TANK_PRESSURE_1_COEFF = - TANK_PRESSURE_1_MAX / - (TANK_PRESSURE_1_CURR_MAX - TANK_PRESSURE_1_CURR_MIN); // [bar/A] -constexpr float TANK_PRESSURE_2_MAX = 100; -constexpr float TANK_PRESSURE_2_MIN = 0; -constexpr float TANK_PRESSURE_2_SHUNT = 30; // [ohm] -constexpr float TANK_PRESSURE_2_CURR_MIN = 0.004; // [A] -constexpr float TANK_PRESSURE_2_CURR_MAX = 0.020; // [A] -constexpr float TANK_PRESSURE_2_COEFF = - TANK_PRESSURE_2_MAX / - (TANK_PRESSURE_2_CURR_MAX - TANK_PRESSURE_2_CURR_MIN); // [bar/A] - -// The current sensor used to measure the servo motors current consumption: -// - A range of 0A to 20A -// - Has an output from 0.65V to 2.65V -// - Is connected to a voltage divider with a coefficient of 12 / (38.3 + 12) -// -// We use only a gain coefficient because the offset is removed by the ADC -// offset calibration feature. -constexpr float SERVO_CURRENT_COEFF = - 10 / (2.65 - 0.65) / (12 / (38.3 + 12)); // [A/V] - -// Sampling periods -constexpr uint32_t SAMPLE_PERIOD_ADC = 1000; -constexpr uint32_t SAMPLE_PERIOD_LSM6 = 10; -constexpr uint32_t SAMPLE_PERIOD_H3LIS = 100; -constexpr uint32_t SAMPLE_PERIOD_LIS2 = 10; -constexpr uint32_t SAMPLE_PERIOD_LPS22 = 10; -constexpr uint32_t SAMPLE_PERIOD_ADS131 = 1; -constexpr uint32_t SAMPLE_PERIOD_MAX = 10; - -} // namespace SensorsConfig - -} // namespace Motor \ No newline at end of file +static constexpr uint32_t PERIOD = 10; // [ms] 100Hz +} // namespace H3LIS331DL + +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 + +namespace LSM6DSRX +{ +static constexpr Boardcore::LSM6DSRXConfig::ACC_FULLSCALE ACC_FS = + Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16; +static constexpr Boardcore::LSM6DSRXConfig::ACC_ODR ACC_ODR = + Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_416; +static constexpr Boardcore::LSM6DSRXConfig::OPERATING_MODE ACC_OP_MODE = + Boardcore::LSM6DSRXConfig::OPERATING_MODE::HIGH_PERFORMANCE; + +static constexpr Boardcore::LSM6DSRXConfig::GYR_FULLSCALE GYR_FS = + Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_4000; +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 + +} // namespace Motor diff --git a/src/boards/Motor/Sensors/Sensors.cpp b/src/boards/Motor/Sensors/Sensors.cpp index 0643a6865d1b0d96f9d7ced1d301689efc21f478..e68a6f563ef578cbd300057f06e5ed00d5738e7a 100644 --- a/src/boards/Motor/Sensors/Sensors.cpp +++ b/src/boards/Motor/Sensors/Sensors.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2022 Skyward Experimental Rocketry - * Authors: Luca Erbetta, Luca Conterio, 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 @@ -24,404 +24,183 @@ #include <Motor/Buses.h> #include <Motor/Configs/SensorsConfig.h> -#include <drivers/interrupt/external_interrupts.h> #include <interfaces-impl/hwmapping.h> -#include <logger/Logger.h> -using namespace std; -using namespace miosix; -using namespace Boardcore; -using namespace Motor::SensorsConfig; - -namespace Motor -{ - -InternalADCData Sensors::getADCData() -{ - miosix::PauseKernelLock lock; - return adc != nullptr ? adc->getLastSample() : InternalADCData{}; -} - -BatteryVoltageSensorData Sensors::getBatteryData() -{ - miosix::PauseKernelLock lock; - return battery != nullptr ? battery->getLastSample() - : BatteryVoltageSensorData{}; -} - -// LSM6DSRXData Sensors::getLSM6DSRXData() -// { -// miosix::PauseKernelLock lock; -// return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{}; -// } - -H3LIS331DLData Sensors::getH3LIS331DLData() -{ - miosix::PauseKernelLock lock; - return h3lis331dl != nullptr ? h3lis331dl->getLastSample() - : H3LIS331DLData{}; -} +#include <utils/ModuleManager/ModuleManager.hpp> -LIS2MDLData Sensors::getLIS2MDLData() -{ - miosix::PauseKernelLock lock; - return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{}; -} - -LPS22DFData Sensors::getLPS22DFData() -{ - miosix::PauseKernelLock lock; - return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{}; -} - -ADS131M08Data Sensors::getADS131M08Data() -{ - miosix::PauseKernelLock lock; - return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{}; -} - -MAX31856Data Sensors::getMAX31856Data() -{ - miosix::PauseKernelLock lock; - return max31856 != nullptr ? max31856->getLastSample() : MAX31856Data{}; -} - -ChamberPressureSensorData Sensors::getChamberPressureSensorData() -{ - miosix::PauseKernelLock lock; - return chamberPressure != nullptr ? chamberPressure->getLastSample() - : ChamberPressureSensorData{}; -} - -TankPressureSensor1Data Sensors::getTankPressureSensor1Data() -{ - miosix::PauseKernelLock lock; - return tankPressure1 != nullptr ? tankPressure1->getLastSample() - : TankPressureSensor1Data{}; -} - -TankPressureSensor2Data Sensors::getTankPressureSensor2Data() -{ - miosix::PauseKernelLock lock; - return tankPressure2 != nullptr ? tankPressure2->getLastSample() - : TankPressureSensor2Data{}; -} - -CurrentData Sensors::getServoCurrentData() -{ - miosix::PauseKernelLock lock; - return servosCurrent != nullptr ? servosCurrent->getLastSample() - : CurrentData{}; -} - -Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {} +using namespace Motor; +using namespace Boardcore; +using namespace miosix; -Sensors::~Sensors() {} +bool Sensors::isStarted() { return started; } bool Sensors::start() { - adcInit(); - batteryInit(); - // lsm6dsrxInit(); - h3lis331dlInit(); - // lis2mdlInit(); - lps22dfInit(); - max31856Init(); - ads131m08Init(); - chamberPressureInit(); - tankPressure1Init(); - tankPressure2Init(); - servosCurrentInit(); - - sensorManager = new SensorManager(sensorsMap, scheduler); - - return sensorManager->start(); -} + SensorManager::SensorMap_t map; + lps22dfInit(map); + h3lis331dlInit(map); + lis2mdlInit(map); + lsm6dsrxInit(map); + ads131m08Init(map); + internalAdcInit(map); -void Sensors::calibrate() -{ - if (ads131m08 != nullptr) + manager = std::make_unique<SensorManager>(map, &scheduler); + if (!manager->start()) { - ads131m08->calibrateOffset(ADS131_SERVO_CURRENT_CH); + return false; } -} - -void Sensors::adcInit() -{ - adc = new InternalADC(ADC1); - - adc->enableTemperature(); - adc->enableVbat(); - adc->enableChannel(ADC_BATTERY_VOLTAGE_CH); - SensorInfo info("ADC", SAMPLE_PERIOD_ADC, - bind(&Sensors::adcCallback, this)); - - sensorsMap.emplace(make_pair(adc, info)); + started = true; + return true; } -void Sensors::batteryInit() +std::vector<SensorInfo> Sensors::getSensorInfo() { - function<ADCData()> getADCVoltage( - bind(&InternalADC::getVoltage, adc, ADC_BATTERY_VOLTAGE_CH)); - - battery = - new BatteryVoltageSensor(getADCVoltage, ADC_BATTERY_VOLTAGE_COEFF); - - SensorInfo info("BATTERY", SAMPLE_PERIOD_ADC, - bind(&Sensors::batteryCallback, this)); - - sensorsMap.emplace(make_pair(battery, info)); + return {manager->getSensorInfo(lps22df.get()), + manager->getSensorInfo(h3lis331dl.get()), + manager->getSensorInfo(lis2mdl.get()), + manager->getSensorInfo(lsm6dsrx.get()), + manager->getSensorInfo(ads131m08.get()), + manager->getSensorInfo(internalAdc.get())}; } -// void Sensors::lsm6dsrxInit() -// { -// SPIBus& spi1 = ModuleManager::getInstance().get<Buses>()->spi1; - -// lsm6dsrx = new LSM6DSRX(spi1, peripherals::lsm6dsrx::cs::getPin(), -// LSM6_SPI_CONFIG, LSM6_SENSOR_CONFIG); - -// SensorInfo info("LSM6DSRX", SAMPLE_PERIOD_LSM6, -// bind(&Sensors::lsm6dsrxCallback, this)); - -// sensorsMap.emplace(make_pair(lsm6dsrx, info)); -// } - -void Sensors::h3lis331dlInit() +void Sensors::lps22dfInit(SensorManager::SensorMap_t &map) { - SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3; + ModuleManager &modules = ModuleManager::getInstance(); - h3lis331dl = new H3LIS331DL(spi3, peripherals::h3lis331dl::cs::getPin(), - H3LIS_ODR, H3LIS_BDU, H3LIS_FSR); + SPIBusConfig spiConfig = LPS22DF::getDefaultSPIConfig(); + spiConfig.clockDivider = SPI::ClockDivider::DIV_16; - SensorInfo info("H3LIS331DL", SAMPLE_PERIOD_H3LIS, - bind(&Sensors::h3lis331dlCallback, this)); + LPS22DF::Config config; + config.avg = Config::Sensors::LPS22DF::AVG; + config.odr = Config::Sensors::LPS22DF::ODR; - sensorsMap.emplace(make_pair(h3lis331dl, info)); -} + lps22df = std::make_unique<LPS22DF>(modules.get<Buses>()->getLPS22DF(), + sensors::LPS22DF::cs::getPin(), + spiConfig, config); -void Sensors::lis2mdlInit() -{ - SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3; - - lis2mdl = new LIS2MDL(spi3, peripherals::lis2mdl::cs::getPin(), - LIS2_SPI_CONFIG, LIS2_SENSOR_CONFIG); - - SensorInfo info("LIS2MDL", SAMPLE_PERIOD_LIS2, - bind(&Sensors::lis2mdlCallback, this)); - - sensorsMap.emplace(make_pair(lis2mdl, info)); + SensorInfo info{"LPS22DF", Config::Sensors::LPS22DF::PERIOD, + [this]() { lps22dfCallback(); }}; + map.emplace(lps22df.get(), info); } -void Sensors::lps22dfInit() -{ - SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3; - - lps22df = new LPS22DF(spi3, peripherals::lps22df::cs::getPin(), - LPS22_SPI_CONFIG, LPS22_SENSOR_CONFIG); - - SensorInfo info("LPS22", SAMPLE_PERIOD_LPS22, - bind(&Sensors::lps22dfCallback, this)); +void Sensors::lps22dfCallback() {} - sensorsMap.emplace(make_pair(lps22df, info)); -} - -void Sensors::max31856Init() +void Sensors::h3lis331dlInit(SensorManager::SensorMap_t &map) { - SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3; + ModuleManager &modules = ModuleManager::getInstance(); - max31856 = new MAX31856(spi3, peripherals::max31856::cs::getPin()); + SPIBusConfig spiConfig = H3LIS331DL::getDefaultSPIConfig(); + spiConfig.clockDivider = SPI::ClockDivider::DIV_16; - SensorInfo info("MAX31856", SAMPLE_PERIOD_MAX, - bind(&Sensors::max31856Callback, this)); + h3lis331dl = std::make_unique<H3LIS331DL>( + modules.get<Buses>()->getH3LIS331DL(), + sensors::H3LIS331DL::cs::getPin(), spiConfig, + Config::Sensors::H3LIS331DL::ODR, + H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE, + Config::Sensors::H3LIS331DL::FS); - sensorsMap.emplace(make_pair(max31856, info)); + SensorInfo info{"H3LIS331DL", Config::Sensors::H3LIS331DL::PERIOD, + [this]() { h3lis331dlCallback(); }}; + map.emplace(h3lis331dl.get(), info); } -void Sensors::ads131m08Init() -{ - SPIBus& spi4 = ModuleManager::getInstance().get<Buses>()->spi4; - - ads131m08 = new ADS131M08(spi4, peripherals::ads131m08::cs::getPin(), - ADS131_SPI_CONFIG, ADS131_SENSOR_CONFIG); - - SensorInfo info("ADS131M08", SAMPLE_PERIOD_ADS131, - bind(&Sensors::ads131m08Callback, this)); - - sensorsMap.emplace(make_pair(ads131m08, info)); -} +void Sensors::h3lis331dlCallback() {} -void Sensors::chamberPressureInit() +void Sensors::lis2mdlInit(SensorManager::SensorMap_t &map) { - if (ads131m08 == nullptr) - { - return; - } - - function<ADCData()> getADCVoltage( - [&](void) - { - auto data = ads131m08->getLastSample(); - return data.getVoltage(ADS131_CHAMBER_PRESSURE_CH); - }); - - function<float(float)> voltageToPressure( - [](float voltage) - { - float current = - voltage / CHAMBER_PRESSURE_SHUNT - CHAMBER_PRESSURE_CURR_MIN; - return current * CHAMBER_PRESSURE_COEFF; - }); - - chamberPressure = - new ChamberPressureSensor(getADCVoltage, voltageToPressure); - - SensorInfo info("CHAMBER_PRESSURE", SAMPLE_PERIOD_ADS131, - bind(&Sensors::chamberPressureCallback, this)); - - sensorsMap.emplace(make_pair(chamberPressure, info)); -} - -void Sensors::tankPressure1Init() -{ - if (ads131m08 == nullptr) - { - return; - } - - function<ADCData()> getADCVoltage( - [&](void) - { - auto data = ads131m08->getLastSample(); - return data.getVoltage(ADS131_TANK_PRESSURE_1_CH); - }); + ModuleManager &modules = ModuleManager::getInstance(); - function<float(float)> voltageToPressure( - [](float voltage) - { - float current = - voltage / TANK_PRESSURE_1_SHUNT - TANK_PRESSURE_1_CURR_MIN; - return current * TANK_PRESSURE_1_COEFF; - }); + SPIBusConfig spiConfig = H3LIS331DL::getDefaultSPIConfig(); + spiConfig.clockDivider = SPI::ClockDivider::DIV_16; - tankPressure1 = new TankPressureSensor1(getADCVoltage, voltageToPressure); + LIS2MDL::Config config; + config.deviceMode = LIS2MDL::MD_CONTINUOUS; + config.odr = Config::Sensors::LIS2MDL::ODR; + config.temperatureDivider = Config::Sensors::LIS2MDL::TEMP_DIVIDER; - SensorInfo info("TANK_PRESSURE_1", SAMPLE_PERIOD_ADS131, - bind(&Sensors::tankPressure1Callback, this)); + lis2mdl = std::make_unique<LIS2MDL>(modules.get<Buses>()->getLIS2MDL(), + sensors::LIS2MDL::cs::getPin(), + spiConfig, config); - sensorsMap.emplace(make_pair(tankPressure1, info)); + SensorInfo info{"LIS2MDL", Config::Sensors::LIS2MDL::PERIOD, + [this]() { lis2mdlCallback(); }}; + map.emplace(lis2mdl.get(), info); } -void Sensors::tankPressure2Init() -{ - if (ads131m08 == nullptr) - { - return; - } - - function<ADCData()> getADCVoltage( - [&](void) - { - auto data = ads131m08->getLastSample(); - return data.getVoltage(ADS131_TANK_PRESSURE_2_CH); - }); - - function<float(float)> voltageToPressure( - [](float voltage) - { - float current = - voltage / TANK_PRESSURE_2_SHUNT - TANK_PRESSURE_2_CURR_MIN; - return current * TANK_PRESSURE_2_COEFF; - }); - - tankPressure2 = new TankPressureSensor2(getADCVoltage, voltageToPressure); - - SensorInfo info("TANK_PRESSURE_2", SAMPLE_PERIOD_ADS131, - bind(&Sensors::tankPressure2Callback, this)); +void Sensors::lis2mdlCallback() {} - sensorsMap.emplace(make_pair(tankPressure2, info)); -} - -void Sensors::servosCurrentInit() +void Sensors::lsm6dsrxInit(SensorManager::SensorMap_t &map) { - if (ads131m08 == nullptr) - { - return; - } + ModuleManager &modules = ModuleManager::getInstance(); - function<ADCData()> getADCVoltage( - [&](void) - { - auto data = ads131m08->getLastSample(); - return data.getVoltage(ADS131_SERVO_CURRENT_CH); - }); + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_32; + spiConfig.mode = SPI::Mode::MODE_0; - function<float(float)> voltageToCurrent( - [](float voltage) { return voltage * SERVO_CURRENT_COEFF; }); + LSM6DSRXConfig config; + config.bdu = LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE; - servosCurrent = new CurrentSensor(getADCVoltage, voltageToCurrent); + config.fsAcc = Config::Sensors::LSM6DSRX::ACC_FS; + config.odrAcc = Config::Sensors::LSM6DSRX::ACC_ODR; + config.opModeAcc = Config::Sensors::LSM6DSRX::ACC_OP_MODE; - SensorInfo info("SERVOS_CURRENT", SAMPLE_PERIOD_ADS131, - bind(&Sensors::servosCurrentCallback, this)); + config.fsGyr = Config::Sensors::LSM6DSRX::GYR_FS; + config.odrGyr = Config::Sensors::LSM6DSRX::GYR_ODR; + config.opModeGyr = Config::Sensors::LSM6DSRX::GYR_OP_MODE; - sensorsMap.emplace(make_pair(servosCurrent, info)); -} + config.fifoMode = LSM6DSRXConfig::FIFO_MODE::CONTINUOUS; + config.fifoTimestampDecimation = + LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1; + config.fifoTemperatureBdr = LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED; -void Sensors::adcCallback() { Logger::getInstance().log(adc->getLastSample()); } + lsm6dsrx = std::make_unique<LSM6DSRX>(modules.get<Buses>()->getLSM6DSRX(), + sensors::LSM6DSRX::cs::getPin(), + spiConfig, config); -void Sensors::batteryCallback() -{ - Logger::getInstance().log(battery->getLastSample()); + SensorInfo info{"LSM6DSRX", Config::Sensors::LSM6DSRX::PERIOD, + [this]() { lsm6dsrxCallback(); }}; + map.emplace(lsm6dsrx.get(), info); } -// void Sensors::lsm6dsrxCallback() -// { -// Logger::getInstance().log(lsm6dsrx->getLastSample()); -// } +void Sensors::lsm6dsrxCallback() {} -void Sensors::h3lis331dlCallback() +void Sensors::ads131m08Init(SensorManager::SensorMap_t &map) { - Logger::getInstance().log(h3lis331dl->getLastSample()); -} + ModuleManager &modules = ModuleManager::getInstance(); -void Sensors::lis2mdlCallback() -{ - Logger::getInstance().log(lis2mdl->getLastSample()); -} + SPIBusConfig spiConfig; + spiConfig.clockDivider = SPI::ClockDivider::DIV_32; -void Sensors::lps22dfCallback() -{ - Logger::getInstance().log(lps22df->getLastSample()); -} + ADS131M08::Config config; + config.oversamplingRatio = Config::Sensors::ADS131M08::OSR; + config.globalChopModeEnabled = + Config::Sensors::ADS131M08::GLOBAL_CHOP_MODE_EN; -void Sensors::max31856Callback() -{ - Logger::getInstance().log(max31856->getLastSample()); -} + ads131m08 = std::make_unique<ADS131M08>( + modules.get<Buses>()->getADS131M08(), sensors::ADS131M08::cs::getPin(), + spiConfig, config); -void Sensors::ads131m08Callback() -{ - Logger::getInstance().log(ads131m08->getLastSample()); + SensorInfo info{"ADS131M08", Config::Sensors::ADS131M08::PERIOD, + [this]() { ads131m08Callback(); }}; + map.emplace(ads131m08.get(), info); } -void Sensors::chamberPressureCallback() -{ - Logger::getInstance().log(chamberPressure->getLastSample()); -} +void Sensors::ads131m08Callback() {} -void Sensors::tankPressure1Callback() +void Sensors::internalAdcInit(SensorManager::SensorMap_t &map) { - Logger::getInstance().log(tankPressure1->getLastSample()); -} + ModuleManager &modules = ModuleManager::getInstance(); -void Sensors::tankPressure2Callback() -{ - Logger::getInstance().log(tankPressure2->getLastSample()); -} + internalAdc = std::make_unique<InternalADC>(ADC2); + internalAdc->enableChannel(InternalADC::CH9); + internalAdc->enableChannel(InternalADC::CH14); + internalAdc->enableTemperature(); + internalAdc->enableVbat(); -void Sensors::servosCurrentCallback() -{ - Logger::getInstance().log(servosCurrent->getLastSample()); + SensorInfo info{"InternalADC", Config::Sensors::InternalADC::PERIOD, + [this]() { internalAdcCallback(); }}; + map.emplace(internalAdc.get(), info); } -} // namespace Motor \ No newline at end of file +void Sensors::internalAdcCallback() {} \ No newline at end of file diff --git a/src/boards/Motor/Sensors/Sensors.h b/src/boards/Motor/Sensors/Sensors.h index 780563efec9feef081b3258fed1fc4f66477854a..79013f223f2626d6ed6e8f51dcb54ffdf8bcd131 100644 --- a/src/boards/Motor/Sensors/Sensors.h +++ b/src/boards/Motor/Sensors/Sensors.h @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Alberto Nidasio +/* 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 @@ -22,22 +22,18 @@ #pragma once -#include <Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h> -#include <Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h> -#include <Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h> #include <drivers/adc/InternalADC.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/LSM6DSRX/LSM6DSRX.h> -#include <sensors/MAX31856/MAX31856.h> +#include <sensors/LSM6DSRX/LSM6DSRX.h> #include <sensors/SensorManager.h> -#include <sensors/analog/BatteryVoltageSensor.h> -#include <sensors/analog/CurrentSensor.h> -#include <sensors/analog/pressure/AnalogPressureSensor.h> +#include <memory> #include <utils/ModuleManager/ModuleManager.hpp> +#include <vector> namespace Motor { @@ -45,80 +41,50 @@ namespace Motor class Sensors : public Boardcore::Module { public: - Boardcore::InternalADCData getADCData(); - Boardcore::BatteryVoltageSensorData getBatteryData(); - // Boardcore::LSM6DSRXData getLSM6DSRXData(); - Boardcore::H3LIS331DLData getH3LIS331DLData(); - Boardcore::LIS2MDLData getLIS2MDLData(); - Boardcore::LPS22DFData getLPS22DFData(); - Boardcore::ADS131M08Data getADS131M08Data(); - Boardcore::MAX31856Data getMAX31856Data(); - Boardcore::ChamberPressureSensorData getChamberPressureSensorData(); - Boardcore::TankPressureSensor1Data getTankPressureSensor1Data(); - Boardcore::TankPressureSensor2Data getTankPressureSensor2Data(); - Boardcore::CurrentData getServoCurrentData(); + explicit Sensors(Boardcore::TaskScheduler &scheduler) : scheduler{scheduler} + { + } - explicit Sensors(Boardcore::TaskScheduler* sched); - ~Sensors(); + bool isStarted(); - bool start(); + [[nodiscard]] bool start(); - void calibrate(); + std::vector<Boardcore::SensorInfo> getSensorInfo(); private: - void adcInit(); - void adcCallback(); - - void batteryInit(); - void batteryCallback(); - - // void lsm6dsrxInit(); - // void lsm6dsrxCallback(); + void lps22dfInit(Boardcore::SensorManager::SensorMap_t &map); + void lps22dfCallback(); - void h3lis331dlInit(); + void h3lis331dlInit(Boardcore::SensorManager::SensorMap_t &map); void h3lis331dlCallback(); - void lis2mdlInit(); + void lis2mdlInit(Boardcore::SensorManager::SensorMap_t &map); void lis2mdlCallback(); - void lps22dfInit(); - void lps22dfCallback(); - - void max31856Init(); - void max31856Callback(); + void lsm6dsrxInit(Boardcore::SensorManager::SensorMap_t &map); + void lsm6dsrxCallback(); - void ads131m08Init(); + void ads131m08Init(Boardcore::SensorManager::SensorMap_t &map); void ads131m08Callback(); - void chamberPressureInit(); - void chamberPressureCallback(); - - void tankPressure1Init(); - void tankPressure1Callback(); - - void tankPressure2Init(); - void tankPressure2Callback(); - - void servosCurrentInit(); - void servosCurrentCallback(); - - Boardcore::InternalADC* adc = nullptr; - Boardcore::BatteryVoltageSensor* battery = nullptr; - // Boardcore::LSM6DSRX* lsm6dsrx = nullptr; - Boardcore::H3LIS331DL* h3lis331dl = nullptr; - Boardcore::LIS2MDL* lis2mdl = nullptr; - Boardcore::LPS22DF* lps22df = nullptr; - Boardcore::MAX31856* max31856 = nullptr; - Boardcore::ADS131M08* ads131m08 = nullptr; - Boardcore::ChamberPressureSensor* chamberPressure = nullptr; - Boardcore::TankPressureSensor1* tankPressure1 = nullptr; - Boardcore::TankPressureSensor2* tankPressure2 = nullptr; - Boardcore::CurrentSensor* servosCurrent = nullptr; - - Boardcore::SensorManager::SensorMap_t sensorsMap; - Boardcore::SensorManager* sensorManager = nullptr; - Boardcore::TaskScheduler* scheduler = nullptr; + 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::H3LIS331DL> h3lis331dl; + std::unique_ptr<Boardcore::LIS2MDL> lis2mdl; + 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 Motor \ No newline at end of file diff --git a/src/boards/Motor/Actuators/Actuators.cpp b/src/boards/Motor_old/Actuators/Actuators.cpp similarity index 100% rename from src/boards/Motor/Actuators/Actuators.cpp rename to src/boards/Motor_old/Actuators/Actuators.cpp diff --git a/src/boards/Motor/Actuators/Actuators.h b/src/boards/Motor_old/Actuators/Actuators.h similarity index 100% rename from src/boards/Motor/Actuators/Actuators.h rename to src/boards/Motor_old/Actuators/Actuators.h diff --git a/src/boards/Motor/Actuators/ActuatorsData.h b/src/boards/Motor_old/Actuators/ActuatorsData.h similarity index 100% rename from src/boards/Motor/Actuators/ActuatorsData.h rename to src/boards/Motor_old/Actuators/ActuatorsData.h diff --git a/src/boards/Motor/BoardScheduler.cpp b/src/boards/Motor_old/BoardScheduler.cpp similarity index 100% rename from src/boards/Motor/BoardScheduler.cpp rename to src/boards/Motor_old/BoardScheduler.cpp diff --git a/src/boards/Motor/BoardScheduler.h b/src/boards/Motor_old/BoardScheduler.h similarity index 100% rename from src/boards/Motor/BoardScheduler.h rename to src/boards/Motor_old/BoardScheduler.h diff --git a/src/boards/Motor_old/Buses.h b/src/boards/Motor_old/Buses.h new file mode 100644 index 0000000000000000000000000000000000000000..899fde6bbc0cf07e07fff4fc0588414ab5aef839 --- /dev/null +++ b/src/boards/Motor_old/Buses.h @@ -0,0 +1,46 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * 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/spi/SPIBus.h> +#include <drivers/usart/USART.h> +#include <miosix.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Motor +{ + +class Buses : public Boardcore::Module +{ +public: + Boardcore::SPIBus spi1; + Boardcore::SPIBus spi3; + Boardcore::SPIBus spi4; + + Boardcore::USART usart2; + + Buses() : spi1(SPI1), spi3(SPI3), spi4(SPI4), usart2(USART2, 115200) {} +}; + +} // namespace Motor \ No newline at end of file diff --git a/src/boards/Motor_old/CanHandler/CanHandler.cpp b/src/boards/Motor_old/CanHandler/CanHandler.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0ef05206248bcfc0996d8ac70e146d2d01a5a05a --- /dev/null +++ b/src/boards/Motor_old/CanHandler/CanHandler.cpp @@ -0,0 +1,264 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Federico Mandelli, Alberto Nidasio, 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 "CanHandler.h" + +#include <Motor/Actuators/Actuators.h> +#include <Motor/Configs/CanHandlerConfig.h> +#include <Motor/Sensors/Sensors.h> +#include <common/CanConfig.h> +#include <common/Events.h> + +#include <functional> + +using namespace std; +using namespace placeholders; +using namespace Boardcore; +using namespace Canbus; +using namespace Common; +using namespace CanConfig; +using namespace Motor::CanHandlerConfig; + +namespace Motor +{ + +CanHandler::CanHandler(TaskScheduler *sched) : scheduler(sched) +{ + // Configure the CAN driver + CanbusDriver::AutoBitTiming bitTiming; + bitTiming.baudRate = BAUD_RATE; + bitTiming.samplePoint = SAMPLE_POINT; + CanbusDriver::CanbusConfig config; + + driver = new CanbusDriver(CAN2, config, bitTiming); + + // Create the protocol with the defined driver + protocol = + new CanProtocol(driver, bind(&CanHandler::handleCanMessage, this, _1), + miosix::PRIORITY_MAX - 1); + + // Accept messages only from the main and RIG board + protocol->addFilter(static_cast<uint8_t>(Board::MAIN), + static_cast<uint8_t>(Board::BROADCAST)); + protocol->addFilter(static_cast<uint8_t>(Board::RIG), + static_cast<uint8_t>(Board::BROADCAST)); + driver->init(); +} + +bool CanHandler::start() +{ + // 0 if fail + auto result1 = scheduler->addTask( + [&]() + { + auto sensors = ModuleManager::getInstance().get<Sensors>(); + + protocol->enqueueData(static_cast<uint8_t>(Priority::HIGH), + static_cast<uint8_t>(PrimaryType::SENSORS), + static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(SensorId::CC_PRESSURE), + static_cast<PressureData>( + sensors->getChamberPressureSensorData())); + + protocol->enqueueData( + static_cast<uint8_t>(Priority::HIGH), + static_cast<uint8_t>(PrimaryType::SENSORS), + static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(SensorId::BOTTOM_TANK_PRESSURE), + static_cast<PressureData>( + sensors->getTankPressureSensor1Data())); + + protocol->enqueueData( + static_cast<uint8_t>(Priority::HIGH), + static_cast<uint8_t>(PrimaryType::SENSORS), + static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(SensorId::TOP_TANK_PRESSURE), + static_cast<PressureData>( + sensors->getTankPressureSensor2Data())); + }, + PRESSURES_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER); + + auto result2 = scheduler->addTask( + [&]() + { + auto sensors = ModuleManager::getInstance().get<Sensors>(); + + protocol->enqueueData( + static_cast<uint8_t>(Priority::MEDIUM), + static_cast<uint8_t>(PrimaryType::SENSORS), + static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(SensorId::TANK_TEMPERATURE), + static_cast<TemperatureData>(sensors->getMAX31856Data())); + + protocol->enqueueData( + static_cast<uint8_t>(Priority::MEDIUM), + static_cast<uint8_t>(PrimaryType::SENSORS), + static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(SensorId::MOTOR_ACTUATORS_CURRENT), + static_cast<CurrentData>(sensors->getServoCurrentData())); + + protocol->enqueueData( + static_cast<uint8_t>(Priority::MEDIUM), + static_cast<uint8_t>(PrimaryType::SENSORS), + static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(SensorId::MOTOR_BOARD_VOLTAGE), + static_cast<BatteryVoltageSensorData>( + sensors->getBatteryData())); + }, + TEMPERATURE_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER); + + auto result3 = scheduler->addTask( + [&]() + { + protocol->enqueueSimplePacket( + static_cast<uint8_t>(Priority::MEDIUM), + static_cast<uint8_t>(PrimaryType::STATUS), + static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST), initStatus, initStatus); + }, + STATUS_TRANSMISSION_PERIOD); + + auto result4 = scheduler->addTask( + [&]() + { + ModuleManager &modules = ModuleManager::getInstance(); + + protocol->enqueueData( + static_cast<uint8_t>(Priority::HIGH), + static_cast<uint8_t>(PrimaryType::ACTUATORS), + static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(ServosList::MAIN_VALVE), + ServoData{TimestampTimer::getTimestamp(), 0, 0, + modules.get<Actuators>()->getServoPosition( + ServosList::MAIN_VALVE)}); + + protocol->enqueueData( + static_cast<uint8_t>(Priority::HIGH), + static_cast<uint8_t>(PrimaryType::ACTUATORS), + static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(ServosList::VENTING_VALVE), + ServoData{TimestampTimer::getTimestamp(), 0, 0, + modules.get<Actuators>()->getServoPosition( + ServosList::VENTING_VALVE)}); + }, + ACTUATORS_TRANSMISSION_PERIOD, TaskScheduler::Policy::RECOVER); + + return protocol->start() && result1 != 0 && result2 != 0 && result3 != 0 && + result4 != 0; +} + +bool CanHandler::isStarted() +{ + return protocol->isStarted() && scheduler->isRunning(); +} + +void CanHandler::sendEvent(EventId event) +{ + protocol->enqueueEvent(static_cast<uint8_t>(Priority::CRITICAL), + static_cast<uint8_t>(PrimaryType::EVENTS), + static_cast<uint8_t>(Board::MOTOR), + static_cast<uint8_t>(Board::BROADCAST), + static_cast<uint8_t>(event)); +} + +void CanHandler::setInitStatus(bool initResult) { initStatus = initResult; } + +void CanHandler::handleCanMessage(const CanMessage &msg) +{ + PrimaryType msgType = static_cast<PrimaryType>(msg.getPrimaryType()); + + // Depending on the received message, call the handling method + switch (msgType) + { + case PrimaryType::EVENTS: + { + handleCanEvent(msg); + break; + } + case PrimaryType::COMMAND: + { + handleCanCommand(msg); + break; + } + default: + { + LOG_WARN(logger, "Received unsupported message type: type={}", + msgType); + break; + } + } +} + +void CanHandler::handleCanEvent(const CanMessage &msg) +{ + EventId eventId = static_cast<EventId>(msg.getSecondaryType()); + + switch (eventId) + { + case EventId::CALIBRATE: + { + auto sensors = ModuleManager::getInstance().get<Sensors>(); + sensors->calibrate(); + + break; + } + default: + { + LOG_WARN(logger, "Received unsupported event: id={}", eventId); + break; + } + } +} + +void CanHandler::handleCanCommand(const CanMessage &msg) +{ + uint64_t payload = msg.payload[0]; + ServosList servo = static_cast<ServosList>(msg.getSecondaryType()); + bool targetState = static_cast<uint8_t>(payload); + uint32_t delay = static_cast<uint32_t>(payload >> 8); + + auto actuators = ModuleManager::getInstance().get<Actuators>(); + + if (actuators != nullptr) + { + if (targetState) + { + // printf("Opening %d for %ld\n", msg.getSecondaryType(), delay); + actuators->openServoAtomic(servo, delay); + } + else + { + // printf("Closing %d for %ld\n", msg.getSecondaryType(), delay); + actuators->closeServo(servo); + } + } +} + +} // namespace Motor diff --git a/src/boards/Motor_old/CanHandler/CanHandler.h b/src/boards/Motor_old/CanHandler/CanHandler.h new file mode 100644 index 0000000000000000000000000000000000000000..1ccc2b90a4c43e77250b634882491a866f71e2b5 --- /dev/null +++ b/src/boards/Motor_old/CanHandler/CanHandler.h @@ -0,0 +1,84 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Federico Mandelli, Alberto Nidasio + * + * 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/CanConfig.h> +#include <drivers/canbus/CanProtocol/CanProtocol.h> +#include <scheduler/TaskScheduler.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Motor +{ + +class CanHandler : public Boardcore::Module +{ + +public: + explicit CanHandler(Boardcore::TaskScheduler *sched); + + /** + * @brief Adds the periodic task to the scheduler and starts the protocol + * threads + */ + bool start(); + + /** + * @brief Returns true if the protocol threads are started and the scheduler + * is running + */ + bool isStarted(); + + /** + * @brief Sends a CAN event on the bus + */ + void sendEvent(Common::CanConfig::EventId event); + + /** + * @brief Set the initialization flag to true + */ + void setInitStatus(bool initResult); + +private: + /** + * @brief Handles a generic CAN message and dispatch the message to the + * correct handler + */ + void handleCanMessage(const Boardcore::Canbus::CanMessage &msg); + + // CAN message handlers + void handleCanEvent(const Boardcore::Canbus::CanMessage &msg); + void handleCanCommand(const Boardcore::Canbus::CanMessage &msg); + + // Init status + std::atomic<bool> initStatus{false}; + + // CAN interfaces + Boardcore::Canbus::CanbusDriver *driver; + Boardcore::Canbus::CanProtocol *protocol; + + Boardcore::TaskScheduler *scheduler; + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("canhandler"); +}; + +} // namespace Motor diff --git a/src/boards/Motor/Configs/ActuatorsConfig.h b/src/boards/Motor_old/Configs/ActuatorsConfig.h similarity index 100% rename from src/boards/Motor/Configs/ActuatorsConfig.h rename to src/boards/Motor_old/Configs/ActuatorsConfig.h diff --git a/src/boards/Motor/Configs/CanHandlerConfig.h b/src/boards/Motor_old/Configs/CanHandlerConfig.h similarity index 100% rename from src/boards/Motor/Configs/CanHandlerConfig.h rename to src/boards/Motor_old/Configs/CanHandlerConfig.h diff --git a/src/boards/Motor_old/Configs/SensorsConfig.h b/src/boards/Motor_old/Configs/SensorsConfig.h new file mode 100644 index 0000000000000000000000000000000000000000..44b13b762ad92deb64f7870ab34d8a1fdac827d0 --- /dev/null +++ b/src/boards/Motor_old/Configs/SensorsConfig.h @@ -0,0 +1,199 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Authors: Alberto Nidasio + * + * 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 <Motor/Sensors/Sensors.h> + +namespace Motor +{ + +namespace SensorsConfig +{ + +constexpr Boardcore::InternalADC::Channel ADC_BATTERY_VOLTAGE_CH = + Boardcore::InternalADC::Channel::CH15; +// The battery is connected to the stm32's adc with a voltage divider +constexpr float ADC_BATTERY_VOLTAGE_COEFF = (10 + 20) / 10; + +// Boardcore::LSM6DSRXConfig LSM6_SENSOR_CONFIG{ +// .bdu = Boardcore::LSM6DSRXConfig::BDU::CONTINUOUS_UPDATE, +// .odrAcc = Boardcore::LSM6DSRXConfig::ACC_ODR::HZ_1660, +// .opModeAcc = Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL, +// .fsAcc = Boardcore::LSM6DSRXConfig::ACC_FULLSCALE::G16, +// .odrGyr = Boardcore::LSM6DSRXConfig::GYR_ODR::HZ_1660, +// .opModeGyr = Boardcore::LSM6DSRXConfig::OPERATING_MODE::NORMAL, +// .fsGyr = Boardcore::LSM6DSRXConfig::GYR_FULLSCALE::DPS_1000, +// .fifoMode = Boardcore::LSM6DSRXConfig::FIFO_MODE::CONTINUOUS, +// .fifoTimestampDecimation = +// Boardcore::LSM6DSRXConfig::FIFO_TIMESTAMP_DECIMATION::DEC_1, +// .fifoTemperatureBdr = +// Boardcore::LSM6DSRXConfig::FIFO_TEMPERATURE_BDR::DISABLED, +// .int1InterruptSelection = Boardcore::LSM6DSRXConfig::INTERRUPT::NOTHING, +// .int2InterruptSelection = +// Boardcore::LSM6DSRXConfig::INTERRUPT::FIFO_THRESHOLD, +// .fifoWatermark = 170, +// }; +// Boardcore::SPIBusConfig LSM6_SPI_CONFIG{ +// Boardcore::SPI::ClockDivider::DIV_16, +// Boardcore::SPI::Mode::MODE_0, +// }; + +Boardcore::H3LIS331DLDefs::OutputDataRate H3LIS_ODR = + Boardcore::H3LIS331DLDefs::OutputDataRate::ODR_1000; +Boardcore::H3LIS331DLDefs::BlockDataUpdate H3LIS_BDU = + Boardcore::H3LIS331DLDefs::BlockDataUpdate::BDU_CONTINUOS_UPDATE; +Boardcore::H3LIS331DLDefs::FullScaleRange H3LIS_FSR = + Boardcore::H3LIS331DLDefs::FullScaleRange::FS_100; +Boardcore::SPIBusConfig H3LIS_SPI_CONFIG{ + Boardcore::SPI::ClockDivider::DIV_16, +}; + +Boardcore::LIS2MDL::Config LIS2_SENSOR_CONFIG{ + .odr = Boardcore::LIS2MDL::ODR_100_HZ, + .deviceMode = Boardcore::LIS2MDL::MD_CONTINUOUS, + .temperatureDivider = 100, +}; +Boardcore::SPIBusConfig LIS2_SPI_CONFIG{ + Boardcore::SPI::ClockDivider::DIV_16, + Boardcore::SPI::Mode::MODE_3, + Boardcore::SPI::Order::MSB_FIRST, + Boardcore::SPI::Order::LSB_FIRST, +}; + +Boardcore::LPS22DF::Config LPS22_SENSOR_CONFIG{ + .odr = Boardcore::LPS22DF::ODR_100, + .avg = Boardcore::LPS22DF::AVG_512, +}; +Boardcore::SPIBusConfig LPS22_SPI_CONFIG{ + Boardcore::SPI::ClockDivider::DIV_16, + Boardcore::SPI::Mode::MODE_3, + Boardcore::SPI::Order::MSB_FIRST, + Boardcore::SPI::Order::LSB_FIRST, +}; + +Boardcore::ADS131M08::Config ADS131_SENSOR_CONFIG{ + .channelsConfig = + { + {.enabled = false}, + // For the servo motor's current sensor the current range is from 0A + // to 20A and the output voltage from 0.65V to 2.65V. The sensor is + // connected to the adc through a voltage divider with a gain + // of 4.166666 So at the ADC side we have maximum 0.636 and we have + // to set a gain of 1 on the ADC + { + .enabled = true, + .pga = Boardcore::ADS131M08Defs::PGA::PGA_1, + }, + {.enabled = false}, + {.enabled = false}, + {.enabled = false}, + // The analog pressure sensors are measured with a 30ohm shunt + // resistor. The current ranges from 4mAh to 20mAh. So we can have a + // voltage up to 0.6V We set a gain of 2 to maximize resolution + { + .enabled = true, + .pga = Boardcore::ADS131M08Defs::PGA::PGA_2, + }, + { + .enabled = true, + .pga = Boardcore::ADS131M08Defs::PGA::PGA_2, + }, + { + .enabled = true, + .pga = Boardcore::ADS131M08Defs::PGA::PGA_2, + }, + }, + .oversamplingRatio = Boardcore::ADS131M08Defs::OversamplingRatio::OSR_4096, + .globalChopModeEnabled = true, +}; +Boardcore::SPIBusConfig ADS131_SPI_CONFIG{ + Boardcore::SPI::ClockDivider::DIV_8, + Boardcore::SPI::Mode::MODE_1, +}; +Boardcore::ADS131M08Defs::Channel ADS131_CHAMBER_PRESSURE_CH = + Boardcore::ADS131M08Defs::Channel::CHANNEL_5; +Boardcore::ADS131M08Defs::Channel ADS131_TANK_PRESSURE_1_CH = + Boardcore::ADS131M08Defs::Channel::CHANNEL_6; +Boardcore::ADS131M08Defs::Channel ADS131_TANK_PRESSURE_2_CH = + Boardcore::ADS131M08Defs::Channel::CHANNEL_7; +Boardcore::ADS131M08Defs::Channel ADS131_SERVO_CURRENT_CH = + Boardcore::ADS131M08Defs::Channel::CHANNEL_1; + +// The chamber pressure sensor is a 0-40bar sensor with a 4-20mA output +// On the motor motherboard there a 30ohm shunt. The shunt then goes through a +// filter with 491Hz of bandwidth and unitary gain. +// We use only a gain coefficient because the offset is removed by the ADC +// offset calibration feature. +constexpr float CHAMBER_PRESSURE_MAX = 40; +constexpr float CHAMBER_PRESSURE_MIN = 0; +constexpr float CHAMBER_PRESSURE_SHUNT = 30; // [ohm] +constexpr float CHAMBER_PRESSURE_CURR_MIN = 0.004; // [A] +constexpr float CHAMBER_PRESSURE_CURR_MAX = 0.020; // [A] +constexpr float CHAMBER_PRESSURE_COEFF = + CHAMBER_PRESSURE_MAX / + (CHAMBER_PRESSURE_CURR_MAX - CHAMBER_PRESSURE_CURR_MIN); // [bar/A] + +// The tank pressure sensors are 0-100bar sensors with a 4-20mA output +// On the motor motherboard there a 30ohm shunt. The shunt then goes through a +// filter with 491Hz of bandwidth and unitary gain. +// We use only a gain coefficient because the offset is removed by the ADC +// offset calibration feature. +constexpr float TANK_PRESSURE_1_MAX = 100; +constexpr float TANK_PRESSURE_1_MIN = 0; +constexpr float TANK_PRESSURE_1_SHUNT = 30; // [ohm] +constexpr float TANK_PRESSURE_1_CURR_MIN = 0.004; // [A] +constexpr float TANK_PRESSURE_1_CURR_MAX = 0.020; // [A] +constexpr float TANK_PRESSURE_1_COEFF = + TANK_PRESSURE_1_MAX / + (TANK_PRESSURE_1_CURR_MAX - TANK_PRESSURE_1_CURR_MIN); // [bar/A] +constexpr float TANK_PRESSURE_2_MAX = 100; +constexpr float TANK_PRESSURE_2_MIN = 0; +constexpr float TANK_PRESSURE_2_SHUNT = 30; // [ohm] +constexpr float TANK_PRESSURE_2_CURR_MIN = 0.004; // [A] +constexpr float TANK_PRESSURE_2_CURR_MAX = 0.020; // [A] +constexpr float TANK_PRESSURE_2_COEFF = + TANK_PRESSURE_2_MAX / + (TANK_PRESSURE_2_CURR_MAX - TANK_PRESSURE_2_CURR_MIN); // [bar/A] + +// The current sensor used to measure the servo motors current consumption: +// - A range of 0A to 20A +// - Has an output from 0.65V to 2.65V +// - Is connected to a voltage divider with a coefficient of 12 / (38.3 + 12) +// +// We use only a gain coefficient because the offset is removed by the ADC +// offset calibration feature. +constexpr float SERVO_CURRENT_COEFF = + 10 / (2.65 - 0.65) / (12 / (38.3 + 12)); // [A/V] + +// Sampling periods +constexpr uint32_t SAMPLE_PERIOD_ADC = 1000; +constexpr uint32_t SAMPLE_PERIOD_LSM6 = 10; +constexpr uint32_t SAMPLE_PERIOD_H3LIS = 100; +constexpr uint32_t SAMPLE_PERIOD_LIS2 = 10; +constexpr uint32_t SAMPLE_PERIOD_LPS22 = 10; +constexpr uint32_t SAMPLE_PERIOD_ADS131 = 1; +constexpr uint32_t SAMPLE_PERIOD_MAX = 10; + +} // namespace SensorsConfig + +} // namespace Motor \ No newline at end of file diff --git a/src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h b/src/boards/Motor_old/Sensors/ChamberPressureSensor/ChamberPressureSensor.h similarity index 100% rename from src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h rename to src/boards/Motor_old/Sensors/ChamberPressureSensor/ChamberPressureSensor.h diff --git a/src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h b/src/boards/Motor_old/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h similarity index 100% rename from src/boards/Motor/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h rename to src/boards/Motor_old/Sensors/ChamberPressureSensor/ChamberPressureSensorData.h diff --git a/src/boards/Motor_old/Sensors/Sensors.cpp b/src/boards/Motor_old/Sensors/Sensors.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0643a6865d1b0d96f9d7ced1d301689efc21f478 --- /dev/null +++ b/src/boards/Motor_old/Sensors/Sensors.cpp @@ -0,0 +1,427 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Authors: Luca Erbetta, Luca Conterio, 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 <Motor/Buses.h> +#include <Motor/Configs/SensorsConfig.h> +#include <drivers/interrupt/external_interrupts.h> +#include <interfaces-impl/hwmapping.h> +#include <logger/Logger.h> + +using namespace std; +using namespace miosix; +using namespace Boardcore; +using namespace Motor::SensorsConfig; + +namespace Motor +{ + +InternalADCData Sensors::getADCData() +{ + miosix::PauseKernelLock lock; + return adc != nullptr ? adc->getLastSample() : InternalADCData{}; +} + +BatteryVoltageSensorData Sensors::getBatteryData() +{ + miosix::PauseKernelLock lock; + return battery != nullptr ? battery->getLastSample() + : BatteryVoltageSensorData{}; +} + +// LSM6DSRXData Sensors::getLSM6DSRXData() +// { +// miosix::PauseKernelLock lock; +// return lsm6dsrx != nullptr ? lsm6dsrx->getLastSample() : LSM6DSRXData{}; +// } + +H3LIS331DLData Sensors::getH3LIS331DLData() +{ + miosix::PauseKernelLock lock; + return h3lis331dl != nullptr ? h3lis331dl->getLastSample() + : H3LIS331DLData{}; +} + +LIS2MDLData Sensors::getLIS2MDLData() +{ + miosix::PauseKernelLock lock; + return lis2mdl != nullptr ? lis2mdl->getLastSample() : LIS2MDLData{}; +} + +LPS22DFData Sensors::getLPS22DFData() +{ + miosix::PauseKernelLock lock; + return lps22df != nullptr ? lps22df->getLastSample() : LPS22DFData{}; +} + +ADS131M08Data Sensors::getADS131M08Data() +{ + miosix::PauseKernelLock lock; + return ads131m08 != nullptr ? ads131m08->getLastSample() : ADS131M08Data{}; +} + +MAX31856Data Sensors::getMAX31856Data() +{ + miosix::PauseKernelLock lock; + return max31856 != nullptr ? max31856->getLastSample() : MAX31856Data{}; +} + +ChamberPressureSensorData Sensors::getChamberPressureSensorData() +{ + miosix::PauseKernelLock lock; + return chamberPressure != nullptr ? chamberPressure->getLastSample() + : ChamberPressureSensorData{}; +} + +TankPressureSensor1Data Sensors::getTankPressureSensor1Data() +{ + miosix::PauseKernelLock lock; + return tankPressure1 != nullptr ? tankPressure1->getLastSample() + : TankPressureSensor1Data{}; +} + +TankPressureSensor2Data Sensors::getTankPressureSensor2Data() +{ + miosix::PauseKernelLock lock; + return tankPressure2 != nullptr ? tankPressure2->getLastSample() + : TankPressureSensor2Data{}; +} + +CurrentData Sensors::getServoCurrentData() +{ + miosix::PauseKernelLock lock; + return servosCurrent != nullptr ? servosCurrent->getLastSample() + : CurrentData{}; +} + +Sensors::Sensors(TaskScheduler* sched) : scheduler(sched) {} + +Sensors::~Sensors() {} + +bool Sensors::start() +{ + adcInit(); + batteryInit(); + // lsm6dsrxInit(); + h3lis331dlInit(); + // lis2mdlInit(); + lps22dfInit(); + max31856Init(); + ads131m08Init(); + chamberPressureInit(); + tankPressure1Init(); + tankPressure2Init(); + servosCurrentInit(); + + sensorManager = new SensorManager(sensorsMap, scheduler); + + return sensorManager->start(); +} + +void Sensors::calibrate() +{ + if (ads131m08 != nullptr) + { + ads131m08->calibrateOffset(ADS131_SERVO_CURRENT_CH); + } +} + +void Sensors::adcInit() +{ + adc = new InternalADC(ADC1); + + adc->enableTemperature(); + adc->enableVbat(); + adc->enableChannel(ADC_BATTERY_VOLTAGE_CH); + + SensorInfo info("ADC", SAMPLE_PERIOD_ADC, + bind(&Sensors::adcCallback, this)); + + sensorsMap.emplace(make_pair(adc, info)); +} + +void Sensors::batteryInit() +{ + function<ADCData()> getADCVoltage( + bind(&InternalADC::getVoltage, adc, ADC_BATTERY_VOLTAGE_CH)); + + battery = + new BatteryVoltageSensor(getADCVoltage, ADC_BATTERY_VOLTAGE_COEFF); + + SensorInfo info("BATTERY", SAMPLE_PERIOD_ADC, + bind(&Sensors::batteryCallback, this)); + + sensorsMap.emplace(make_pair(battery, info)); +} + +// void Sensors::lsm6dsrxInit() +// { +// SPIBus& spi1 = ModuleManager::getInstance().get<Buses>()->spi1; + +// lsm6dsrx = new LSM6DSRX(spi1, peripherals::lsm6dsrx::cs::getPin(), +// LSM6_SPI_CONFIG, LSM6_SENSOR_CONFIG); + +// SensorInfo info("LSM6DSRX", SAMPLE_PERIOD_LSM6, +// bind(&Sensors::lsm6dsrxCallback, this)); + +// sensorsMap.emplace(make_pair(lsm6dsrx, info)); +// } + +void Sensors::h3lis331dlInit() +{ + SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3; + + h3lis331dl = new H3LIS331DL(spi3, peripherals::h3lis331dl::cs::getPin(), + H3LIS_ODR, H3LIS_BDU, H3LIS_FSR); + + SensorInfo info("H3LIS331DL", SAMPLE_PERIOD_H3LIS, + bind(&Sensors::h3lis331dlCallback, this)); + + sensorsMap.emplace(make_pair(h3lis331dl, info)); +} + +void Sensors::lis2mdlInit() +{ + SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3; + + lis2mdl = new LIS2MDL(spi3, peripherals::lis2mdl::cs::getPin(), + LIS2_SPI_CONFIG, LIS2_SENSOR_CONFIG); + + SensorInfo info("LIS2MDL", SAMPLE_PERIOD_LIS2, + bind(&Sensors::lis2mdlCallback, this)); + + sensorsMap.emplace(make_pair(lis2mdl, info)); +} + +void Sensors::lps22dfInit() +{ + SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3; + + lps22df = new LPS22DF(spi3, peripherals::lps22df::cs::getPin(), + LPS22_SPI_CONFIG, LPS22_SENSOR_CONFIG); + + SensorInfo info("LPS22", SAMPLE_PERIOD_LPS22, + bind(&Sensors::lps22dfCallback, this)); + + sensorsMap.emplace(make_pair(lps22df, info)); +} + +void Sensors::max31856Init() +{ + SPIBus& spi3 = ModuleManager::getInstance().get<Buses>()->spi3; + + max31856 = new MAX31856(spi3, peripherals::max31856::cs::getPin()); + + SensorInfo info("MAX31856", SAMPLE_PERIOD_MAX, + bind(&Sensors::max31856Callback, this)); + + sensorsMap.emplace(make_pair(max31856, info)); +} + +void Sensors::ads131m08Init() +{ + SPIBus& spi4 = ModuleManager::getInstance().get<Buses>()->spi4; + + ads131m08 = new ADS131M08(spi4, peripherals::ads131m08::cs::getPin(), + ADS131_SPI_CONFIG, ADS131_SENSOR_CONFIG); + + SensorInfo info("ADS131M08", SAMPLE_PERIOD_ADS131, + bind(&Sensors::ads131m08Callback, this)); + + sensorsMap.emplace(make_pair(ads131m08, info)); +} + +void Sensors::chamberPressureInit() +{ + if (ads131m08 == nullptr) + { + return; + } + + function<ADCData()> getADCVoltage( + [&](void) + { + auto data = ads131m08->getLastSample(); + return data.getVoltage(ADS131_CHAMBER_PRESSURE_CH); + }); + + function<float(float)> voltageToPressure( + [](float voltage) + { + float current = + voltage / CHAMBER_PRESSURE_SHUNT - CHAMBER_PRESSURE_CURR_MIN; + return current * CHAMBER_PRESSURE_COEFF; + }); + + chamberPressure = + new ChamberPressureSensor(getADCVoltage, voltageToPressure); + + SensorInfo info("CHAMBER_PRESSURE", SAMPLE_PERIOD_ADS131, + bind(&Sensors::chamberPressureCallback, this)); + + sensorsMap.emplace(make_pair(chamberPressure, info)); +} + +void Sensors::tankPressure1Init() +{ + if (ads131m08 == nullptr) + { + return; + } + + function<ADCData()> getADCVoltage( + [&](void) + { + auto data = ads131m08->getLastSample(); + return data.getVoltage(ADS131_TANK_PRESSURE_1_CH); + }); + + function<float(float)> voltageToPressure( + [](float voltage) + { + float current = + voltage / TANK_PRESSURE_1_SHUNT - TANK_PRESSURE_1_CURR_MIN; + return current * TANK_PRESSURE_1_COEFF; + }); + + tankPressure1 = new TankPressureSensor1(getADCVoltage, voltageToPressure); + + SensorInfo info("TANK_PRESSURE_1", SAMPLE_PERIOD_ADS131, + bind(&Sensors::tankPressure1Callback, this)); + + sensorsMap.emplace(make_pair(tankPressure1, info)); +} + +void Sensors::tankPressure2Init() +{ + if (ads131m08 == nullptr) + { + return; + } + + function<ADCData()> getADCVoltage( + [&](void) + { + auto data = ads131m08->getLastSample(); + return data.getVoltage(ADS131_TANK_PRESSURE_2_CH); + }); + + function<float(float)> voltageToPressure( + [](float voltage) + { + float current = + voltage / TANK_PRESSURE_2_SHUNT - TANK_PRESSURE_2_CURR_MIN; + return current * TANK_PRESSURE_2_COEFF; + }); + + tankPressure2 = new TankPressureSensor2(getADCVoltage, voltageToPressure); + + SensorInfo info("TANK_PRESSURE_2", SAMPLE_PERIOD_ADS131, + bind(&Sensors::tankPressure2Callback, this)); + + sensorsMap.emplace(make_pair(tankPressure2, info)); +} + +void Sensors::servosCurrentInit() +{ + if (ads131m08 == nullptr) + { + return; + } + + function<ADCData()> getADCVoltage( + [&](void) + { + auto data = ads131m08->getLastSample(); + return data.getVoltage(ADS131_SERVO_CURRENT_CH); + }); + + function<float(float)> voltageToCurrent( + [](float voltage) { return voltage * SERVO_CURRENT_COEFF; }); + + servosCurrent = new CurrentSensor(getADCVoltage, voltageToCurrent); + + SensorInfo info("SERVOS_CURRENT", SAMPLE_PERIOD_ADS131, + bind(&Sensors::servosCurrentCallback, this)); + + sensorsMap.emplace(make_pair(servosCurrent, info)); +} + +void Sensors::adcCallback() { Logger::getInstance().log(adc->getLastSample()); } + +void Sensors::batteryCallback() +{ + Logger::getInstance().log(battery->getLastSample()); +} + +// void Sensors::lsm6dsrxCallback() +// { +// Logger::getInstance().log(lsm6dsrx->getLastSample()); +// } + +void Sensors::h3lis331dlCallback() +{ + Logger::getInstance().log(h3lis331dl->getLastSample()); +} + +void Sensors::lis2mdlCallback() +{ + Logger::getInstance().log(lis2mdl->getLastSample()); +} + +void Sensors::lps22dfCallback() +{ + Logger::getInstance().log(lps22df->getLastSample()); +} + +void Sensors::max31856Callback() +{ + Logger::getInstance().log(max31856->getLastSample()); +} + +void Sensors::ads131m08Callback() +{ + Logger::getInstance().log(ads131m08->getLastSample()); +} + +void Sensors::chamberPressureCallback() +{ + Logger::getInstance().log(chamberPressure->getLastSample()); +} + +void Sensors::tankPressure1Callback() +{ + Logger::getInstance().log(tankPressure1->getLastSample()); +} + +void Sensors::tankPressure2Callback() +{ + Logger::getInstance().log(tankPressure2->getLastSample()); +} + +void Sensors::servosCurrentCallback() +{ + Logger::getInstance().log(servosCurrent->getLastSample()); +} + +} // namespace Motor \ No newline at end of file diff --git a/src/boards/Motor_old/Sensors/Sensors.h b/src/boards/Motor_old/Sensors/Sensors.h new file mode 100644 index 0000000000000000000000000000000000000000..780563efec9feef081b3258fed1fc4f66477854a --- /dev/null +++ b/src/boards/Motor_old/Sensors/Sensors.h @@ -0,0 +1,124 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * 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 <Motor/Sensors/ChamberPressureSensor/ChamberPressureSensor.h> +#include <Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h> +#include <Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h> +#include <drivers/adc/InternalADC.h> +#include <sensors/ADS131M08/ADS131M08.h> +#include <sensors/H3LIS331DL/H3LIS331DL.h> +#include <sensors/LIS2MDL/LIS2MDL.h> +#include <sensors/LPS22DF/LPS22DF.h> +// #include <sensors/LSM6DSRX/LSM6DSRX.h> +#include <sensors/MAX31856/MAX31856.h> +#include <sensors/SensorManager.h> +#include <sensors/analog/BatteryVoltageSensor.h> +#include <sensors/analog/CurrentSensor.h> +#include <sensors/analog/pressure/AnalogPressureSensor.h> + +#include <utils/ModuleManager/ModuleManager.hpp> + +namespace Motor +{ + +class Sensors : public Boardcore::Module +{ +public: + Boardcore::InternalADCData getADCData(); + Boardcore::BatteryVoltageSensorData getBatteryData(); + // Boardcore::LSM6DSRXData getLSM6DSRXData(); + Boardcore::H3LIS331DLData getH3LIS331DLData(); + Boardcore::LIS2MDLData getLIS2MDLData(); + Boardcore::LPS22DFData getLPS22DFData(); + Boardcore::ADS131M08Data getADS131M08Data(); + Boardcore::MAX31856Data getMAX31856Data(); + Boardcore::ChamberPressureSensorData getChamberPressureSensorData(); + Boardcore::TankPressureSensor1Data getTankPressureSensor1Data(); + Boardcore::TankPressureSensor2Data getTankPressureSensor2Data(); + Boardcore::CurrentData getServoCurrentData(); + + explicit Sensors(Boardcore::TaskScheduler* sched); + + ~Sensors(); + + bool start(); + + void calibrate(); + +private: + void adcInit(); + void adcCallback(); + + void batteryInit(); + void batteryCallback(); + + // void lsm6dsrxInit(); + // void lsm6dsrxCallback(); + + void h3lis331dlInit(); + void h3lis331dlCallback(); + + void lis2mdlInit(); + void lis2mdlCallback(); + + void lps22dfInit(); + void lps22dfCallback(); + + void max31856Init(); + void max31856Callback(); + + void ads131m08Init(); + void ads131m08Callback(); + + void chamberPressureInit(); + void chamberPressureCallback(); + + void tankPressure1Init(); + void tankPressure1Callback(); + + void tankPressure2Init(); + void tankPressure2Callback(); + + void servosCurrentInit(); + void servosCurrentCallback(); + + Boardcore::InternalADC* adc = nullptr; + Boardcore::BatteryVoltageSensor* battery = nullptr; + // Boardcore::LSM6DSRX* lsm6dsrx = nullptr; + Boardcore::H3LIS331DL* h3lis331dl = nullptr; + Boardcore::LIS2MDL* lis2mdl = nullptr; + Boardcore::LPS22DF* lps22df = nullptr; + Boardcore::MAX31856* max31856 = nullptr; + Boardcore::ADS131M08* ads131m08 = nullptr; + Boardcore::ChamberPressureSensor* chamberPressure = nullptr; + Boardcore::TankPressureSensor1* tankPressure1 = nullptr; + Boardcore::TankPressureSensor2* tankPressure2 = nullptr; + Boardcore::CurrentSensor* servosCurrent = nullptr; + + Boardcore::SensorManager::SensorMap_t sensorsMap; + Boardcore::SensorManager* sensorManager = nullptr; + Boardcore::TaskScheduler* scheduler = nullptr; +}; + +} // namespace Motor \ No newline at end of file diff --git a/src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h b/src/boards/Motor_old/Sensors/TankPressureSensor1/TankPressureSensor1.h similarity index 100% rename from src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1.h rename to src/boards/Motor_old/Sensors/TankPressureSensor1/TankPressureSensor1.h diff --git a/src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1Data.h b/src/boards/Motor_old/Sensors/TankPressureSensor1/TankPressureSensor1Data.h similarity index 100% rename from src/boards/Motor/Sensors/TankPressureSensor1/TankPressureSensor1Data.h rename to src/boards/Motor_old/Sensors/TankPressureSensor1/TankPressureSensor1Data.h diff --git a/src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h b/src/boards/Motor_old/Sensors/TankPressureSensor2/TankPressureSensor2.h similarity index 100% rename from src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2.h rename to src/boards/Motor_old/Sensors/TankPressureSensor2/TankPressureSensor2.h diff --git a/src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2Data.h b/src/boards/Motor_old/Sensors/TankPressureSensor2/TankPressureSensor2Data.h similarity index 100% rename from src/boards/Motor/Sensors/TankPressureSensor2/TankPressureSensor2Data.h rename to src/boards/Motor_old/Sensors/TankPressureSensor2/TankPressureSensor2Data.h diff --git a/src/entrypoints/Motor/motor-entry.cpp b/src/entrypoints/Motor/motor-entry.cpp index 8d42fd5b4c35dc790a9990fd17f86ae753d08f88..61ad36f243e172e4fb848ac0a3db8c81c28ae97d 100644 --- a/src/entrypoints/Motor/motor-entry.cpp +++ b/src/entrypoints/Motor/motor-entry.cpp @@ -1,5 +1,5 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Alberto Nidasio +/* 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,189 +20,86 @@ * THE SOFTWARE. */ -#include <Motor/Actuators/Actuators.h> -#include <Motor/BoardScheduler.h> #include <Motor/Buses.h> #include <Motor/CanHandler/CanHandler.h> #include <Motor/Sensors/Sensors.h> -#include <diagnostic/CpuMeter/CpuMeter.h> +#include <diagnostic/PrintLogger.h> +#include <interfaces-impl/hwmapping.h> #include <miosix.h> -using namespace miosix; +#include <utils/ModuleManager/ModuleManager.hpp> + using namespace Boardcore; using namespace Motor; +using namespace miosix; 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(); - auto actuators = - new Actuators(scheduler->getScheduler(miosix::PRIORITY_MAX)); - auto sensors = - new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1)); - auto canHandler = - new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2)); + ModuleManager &modules = ModuleManager::getInstance(); + PrintLogger logger = Logging::getLogger("main"); - // Insert modules - if (!modules.insert<BoardScheduler>(scheduler)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the board scheduler module"); - } + // TODO: Move this to a dedicated board scheduler + TaskScheduler *scheduler1 = new TaskScheduler(2); + TaskScheduler *scheduler2 = new TaskScheduler(3); - if (!modules.insert<Buses>(buses)) - { - initResult = false; - LOG_ERR(logger, "Error inserting the buses module"); - } + Buses *buses = new Buses(); + Sensors *sensors = new Sensors(*scheduler2); + CanHandler *canHandler = new CanHandler(); + + bool initResult = true; - if (!modules.insert<Actuators>(actuators)) + // Insert modules + if (!modules.insert<Buses>(buses)) { initResult = false; - LOG_ERR(logger, "Error inserting the sensor module"); } if (!modules.insert<Sensors>(sensors)) { initResult = false; - LOG_ERR(logger, "Error inserting the sensor module"); } if (!modules.insert<CanHandler>(canHandler)) { initResult = false; - LOG_ERR(logger, "Error inserting the CanHandler module"); } // Start modules - if (!Logger::getInstance().start()) - { - initResult = false; - LOG_ERR(logger, "Error starting the logger module"); - } - - if (!modules.get<BoardScheduler>()->start()) + if (!sensors->start()) { initResult = false; - LOG_ERR(logger, "Error starting the board scheduler module"); + LOG_ERR(logger, "Error failed to start Sensors module"); } - if (!modules.get<Actuators>()->start()) + if (!canHandler->start()) { initResult = false; - LOG_ERR(logger, "Error starting the sensors module"); + LOG_ERR(logger, "Error failed to start CanHandler module"); } - if (!modules.get<Sensors>()->start()) + if (initResult) { - initResult = false; - LOG_ERR(logger, "Error starting the sensors module"); + LOG_INFO(logger, "All good!"); } - - if (!modules.get<CanHandler>()->start()) + else { - initResult = false; - LOG_ERR(logger, "Error starting the CanHandler module"); + LOG_ERR(logger, "Init failure!"); } - // Calibration - modules.get<Sensors>()->calibrate(); - - // Set the init status inside the CAN handler - modules.get<CanHandler>()->setInitStatus(initResult); - - // Check the init result and launch an event - if (initResult) - { - miosix::led1On(); - } - else + for (auto info : sensors->getSensorInfo()) { - miosix::led2On(); + LOG_INFO(logger, "{} {}", info.isInitialized, info.id); } while (true) { - printf("Average CPU usage: %.1f%%\n", CpuMeter::getCpuStats().mean); - - auto adcData = sensors->getADCData(); - auto batteryData = sensors->getBatteryData(); - printf("[%.2fs]\tADC:\t%f %f %f\n", adcData.timestamp / 1e6, - adcData.temperature, adcData.vBat, batteryData.batVoltage); - - // // WARNING: Fails self test - // auto lsm6Data = sensors->getLSM6DSRXData(); - // printf("[%.2fs]\tLSM6:\t%fm/s^2 %fm/s^2 %fm/s^2\n", - // lsm6Data.accelerationTimestamp / 1e6, - // lsm6Data.accelerationX * 0.001, lsm6Data.accelerationY * - // 0.001, lsm6Data.accelerationZ * 0.001); - // printf("[%.2fs]\tLSM6:\t%frad/s %frad/s %frad/s\n", - // lsm6Data.angularSpeedTimestamp / 1e6, - // lsm6Data.angularSpeedX * 0.001, lsm6Data.angularSpeedY * - // 0.001, lsm6Data.angularSpeedZ * 0.001); - - // // WARNING: The values are wrong - // auto h3lisData = sensors->getH3LIS331DLData(); - // printf("[%.2fs]\tH3LIS:\t%fm/s %fm/s %fm/s\n", - // h3lisData.accelerationTimestamp / 1e6, - // h3lisData.accelerationX, h3lisData.accelerationY, - // h3lisData.accelerationZ); - - // auto lis2Data = sensors->getLIS2MDLData(); - // printf("[%.2fs]\tLIS2:\t%f %f %f\n", - // lis2Data.magneticFieldTimestamp / 1e6, - // lis2Data.magneticFieldX, lis2Data.magneticFieldY, - // lis2Data.magneticFieldZ); - - // auto lps22Data = sensors->getLPS22DFData(); - // printf("[%.2fs]\tLPS22:\t%f %f\n", lps22Data.pressureTimestamp / 1e6, - // lps22Data.pressure, lps22Data.temperature); - - // auto maxData = sensors->getMAX31856Data(); - // printf("[%.2fs]\tMAX:\t%f° %f°\n", maxData.temperatureTimestamp / - // 1e6, - // maxData.temperature, maxData.coldJunctionTemperature); - - auto adsData = sensors->getADS131M08Data(); - printf("[%.2fs]\tADS131:\t%f %f %f %f\n", adsData.timestamp / 1e6, - adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage, - adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage, - adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage, - adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage); - - auto chamberData = sensors->getChamberPressureSensorData(); - printf("[%.2fs]\tCHAMBER:\t%fbar\n", - chamberData.pressureTimestamp / 1e6, chamberData.pressure); - - auto tank1Data = sensors->getTankPressureSensor1Data(); - printf("[%.2fs]\tTANK1:\t\t%fbar\n", tank1Data.pressureTimestamp / 1e6, - tank1Data.pressure); - - auto tank2Data = sensors->getTankPressureSensor2Data(); - printf("[%.2fs]\tTANK2:\t\t%fbar\n", tank2Data.pressureTimestamp / 1e6, - tank2Data.pressure); - - auto servoCurrent = sensors->getServoCurrentData(); - printf("[%.2fs]\tSERVO:\t\t%fA\n", servoCurrent.currentTimestamp / 1e6, - servoCurrent.current); - - auto batteryVoltage = sensors->getBatteryData(); - printf("[%.2fs]\tBATTERY:\t%fV\n", - batteryVoltage.voltageTimestamp / 1e6, - batteryVoltage.batVoltage); - - auto loggerData = Logger::getInstance().getStats(); - printf("[%.2fs]\tLOGGER:\t%d\n", loggerData.timestamp / 1e6, - loggerData.writesFailed); - + gpios::boardLed::low(); + canHandler->sendEvent(Common::CanConfig::EventId::ARM); + Thread::sleep(1000); + gpios::boardLed::high(); + canHandler->sendEvent(Common::CanConfig::EventId::DISARM); Thread::sleep(1000); } -} + + return 0; +} \ No newline at end of file diff --git a/src/entrypoints/Motor_old/motor-entry.cpp b/src/entrypoints/Motor_old/motor-entry.cpp new file mode 100644 index 0000000000000000000000000000000000000000..8d42fd5b4c35dc790a9990fd17f86ae753d08f88 --- /dev/null +++ b/src/entrypoints/Motor_old/motor-entry.cpp @@ -0,0 +1,208 @@ +/* Copyright (c) 2023 Skyward Experimental Rocketry + * Author: Alberto Nidasio + * + * 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 <Motor/Actuators/Actuators.h> +#include <Motor/BoardScheduler.h> +#include <Motor/Buses.h> +#include <Motor/CanHandler/CanHandler.h> +#include <Motor/Sensors/Sensors.h> +#include <diagnostic/CpuMeter/CpuMeter.h> +#include <miosix.h> + +using namespace miosix; +using namespace Boardcore; +using namespace Motor; + +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(); + auto actuators = + new Actuators(scheduler->getScheduler(miosix::PRIORITY_MAX)); + auto sensors = + new Sensors(scheduler->getScheduler(miosix::PRIORITY_MAX - 1)); + auto canHandler = + new CanHandler(scheduler->getScheduler(miosix::PRIORITY_MAX - 2)); + + // 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<Actuators>(actuators)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the sensor module"); + } + + if (!modules.insert<Sensors>(sensors)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the sensor module"); + } + + if (!modules.insert<CanHandler>(canHandler)) + { + initResult = false; + LOG_ERR(logger, "Error inserting the CanHandler module"); + } + + // Start modules + if (!Logger::getInstance().start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the logger 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 sensors module"); + } + + if (!modules.get<Sensors>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the sensors module"); + } + + if (!modules.get<CanHandler>()->start()) + { + initResult = false; + LOG_ERR(logger, "Error starting the CanHandler module"); + } + + // Calibration + modules.get<Sensors>()->calibrate(); + + // Set the init status inside the CAN handler + modules.get<CanHandler>()->setInitStatus(initResult); + + // Check the init result and launch an event + if (initResult) + { + miosix::led1On(); + } + else + { + miosix::led2On(); + } + + while (true) + { + printf("Average CPU usage: %.1f%%\n", CpuMeter::getCpuStats().mean); + + auto adcData = sensors->getADCData(); + auto batteryData = sensors->getBatteryData(); + printf("[%.2fs]\tADC:\t%f %f %f\n", adcData.timestamp / 1e6, + adcData.temperature, adcData.vBat, batteryData.batVoltage); + + // // WARNING: Fails self test + // auto lsm6Data = sensors->getLSM6DSRXData(); + // printf("[%.2fs]\tLSM6:\t%fm/s^2 %fm/s^2 %fm/s^2\n", + // lsm6Data.accelerationTimestamp / 1e6, + // lsm6Data.accelerationX * 0.001, lsm6Data.accelerationY * + // 0.001, lsm6Data.accelerationZ * 0.001); + // printf("[%.2fs]\tLSM6:\t%frad/s %frad/s %frad/s\n", + // lsm6Data.angularSpeedTimestamp / 1e6, + // lsm6Data.angularSpeedX * 0.001, lsm6Data.angularSpeedY * + // 0.001, lsm6Data.angularSpeedZ * 0.001); + + // // WARNING: The values are wrong + // auto h3lisData = sensors->getH3LIS331DLData(); + // printf("[%.2fs]\tH3LIS:\t%fm/s %fm/s %fm/s\n", + // h3lisData.accelerationTimestamp / 1e6, + // h3lisData.accelerationX, h3lisData.accelerationY, + // h3lisData.accelerationZ); + + // auto lis2Data = sensors->getLIS2MDLData(); + // printf("[%.2fs]\tLIS2:\t%f %f %f\n", + // lis2Data.magneticFieldTimestamp / 1e6, + // lis2Data.magneticFieldX, lis2Data.magneticFieldY, + // lis2Data.magneticFieldZ); + + // auto lps22Data = sensors->getLPS22DFData(); + // printf("[%.2fs]\tLPS22:\t%f %f\n", lps22Data.pressureTimestamp / 1e6, + // lps22Data.pressure, lps22Data.temperature); + + // auto maxData = sensors->getMAX31856Data(); + // printf("[%.2fs]\tMAX:\t%f° %f°\n", maxData.temperatureTimestamp / + // 1e6, + // maxData.temperature, maxData.coldJunctionTemperature); + + auto adsData = sensors->getADS131M08Data(); + printf("[%.2fs]\tADS131:\t%f %f %f %f\n", adsData.timestamp / 1e6, + adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_5).voltage, + adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_6).voltage, + adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_7).voltage, + adsData.getVoltage(ADS131M08Defs::Channel::CHANNEL_1).voltage); + + auto chamberData = sensors->getChamberPressureSensorData(); + printf("[%.2fs]\tCHAMBER:\t%fbar\n", + chamberData.pressureTimestamp / 1e6, chamberData.pressure); + + auto tank1Data = sensors->getTankPressureSensor1Data(); + printf("[%.2fs]\tTANK1:\t\t%fbar\n", tank1Data.pressureTimestamp / 1e6, + tank1Data.pressure); + + auto tank2Data = sensors->getTankPressureSensor2Data(); + printf("[%.2fs]\tTANK2:\t\t%fbar\n", tank2Data.pressureTimestamp / 1e6, + tank2Data.pressure); + + auto servoCurrent = sensors->getServoCurrentData(); + printf("[%.2fs]\tSERVO:\t\t%fA\n", servoCurrent.currentTimestamp / 1e6, + servoCurrent.current); + + auto batteryVoltage = sensors->getBatteryData(); + printf("[%.2fs]\tBATTERY:\t%fV\n", + batteryVoltage.voltageTimestamp / 1e6, + batteryVoltage.batVoltage); + + auto loggerData = Logger::getInstance().getStats(); + printf("[%.2fs]\tLOGGER:\t%d\n", loggerData.timestamp / 1e6, + loggerData.writesFailed); + + Thread::sleep(1000); + } +}