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:
parent
3f204d976e
commit
45f7cb0c82
libraries/AP_Frsky_Telem
@ -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
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user