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