From c6c285b5a2e99f816d25ce36f69dd47436012eac Mon Sep 17 00:00:00 2001 From: yaapu Date: Tue, 6 Jul 2021 21:47:35 +0200 Subject: [PATCH] AP_Frsky_Telem: added frame 0x500D for waypoint data --- libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp | 4 +- libraries/AP_Frsky_Telem/AP_Frsky_SPort.h | 6 +-- .../AP_Frsky_SPort_Passthrough.cpp | 43 +++++++++++++++++++ .../AP_Frsky_SPort_Passthrough.h | 2 + 4 files changed, 50 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp index 1ad9523b92..8f69544c60 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp @@ -421,7 +421,7 @@ uint16_t AP_Frsky_SPort::prep_number(int32_t number, uint8_t digits, uint8_t pow res = abs_number<<1; } else if (abs_number < 10240) { res = ((uint16_t)roundf(abs_number * 0.1f)<<1)|0x1; - } else { // transmit max possible value (0x3FF x 10^1 = 10240) + } else { // transmit max possible value (0x3FF x 10^1 = 10230) res = 0x7FF; } if (number < 0) { // if number is negative, add sign bit in front @@ -436,7 +436,7 @@ uint16_t AP_Frsky_SPort::prep_number(int32_t number, uint8_t digits, uint8_t pow res = ((uint16_t)roundf(abs_number * 0.01f)<<2)|0x2; } else if (abs_number < 1024000) { res = ((uint16_t)roundf(abs_number * 0.001f)<<2)|0x3; - } else { // transmit max possible value (0x3FF x 10^3 = 127000) + } else { // transmit max possible value (0x3FF x 10^3 = 1023000) res = 0xFFF; } if (number < 0) { // if number is negative, add sign bit in front diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h index 33ce14af99..fb76f5ad38 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h @@ -29,9 +29,9 @@ protected: void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data); - struct PACKED { - bool send_latitude; // sizeof(bool) = 4 ? - bool send_airspeed; // toggles 0x5005 between airspeed and groundspeed + struct { + bool send_latitude; + bool send_airspeed; // toggles 0x5005 between airspeed and groundspeed uint32_t gps_lng_sample; uint8_t new_byte; } _passthrough; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index 60049328d5..32f4ba6106 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #if APM_BUILD_TYPE(APM_BUILD_Rover) #include @@ -75,6 +76,11 @@ for FrSky SPort Passthrough #define WIND_SPEED_OFFSET 7 #define WIND_APPARENT_ANGLE_OFFSET 15 #define WIND_APPARENT_SPEED_OFFSET 23 +// for waypoint data +#define WP_NUMBER_LIMIT 2047 +#define WP_DISTANCE_LIMIT 1023000 +#define WP_DISTANCE_OFFSET 11 +#define WP_BEARING_OFFSET 23 extern const AP_HAL::HAL& hal; @@ -131,6 +137,7 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void) set_scheduler_entry(RPM, 300, 330); // 0x500A rpm sensors 1 and 2 set_scheduler_entry(TERRAIN, 700, 500); // 0x500B terrain data set_scheduler_entry(WIND, 700, 500); // 0x500C wind data + set_scheduler_entry(WAYPOINT, 750, 500); // 0x500D waypoint data set_scheduler_entry(UDATA, 5000, 200); // user data // initialize default sport sensor ID @@ -248,6 +255,12 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty) } #endif break; + case WAYPOINT: + { + const AP_Mission *mission = AP::mission(); + packet_ready = mission != nullptr && mission->get_current_nav_index() > 0; + } + break; case UDATA: // when using fport user data is sent by scheduler // when using sport user data is sent responding to custom polling @@ -323,6 +336,9 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx) case WIND: // 0x500C terrain data send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0C, calc_wind()); break; + case WAYPOINT: // 0x500D waypoint data + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0D, calc_waypoint()); + break; case UDATA: // user data { WITH_SEMAPHORE(_sport_push_buffer.sem); @@ -754,6 +770,33 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_wind(void) #endif return value; } + /* + * prepare waypoint data + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +uint32_t AP_Frsky_SPort_Passthrough::calc_waypoint(void) +{ + const AP_Mission *mission = AP::mission(); + const AP_Vehicle *vehicle = AP::vehicle(); + if (mission == nullptr || vehicle == nullptr) { + return 0U; + } + float wp_distance; + if (!vehicle->get_wp_distance_m(wp_distance)) { + return 0U; + } + float angle; + if (!vehicle->get_wp_bearing_deg(angle)) { + return 0U; + } + // waypoint current nav index + uint32_t value = MIN(mission->get_current_nav_index(), WP_NUMBER_LIMIT); + // distance to next waypoint + value |= prep_number(wp_distance, 3, 2) << WP_DISTANCE_OFFSET; + // bearing encoded in 3 degrees increments + value |= ((uint8_t)roundf(wrap_360(angle) * 0.333f)) << WP_BEARING_OFFSET; + return value; +} /* fetch Sport data for an external transport, such as FPort or crossfire diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index 7501a7fb6a..2efdca4fd0 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -74,6 +74,7 @@ public: #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL TERRAIN = 14, // 0x500B terrain data WIND = 15, // 0x500C wind data + WAYPOINT = 16, // 0x500D waypoint data WFQ_LAST_ITEM // must be last }; @@ -110,6 +111,7 @@ private: uint32_t calc_rpm(void); uint32_t calc_terrain(void); uint32_t calc_wind(void); + uint32_t calc_waypoint(void); // use_external_data is set when this library will // be providing data to another transport, such as FPort