mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
AP_Frsky_Telem: added passthrough frame 0x500B for height above terrain
This commit is contained in:
parent
3f204d976e
commit
45f7cb0c82
@ -8,6 +8,7 @@
|
|||||||
#include <AP_Notify/AP_Notify.h>
|
#include <AP_Notify/AP_Notify.h>
|
||||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||||
#include <AP_RPM/AP_RPM.h>
|
#include <AP_RPM/AP_RPM.h>
|
||||||
|
#include <AP_Terrain/AP_Terrain.h>
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
|
||||||
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
#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(BATT_1, 1300, 500); // 0x5003 Battery 1 status
|
||||||
set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters
|
set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters
|
||||||
set_scheduler_entry(RPM, 300, 330); // 0x500A rpm sensors 1 and 2
|
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
|
set_scheduler_entry(UDATA, 5000, 200); // user data
|
||||||
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
set_scheduler_entry(MAV, 35, 25); // mavlite
|
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;
|
packet_ready = rpm->num_sensors() > 0;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
case TERRAIN:
|
||||||
|
{
|
||||||
|
packet_ready = false;
|
||||||
|
#if AP_TERRAIN_AVAILABLE
|
||||||
|
const AP_Terrain &terrain = AP::terrain();
|
||||||
|
packet_ready = terrain.enabled();
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
break;
|
||||||
case UDATA:
|
case UDATA:
|
||||||
// when using fport user data is sent by scheduler
|
// when using fport user data is sent by scheduler
|
||||||
// when using sport user data is sent responding to custom polling
|
// 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
|
case RPM: // 0x500A rpm sensors 1 and 2
|
||||||
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0A, calc_rpm());
|
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0A, calc_rpm());
|
||||||
break;
|
break;
|
||||||
|
case TERRAIN: // 0x500B terrain data
|
||||||
|
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0B, calc_terrain());
|
||||||
|
break;
|
||||||
case UDATA: // user data
|
case UDATA: // user data
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(_sport_push_buffer.sem);
|
WITH_SEMAPHORE(_sport_push_buffer.sem);
|
||||||
@ -616,6 +630,27 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_rpm(void)
|
|||||||
return value;
|
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
|
fetch Sport data for an external transport, such as FPort or crossfire
|
||||||
Note: we need to create a packet array with unique packet types
|
Note: we need to create a packet array with unique packet types
|
||||||
|
@ -72,6 +72,7 @@ public:
|
|||||||
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
MAV = 13, // mavlite
|
MAV = 13, // mavlite
|
||||||
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
|
TERRAIN = 14, // 0x500B terrain data
|
||||||
WFQ_LAST_ITEM // must be last
|
WFQ_LAST_ITEM // must be last
|
||||||
};
|
};
|
||||||
|
|
||||||
@ -99,6 +100,7 @@ private:
|
|||||||
uint32_t calc_velandyaw(void);
|
uint32_t calc_velandyaw(void);
|
||||||
uint32_t calc_attiandrng(void);
|
uint32_t calc_attiandrng(void);
|
||||||
uint32_t calc_rpm(void);
|
uint32_t calc_rpm(void);
|
||||||
|
uint32_t calc_terrain(void);
|
||||||
|
|
||||||
// use_external_data is set when this library will
|
// use_external_data is set when this library will
|
||||||
// be providing data to another transport, such as FPort
|
// be providing data to another transport, such as FPort
|
||||||
|
Loading…
Reference in New Issue
Block a user