diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index ee8f48e54f..9a7936791a 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #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 diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index 62f20ba256..af92135bde 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -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