From 82c2ccf1120c62242ab295b4b9c3399c6084b3e2 Mon Sep 17 00:00:00 2001 From: Alberto Nidasio <alberto.nidasio@skywarder.eu> Date: Sat, 5 Mar 2022 18:19:33 +0100 Subject: [PATCH] Fixed pipeline --- src/boards/Parafoil/Configs/MavlinkConfig.h | 30 ++- src/boards/Parafoil/Configs/RadioConfig.h | 12 +- src/boards/Parafoil/Configs/SensorsConfig.h | 56 ++-- src/boards/Parafoil/Configs/XbeeConfig.h | 29 +- .../FlightModeManager/FMMController.cpp | 214 ++++++++------- .../FlightModeManager/FMMController.h | 57 ++-- .../Parafoil/FlightModeManager/FMMData.h | 11 +- src/boards/Parafoil/Main/Radio.cpp | 13 +- src/boards/Parafoil/Main/Radio.h | 253 +++++++++--------- src/boards/Parafoil/Main/Sensors.cpp | 15 +- src/boards/Parafoil/Main/Sensors.h | 188 +++++++------ src/boards/Parafoil/ParafoilTest.h | 36 +-- src/boards/Parafoil/ParafoilTestStatus.h | 78 +++--- .../TelemetriesTelecommands/Mavlink.h | 10 +- .../TelemetriesTelecommands/TMRepository.cpp | 203 +++++++------- .../TelemetriesTelecommands/TMRepository.h | 119 ++++---- src/boards/Parafoil/Wing/WingAlgorithm.cpp | 217 ++++++++------- src/boards/Parafoil/Wing/WingAlgorithm.h | 174 ++++++------ src/boards/Parafoil/Wing/WingConfig.h | 56 ++-- src/boards/Parafoil/Wing/WingController.cpp | 212 +++++++-------- src/boards/Parafoil/Wing/WingController.h | 199 +++++++------- src/boards/Parafoil/Wing/WingServo.cpp | 82 +++--- src/boards/Parafoil/Wing/WingServo.h | 110 ++++---- .../Parafoil/logdecoder/LogTypes.h | 7 - src/entrypoints/Parafoil/parafoil-entry.cpp | 2 +- 25 files changed, 1177 insertions(+), 1206 deletions(-) diff --git a/src/boards/Parafoil/Configs/MavlinkConfig.h b/src/boards/Parafoil/Configs/MavlinkConfig.h index e50257717..9c702ae66 100644 --- a/src/boards/Parafoil/Configs/MavlinkConfig.h +++ b/src/boards/Parafoil/Configs/MavlinkConfig.h @@ -1,5 +1,5 @@ /* Copyright (c) 2021 Skyward Experimental Rocketry - * Authors: Luca Conterio + * Author: Luca Conterio * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -19,20 +19,26 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #pragma once -namespace ParafoilTestDev +#include <stdint.h> + +#include <cstdio> + +namespace Parafoil { - /* Mavlink Driver queue settings */ - static constexpr unsigned int MAV_OUT_QUEUE_LEN = 10; - static constexpr unsigned int MAV_PKT_SIZE = 255; - static constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 200; +// Mavlink Driver queue settings +static constexpr unsigned int MAV_OUT_QUEUE_LEN = 10; +static constexpr unsigned int MAV_PKT_SIZE = 255; +static constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 200; + +// These two values are taken as is +static const unsigned int TMTC_MAV_SYSID = 171; +static const unsigned int TMTC_MAV_COMPID = 96; - //These two values are taken as is - static const unsigned int TMTC_MAV_SYSID = 171; - static const unsigned int TMTC_MAV_COMPID = 96; +// Min guaranteed sleep time after each packet sent +static const uint16_t SLEEP_AFTER_SEND = 0; // [ms] - /* Min guaranteed sleep time after each packet sent (milliseconds) */ - static const uint16_t SLEEP_AFTER_SEND = 0; -} +} // namespace Parafoil diff --git a/src/boards/Parafoil/Configs/RadioConfig.h b/src/boards/Parafoil/Configs/RadioConfig.h index ca9beaf91..5248aedd1 100644 --- a/src/boards/Parafoil/Configs/RadioConfig.h +++ b/src/boards/Parafoil/Configs/RadioConfig.h @@ -24,16 +24,16 @@ #include <stdint.h> -namespace ParafoilTestDev +namespace Parafoil { -// TODO update with the correct values -static const uint32_t HR_GROUND_UPDATE_PERIOD = 62; // Milliseconds +// TODO: update with the correct values +static const uint32_t HR_GROUND_UPDATE_PERIOD = 62; // [ms] static const uint32_t HR_FLIGHT_UPDATE_PERIOD = 10; -static const uint32_t LR_UPDATE_PERIOD = 100; // Milliseconds +static const uint32_t LR_UPDATE_PERIOD = 100; // [ms] -// TODO define the correct ids for task scheduler +// TODO: define the correct ids for task scheduler static const uint8_t RADIO_HR_ID = 200; static const uint8_t RADIO_LR_ID = 201; -} // namespace ParafoilTestDev \ No newline at end of file +} // namespace Parafoil diff --git a/src/boards/Parafoil/Configs/SensorsConfig.h b/src/boards/Parafoil/Configs/SensorsConfig.h index ce12d431d..a2629c6ea 100644 --- a/src/boards/Parafoil/Configs/SensorsConfig.h +++ b/src/boards/Parafoil/Configs/SensorsConfig.h @@ -29,36 +29,38 @@ #pragma once -#include <sensors/UbloxGPS/UbloxGPS.h> -#include <sensors/MPU9250/MPU9250.h> #include <sensors/BME280/BME280.h> +#include <sensors/MPU9250/MPU9250.h> +#include <sensors/UbloxGPS/UbloxGPS.h> -using namespace Boardcore; - -namespace ParafoilTestDev +namespace Parafoil { - //SPI pinouts - static miosix::GpioPin SCK(GPIOA_BASE, 5); - static miosix::GpioPin MISO(GPIOB_BASE, 4); - static miosix::GpioPin MOSI(GPIOA_BASE, 7); - //GPS settings - //static miosix::GpioPin GPS_CS(GPIOG_BASE, 3); - static miosix::GpioPin GPS_TX(GPIOA_BASE, 2); - static miosix::GpioPin GPS_RX(GPIOA_BASE, 3); - static constexpr unsigned int GPS_SAMPLE_RATE = 25; - static constexpr unsigned int GPS_SAMPLE_PERIOD = 1000 / GPS_SAMPLE_RATE; - +// SPI pinouts +static miosix::GpioPin SCK(GPIOA_BASE, 5); +static miosix::GpioPin MISO(GPIOB_BASE, 4); +static miosix::GpioPin MOSI(GPIOA_BASE, 7); + +// GPS settings +// static miosix::GpioPin GPS_CS(GPIOG_BASE, 3); +static miosix::GpioPin GPS_TX(GPIOA_BASE, 2); +static miosix::GpioPin GPS_RX(GPIOA_BASE, 3); +static constexpr unsigned int GPS_SAMPLE_RATE = 25; +static constexpr unsigned int GPS_SAMPLE_PERIOD = 1000 / GPS_SAMPLE_RATE; + +// IMU MPU9250 settings +static miosix::GpioPin IMU_CS(GPIOB_BASE, 2); +static const Boardcore::MPU9250::MPU9250GyroFSR IMU_GYRO_SCALE = + Boardcore::MPU9250::GYRO_FSR_500DPS; +static const Boardcore::MPU9250::MPU9250AccelFSR IMU_ACCEL_SCALE = + Boardcore::MPU9250::ACCEL_FSR_16G; +static constexpr unsigned short IMU_SAMPLE_RATE = 500; +static constexpr unsigned int IMU_SAMPLE_PERIOD = 1000 / IMU_SAMPLE_RATE; - //IMU MPU9250 settings - static miosix::GpioPin IMU_CS(GPIOB_BASE, 2); - static const MPU9250::MPU9250GyroFSR IMU_GYRO_SCALE = MPU9250::GYRO_FSR_500DPS; - static const MPU9250::MPU9250AccelFSR IMU_ACCEL_SCALE = MPU9250::ACCEL_FSR_16G; - static constexpr unsigned short IMU_SAMPLE_RATE = 500; - static constexpr unsigned int IMU_SAMPLE_PERIOD = 1000 / IMU_SAMPLE_RATE; +// Barometer BME280 settings +static miosix::GpioPin PRESS_CS(GPIOC_BASE, 11); +static const Boardcore::BME280::StandbyTime PRESS_SAMPLE_RATE = + Boardcore::BME280::STB_TIME_0_5; +static constexpr unsigned int PRESS_SAMPLE_PERIOD = 20; - //Barometer BME280 settings - static miosix::GpioPin PRESS_CS(GPIOC_BASE, 11); - static const BME280::StandbyTime PRESS_SAMPLE_RATE = BME280::STB_TIME_0_5; - static constexpr unsigned int PRESS_SAMPLE_PERIOD = 20; -} \ No newline at end of file +} // namespace Parafoil diff --git a/src/boards/Parafoil/Configs/XbeeConfig.h b/src/boards/Parafoil/Configs/XbeeConfig.h index a5bb02754..e254ea5a0 100644 --- a/src/boards/Parafoil/Configs/XbeeConfig.h +++ b/src/boards/Parafoil/Configs/XbeeConfig.h @@ -19,24 +19,25 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #pragma once #include <radio/Xbee/Xbee.h> -using namespace Boardcore; - -namespace ParafoilTestDev +namespace Parafoil { - //TODO change the pins to something correct - static miosix::GpioPin XBEE_CS(GPIOC_BASE, 1); - static miosix::GpioPin XBEE_ATTN(GPIOF_BASE, 10); //Interrupt pin (THE SAME AS THE RADIO.CPP) - static miosix::GpioPin XBEE_RESET(GPIOC_BASE, 5); - static miosix::GpioPin XBEE_SCK(GPIOE_BASE, 2); - static miosix::GpioPin XBEE_MISO(GPIOE_BASE, 5); - static miosix::GpioPin XBEE_MOSI(GPIOE_BASE, 6); +// TODO change the pins to something correct +static miosix::GpioPin XBEE_CS(GPIOC_BASE, 1); +static miosix::GpioPin XBEE_ATTN(GPIOF_BASE, 10); // Interrupt pin +static miosix::GpioPin XBEE_RESET(GPIOC_BASE, 5); + +static miosix::GpioPin XBEE_SCK(GPIOE_BASE, 2); +static miosix::GpioPin XBEE_MISO(GPIOE_BASE, 5); +static miosix::GpioPin XBEE_MOSI(GPIOE_BASE, 6); + +// Data rate +static const bool XBEE_80KBPS_DATA_RATE = true; +static const int XBEE_TIMEOUT = 5000; // [ms] - //Data rate - static const bool XBEE_80KBPS_DATA_RATE = true; - static const int XBEE_TIMEOUT = 5000; //5 seconds -} \ No newline at end of file +} // namespace Parafoil diff --git a/src/boards/Parafoil/FlightModeManager/FMMController.cpp b/src/boards/Parafoil/FlightModeManager/FMMController.cpp index b40951fdb..5c43d55d8 100644 --- a/src/boards/Parafoil/FlightModeManager/FMMController.cpp +++ b/src/boards/Parafoil/FlightModeManager/FMMController.cpp @@ -1,5 +1,5 @@ /* Copyright (c) 2022 Skyward Experimental Rocketry - * Authors: Matteo Pignataro + * Author: Matteo Pignataro * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -22,136 +22,132 @@ #include "FMMController.h" +#include <common/events/Events.h> +#include <events/EventBroker.h> #include <miosix.h> -#include <events/EventBroker.h> -#include <common/events/Events.h> +using namespace Boardcore; -namespace ParafoilTestDev +namespace Parafoil { - FMMController::FMMController() : FSM(&FMMController::state_on_ground) - { - memset(&status, 0, sizeof(FMMControllerStatus)); - sEventBroker.subscribe(this, TOPIC_FMM); - sEventBroker.subscribe(this, TOPIC_TMTC); - } +FMMController::FMMController() : FSM(&FMMController::state_on_ground) +{ + memset(&status, 0, sizeof(FMMControllerStatus)); + sEventBroker.subscribe(this, TOPIC_FMM); + sEventBroker.subscribe(this, TOPIC_TMTC); +} + +FMMController::~FMMController() { sEventBroker.unsubscribe(this); } - FMMController::~FMMController() +void FMMController::state_on_ground(const Event& ev) +{ + switch (ev.code) { - sEventBroker.unsubscribe(this); + case EV_ENTRY: + { + // ... + + logStatus(ON_GROUND); + + LOG_DEBUG(logger, "[FMM] entering state on_ground\n"); + break; + } + case EV_EXIT: + { + // ... + + LOG_DEBUG(logger, "[FMM] exiting state on_ground\n"); + break; + } + case EV_LIFTOFF: + { + transition(&FMMController::state_flying); + break; + } + case EV_TC_TEST_MODE: + { + transition(&FMMController::state_debug); + break; + } + default: + { + break; + } } +} - void FMMController::state_on_ground(const Event& ev) +void FMMController::state_flying(const Event& ev) +{ + switch (ev.code) { - switch (ev.code) + case EV_ENTRY: { - case EV_ENTRY: - { - // ... - - logStatus(ON_GROUND); - - LOG_DEBUG(logger, "[FMM] entering state on_ground\n"); - break; - } - case EV_EXIT: - { - // ... - - LOG_DEBUG(logger, "[FMM] exiting state on_ground\n"); - break; - } - case EV_LIFTOFF: - { - transition(&FMMController::state_flying); - break; - } - case EV_TC_TEST_MODE: - { - transition(&FMMController::state_debug); - break; - } - default: - { - break; - } + // ... + + logStatus(FLYING_STATE); + + LOG_DEBUG(logger, "[FMM] entering state flying\n"); + break; } - } + case EV_EXIT: + { + // ... + LOG_DEBUG(logger, "[FMM] exiting state flying\n"); + break; + } - void FMMController::state_flying(const Event& ev) - { - switch (ev.code) + default: { - case EV_ENTRY: - { - // ... - - logStatus(FLYING_STATE); - - LOG_DEBUG(logger, "[FMM] entering state flying\n"); - break; - } - case EV_EXIT: - { - // ... - - LOG_DEBUG(logger, "[FMM] exiting state flying\n"); - break; - } - - default: - { - break; - } + break; } } +} - - void FMMController::state_debug(const Event& ev) +void FMMController::state_debug(const Event& ev) +{ + switch (ev.code) { - switch (ev.code) + case EV_ENTRY: { - case EV_ENTRY: - { - // ... - - logStatus(DEBUG_STATE); - - LOG_DEBUG(logger, "[FMM] entering state debug\n"); - break; - } - case EV_EXIT: - { - // ... - - LOG_DEBUG(logger, "[FMM] exiting state debug\n"); - break; - } - case EV_TC_EXIT_TEST_MODE: - { - transition(&FMMController::state_on_ground); - break; - } - default: - { - break; - } + // ... + + logStatus(DEBUG_STATE); + + LOG_DEBUG(logger, "[FMM] entering state debug\n"); + break; + } + case EV_EXIT: + { + // ... + + LOG_DEBUG(logger, "[FMM] exiting state debug\n"); + break; + } + case EV_TC_EXIT_TEST_MODE: + { + transition(&FMMController::state_on_ground); + break; + } + default: + { + break; } } +} - /* --- LOGGER --- */ +/* --- LOGGER --- */ - void FMMController::logStatus(FMMControllerState state) - { - status.state = state; - logStatus(); - } +void FMMController::logStatus(FMMControllerState state) +{ + status.state = state; + logStatus(); +} - void FMMController::logStatus() - { - status.timestamp = miosix::getTick(); - SDlogger -> log(status); - } +void FMMController::logStatus() +{ + status.timestamp = miosix::getTick(); + SDlogger->log(status); +} -} // namespace ParafoilTestDev +} // namespace Parafoil diff --git a/src/boards/Parafoil/FlightModeManager/FMMController.h b/src/boards/Parafoil/FlightModeManager/FMMController.h index f185f9452..1f03ed297 100644 --- a/src/boards/Parafoil/FlightModeManager/FMMController.h +++ b/src/boards/Parafoil/FlightModeManager/FMMController.h @@ -1,5 +1,5 @@ /* Copyright (c) 2022 Skyward Experimental Rocketry - * Authors: Matteo Pignataro + * Author: Matteo Pignataro * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -22,35 +22,34 @@ #pragma once -#include "FMMData.h" -#include <events/FSM.h> #include <diagnostic/PrintLogger.h> +#include <events/FSM.h> -using namespace Boardcore; +#include "FMMData.h" -namespace ParafoilTestDev +namespace Parafoil +{ +/** + * @brief FMM state machine + */ +class FMMController : public Boardcore::FSM<FMMController> { - /** - * @brief FMM state machine - */ - class FMMController : public FSM<FMMController> - { - public: - FMMController(); - ~FMMController(); - - void state_on_ground(const Event& ev); - void state_flying(const Event& ev); - void state_debug(const Event& ev); - - private: - FMMControllerStatus status; - PrintLogger logger = Logging::getLogger("FMM"); - Logger* SDlogger = &Logger::getInstance(); - - /* --- LOGGER --- */ - - void logStatus(FMMControllerState state); - void logStatus(); - }; -} // namespace ParafoilTestDev +public: + FMMController(); + ~FMMController(); + + void state_on_ground(const Boardcore::Event& ev); + void state_flying(const Boardcore::Event& ev); + void state_debug(const Boardcore::Event& ev); + +private: + FMMControllerStatus status; + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("FMM"); + Boardcore::Logger* SDlogger = &Boardcore::Logger::getInstance(); + + /* --- LOGGER --- */ + + void logStatus(FMMControllerState state); + void logStatus(); +}; +} // namespace Parafoil diff --git a/src/boards/Parafoil/FlightModeManager/FMMData.h b/src/boards/Parafoil/FlightModeManager/FMMData.h index a57776517..09a17d601 100644 --- a/src/boards/Parafoil/FlightModeManager/FMMData.h +++ b/src/boards/Parafoil/FlightModeManager/FMMData.h @@ -1,5 +1,5 @@ /* Copyright (c) 2022 Skyward Experimental Rocketry - * Authors: Matteo Pignataro + * Author: Matteo Pignataro * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -27,7 +27,7 @@ #include <iostream> #include <string> -namespace ParafoilTestDev +namespace Parafoil { /** @@ -48,10 +48,7 @@ struct FMMControllerStatus long long timestamp; FMMControllerState state; - static std::string header() - { - return "timestamp,state\n"; - } + static std::string header() { return "timestamp,state\n"; } void print(std::ostream& os) const { @@ -59,4 +56,4 @@ struct FMMControllerStatus } }; -} // namespace ParafoilTestDev +} // namespace Parafoil diff --git a/src/boards/Parafoil/Main/Radio.cpp b/src/boards/Parafoil/Main/Radio.cpp index dcee16022..20158cf3a 100644 --- a/src/boards/Parafoil/Main/Radio.cpp +++ b/src/boards/Parafoil/Main/Radio.cpp @@ -41,7 +41,7 @@ using namespace Boardcore; */ void __attribute__((used)) EXTI10_IRQHandlerImpl() { - using namespace ParafoilTestDev; + using namespace Parafoil; /*if (ParafoilTest::getInstance().radio->xbee != nullptr) { @@ -53,11 +53,9 @@ void __attribute__((used)) EXTI10_IRQHandlerImpl() } } -namespace ParafoilTestDev +namespace Parafoil { -/** - * PUBLIC METHODS - */ + Radio::Radio(SPIBusInterface& xbee_bus, TaskScheduler* scheduler) : xbee_bus(xbee_bus) { @@ -260,9 +258,6 @@ void Radio::logStatus() // TODO log } -/** - * PRIVATE METHODS - */ void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) { // Log the thing @@ -291,4 +286,4 @@ void Radio::init() // Enable external interrupt on F10 pin enableExternalInterrupt(GPIOF_BASE, 10, InterruptTrigger::FALLING_EDGE); } -} // namespace ParafoilTestDev \ No newline at end of file +} // namespace Parafoil diff --git a/src/boards/Parafoil/Main/Radio.h b/src/boards/Parafoil/Main/Radio.h index 2bb6dc053..9ce2c5327 100644 --- a/src/boards/Parafoil/Main/Radio.h +++ b/src/boards/Parafoil/Main/Radio.h @@ -19,96 +19,98 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #pragma once #include <Parafoil/TelemetriesTelecommands/Mavlink.h> -#include <radio/Xbee/Xbee.h> +#include <common/events/Events.h> #include <radio/SX1278/SX1278.h> +#include <radio/Xbee/Xbee.h> #include <scheduler/TaskScheduler.h> -#include <common/events/Events.h> /** * @brief This class represents the radio module. It allows the communications - * between the xbee radio module and the on board software. The main tasks are about - * sending automatic updates to ground based on the current state (High rate telemetry - * or Low rate telemetry) and handle the messages from the ground station. - * To handle the various messages there is an apposit callback method and, - * to pack the optional reply data we use the TMRepository singleton. + * between the xbee radio module and the on board software. The main tasks are + * about sending automatic updates to ground based on the current state (High + * rate telemetry or Low rate telemetry) and handle the messages from the ground + * station. To handle the various messages there is an apposit callback method + * and, to pack the optional reply data we use the TMRepository singleton. */ -namespace ParafoilTestDev +namespace Parafoil +{ + +class Radio { - class Radio - { - public: - /** - * @brief The xbee module driver - */ - //Xbee::Xbee* xbee; - SX1278* module; - - /** - * @brief Construct a new Radio object - * - * @param xbee_bus The Xbee SPI bus - */ - Radio(SPIBusInterface& xbee_bus, TaskScheduler* scheduler); - - /** - * @brief Destroy the Radio object - */ - ~Radio(); - - /** - * @brief Method called when a mavlink message is received - * - * @param msg The parsed message - */ - void handleMavlinkMessage(MavDriver* driver, const mavlink_message_t& msg); - - /** - * @brief This method is used to add in send queue - * the requested message. It is necessary to call the - * TMrepository to process the actual packet. - * - * @param tm_id The requested message id - * @return boolean that indicates the operation's result - */ - bool sendTelemetry(const uint8_t tm_id); - - /** - * @brief Method automatically called by the task - * scheduler that sends the high rate telemetry - */ - void sendHRTelemetry(); - - /** - * @brief Method automatically called by the task - * scheduler that sends the high rate telemetry - */ - void sendLRTelemetry(); - - /** - * @brief Every time a message is received we send - * an ack message to tell the ground station that we - * received the message - * - * @param msg The message received that we need to ack - */ - void sendAck(const mavlink_message_t& msg); - - /** - * @brief Method to change the telemetry rate - */ - void toggleHRTelemetry(); - - bool start(); - - void logStatus(); - - private: - - //1 to 1 corrispondence of every message to its event - const std::map<uint8_t, uint8_t> tcMap = { +public: + /** + * @brief The xbee module driver + */ + // Xbee::Xbee* xbee; + Boardcore::SX1278* module; + + /** + * @brief Construct a new Radio object + * + * @param xbee_bus The Xbee SPI bus + */ + Radio(Boardcore::SPIBusInterface& xbee_bus, + Boardcore::TaskScheduler* scheduler); + + /** + * @brief Destroy the Radio object + */ + ~Radio(); + + /** + * @brief Method called when a mavlink message is received + * + * @param msg The parsed message + */ + void handleMavlinkMessage(MavDriver* driver, const mavlink_message_t& msg); + + /** + * @brief This method is used to add in send queue + * the requested message. It is necessary to call the + * TMrepository to process the actual packet. + * + * @param tm_id The requested message id + * @return boolean that indicates the operation's result + */ + bool sendTelemetry(const uint8_t tm_id); + + /** + * @brief Method automatically called by the task + * scheduler that sends the high rate telemetry + */ + void sendHRTelemetry(); + + /** + * @brief Method automatically called by the task + * scheduler that sends the high rate telemetry + */ + void sendLRTelemetry(); + + /** + * @brief Every time a message is received we send + * an ack message to tell the ground station that we + * received the message + * + * @param msg The message received that we need to ack + */ + void sendAck(const mavlink_message_t& msg); + + /** + * @brief Method to change the telemetry rate + */ + void toggleHRTelemetry(); + + bool start(); + + void logStatus(); + +private: + // 1 to 1 corrispondence of every message to its event + const std::map<uint8_t, uint8_t> tcMap = { {MAV_CMD_ARM, EV_TC_ARM}, {MAV_CMD_DISARM, EV_TC_DISARM}, @@ -138,48 +140,49 @@ namespace ParafoilTestDev {MAV_CMD_BOARD_RESET, EV_TC_RESET_BOARD}, {MAV_CMD_END_MISSION, EV_TC_END_MISSION}}; - /** - * @brief The mavlink driver - */ - MavDriver* mav_driver; - - /** - * @brief SPI bus - */ - SPIBusInterface& xbee_bus; - - /** - * @brief Main task scheduler - */ - TaskScheduler* scheduler; - - /** - * @brief high rate telemetry status. - * false -> LR telemetry - * true -> HR telemetry - */ - bool HRtelemetry; - - /** - * @brief Logger - */ - PrintLogger logger = Logging::getLogger("Radio"); - - /** - * @brief SD logger (pre started because of the ParafoilTest.h main class) - */ - Logger* SDlogger = &Logger::getInstance(); - - /** - * @brief Radio frame received callback method. - * It's used for logging purposes - */ - void onXbeeFrameReceived(Xbee::APIFrame& frame); - - /** - * @brief Method to initialize all the variables and objects - * and to register the HR and LR tasks into the task scheduler - */ - void init(); - }; -} \ No newline at end of file + /** + * @brief The mavlink driver + */ + MavDriver* mav_driver; + + /** + * @brief SPI bus + */ + Boardcore::SPIBusInterface& xbee_bus; + + /** + * @brief Main task scheduler + */ + Boardcore::TaskScheduler* scheduler; + + /** + * @brief high rate telemetry status. + * false -> LR telemetry + * true -> HR telemetry + */ + bool HRtelemetry; + + /** + * @brief Logger + */ + Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("Radio"); + + /** + * @brief SD logger (pre started because of the ParafoilTest.h main class) + */ + Boardcore::Logger* SDlogger = &Boardcore::Logger::getInstance(); + + /** + * @brief Radio frame received callback method. + * It's used for logging purposes + */ + void onXbeeFrameReceived(Boardcore::Xbee::APIFrame& frame); + + /** + * @brief Method to initialize all the variables and objects + * and to register the HR and LR tasks into the task scheduler + */ + void init(); +}; + +} // namespace Parafoil diff --git a/src/boards/Parafoil/Main/Sensors.cpp b/src/boards/Parafoil/Main/Sensors.cpp index 1f3f57de8..36aa8ac56 100644 --- a/src/boards/Parafoil/Main/Sensors.cpp +++ b/src/boards/Parafoil/Main/Sensors.cpp @@ -29,11 +29,9 @@ using std::bind; using namespace Boardcore; -namespace ParafoilTestDev +namespace Parafoil { -/** - * PRIVATE METHODS - */ + void Sensors::MPU9250init() { SPIBusConfig spiConfig{}; @@ -109,7 +107,7 @@ void Sensors::BME280init() slave.config.clockDivider = SPI::ClockDivider::DIV_16; - auto config = BME280::BME280_CONFIG_ALL_ENABLED; + auto config = BME280::BME280_CONFIG_ALL_ENABLED; config.bits.oversamplingPressure = BME280::OVERSAMPLING_4; // Instantiate the object @@ -140,9 +138,6 @@ void Sensors::BME280Callback() TMRepository::getInstance().update(d); } -/** - * PUBLIC METHODS - */ Sensors::Sensors(SPIBusInterface& spi, TaskScheduler* scheduler) : spiInterface(spi) { @@ -156,6 +151,8 @@ Sensors::Sensors(SPIBusInterface& spi, TaskScheduler* scheduler) // Sensor manager instance // At this point sensors_map contains all the initialized sensors + // cppcheck-suppress noCopyConstructor + // cppcheck-suppress noOperatorEq manager = new SensorManager(sensors_map, scheduler); } @@ -175,4 +172,4 @@ bool Sensors::start() { return manager->start(); } void Sensors::calibrate() {} -} // namespace ParafoilTestDev \ No newline at end of file +} // namespace Parafoil diff --git a/src/boards/Parafoil/Main/Sensors.h b/src/boards/Parafoil/Main/Sensors.h index 8f6fd5b58..8c2604e0d 100644 --- a/src/boards/Parafoil/Main/Sensors.h +++ b/src/boards/Parafoil/Main/Sensors.h @@ -21,107 +21,105 @@ */ #pragma once -#include <sensors/SensorManager.h> +#include <sensors/BME280/BME280.h> #include <sensors/MPU9250/MPU9250.h> +#include <sensors/SensorManager.h> #include <sensors/UbloxGPS/UbloxGPS.h> -#include <sensors/BME280/BME280.h> - -using namespace Boardcore; /** * This class memorizes all the sensors instances. * Every instance is initialized by the constructor. Also every * sensor has its own status */ -namespace ParafoilTestDev +namespace Parafoil +{ + +class Sensors { - class Sensors - { - private: - - /** - * @brief Sensor manager object - */ - SensorManager* manager; - - /** - * @brief Sensor map that contains every sensor istance - * that needs to be sampled by the sensor manager - */ - SensorManager::SensorMap_t sensors_map; - - /** - * @brief SPI interface passed via constructor - */ - SPIBusInterface& spiInterface; - - /** - * @brief Sensors serial logger - */ - PrintLogger log = Logging::getLogger("sensors"); - - /** - * @brief SD logger singleton - */ - Logger* logger; - - /** - * @brief The MPU9250 IMU init function and - * sample callback - */ - void MPU9250init(); - void MPU9250Callback(); - - /** - * @brief GPS init function and sample callback - */ - void UbloxGPSinit(); - void UbloxGPSCallback(); - - /** - * @brief Barometer BME280 init function and sample callback - */ - void BME280init(); - void BME280Callback(); - - public: - - /** - * @brief MPU9250IMU - */ - MPU9250* imu_mpu9250; - - /** - * @brief GPS - */ - UbloxGPS* gps_ublox; - - /** - * @brief Barometer - */ - BME280* press_bme280; - - /** - * @brief Constructor - * @param The spi interface used for the sensors - * @param The task scheduler - */ - Sensors(SPIBusInterface& spi, TaskScheduler* scheduler); - - /** - * @brief Deconstructor - */ - ~Sensors(); - - /** - * @brief Starts the sensor manager to sample data - * @return boolean that indicates operation's result - */ - bool start(); - - /** - * @brief Calibrates the sensors that need to - */ - void calibrate(); - }; -} \ No newline at end of file +private: + /** + * @brief Sensor manager object + */ + Boardcore::SensorManager* manager; + + /** + * @brief Sensor map that contains every sensor istance + * that needs to be sampled by the sensor manager + */ + Boardcore::SensorManager::SensorMap_t sensors_map; + + /** + * @brief SPI interface passed via constructor + */ + Boardcore::SPIBusInterface& spiInterface; + + /** + * @brief Sensors serial logger + */ + Boardcore::PrintLogger log = Boardcore::Logging::getLogger("sensors"); + + /** + * @brief SD logger singleton + */ + Boardcore::Logger* logger; + + /** + * @brief The MPU9250 IMU init function and + * sample callback + */ + void MPU9250init(); + void MPU9250Callback(); + + /** + * @brief GPS init function and sample callback + */ + void UbloxGPSinit(); + void UbloxGPSCallback(); + + /** + * @brief Barometer BME280 init function and sample callback + */ + void BME280init(); + void BME280Callback(); + +public: + /** + * @brief MPU9250IMU + */ + Boardcore::MPU9250* imu_mpu9250; + + /** + * @brief GPS + */ + Boardcore::UbloxGPS* gps_ublox; + + /** + * @brief Barometer + */ + Boardcore::BME280* press_bme280; + + /** + * @brief Constructor + * @param The spi interface used for the sensors + * @param The task scheduler + */ + Sensors(Boardcore::SPIBusInterface& spi, + Boardcore::TaskScheduler* scheduler); + + /** + * @brief Deconstructor + */ + ~Sensors(); + + /** + * @brief Starts the sensor manager to sample data + * @return boolean that indicates operation's result + */ + bool start(); + + /** + * @brief Calibrates the sensors that need to + */ + void calibrate(); +}; +} // namespace Parafoil diff --git a/src/boards/Parafoil/ParafoilTest.h b/src/boards/Parafoil/ParafoilTest.h index 7482ef9a5..3706d6c63 100644 --- a/src/boards/Parafoil/ParafoilTest.h +++ b/src/boards/Parafoil/ParafoilTest.h @@ -31,17 +31,16 @@ #include <events/EventBroker.h> #include <miosix.h> -using namespace Boardcore; - /** * This class is the main singleton that keeps all the project objects. * It has all the instances and initializes all of them. */ -namespace ParafoilTestDev +namespace Parafoil { + enum ThreadIds : uint8_t { - THID_ENTRYPOINT = THID_FIRST_AVAILABLE_ID, + THID_ENTRYPOINT = Boardcore::THID_FIRST_AVAILABLE_ID, THID_FMM_FSM, THID_TMTC_FSM, THID_STATS_FSM, @@ -63,15 +62,15 @@ enum TaskIDs : uint8_t TASK_NAS_ID = 9 }; -class ParafoilTest : public Singleton<ParafoilTest> +class ParafoilTest : public Boardcore::Singleton<ParafoilTest> { - friend class Singleton<ParafoilTest>; + friend class Boardcore::Singleton<ParafoilTest>; public: /** * @brief Event broker */ - EventBroker* broker; + Boardcore::EventBroker* broker; /** * @brief Sensors collection @@ -97,7 +96,7 @@ public: /** * @brief Task scheduler */ - TaskScheduler* scheduler; + Boardcore::TaskScheduler* scheduler; /** * @brief Start method @@ -172,12 +171,12 @@ private: /** * @brief SDlogger in debug mode */ - PrintLogger log = Logging::getLogger("ParafoilTest"); + Boardcore::PrintLogger log = Boardcore::Logging::getLogger("ParafoilTest"); /** * @brief SDlogger singleton for SD */ - Logger* SDlogger; + Boardcore::Logger* SDlogger; /** * @brief Status memory @@ -190,7 +189,7 @@ private: ParafoilTest() { // Take the singleton instance of SD logger - SDlogger = &Logger::getInstance(); + SDlogger = &Boardcore::Logger::getInstance(); // Start the logging startSDlogger(); @@ -199,12 +198,12 @@ private: broker = &sEventBroker; // Create the task scheduler - scheduler = new TaskScheduler(); + scheduler = new Boardcore::TaskScheduler(); addSchedulerStatsTask(); // Create the sensors - SPIBusInterface* spiInterface1 = new SPIBus(SPI1); - sensors = new Sensors(*spiInterface1, scheduler); + Boardcore::SPIBusInterface* spiInterface1 = new Boardcore::SPIBus(SPI1); + sensors = new Sensors(*spiInterface1, scheduler); // Create the wing controller // wingController = new WingController(scheduler); @@ -213,8 +212,8 @@ private: // FMM = new FMMController(); // Create a new radio - SPIBusInterface* spiInterface4 = new SPIBus(SPI4); - radio = new Radio(*spiInterface4, scheduler); + Boardcore::SPIBusInterface* spiInterface4 = new Boardcore::SPIBus(SPI4); + radio = new Radio(*spiInterface4, scheduler); } void addSchedulerStatsTask() @@ -226,7 +225,7 @@ private: std::vector<Boardcore::TaskStatsResult> scheduler_stats = scheduler->getTaskStats(); - for (TaskStatsResult stat : scheduler_stats) + for (Boardcore::TaskStatsResult stat : scheduler_stats) { SDlogger->log(stat); } @@ -239,4 +238,5 @@ private: miosix::getTick()); } }; -} // namespace ParafoilTestDev \ No newline at end of file + +} // namespace Parafoil diff --git a/src/boards/Parafoil/ParafoilTestStatus.h b/src/boards/Parafoil/ParafoilTestStatus.h index 005deebd8..73f27c767 100644 --- a/src/boards/Parafoil/ParafoilTestStatus.h +++ b/src/boards/Parafoil/ParafoilTestStatus.h @@ -18,7 +18,7 @@ * 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. - */ + */ /** * This class is used to keep track of various main class @@ -27,49 +27,49 @@ #pragma once -#include <string> #include <ostream> +#include <string> -namespace ParafoilTestDev +namespace Parafoil { - enum ParafoilTestComponentStatus - { - ERROR = 0, - OK = 1 - }; - - struct ParafoilTestStatus - { - //If there is an error, this uint8_t reports it(OR) - uint8_t parafoil_test = OK; +enum ParafoilTestComponentStatus +{ + ERROR = 0, + OK = 1 +}; - //Specific errors - uint8_t logger = OK; - uint8_t eventBroker = OK; - uint8_t sensors = OK; - uint8_t FMM = OK; - uint8_t radio = OK; +struct ParafoilTestStatus +{ + // If there is an error, this uint8_t reports it(OR) + uint8_t parafoil_test = OK; - /** - * @brief Method to set a specific component in an error state - */ - void setError(uint8_t ParafoilTestStatus::*component) - { - //Put the passed component to error state - this->*component = ERROR; - //Logic OR - parafoil_test = ERROR; - } + // Specific errors + uint8_t logger = OK; + uint8_t eventBroker = OK; + uint8_t sensors = OK; + uint8_t FMM = OK; + uint8_t radio = OK; + /** + * @brief Method to set a specific component in an error state + */ + void setError(uint8_t ParafoilTestStatus::*component) + { + // Put the passed component to error state + this->*component = ERROR; + // Logic OR + parafoil_test = ERROR; + } - static std::string header() - { - return "logger, eventBorker, sensors, radio\n"; - } + static std::string header() + { + return "logger, eventBorker, sensors, radio\n"; + } - void print(std::ostream& os) - { - os << (int)logger << "," << (int)eventBroker << "," << (int)sensors << "," << (int)radio << "\n"; - } - }; -} \ No newline at end of file + void print(std::ostream& os) + { + os << (int)logger << "," << (int)eventBroker << "," << (int)sensors + << "," << (int)radio << "\n"; + } +}; +} // namespace Parafoil diff --git a/src/boards/Parafoil/TelemetriesTelecommands/Mavlink.h b/src/boards/Parafoil/TelemetriesTelecommands/Mavlink.h index 59f1fd075..2ddad64c8 100644 --- a/src/boards/Parafoil/TelemetriesTelecommands/Mavlink.h +++ b/src/boards/Parafoil/TelemetriesTelecommands/Mavlink.h @@ -30,12 +30,12 @@ #include <mavlink_lib/lynx/mavlink.h> #pragma GCC diagnostic pop -#include <radio/MavlinkDriver/MavlinkDriver.h> #include <Parafoil/Configs/MavlinkConfig.h> +#include <radio/MavlinkDriver/MavlinkDriver.h> -using namespace Boardcore; - -namespace ParafoilTestDev +namespace Parafoil { - using MavDriver = MavlinkDriver<MAV_PKT_SIZE, MAV_OUT_QUEUE_LEN>; + +using MavDriver = Boardcore::MavlinkDriver<MAV_PKT_SIZE, MAV_OUT_QUEUE_LEN>; + } diff --git a/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.cpp b/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.cpp index 8f73ce12c..31df13ba3 100644 --- a/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.cpp +++ b/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.cpp @@ -19,117 +19,120 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #include <Parafoil/TelemetriesTelecommands/TMRepository.h> -namespace ParafoilTestDev +using namespace Boardcore; + +namespace Parafoil { - /** - * PUBLIC METHODS - */ - mavlink_message_t TMRepository::packTM(uint8_t req_tm, uint8_t sys_id, uint8_t comp_id) - { - mavlink_message_t m; - mavlink_nack_tm_t nack_tm; - miosix::PauseKernelLock kLock; +mavlink_message_t TMRepository::packTM(uint8_t req_tm, uint8_t sys_id, + uint8_t comp_id) +{ + mavlink_message_t m; + mavlink_nack_tm_t nack_tm; + + miosix::PauseKernelLock kLock; - switch (req_tm) + switch (req_tm) + { + case MavTMList::MAV_SENSORS_TM_ID: + tm_repository.sensors_tm.timestamp = miosix::getTick(); + mavlink_msg_sensors_tm_encode(sys_id, comp_id, &m, + &(tm_repository.sensors_tm)); + break; + /*case MavTMList::MAV_SYS_TM_ID: + tm_repository.sys_tm.timestamp = miosix::getTick(); + mavlink_msg_sys_tm_encode(sys_id, comp_id, &m, + &(tm_repository.sys_tm)); + break; + case MavTMList::MAV_FMM_TM_ID: + tm_repository.fmm_tm.timestamp = miosix::getTick(); + mavlink_msg_fmm_tm_encode(sys_id, comp_id, &m, + &(tm_repository.fmm_tm)); + break; + case MavTMList::MAV_LOGGER_TM_ID: + tm_repository.logger_tm.timestamp = miosix::getTick(); + mavlink_msg_logger_tm_encode(sys_id, comp_id, &m, + &(tm_repository.logger_tm)); + break; + case MavTMList::MAV_TMTC_TM_ID: + tm_repository.tmtc_tm.timestamp = miosix::getTick(); + mavlink_msg_tmtc_tm_encode(sys_id, comp_id, &m, + &(tm_repository.tmtc_tm)); + break; + case MavTMList::MAV_TASK_STATS_TM_ID: + tm_repository.task_stats_tm.timestamp = miosix::getTick(); + mavlink_msg_task_stats_tm_encode(sys_id, comp_id, &m, + &(tm_repository.task_stats_tm)); + break; + case MavTMList::MAV_GPS_TM_ID: + tm_repository.gps_tm.timestamp = miosix::getTick(); + mavlink_msg_gps_tm_encode(sys_id, comp_id, &m, + &(tm_repository.gps_tm)); + break; + case MavTMList::MAV_HR_TM_ID: + tm_repository.hr_tm.timestamp = miosix::getTick(); + mavlink_msg_hr_tm_encode(sys_id, comp_id, &m, + &(tm_repository.hr_tm)); + break; + case MavTMList::MAV_LR_TM_ID: + //tm_repository.tm_repository.lr_tm.timestamp = miosix::getTick(); + mavlink_msg_lr_tm_encode(sys_id, comp_id, &m, + &(tm_repository.lr_tm)); + break;*/ + default: { - case MavTMList::MAV_SENSORS_TM_ID: - tm_repository.sensors_tm.timestamp = miosix::getTick(); - mavlink_msg_sensors_tm_encode(sys_id, comp_id, &m, - &(tm_repository.sensors_tm)); - break; - /*case MavTMList::MAV_SYS_TM_ID: - tm_repository.sys_tm.timestamp = miosix::getTick(); - mavlink_msg_sys_tm_encode(sys_id, comp_id, &m, - &(tm_repository.sys_tm)); - break; - case MavTMList::MAV_FMM_TM_ID: - tm_repository.fmm_tm.timestamp = miosix::getTick(); - mavlink_msg_fmm_tm_encode(sys_id, comp_id, &m, - &(tm_repository.fmm_tm)); - break; - case MavTMList::MAV_LOGGER_TM_ID: - tm_repository.logger_tm.timestamp = miosix::getTick(); - mavlink_msg_logger_tm_encode(sys_id, comp_id, &m, - &(tm_repository.logger_tm)); - break; - case MavTMList::MAV_TMTC_TM_ID: - tm_repository.tmtc_tm.timestamp = miosix::getTick(); - mavlink_msg_tmtc_tm_encode(sys_id, comp_id, &m, - &(tm_repository.tmtc_tm)); - break; - case MavTMList::MAV_TASK_STATS_TM_ID: - tm_repository.task_stats_tm.timestamp = miosix::getTick(); - mavlink_msg_task_stats_tm_encode(sys_id, comp_id, &m, - &(tm_repository.task_stats_tm)); - break; - case MavTMList::MAV_GPS_TM_ID: - tm_repository.gps_tm.timestamp = miosix::getTick(); - mavlink_msg_gps_tm_encode(sys_id, comp_id, &m, - &(tm_repository.gps_tm)); - break; - case MavTMList::MAV_HR_TM_ID: - tm_repository.hr_tm.timestamp = miosix::getTick(); - mavlink_msg_hr_tm_encode(sys_id, comp_id, &m, - &(tm_repository.hr_tm)); - break; - case MavTMList::MAV_LR_TM_ID: - //tm_repository.tm_repository.lr_tm.timestamp = miosix::getTick(); - mavlink_msg_lr_tm_encode(sys_id, comp_id, &m, - &(tm_repository.lr_tm)); - break;*/ - default: - { - LOG_DEBUG(logger, "Unknown telemetry id: %d", req_tm); - nack_tm.recv_msgid = 0; - nack_tm.seq_ack = 0; - mavlink_msg_nack_tm_encode(sys_id, comp_id, &m, &nack_tm); - break; - } + LOG_DEBUG(logger, "Unknown telemetry id: %d", req_tm); + nack_tm.recv_msgid = 0; + nack_tm.seq_ack = 0; + mavlink_msg_nack_tm_encode(sys_id, comp_id, &m, &nack_tm); + break; } - return m; } + return m; +} - //Implement all the update functions - void TMRepository::update(MPU9250Data data) - { - //Pause the kernel to avoid interructions during this fast operation - miosix::PauseKernelLock kLock; - //Update only the sensors message TODO update all the things - tm_repository.sensors_tm.bmx160_acc_x = data.accelerationX; - tm_repository.sensors_tm.bmx160_acc_y = data.accelerationY; - tm_repository.sensors_tm.bmx160_acc_z = data.accelerationZ; +// Implement all the update functions +void TMRepository::update(MPU9250Data data) +{ + // Pause the kernel to avoid interructions during this fast operation + miosix::PauseKernelLock kLock; + // Update only the sensors message TODO update all the things + tm_repository.sensors_tm.bmx160_acc_x = data.accelerationX; + tm_repository.sensors_tm.bmx160_acc_y = data.accelerationY; + tm_repository.sensors_tm.bmx160_acc_z = data.accelerationZ; - tm_repository.sensors_tm.bmx160_gyro_x = data.angularVelocityX; - tm_repository.sensors_tm.bmx160_gyro_y = data.angularVelocityY; - tm_repository.sensors_tm.bmx160_gyro_z = data.angularVelocityZ; + tm_repository.sensors_tm.bmx160_gyro_x = data.angularVelocityX; + tm_repository.sensors_tm.bmx160_gyro_y = data.angularVelocityY; + tm_repository.sensors_tm.bmx160_gyro_z = data.angularVelocityZ; - tm_repository.sensors_tm.bmx160_mag_x = data.magneticFieldX; - tm_repository.sensors_tm.bmx160_mag_y = data.magneticFieldY; - tm_repository.sensors_tm.bmx160_mag_z = data.magneticFieldZ; + tm_repository.sensors_tm.bmx160_mag_x = data.magneticFieldX; + tm_repository.sensors_tm.bmx160_mag_y = data.magneticFieldY; + tm_repository.sensors_tm.bmx160_mag_z = data.magneticFieldZ; - tm_repository.sensors_tm.bmx160_temp = data.temperature; - } + tm_repository.sensors_tm.bmx160_temp = data.temperature; +} - void TMRepository::update(UbloxGPSData data) - { - //Pause the kernel to avoid interructions during this fast operation - miosix::PauseKernelLock kLock; - //Update only the sensors message TODO update all the things - tm_repository.sensors_tm.gps_alt = data.height; - tm_repository.sensors_tm.gps_fix = data.fix; - tm_repository.sensors_tm.gps_lon = data.longitude; - tm_repository.sensors_tm.gps_lat = data.latitude; - } +void TMRepository::update(UbloxGPSData data) +{ + // Pause the kernel to avoid interructions during this fast operation + miosix::PauseKernelLock kLock; + // Update only the sensors message TODO update all the things + tm_repository.sensors_tm.gps_alt = data.height; + tm_repository.sensors_tm.gps_fix = data.fix; + tm_repository.sensors_tm.gps_lon = data.longitude; + tm_repository.sensors_tm.gps_lat = data.latitude; +} - void TMRepository::update(BME280Data data) - { - //Pause the kernel to avoid interructions during this fast operation - miosix::PauseKernelLock kLock; - //Update only the sensors message TODO update all the things - tm_repository.sensors_tm.ms5803_press = data.pressure; - tm_repository.sensors_tm.ms5803_temp = data.temperature; - } -} \ No newline at end of file +void TMRepository::update(BME280Data data) +{ + // Pause the kernel to avoid interructions during this fast operation + miosix::PauseKernelLock kLock; + // Update only the sensors message TODO update all the things + tm_repository.sensors_tm.ms5803_press = data.pressure; + tm_repository.sensors_tm.ms5803_temp = data.temperature; +} + +} // namespace Parafoil diff --git a/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.h b/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.h index 38eba7747..744f22bb9 100644 --- a/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.h +++ b/src/boards/Parafoil/TelemetriesTelecommands/TMRepository.h @@ -21,11 +21,11 @@ */ #pragma once +#include <Parafoil/TelemetriesTelecommands/Mavlink.h> #include <Singleton.h> +#include <sensors/BME280/BME280Data.h> #include <sensors/MPU9250/MPU9250Data.h> #include <sensors/UbloxGPS/UbloxGPSData.h> -#include <sensors/BME280/BME280Data.h> -#include <Parafoil/TelemetriesTelecommands/Mavlink.h> /** * @brief This class represents the collection of data that can @@ -35,71 +35,70 @@ * It is necessary that this singleton class handles the structure update * (when a message pack is requested). * The pack method is the core of the class. It returns a mavlink_message - * with the message data(specified with the id) requested. + * with the message data(specified with the id) requested. */ -using namespace Boardcore; - -namespace ParafoilTestDev +namespace Parafoil { - class TMRepository : public Singleton<TMRepository> - { - friend class Singleton<TMRepository>; - public: +class TMRepository : public Boardcore::Singleton<TMRepository> +{ + friend class Boardcore::Singleton<TMRepository>; - /** - * @brief Retrieve a telemetry message in packed form. - * - * @param req_tm required telemetry - * @param sys_id system id to pack it with - * @param comp_id component id to pack it with - * @return packed mavlink struct of that telemetry or a NACK_TM if - * the telemetry id was not found. - */ - mavlink_message_t packTM(uint8_t req_tm, uint8_t sys_id = TMTC_MAV_SYSID, - uint8_t comp_id = TMTC_MAV_COMPID); +public: + /** + * @brief Retrieve a telemetry message in packed form. + * + * @param req_tm required telemetry + * @param sys_id system id to pack it with + * @param comp_id component id to pack it with + * @return packed mavlink struct of that telemetry or a NACK_TM if + * the telemetry id was not found. + */ + mavlink_message_t packTM(uint8_t req_tm, uint8_t sys_id = TMTC_MAV_SYSID, + uint8_t comp_id = TMTC_MAV_COMPID); - /** - * @brief Update functions - */ - void update(MPU9250Data data); - void update(UbloxGPSData data); - void update(BME280Data data); + /** + * @brief Update functions + */ + void update(Boardcore::MPU9250Data data); + void update(Boardcore::UbloxGPSData data); + void update(Boardcore::BME280Data data); - private: - /** - * @brief Struct containing all TMs in the form of mavlink messages. - */ - struct TmRepository_t - { - mavlink_sys_tm_t sys_tm; - mavlink_pin_obs_tm_t pin_obs_tm; - mavlink_logger_tm_t logger_tm; - mavlink_fmm_tm_t fmm_tm; - mavlink_tmtc_tm_t tmtc_tm; - mavlink_task_stats_tm_t task_stats_tm; - mavlink_dpl_tm_t dpl_tm; - mavlink_ada_tm_t ada_tm; - mavlink_abk_tm_t abk_tm; - mavlink_nas_tm_t nas_tm; +private: + /** + * @brief Struct containing all TMs in the form of mavlink messages. + */ + struct TmRepository_t + { + mavlink_sys_tm_t sys_tm; + mavlink_pin_obs_tm_t pin_obs_tm; + mavlink_logger_tm_t logger_tm; + mavlink_fmm_tm_t fmm_tm; + mavlink_tmtc_tm_t tmtc_tm; + mavlink_task_stats_tm_t task_stats_tm; + mavlink_dpl_tm_t dpl_tm; + mavlink_ada_tm_t ada_tm; + mavlink_abk_tm_t abk_tm; + mavlink_nas_tm_t nas_tm; - mavlink_ms5803_tm_t digital_baro_tm; - mavlink_bmx160_tm_t bmx_tm; - mavlink_lis3mdl_tm_t lis3mdl_tm; - mavlink_adc_tm_t adc_tm; - mavlink_gps_tm_t gps_tm; + mavlink_ms5803_tm_t digital_baro_tm; + mavlink_bmx160_tm_t bmx_tm; + mavlink_lis3mdl_tm_t lis3mdl_tm; + mavlink_adc_tm_t adc_tm; + mavlink_gps_tm_t gps_tm; - mavlink_hr_tm_t hr_tm; - mavlink_lr_tm_t lr_tm; - mavlink_windtunnel_tm_t wind_tm; - mavlink_sensors_tm_t sensors_tm; - mavlink_test_tm_t test_tm; - } tm_repository; + mavlink_hr_tm_t hr_tm; + mavlink_lr_tm_t lr_tm; + mavlink_windtunnel_tm_t wind_tm; + mavlink_sensors_tm_t sensors_tm; + mavlink_test_tm_t test_tm; + } tm_repository; - /** - * @brief Logger - */ - PrintLogger logger = Logging::getLogger("TMRepository"); - }; -} \ No newline at end of file + /** + * @brief Logger + */ + Boardcore::PrintLogger logger = + Boardcore::Logging::getLogger("TMRepository"); +}; +} // namespace Parafoil diff --git a/src/boards/Parafoil/Wing/WingAlgorithm.cpp b/src/boards/Parafoil/Wing/WingAlgorithm.cpp index 38f120d2c..641c70ae4 100644 --- a/src/boards/Parafoil/Wing/WingAlgorithm.cpp +++ b/src/boards/Parafoil/Wing/WingAlgorithm.cpp @@ -23,140 +23,137 @@ #include <Parafoil/Wing/WingAlgorithm.h> #include <drivers/timer/TimestampTimer.h> -namespace ParafoilTestDev +using namespace Boardcore; + +namespace Parafoil +{ +std::istream& operator>>(std::istream& input, WingAlgorithmData& data) +{ + input >> data.timestamp; + input.ignore(1, ','); + input >> data.servo1Angle; + input.ignore(1, ','); + input >> data.servo2Angle; + return input; +} + +WingAlgorithm::WingAlgorithm(ServoInterface* servo1, ServoInterface* servo2, + const char* filename) + : parser(filename) +{ + setServo(servo1, servo2); +} + +WingAlgorithm::WingAlgorithm(const char* filename) : parser(filename) {} + +bool WingAlgorithm::init() +{ + // Returns a std::vector which contains + // all the csv parsed with the data structure in mind + steps = parser.collect(); + + // Return if the size collected is greater than 0 + fileValid = steps.size() > 0; + return fileValid; +} + +void WingAlgorithm::setServo(ServoInterface* servo1, ServoInterface* servo2) { - std::istream& operator>>(std::istream& input, WingAlgorithmData& data) + if (servo1 != NULL) { - input >> data.timestamp; - input.ignore(1, ','); - input >> data.servo1Angle; - input.ignore(1, ','); - input >> data.servo2Angle; - return input; + this->servo1 = servo1; } - - /** - * PUBLIC METHODS - */ - WingAlgorithm::WingAlgorithm(ServoInterface* servo1, ServoInterface* servo2, const char* filename) - : parser(filename) + else { - setServo(servo1, servo2); + // In this case i create a standard servo from the wing config file + servo1 = + new WingServo(WING_SERVO1_TIMER, WING_SERVO1_PWM_CHANNEL, + WING_SERVO1_MIN_POSITION, WING_SERVO1_MAX_POSITION, + WING_SERVO1_RESET_POSITION); } - WingAlgorithm::WingAlgorithm(const char* filename) - : parser(filename){} - - bool WingAlgorithm::init() + if (servo2 != NULL) { - //Returns a std::vector which contains - //all the csv parsed with the data structure in mind - steps = parser.collect(); - - //Return if the size collected is greater than 0 - fileValid = steps.size() > 0; - return fileValid; + this->servo2 = servo2; } - - void WingAlgorithm::setServo(ServoInterface* servo1, ServoInterface* servo2) + else { - if(servo1 != NULL) - { - this -> servo1 = servo1; - } - else - { - //In this case i create a standard servo from the wing config file - servo1 = new WingServo(WING_SERVO1_TIMER, - WING_SERVO1_PWM_CHANNEL, - WING_SERVO1_MIN_POSITION, - WING_SERVO1_MAX_POSITION, - WING_SERVO1_RESET_POSITION); - } - - if(servo2 != NULL) - { - this -> servo2 = servo2; - } - else - { - //In this case i create a standard servo from the wing config file - servo2 = new WingServo(WING_SERVO2_TIMER, - WING_SERVO2_PWM_CHANNEL, - WING_SERVO2_MIN_POSITION, - WING_SERVO2_MAX_POSITION, - WING_SERVO2_RESET_POSITION); - } + // In this case i create a standard servo from the wing config file + servo2 = + new WingServo(WING_SERVO2_TIMER, WING_SERVO2_PWM_CHANNEL, + WING_SERVO2_MIN_POSITION, WING_SERVO2_MAX_POSITION, + WING_SERVO2_RESET_POSITION); } +} - void WingAlgorithm::addStep(WingAlgorithmData step) +void WingAlgorithm::addStep(WingAlgorithmData step) +{ + // I do it if and only if the file is invalid, because + // i don't want to mess up with the timestamp order + if (!fileValid) { - //I do it if and only if the file is invalid, because - //i don't want to mess up with the timestamp order - if(!fileValid) - { - //Add it to the std::vector at the end - steps.push_back(step); - } + // Add it to the std::vector at the end + steps.push_back(step); } +} + +void WingAlgorithm::begin() +{ + running = true; + shouldReset = true; + // Set the current timestamp + timeStart = TimestampTimer::getInstance().getTimestamp(); +} - void WingAlgorithm::begin() +void WingAlgorithm::end() +{ + running = false; + // Set the offset timestamp to 0 + timeStart = 0; +} + +/** + * PROTECTED METHODS + */ +void WingAlgorithm::step() +{ + // Variable to remember what is the step that has to be done + static unsigned int stepIndex = 0; + uint64_t currentTimestamp = TimestampTimer::getInstance().getTimestamp(); + + if (shouldReset) { - running = true; - shouldReset = true; - //Set the current timestamp - timeStart = TimestampTimer::getInstance().getTimestamp(); + // If the algorithm has been stopped + // i want to start from the beginning + stepIndex = 0; + shouldReset = false; } - void WingAlgorithm::end() + if (stepIndex >= steps.size()) { - running = false; - //Set the offset timestamp to 0 - timeStart = 0; + // End the procedure so it won't be executed + end(); + // Set the index to 0 in case of another future execution + stepIndex = 0; + // Terminate here + return; } - /** - * PROTECTED METHODS - */ - void WingAlgorithm::step() + if (currentTimestamp - timeStart >= steps[stepIndex].timestamp) { - //Variable to remember what is the step that has to be done - static unsigned int stepIndex = 0; - uint64_t currentTimestamp = TimestampTimer::getInstance().getTimestamp(); - - if(shouldReset) + // TODO log the action + // I need to execute the current step (if not null servos) + if (servo1 != NULL) { - //If the algorithm has been stopped - //i want to start from the beginning - stepIndex = 0; - shouldReset = false; + servo1->set(steps[stepIndex].servo1Angle); } - - if(stepIndex >= steps.size()) + if (servo2 != NULL) { - //End the procedure so it won't be executed - end(); - //Set the index to 0 in case of another future execution - stepIndex = 0; - //Terminate here - return; + servo2->set(steps[stepIndex].servo2Angle); } - if(currentTimestamp - timeStart >= steps[stepIndex].timestamp) - { - //TODO log the action - //I need to execute the current step (if not null servos) - if(servo1 != NULL) - { - servo1->set(steps[stepIndex].servo1Angle); - } - if(servo2 != NULL) - { - servo2->set(steps[stepIndex].servo2Angle); - } - - //finally increment the stepIndex - stepIndex++; - } + // finally increment the stepIndex + stepIndex++; } -} \ No newline at end of file +} +} // namespace Parafoil diff --git a/src/boards/Parafoil/Wing/WingAlgorithm.h b/src/boards/Parafoil/Wing/WingAlgorithm.h index ad4aa33f3..bd7158f2b 100644 --- a/src/boards/Parafoil/Wing/WingAlgorithm.h +++ b/src/boards/Parafoil/Wing/WingAlgorithm.h @@ -22,9 +22,9 @@ #pragma once -#include <utils/CSVReader/CSVReader.h> -#include <common/Algorithm.h> #include <Parafoil/Wing/WingServo.h> +#include <common/Algorithm.h> +#include <utils/CSVReader/CSVReader.h> /** * @brief This class abstracts the concept of wing timestamp based @@ -32,114 +32,114 @@ * We use a CSV parser to properly parse the procedure and every step * we check if it is time to advance and in case actuate teh step with * the two servos. - * + * * Brief use case: * WingServo servo1... * WingServo servo2... - * + * * servo1.enable(); * servo2.enable(); - * + * * WingAlgorithm algorithm(&servo1, &servo2, "Optional File") * algorithm.init(); - * + * * //In case of a non valid file/empty string you can add the steps * algorithm.addStep(WingAlgorithmData{timestamp, angle1, angle2}); - * + * * algorithm.begin(); - * + * * algorithm.update()... - * + * * //End of algorithm - * + * * algorithm.begin(); - * + * * algorithm.update()... */ -using namespace Boardcore; - -namespace ParafoilTestDev +namespace Parafoil { - struct WingAlgorithmData - { - uint64_t timestamp; //First timestamp is 0 (in microseconds) - float servo1Angle; //degrees - float servo2Angle; //degrees - }; - - class WingAlgorithm : public Algorithm - { - public: +struct WingAlgorithmData +{ + uint64_t timestamp; // First timestamp is 0 (in microseconds) + float servo1Angle; // degrees + float servo2Angle; // degrees +}; - /** - * @brief Construct a new Wing Algorithm object - * - * @param servo1 The first servo - * @param servo2 The second servo - * @param filename The csv file where all the operations are stored - */ - WingAlgorithm(ServoInterface* servo1, ServoInterface* servo2, const char* filename); +class WingAlgorithm : public Algorithm +{ +public: + /** + * @brief Construct a new Wing Algorithm object + * + * @param servo1 The first servo + * @param servo2 The second servo + * @param filename The csv file where all the operations are stored + */ + WingAlgorithm(ServoInterface* servo1, ServoInterface* servo2, + const char* filename); - /** - * @brief Construct a new Wing Algorithm object - * - * @param filename The csv file where all the operations are stored - */ - WingAlgorithm(const char* filename); + /** + * @brief Construct a new Wing Algorithm object + * + * @param filename The csv file where all the operations are stored + */ + WingAlgorithm(const char* filename); - /** - * @brief This method parses the file and stores it into a std::vector - * - * @return true If the initialization finished correctly - * @return false If something went wrong - */ - bool init() override; + /** + * @brief This method parses the file and stores it into a std::vector + * + * @return true If the initialization finished correctly + * @return false If something went wrong + */ + bool init() override; - /** - * @brief Set the Servos objects - * @param servo1 The first algorithm servo - * @param servo2 The second algorithm servo - */ - void setServo(ServoInterface* servo1, ServoInterface* servo2); + /** + * @brief Set the Servos objects + * @param servo1 The first algorithm servo + * @param servo2 The second algorithm servo + */ + void setServo(ServoInterface* servo1, ServoInterface* servo2); - /** - * @brief Adds manually the step in case of fast debug needs - * - * @param step The data that describes a step(timestamp, servo1 angle, servo2 angle) - */ - void addStep(WingAlgorithmData step); + /** + * @brief Adds manually the step in case of fast debug needs + * + * @param step The data that describes a step(timestamp, servo1 angle, + * servo2 angle) + */ + void addStep(WingAlgorithmData step); - /** - * @brief This sets the reference timestamp for the algorithm - */ - void begin(); + /** + * @brief This sets the reference timestamp for the algorithm + */ + void begin(); - /** - * @brief This disables the algorithm - */ - void end(); + /** + * @brief This disables the algorithm + */ + void end(); - protected: - //Actuators - ServoInterface* servo1; - ServoInterface* servo2; - //Offset timestamp - uint64_t timeStart; - //Procedure - std::vector<WingAlgorithmData> steps; - //File parser - CSVParser<WingAlgorithmData> parser; - bool fileValid = false; - //This boolean is used to understand when to reset - //the index where the algorithm has stopped. - //In case of end call, we want to be able to perform - //another time this algorithm starting from 0 - bool shouldReset = false; +protected: + // Actuators + ServoInterface* servo1; + ServoInterface* servo2; + // Offset timestamp + uint64_t timeStart; + // Procedure + std::vector<WingAlgorithmData> steps; + // File parser + Boardcore::CSVParser<WingAlgorithmData> parser; + bool fileValid = false; + // This boolean is used to understand when to reset + // the index where the algorithm has stopped. + // In case of end call, we want to be able to perform + // another time this algorithm starting from 0 + bool shouldReset = false; - /** - * @brief This method implements the algorithm step based on the current timestamp - */ - void step() override; - }; -} \ No newline at end of file + /** + * @brief This method implements the algorithm step based on the current + * timestamp + */ + void step() override; +}; +} // namespace Parafoil diff --git a/src/boards/Parafoil/Wing/WingConfig.h b/src/boards/Parafoil/Wing/WingConfig.h index 213f00bf3..c11bea6c1 100644 --- a/src/boards/Parafoil/Wing/WingConfig.h +++ b/src/boards/Parafoil/Wing/WingConfig.h @@ -18,47 +18,47 @@ * 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/timer/PWM.h> -using namespace Boardcore; - /** * @brief This class defines all the wing configs * (E.g. Servos PWM channels, Servos max and min positions, etc..) */ -namespace ParafoilTestDev +namespace Parafoil { - /** - * ALGORITHM CONFIGURATION - */ - static const uint32_t WING_UPDATE_PERIOD = 10; //milliseconds - static const uint8_t WING_CONTROLLER_ID = 100; //TODO define a correct ID +/** + * ALGORITHM CONFIGURATION + */ +static const uint32_t WING_UPDATE_PERIOD = 10; // milliseconds +static const uint8_t WING_CONTROLLER_ID = 100; // TODO define a correct ID - /** - * SERVOS CONFIGURATIONS - */ +/** + * SERVOS CONFIGURATIONS + */ - //16 bit, 45MHz, no DMA timer - static TIM_TypeDef* WING_SERVO1_TIMER = TIM13; - static TIM_TypeDef* WING_SERVO2_TIMER = TIM14; +// 16 bit, 45MHz, no DMA timer +static TIM_TypeDef* WING_SERVO1_TIMER = TIM13; +static TIM_TypeDef* WING_SERVO2_TIMER = TIM14; - static const TimerUtils::Channel WING_SERVO1_PWM_CHANNEL = TimerUtils::Channel::CHANNEL_1; - static const TimerUtils::Channel WING_SERVO2_PWM_CHANNEL = TimerUtils::Channel::CHANNEL_1; +static const Boardcore::TimerUtils::Channel WING_SERVO1_PWM_CHANNEL = + Boardcore::TimerUtils::Channel::CHANNEL_1; +static const Boardcore::TimerUtils::Channel WING_SERVO2_PWM_CHANNEL = + Boardcore::TimerUtils::Channel::CHANNEL_1; - //Servo dipendent variables - static const unsigned int WING_SERVO_MIN_PULSE = 1000; - static const unsigned int WING_SERVO_MAX_PULSE = 2500; - static const unsigned int WING_SERVO_FREQUENCY = 50; +// Servo dipendent variables +static const unsigned int WING_SERVO_MIN_PULSE = 1000; +static const unsigned int WING_SERVO_MAX_PULSE = 2500; +static const unsigned int WING_SERVO_FREQUENCY = 50; - static const float WING_SERVO1_MAX_POSITION = 180; //degrees - static const float WING_SERVO2_MAX_POSITION = 180; //degrees +static const float WING_SERVO1_MAX_POSITION = 180; // degrees +static const float WING_SERVO2_MAX_POSITION = 180; // degrees - static const float WING_SERVO1_MIN_POSITION = 0; //degrees - static const float WING_SERVO2_MIN_POSITION = 0; //degrees +static const float WING_SERVO1_MIN_POSITION = 0; // degrees +static const float WING_SERVO2_MIN_POSITION = 0; // degrees - static const float WING_SERVO1_RESET_POSITION = 90; //degrees - static const float WING_SERVO2_RESET_POSITION = 90; //degrees -} \ No newline at end of file +static const float WING_SERVO1_RESET_POSITION = 90; // degrees +static const float WING_SERVO2_RESET_POSITION = 90; // degrees +} // namespace Parafoil diff --git a/src/boards/Parafoil/Wing/WingController.cpp b/src/boards/Parafoil/Wing/WingController.cpp index a32ad0580..6e3ead4f6 100644 --- a/src/boards/Parafoil/Wing/WingController.cpp +++ b/src/boards/Parafoil/Wing/WingController.cpp @@ -22,137 +22,129 @@ #include <Parafoil/Wing/WingConfig.h> #include <Parafoil/Wing/WingController.h> -namespace ParafoilTestDev +using namespace Boardcore; + +namespace Parafoil { - /** - * PUBLIC METHODS - */ - WingController::WingController(TaskScheduler* scheduler) - { - //Assign the task scheduler - this -> scheduler = scheduler; - //Set the current running state - this -> running = false; +WingController::WingController(TaskScheduler* scheduler) +{ + // Assign the task scheduler + this->scheduler = scheduler; - //Set the current selected algorithm to 0 - this -> selectedAlgorithm = 0; + // Set the current running state + this->running = false; - //Initialize the servos, enable them, - //register the task into the task scheduler - init(); - } + // Set the current selected algorithm to 0 + this->selectedAlgorithm = 0; + + // Initialize the servos, enable them, + // register the task into the task scheduler + init(); +} + +WingController::~WingController() +{ + // Delete the servos + delete servo1; + delete servo2; +} - WingController::~WingController() +void WingController::addAlgorithm(const char* filename) +{ + // Create the algorithm + WingAlgorithm* algorithm = new WingAlgorithm(servo1, servo2, filename); + + // Add the algorithm to the vector and init it + algorithms.push_back(algorithm); + // If init fails[Beacuse of inexistent file or stuff] doesn't matter + // Because the algorithm is empty and so it won't do anything + algorithms[algorithms.size() - 1]->init(); +} + +void WingController::addAlgorithm(WingAlgorithm* algorithm) +{ + // Ensure that the servos are correct + algorithm->setServo(servo1, servo2); + + // Add the algorithm to the vector + algorithms.push_back(algorithm); +} + +void WingController::selectAlgorithm(unsigned int index) +{ + if (index >= 0 && index < algorithms.size()) { - //Delete the servos - delete servo1; - delete servo2; + selectedAlgorithm = index; } - - void WingController::addAlgorithm(const char* filename) + else { - //Create the algorithm - WingAlgorithm* algorithm = new WingAlgorithm(servo1, servo2, filename); - - //Add the algorithm to the vector and init it - algorithms.push_back(algorithm); - //If init fails[Beacuse of inexistent file or stuff] doesn't matter - //Because the algorithm is empty and so it won't do anything - algorithms[algorithms.size() - 1] -> init(); + // I select the 0 algorithm + selectedAlgorithm = 0; } +} - void WingController::addAlgorithm(WingAlgorithm* algorithm) +void WingController::start() +{ + // If the selected algorithm is valid --> also the + // algorithms array is not empty i start the whole thing + if (selectedAlgorithm >= 0 && selectedAlgorithm < algorithms.size()) { - //Ensure that the servos are correct - algorithm->setServo(servo1, servo2); + // Set the boolean that enables the update method to true + running = true; - //Add the algorithm to the vector - algorithms.push_back(algorithm); - } + // Begin the selected algorithm + algorithms[selectedAlgorithm]->begin(); - void WingController::selectAlgorithm(unsigned int index) - { - if(index >= 0 && index < algorithms.size()) - { - selectedAlgorithm = index; - } - else - { - //I select the 0 algorithm - selectedAlgorithm = 0; - } + // Also start the scheduler + scheduler->start(); } +} - void WingController::start() +void WingController::stop() +{ + // Set running to false so that the update method doesn't act + running = false; + // Stop the algorithm if selected + if (selectedAlgorithm >= 0 && selectedAlgorithm < algorithms.size()) { - //If the selected algorithm is valid --> also the - //algorithms array is not empty i start the whole thing - if(selectedAlgorithm >= 0 && selectedAlgorithm < algorithms.size()) - { - //Set the boolean that enables the update method to true - running = true; - - //Begin the selected algorithm - algorithms[selectedAlgorithm] -> begin(); - - //Also start the scheduler - scheduler -> start(); - } + algorithms[selectedAlgorithm]->end(); } +} - void WingController::stop() +void WingController::update() +{ + // Check if the algorithm is running + if (!running) { - //Set running to false so that the update method doesn't act - running = false; - //Stop the algorithm if selected - if(selectedAlgorithm >= 0 && selectedAlgorithm < algorithms.size()) - { - algorithms[selectedAlgorithm] -> end(); - } + return; } - void WingController::update() + // If the selected algorithm is valid i can update it + if (selectedAlgorithm >= 0 && selectedAlgorithm < algorithms.size()) { - //Check if the algorithm is running - if(!running) - { - return; - } - - //If the selected algorithm is valid i can update it - if(selectedAlgorithm >= 0 && selectedAlgorithm < algorithms.size()) - { - algorithms[selectedAlgorithm] -> update(); - } + algorithms[selectedAlgorithm]->update(); } +} - /** - * PRIVATE METHODS - */ - void WingController::init() - { - //Instanciate the servos - servo1 = new WingServo(WING_SERVO1_TIMER, - WING_SERVO1_PWM_CHANNEL, - WING_SERVO1_MIN_POSITION, - WING_SERVO1_MAX_POSITION, - WING_SERVO1_RESET_POSITION); - - servo2 = new WingServo(WING_SERVO2_TIMER, - WING_SERVO2_PWM_CHANNEL, - WING_SERVO2_MIN_POSITION, - WING_SERVO2_MAX_POSITION, - WING_SERVO2_RESET_POSITION); - - //Enable servos - servo1 -> enable(); - servo2 -> enable(); - - //Register the task - TaskScheduler::function_t updateFunction([=]() {update();}); - - scheduler -> addTask(updateFunction, WING_UPDATE_PERIOD, - WING_CONTROLLER_ID); - } -} \ No newline at end of file +void WingController::init() +{ + // Instanciate the servos + servo1 = new WingServo(WING_SERVO1_TIMER, WING_SERVO1_PWM_CHANNEL, + WING_SERVO1_MIN_POSITION, WING_SERVO1_MAX_POSITION, + WING_SERVO1_RESET_POSITION); + + servo2 = new WingServo(WING_SERVO2_TIMER, WING_SERVO2_PWM_CHANNEL, + WING_SERVO2_MIN_POSITION, WING_SERVO2_MAX_POSITION, + WING_SERVO2_RESET_POSITION); + + // Enable servos + servo1->enable(); + servo2->enable(); + + // Register the task + TaskScheduler::function_t updateFunction([=]() { update(); }); + + scheduler->addTask(updateFunction, WING_UPDATE_PERIOD, WING_CONTROLLER_ID); +} +} // namespace Parafoil diff --git a/src/boards/Parafoil/Wing/WingController.h b/src/boards/Parafoil/Wing/WingController.h index 1e9e0c488..0cd099061 100644 --- a/src/boards/Parafoil/Wing/WingController.h +++ b/src/boards/Parafoil/Wing/WingController.h @@ -19,6 +19,7 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #pragma once #include <Parafoil/Wing/WingAlgorithm.h> @@ -26,118 +27,116 @@ /** * @brief This class allows the user to select the wing algorithm - * that has to be used during the tests. It also registers his + * that has to be used during the tests. It also registers his * dedicated function in the task schduler in order to be * executed every fixed period and to update the two servos position * depending on the selected algorithm. - * + * * Use case example: * controller = new WingController(scheduler); - * + * * controller.addAlgorithm("filename"); * OR * controller.addAlgorithm(algorithm); - * + * * controller.selectAlgorithm(index); - * + * * controller.start(); * controller.stop(); //If you want to abort the execution * controller.start(); //If you want to start again from the beginning */ -using namespace Boardcore; - -namespace ParafoilTestDev +namespace Parafoil +{ +class WingController { - class WingController - { - private: - /** - * @brief Servo actuators - */ - ServoInterface* servo1; - ServoInterface* servo2; - - /** - * @brief List of loaded algorithms (from SD or not) - */ - std::vector<WingAlgorithm*> algorithms; - - /** - * @brief The common task scheduler - */ - TaskScheduler* scheduler; - - /** - * @brief This attribute is modified by the mavlink radio section. - * The user using the Ground Station can select the pre-enumered algorithm to execute - */ - unsigned int selectedAlgorithm; - - /** - * @brief Internal running state - */ - bool running; - - /** - * @brief Initialization method. It registers the update method - * into the task scheduler - */ - void init(); - - public: - - /** - * @brief Construct a new Wing Controller object - * - * @param scheduler The main used task scheduler - */ - WingController(TaskScheduler* scheduler); - - /** - * @brief Destroy the Wing Controller object. - * In particular destroys the servo instances - */ - ~WingController(); - - /** - * @brief Method to add the algorithm in the list - * - * @param filename The SD file where to read the instructions - */ - void addAlgorithm(const char* filename); - - /** - * @brief Method to add the algorithm in the list - * - * @param algorithm The algorithm with - * all already done (e.g. steps already registered) - */ - void addAlgorithm(WingAlgorithm* algorithm); - - /** - * @brief Selects the algorithm if present. - * - * @param index The algorithms vector index - */ - void selectAlgorithm(unsigned int index); - - /** - * @brief Sets the internal state running and - * starts the selected algorithm - */ - void start(); - - /** - * @brief Sets the internal state to stop and - * stops the selected algorithm - */ - void stop(); - - /** - * @brief Method that is called every time period - * to update the internal wing servos states - */ - void update(); - }; -} \ No newline at end of file +private: + /** + * @brief Servo actuators + */ + ServoInterface* servo1; + ServoInterface* servo2; + + /** + * @brief List of loaded algorithms (from SD or not) + */ + std::vector<WingAlgorithm*> algorithms; + + /** + * @brief The common task scheduler + */ + Boardcore::TaskScheduler* scheduler; + + /** + * @brief This attribute is modified by the mavlink radio section. + * The user using the Ground Station can select the pre-enumered algorithm + * to execute + */ + unsigned int selectedAlgorithm; + + /** + * @brief Internal running state + */ + bool running; + + /** + * @brief Initialization method. It registers the update method + * into the task scheduler + */ + void init(); + +public: + /** + * @brief Construct a new Wing Controller object + * + * @param scheduler The main used task scheduler + */ + WingController(Boardcore::TaskScheduler* scheduler); + + /** + * @brief Destroy the Wing Controller object. + * In particular destroys the servo instances + */ + ~WingController(); + + /** + * @brief Method to add the algorithm in the list + * + * @param filename The SD file where to read the instructions + */ + void addAlgorithm(const char* filename); + + /** + * @brief Method to add the algorithm in the list + * + * @param algorithm The algorithm with + * all already done (e.g. steps already registered) + */ + void addAlgorithm(WingAlgorithm* algorithm); + + /** + * @brief Selects the algorithm if present. + * + * @param index The algorithms vector index + */ + void selectAlgorithm(unsigned int index); + + /** + * @brief Sets the internal state running and + * starts the selected algorithm + */ + void start(); + + /** + * @brief Sets the internal state to stop and + * stops the selected algorithm + */ + void stop(); + + /** + * @brief Method that is called every time period + * to update the internal wing servos states + */ + void update(); +}; +} // namespace Parafoil diff --git a/src/boards/Parafoil/Wing/WingServo.cpp b/src/boards/Parafoil/Wing/WingServo.cpp index d10aabb4e..26167e703 100644 --- a/src/boards/Parafoil/Wing/WingServo.cpp +++ b/src/boards/Parafoil/Wing/WingServo.cpp @@ -19,57 +19,49 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #include <Parafoil/Wing/WingServo.h> -namespace ParafoilTestDev +using namespace Boardcore; + +namespace Parafoil +{ + +WingServo::WingServo(TIM_TypeDef* timer, TimerUtils::Channel pwmChannel, + float minPosition, float maxPosition) + : WingServo(timer, pwmChannel, minPosition, maxPosition, minPosition) { - /** - * PUBLIC METHODS - */ - WingServo::WingServo(TIM_TypeDef* timer, TimerUtils::Channel pwmChannel, float minPosition, float maxPosition) - :WingServo(timer, pwmChannel, minPosition, maxPosition, minPosition){} - - WingServo::WingServo(TIM_TypeDef* timer, TimerUtils::Channel pwmChannel, float minPosition, float maxPosition, float resetPosition) - :ServoInterface(minPosition, maxPosition, resetPosition) - { - servo = new Servo(timer, pwmChannel, WING_SERVO_MIN_PULSE - , WING_SERVO_MAX_PULSE - , WING_SERVO_FREQUENCY); - } +} - WingServo::~WingServo(){} +WingServo::WingServo(TIM_TypeDef* timer, TimerUtils::Channel pwmChannel, + float minPosition, float maxPosition, float resetPosition) + : ServoInterface(minPosition, maxPosition, resetPosition) +{ + servo = new Servo(timer, pwmChannel, WING_SERVO_MIN_PULSE, + WING_SERVO_MAX_PULSE, WING_SERVO_FREQUENCY); +} + +WingServo::~WingServo() {} - void WingServo::enable() - { - servo -> enable(); - //Set the servo position to reset - setPosition(RESET_POS); - } +void WingServo::enable() +{ + servo->enable(); + // Set the servo position to reset + setPosition(RESET_POS); +} - void WingServo::disable() - { - servo -> disable(); - } +void WingServo::disable() { servo->disable(); } - void WingServo::selfTest() - { - setPosition(MIN_POS); - miosix::Thread::sleep(1000); - setPosition(RESET_POS); - miosix::Thread::sleep(1000); - setPosition(MAX_POS); - } +void WingServo::selfTest() +{ + setPosition(MIN_POS); + miosix::Thread::sleep(1000); + setPosition(RESET_POS); + miosix::Thread::sleep(1000); + setPosition(MAX_POS); +} - /** - * PRIVATE METHODS - */ - void WingServo::setPosition(float angle) - { - servo->setPosition(angle / 180); - } +void WingServo::setPosition(float angle) { servo->setPosition(angle / 180); } - float WingServo::preprocessPosition(float angle) - { - return angle; - } -} \ No newline at end of file +float WingServo::preprocessPosition(float angle) { return angle; } +} // namespace Parafoil diff --git a/src/boards/Parafoil/Wing/WingServo.h b/src/boards/Parafoil/Wing/WingServo.h index 2711a3c4c..74bcc163c 100644 --- a/src/boards/Parafoil/Wing/WingServo.h +++ b/src/boards/Parafoil/Wing/WingServo.h @@ -19,72 +19,74 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ + #pragma once +#include <Parafoil/Wing/WingConfig.h> #include <common/ServoInterface.h> #include <drivers/servo/Servo.h> -#include <Parafoil/Wing/WingConfig.h> -using namespace Boardcore; - -namespace ParafoilTestDev +namespace Parafoil { - class WingServo : public ServoInterface - { - public: - /** - * @brief Construct a new Wing Servo object - * It will use the min position as reset - * - * @param timer The servo timer that needs to be used - * @param pwmChannel The servo pwm port used - * @param minPosition The min allowed servo position(degrees) - * @param maxPosition The max allowed servo position(degrees) - */ - WingServo(TIM_TypeDef* timer, TimerUtils::Channel pwmChannel, float minPosition, float maxPosition); +class WingServo : public ServoInterface +{ +public: + /** + * @brief Construct a new Wing Servo object + * It will use the min position as reset + * + * @param timer The servo timer that needs to be used + * @param pwmChannel The servo pwm port used + * @param minPosition The min allowed servo position(degrees) + * @param maxPosition The max allowed servo position(degrees) + */ + WingServo(TIM_TypeDef* timer, Boardcore::TimerUtils::Channel pwmChannel, + float minPosition, float maxPosition); - /** - * @brief Construct a new Wing Servo object - * - * @param timer The servo timer that needs to be used - * @param pwmChannel The servo pwm port used - * @param minPosition The min allowed servo position(degrees) - * @param maxPosition The max allowed servo position(degrees) - * @param resetPosition The position where the servo is when it resets(degrees) - */ - WingServo(TIM_TypeDef* timer, TimerUtils::Channel pwmChannel, float minPosition, float maxPosition, float resetPosition); + /** + * @brief Construct a new Wing Servo object + * + * @param timer The servo timer that needs to be used + * @param pwmChannel The servo pwm port used + * @param minPosition The min allowed servo position(degrees) + * @param maxPosition The max allowed servo position(degrees) + * @param resetPosition The position where the servo is when it + * resets(degrees) + */ + WingServo(TIM_TypeDef* timer, Boardcore::TimerUtils::Channel pwmChannel, + float minPosition, float maxPosition, float resetPosition); - /** - * @brief Destroy the Wing Servo object - */ - ~WingServo(); + /** + * @brief Destroy the Wing Servo object + */ + ~WingServo(); - void enable() override; + void enable() override; - void disable() override; + void disable() override; - /** - * @brief Execute a self test with the servo - */ - void selfTest() override; + /** + * @brief Execute a self test with the servo + */ + void selfTest() override; - private: - Servo* servo; +private: + Boardcore::Servo* servo; - protected: - /** - * @brief Set the servo position - * @param angle sevo position(degrees) - */ - void setPosition(float angle) override; +protected: + /** + * @brief Set the servo position + * @param angle sevo position(degrees) + */ + void setPosition(float angle) override; - /** - * @brief Method to convert the angle passed by the user to an - * useful angle for the servo interface - * @param angle The angle(degrees) - * @return float The converted angle - */ - float preprocessPosition(float angle) override; - }; -} \ No newline at end of file + /** + * @brief Method to convert the angle passed by the user to an + * useful angle for the servo interface + * @param angle The angle(degrees) + * @return float The converted angle + */ + float preprocessPosition(float angle) override; +}; +} // namespace Parafoil diff --git a/src/entrypoints/Parafoil/logdecoder/LogTypes.h b/src/entrypoints/Parafoil/logdecoder/LogTypes.h index 9a2ca5f89..8e7500d1a 100644 --- a/src/entrypoints/Parafoil/logdecoder/LogTypes.h +++ b/src/entrypoints/Parafoil/logdecoder/LogTypes.h @@ -65,13 +65,6 @@ #include "FlightStatsRecorder/FSRData.h" #include "scheduler/TaskSchedulerData.h" -// Serialized classes -using std::ofstream; - -using namespace DeathStackBoard; -using namespace Boardcore; -using namespace ParafoilTestDev; - template <typename T> void print(T& t, ostream& os) { diff --git a/src/entrypoints/Parafoil/parafoil-entry.cpp b/src/entrypoints/Parafoil/parafoil-entry.cpp index b5abd84ee..ab4843c59 100644 --- a/src/entrypoints/Parafoil/parafoil-entry.cpp +++ b/src/entrypoints/Parafoil/parafoil-entry.cpp @@ -27,7 +27,7 @@ #include <miosix.h> using namespace miosix; -using namespace ParafoilTestDev; +using namespace Parafoil; using namespace Boardcore; void enablePin() -- GitLab