diff --git a/src/boards/ParafoilTest/Configs/RadioConfig.h b/src/boards/ParafoilTest/Configs/RadioConfig.h index f70012e885f02fca0cc3c9875b2bac4509a7125f..1fe3f2909759ed919ba53f748d331bea58b39f4b 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 4a64aafc56725633f310f8a4bf444fc1c70fcb73..a967128750dcf3bbaebe307226fb03114e313fe6 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(