From 5df84a1fee720bbc9cd81745c24c9b64ae11b0a7 Mon Sep 17 00:00:00 2001 From: Matteo Pignataro <matteo.pignataro@skywarder.eu> Date: Sat, 12 Feb 2022 20:32:08 +0100 Subject: [PATCH] [ParafoilTestDev] First mavlink send. - Not calling handleMavlinkMessage function(to be fixed) --- data/ParafoilTestDev/UMLMavlink.dia | Bin 0 -> 519 bytes .../ParafoilTest/Configs/MavlinkConfig.h | 8 +- .../ParafoilTest/Configs/SensorsConfig.h | 3 +- src/boards/ParafoilTest/Configs/XbeeConfig.h | 6 +- src/boards/ParafoilTest/Main/Radio.cpp | 79 +++++++++++++-- src/boards/ParafoilTest/Main/Radio.h | 74 +++++++++++--- src/boards/ParafoilTest/Main/Sensors.cpp | 5 +- src/boards/ParafoilTest/ParafoilTest.h | 13 +-- .../TelemetriesTelecommands/TMRepository.cpp | 0 .../TelemetriesTelecommands/TMRepository.h | 91 ++++++++++++++++++ src/boards/ParafoilTest/Wing/WingAlgorithm.h | 10 +- src/entrypoints/parafoilTest-entry.cpp | 37 ++++--- 12 files changed, 273 insertions(+), 53 deletions(-) create mode 100644 data/ParafoilTestDev/UMLMavlink.dia create mode 100644 src/boards/ParafoilTest/TelemetriesTelecommands/TMRepository.cpp create mode 100644 src/boards/ParafoilTest/TelemetriesTelecommands/TMRepository.h diff --git a/data/ParafoilTestDev/UMLMavlink.dia b/data/ParafoilTestDev/UMLMavlink.dia new file mode 100644 index 0000000000000000000000000000000000000000..91d826341f788f0f8060e481f3e3f2fc5232f174 GIT binary patch literal 519 zcmb2|=3oE;Cg#0i2lE~o2<-hVI?-}+KuE|6sa1Qwu<xo~r?!!Eb7{`$GLPb$`SF)4 z#5s5aJT^>mk+j`!ZY<aT_sgG&pA*h*anwvI67Tao;G(73rK7gLZeLH@lS;#iMI8O_ zPMSPvoUM4~nZBBK_SIyOsi~5mPBl!mdJ@sn=33uS+##c()a@wnMkrvzhK-fl5vvU~ zb+}oc=%{fnc|UVn>w+b2l`&d7uN;)B@M-h5_juB|$ab0X8!n@18Eo(0K1nuz_WAS) z(aXM%VmzDqlC(=1G9TXDQ*I!aD&vvT{jmRc;@e5)IXm|iZ3#W{Y=N;zyOo~x9>1e1 zb9~i=r}w?AF_X8yV}IxG>671|PP%>S<Ly%~Z=YIN85|VNo58OttZB&{xH>9!!|75V zt~*n2Y}~M}BdPP+DdB(mjkAlHR-FA`e!ILpZ`nk-`#Y*T#KfX@>Mz@?=h=B>-95Xi zs7bT-zUr*)WfMNP?~`2X+n#-|+V({<&QCv5_G_wW_~xy0P9?{d<y4(ioFeKm(=zbP z?-^ptrnb8-|8(z2>`bXHx55>D?H5?K7Bp^nQ6(YaX8p<}cWqFERHS9rmrl8<DW7I9 zwf+8os?Pb&70gpZxE>xVynbQgq`cj;@>I_29=&yC$HKZ*yOfe|RsY&GrO8X^hu`D9 f9DCkxNM;g0H7ob-oyYM!|Cm1Nzg4Q>W?%pSM}z$% literal 0 HcmV?d00001 diff --git a/src/boards/ParafoilTest/Configs/MavlinkConfig.h b/src/boards/ParafoilTest/Configs/MavlinkConfig.h index 1ce38e2f4..3d09ea284 100644 --- a/src/boards/ParafoilTest/Configs/MavlinkConfig.h +++ b/src/boards/ParafoilTest/Configs/MavlinkConfig.h @@ -27,6 +27,12 @@ namespace ParafoilTestDev /* Mavlink Driver queue settings */ static constexpr unsigned int MAV_OUT_QUEUE_LEN = 10; static constexpr unsigned int MAV_PKT_SIZE = 255; - static constexpr long long MAV_OUT_BUFFER_MAX_AGE = 200; + 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; + + /* Min guaranteed sleep time after each packet sent (milliseconds) */ + static const uint16_t SLEEP_AFTER_SEND = 0; } \ No newline at end of file diff --git a/src/boards/ParafoilTest/Configs/SensorsConfig.h b/src/boards/ParafoilTest/Configs/SensorsConfig.h index 5cb6916fd..67b468f91 100644 --- a/src/boards/ParafoilTest/Configs/SensorsConfig.h +++ b/src/boards/ParafoilTest/Configs/SensorsConfig.h @@ -37,7 +37,8 @@ using namespace Boardcore; namespace ParafoilTestDev { - //GPS settings + //GPS settings TODO SPI + static miosix::GpioPin GPS_CS(GPIOA_BASE, 4); static constexpr unsigned int GPS_SAMPLE_RATE = 25; static constexpr unsigned int GPS_SAMPLE_PERIOD = 1000 / GPS_SAMPLE_RATE; static constexpr unsigned int GPS_BAUD_RATE = 460800; diff --git a/src/boards/ParafoilTest/Configs/XbeeConfig.h b/src/boards/ParafoilTest/Configs/XbeeConfig.h index 22418d435..d34d9e3be 100644 --- a/src/boards/ParafoilTest/Configs/XbeeConfig.h +++ b/src/boards/ParafoilTest/Configs/XbeeConfig.h @@ -28,9 +28,9 @@ using namespace Boardcore; namespace ParafoilTestDev { //TODO change the pins to something correct - static miosix::GpioPin XBEE_CS(GPIOB_BASE, 6); - static miosix::GpioPin XBEE_ATTN(GPIOB_BASE, 7); - static miosix::GpioPin XBEE_RESET(GPIOB_BASE, 8); + static miosix::GpioPin XBEE_CS(GPIOA_BASE, 3); + static miosix::GpioPin XBEE_ATTN(GPIOF_BASE, 10); //Interrupt pin (THE SAME AS THE RADIO.CPP) + static miosix::GpioPin XBEE_RESET(GPIOA_BASE, 4); //Data rate static const bool XBEE_80KBPS_DATA_RATE = true; diff --git a/src/boards/ParafoilTest/Main/Radio.cpp b/src/boards/ParafoilTest/Main/Radio.cpp index 1020f5a25..5f639eeab 100644 --- a/src/boards/ParafoilTest/Main/Radio.cpp +++ b/src/boards/ParafoilTest/Main/Radio.cpp @@ -21,24 +21,45 @@ */ #include <Main/Radio.h> +#include <TelemetriesTelecommands/TMRepository.h> #include <Configs/XbeeConfig.h> #include <drivers/spi/SPIDriver.h> #include <drivers/Xbee/ATCommands.h> +#include <drivers/interrupt/external_interrupts.h> +#include <ParafoilTest.h> using std::bind; using namespace std::placeholders; using namespace Boardcore; +// Xbee ATTN interrupt +/** + * @brief We must define the interrupt handler. This calls + * the message handler which is: handleMavlinkMessage + */ +void __attribute__((used)) EXTI10_IRQHandlerImpl() +{ + using namespace ParafoilTestDev; + + if (ParafoilTest::getInstance().radio->xbee != nullptr) + { + ParafoilTest::getInstance().radio->xbee->handleATTNInterrupt(); + } +} + namespace ParafoilTestDev { /** * PUBLIC METHODS */ - Radio::Radio(SPIBusInterface& xbee_bus) : xbee_bus(xbee_bus) + Radio::Radio(SPIBusInterface& xbee_bus, TaskScheduler* scheduler) : xbee_bus(xbee_bus) { //Create a SPI configuration object SPIBusConfig config{}; + //Set the internal scheduler + this -> scheduler = scheduler; + //Add a clock divider config config.clockDivider = SPI::ClockDivider::DIV_16; @@ -55,9 +76,17 @@ namespace ParafoilTestDev //Set the data rate Xbee::setDataRate(*xbee, XBEE_80KBPS_DATA_RATE, XBEE_TIMEOUT); - //TODO Create the mavlink driver - - + //Create a lambda function that interfaces the driver with + //the object handleMavlikMessage of this class + std::function<void (MavDriver* driver, const mavlink_message_t& msg)> + handleMessage([=](MavDriver* driver, const mavlink_message_t& msg) + { + this -> handleMavlinkMessage(msg); + }); + mav_driver = new MavDriver(xbee, handleMessage, SLEEP_AFTER_SEND, MAV_OUT_BUFFER_MAX_AGE); + + //Enable external interrupt on F10 pin + enableExternalInterrupt(GPIOF_BASE, 10, InterruptTrigger::FALLING_EDGE); } Radio::~Radio() @@ -65,9 +94,47 @@ namespace ParafoilTestDev } + void Radio::handleMavlinkMessage(const mavlink_message_t& msg) + { + printf("provola\n"); + } + + bool Radio::sendTelemetry(const uint8_t tm_id) + { + //Enqueue the message + bool result = mav_driver -> enqueueMsg(TMRepository::getInstance().packTM(tm_id)); + //TODO log the operation + return result; + } + + void Radio::sendHRTelemetry() + { + mavlink_message_t msg; + msg.msgid = 10; + mav_driver -> enqueueMsg(msg); + } + + void Radio::sendLRTelemetry() + { + + } + + void Radio::sendAck(const mavlink_message_t& msg) + { + mavlink_message_t ackMsg; + //Pack the ack message based on the received message + mavlink_msg_ack_tm_pack(TMTC_MAV_SYSID, TMTC_MAV_COMPID, &ackMsg, msg.msgid, msg.seq); + + //Put the message in the queue + mav_driver -> enqueueMsg(ackMsg); + //TODO log the thing + } + bool Radio::start() { - return false; + bool result = mav_driver -> start(); + //TODO start the scheduler + return result; } void Radio::logStatus() @@ -80,6 +147,6 @@ namespace ParafoilTestDev */ void Radio::onXbeeFrameReceived(Xbee::APIFrame& frame) { - + printf("frame received\n"); } } \ No newline at end of file diff --git a/src/boards/ParafoilTest/Main/Radio.h b/src/boards/ParafoilTest/Main/Radio.h index f504893df..f16287da2 100644 --- a/src/boards/ParafoilTest/Main/Radio.h +++ b/src/boards/ParafoilTest/Main/Radio.h @@ -23,11 +23,15 @@ #include <TelemetriesTelecommands/Mavlink.h> #include <drivers/Xbee/Xbee.h> +#include <scheduler/TaskScheduler.h> /** - * @brief This class defines the interactions beetween the radio module (xbee) - * and the mavlink messages enumeration. It is used as a main class about these radio - * sub modules such as TMRepository and TMCController + * @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. */ namespace ParafoilTestDev @@ -35,44 +39,86 @@ namespace ParafoilTestDev class Radio { public: - /** * @brief The xbee module driver */ Xbee::Xbee* xbee; - /** - * @brief The mavlink driver - */ - MavDriver* mav_driver; - /** * @brief Construct a new Radio object * * @param xbee_bus The Xbee SPI bus */ - Radio(SPIBusInterface& xbee_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(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); + bool start(); void logStatus(); private: - + /** - * @brief Radio frame received callback method. - * It's used for logging purposes + * @brief The mavlink driver */ - void onXbeeFrameReceived(Xbee::APIFrame& frame); + MavDriver* mav_driver; /** * @brief SPI bus */ SPIBusInterface& xbee_bus; + + /** + * @brief Main task scheduler + */ + TaskScheduler* scheduler; + + /** + * @brief Radio frame received callback method. + * It's used for logging purposes + */ + void onXbeeFrameReceived(Xbee::APIFrame& frame); }; } \ No newline at end of file diff --git a/src/boards/ParafoilTest/Main/Sensors.cpp b/src/boards/ParafoilTest/Main/Sensors.cpp index f0cdd1dff..acf076753 100644 --- a/src/boards/ParafoilTest/Main/Sensors.cpp +++ b/src/boards/ParafoilTest/Main/Sensors.cpp @@ -25,7 +25,6 @@ #include "Sensors.h" using std::bind; -//using std::function; using namespace Boardcore; @@ -64,8 +63,8 @@ namespace ParafoilTestDev void Sensors::UbloxGPSinit() { - //Instantiate the object - gps_ublox = new UbloxGPSSerial(GPS_BAUD_RATE, GPS_SAMPLE_RATE); + //Instantiate the object TODO set the sample rate and stuff + gps_ublox = new UbloxGPSSPI(spiInterface, GPS_CS); //Bind the information with the callback method SensorInfo info("UbloxGPS", GPS_SAMPLE_PERIOD, diff --git a/src/boards/ParafoilTest/ParafoilTest.h b/src/boards/ParafoilTest/ParafoilTest.h index 15922e2bc..1a1c3cc4c 100644 --- a/src/boards/ParafoilTest/ParafoilTest.h +++ b/src/boards/ParafoilTest/ParafoilTest.h @@ -82,9 +82,6 @@ namespace ParafoilTestDev status.setError(&ParafoilTestStatus::eventBroker); } - //Start the wing controller - wingController -> start(); - //Start the sensors sampling /*if(!sensors -> start()) { @@ -93,11 +90,11 @@ namespace ParafoilTestDev }*/ //Start the radio - /*if(!radio -> start()) + if(!radio -> start()) { LOG_ERR(log, "Error starting the radio"); status.setError(&ParafoilTestStatus::radio); - }*/ + } //After all the initializations i log the status logger -> log(status); @@ -150,14 +147,14 @@ namespace ParafoilTestDev scheduler = new TaskScheduler(); //Create the wing controller - wingController = new WingController(scheduler); + //wingController = new WingController(scheduler); //Create the sensors - //SPIBusInterface *spiInterface1 = new SPIBus(SPI1); + SPIBusInterface *spiInterface1 = new SPIBus(SPI5); //sensors = new Sensors(*spiInterface1, scheduler); //Create a new radio - //radio = new Radio(*spiInterface1); + radio = new Radio(*spiInterface1, scheduler); } /** diff --git a/src/boards/ParafoilTest/TelemetriesTelecommands/TMRepository.cpp b/src/boards/ParafoilTest/TelemetriesTelecommands/TMRepository.cpp new file mode 100644 index 000000000..e69de29bb diff --git a/src/boards/ParafoilTest/TelemetriesTelecommands/TMRepository.h b/src/boards/ParafoilTest/TelemetriesTelecommands/TMRepository.h new file mode 100644 index 000000000..1664fd3e7 --- /dev/null +++ b/src/boards/ParafoilTest/TelemetriesTelecommands/TMRepository.h @@ -0,0 +1,91 @@ +/* Copyright (c) 2022 Skyward Experimental Rocketry + * Author: Matteo Pignataro + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +#pragma once + +#include <Singleton.h> +#include <TelemetriesTelecommands/Mavlink.h> + +/** + * @brief This class represents the collection of data that can + * be sent via radio communication. This refers to mavlink libriaries + * and structures created in the correct .xml file. + * + * 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. + */ + +using namespace Boardcore; + +namespace ParafoilTestDev +{ + class TMRepository : public Singleton<TMRepository> + { + friend class Singleton<TMRepository>; + + 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); + + 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_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; + }; +} \ No newline at end of file diff --git a/src/boards/ParafoilTest/Wing/WingAlgorithm.h b/src/boards/ParafoilTest/Wing/WingAlgorithm.h index 2887948de..028a2a118 100644 --- a/src/boards/ParafoilTest/Wing/WingAlgorithm.h +++ b/src/boards/ParafoilTest/Wing/WingAlgorithm.h @@ -121,11 +121,6 @@ namespace ParafoilTestDev void end(); protected: - /** - * @brief This method implements the algorithm step based on the current timestamp - */ - void step() override; - //Actuators ServoInterface* servo1; ServoInterface* servo2; @@ -141,5 +136,10 @@ namespace ParafoilTestDev //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 diff --git a/src/entrypoints/parafoilTest-entry.cpp b/src/entrypoints/parafoilTest-entry.cpp index e808da322..e012573c1 100644 --- a/src/entrypoints/parafoilTest-entry.cpp +++ b/src/entrypoints/parafoilTest-entry.cpp @@ -21,28 +21,41 @@ */ #include <miosix.h> #include <ParafoilTest.h> +#include <Configs/XbeeConfig.h> -using namespace ParafoilTestDev; +using namespace ParafoilTestDev; int main() { - miosix::GpioPin servo(GPIOA_BASE, 6); - servo.mode(miosix::Mode::ALTERNATE); - servo.alternateFunction(9); + miosix::GpioPin spiSck(GPIOF_BASE, 7); + miosix::GpioPin spiMiso(GPIOF_BASE, 8); + miosix::GpioPin spiMosi(GPIOF_BASE, 9); - miosix::GpioPin servo2(GPIOA_BASE, 7); - servo2.mode(miosix::Mode::ALTERNATE); - servo2.alternateFunction(9); + spiSck.mode(miosix::Mode::ALTERNATE); + spiSck.alternateFunction(5); + spiMiso.mode(miosix::Mode::ALTERNATE); + spiMiso.alternateFunction(5); + spiMosi.mode(miosix::Mode::ALTERNATE); + spiMosi.alternateFunction(5); + + XBEE_CS.mode(miosix::Mode::OUTPUT); + XBEE_CS.high(); + + XBEE_ATTN.mode(miosix::Mode::INPUT); + + XBEE_RESET.mode(miosix::Mode::OUTPUT); + XBEE_RESET.low(); + + RCC->APB2ENR |= RCC_APB2ENR_SPI5EN; // Enable SPI5 bus //TODO integrate all the logging stuff ParafoilTest::getInstance().start(); - miosix::Thread::sleep(2000); - ParafoilTest::getInstance().wingController->stop(); - ParafoilTest::getInstance().wingController->start(); - while(true) - {} + { + ParafoilTest::getInstance().radio ->sendHRTelemetry(); + miosix::Thread::sleep(100); + } return 0; } -- GitLab