5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-02 22:18:28 -04:00

AP_Frsky_Telem: added passthrough frame 0x500B for height above terrain

This commit is contained in:
yaapu 2021-03-07 10:23:02 +01:00 committed by Peter Barker
parent 3f204d976e
commit 45f7cb0c82
2 changed files with 37 additions and 0 deletions

View File

@ -8,6 +8,7 @@
#include <AP_Notify/AP_Notify.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_RPM/AP_RPM.h>
#include <AP_Terrain/AP_Terrain.h>
#include <GCS_MAVLink/GCS.h>
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
@ -112,6 +113,7 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void)
set_scheduler_entry(BATT_1, 1300, 500); // 0x5003 Battery 1 status
set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters
set_scheduler_entry(RPM, 300, 330); // 0x500A rpm sensors 1 and 2
set_scheduler_entry(TERRAIN, 700, 500); // 0x500B terrain data
set_scheduler_entry(UDATA, 5000, 200); // user data
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
set_scheduler_entry(MAV, 35, 25); // mavlite
@ -200,6 +202,15 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
packet_ready = rpm->num_sensors() > 0;
}
break;
case TERRAIN:
{
packet_ready = false;
#if AP_TERRAIN_AVAILABLE
const AP_Terrain &terrain = AP::terrain();
packet_ready = terrain.enabled();
#endif
}
break;
case UDATA:
// when using fport user data is sent by scheduler
// when using sport user data is sent responding to custom polling
@ -269,6 +280,9 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
case RPM: // 0x500A rpm sensors 1 and 2
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0A, calc_rpm());
break;
case TERRAIN: // 0x500B terrain data
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0B, calc_terrain());
break;
case UDATA: // user data
{
WITH_SEMAPHORE(_sport_push_buffer.sem);
@ -616,6 +630,27 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_rpm(void)
return value;
}
/*
* prepare terrain data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/
uint32_t AP_Frsky_SPort_Passthrough::calc_terrain(void)
{
uint32_t value = 0;
#if AP_TERRAIN_AVAILABLE
AP_Terrain &terrain = AP::terrain();
if (!terrain.enabled()) {
return value;
}
float height_above_terrain;
if (terrain.height_above_terrain(height_above_terrain, true)) {
// vehicle height above terrain
value |= prep_number(roundf(height_above_terrain * 10), 3, 2);
}
#endif
return value;
}
/*
fetch Sport data for an external transport, such as FPort or crossfire
Note: we need to create a packet array with unique packet types

View File

@ -72,6 +72,7 @@ public:
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
MAV = 13, // mavlite
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
TERRAIN = 14, // 0x500B terrain data
WFQ_LAST_ITEM // must be last
};
@ -99,6 +100,7 @@ private:
uint32_t calc_velandyaw(void);
uint32_t calc_attiandrng(void);
uint32_t calc_rpm(void);
uint32_t calc_terrain(void);
// use_external_data is set when this library will
// be providing data to another transport, such as FPort