From df26b0318059372bd43751fc4e597cbdb20b4687 Mon Sep 17 00:00:00 2001
From: Matteo Pignataro <matteo.pignataro@skywarder.eu>
Date: Wed, 2 Mar 2022 16:32:46 +0100
Subject: [PATCH] [ParafoilTestDev] HR and LR telemetry concept correction
---
src/boards/ParafoilTest/Configs/RadioConfig.h | 11 +++++---
src/boards/ParafoilTest/Main/Radio.cpp | 25 ++++++++++++++-----
2 files changed, 26 insertions(+), 10 deletions(-)
diff --git a/src/boards/ParafoilTest/Configs/RadioConfig.h b/src/boards/ParafoilTest/Configs/RadioConfig.h
index f70012e88..1fe3f2909 100644
--- a/src/boards/ParafoilTest/Configs/RadioConfig.h
+++ b/src/boards/ParafoilTest/Configs/RadioConfig.h
@@ -24,9 +24,12 @@
namespace ParafoilTestDev
{
//TODO update with the correct values
- static const uint32_t HR_UPDATE_PERIOD = 62; //Milliseconds
- static const uint32_t LR_UPDATE_PERIOD = 100; //Milliseconds
+ static const uint32_t HR_GROUND_UPDATE_PERIOD = 62; //Milliseconds
+ static const uint32_t HR_FLIGHT_UPDATE_PERIOD = 10;
+ static const uint32_t LR_UPDATE_PERIOD = 100; //Milliseconds
+
+ //TODO define the correct ids for task scheduler
+ static const uint8_t RADIO_HR_ID = 200;
+ static const uint8_t RADIO_LR_ID = 201;
- //TODO define the correct id
- static const uint8_t RADIO_ID = 200;
}
\ No newline at end of file
diff --git a/src/boards/ParafoilTest/Main/Radio.cpp b/src/boards/ParafoilTest/Main/Radio.cpp
index 4a64aafc5..a96712875 100644
--- a/src/boards/ParafoilTest/Main/Radio.cpp
+++ b/src/boards/ParafoilTest/Main/Radio.cpp
@@ -201,10 +201,7 @@ namespace ParafoilTestDev
{
//I send this telemetry if and only if the status is
//in low rate telemetry
- if(!HRtelemetry)
- {
- sendTelemetry(MAV_LR_TM_ID);
- }
+ sendTelemetry(MAV_LR_TM_ID);
}
void Radio::sendAck(const mavlink_message_t& msg)
@@ -222,7 +219,23 @@ namespace ParafoilTestDev
{
//Lock the kernel to avoid context switch during this toggle
miosix::PauseKernelLock kLock;
+ TaskScheduler::function_t HRfunction([=]() {sendHRTelemetry();});
HRtelemetry = !HRtelemetry;
+
+ if(HRtelemetry)
+ {
+ //If we switched to hr telemetry on flight i change the period
+ scheduler -> removeTask(RADIO_HR_ID);
+ //Add the same task with a faster period
+ scheduler -> addTask(HRfunction, HR_FLIGHT_UPDATE_PERIOD, RADIO_HR_ID);
+ }
+ else
+ {
+ //If we switched to hr telemetry on ground i change the period
+ scheduler -> removeTask(RADIO_HR_ID);
+ //Add the same task with a slower period
+ scheduler -> addTask(HRfunction, HR_GROUND_UPDATE_PERIOD, RADIO_HR_ID);
+ }
}
bool Radio::start()
@@ -259,8 +272,8 @@ namespace ParafoilTestDev
HRtelemetry = true;
//Register the LR and HR tasks in the scheduler
- scheduler -> addTask(HRfunction, HR_UPDATE_PERIOD, RADIO_ID);
- //scheduler -> addTask(LRfunction, LR_UPDATE_PERIOD, RADIO_ID);
+ scheduler -> addTask(HRfunction, HR_GROUND_UPDATE_PERIOD, RADIO_HR_ID);
+ scheduler -> addTask(LRfunction, LR_UPDATE_PERIOD, RADIO_LR_ID);
//Set the frame receive callback
//module -> setOnFrameReceivedListener(
--
GitLab