From c05933838633bef3e55f788e7e77172e7943a464 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 25 Feb 2021 10:59:55 +1100 Subject: [PATCH] AP_Frsky_Telem: use location from AHRS not GPS The AHRS may be able to supply information even in the absense of an actual GPS unit. --- libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp index 63c2b0afd7..422fbf673d 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp @@ -2,7 +2,6 @@ #include #include -#include #include extern const AP_HAL::HAL& hal; @@ -106,8 +105,11 @@ float AP_Frsky_Backend::format_gps(float dec) */ void AP_Frsky_Backend::calc_gps_position(void) { - if (AP::gps().status() >= 3) { - const Location &loc = AP::gps().location(); //get gps instance 0 + AP_AHRS &_ahrs = AP::ahrs(); + + Location loc; + + if (_ahrs.get_position(loc)) { float lat = format_gps(fabsf(loc.lat/10000000.0f)); _SPort_data.latdddmm = lat; _SPort_data.latmmmm = (lat - _SPort_data.latdddmm) * 10000; @@ -122,7 +124,7 @@ void AP_Frsky_Backend::calc_gps_position(void) _SPort_data.alt_gps_meters = (int16_t)alt; _SPort_data.alt_gps_cm = (alt - _SPort_data.alt_gps_meters) * 100; - float speed = AP::gps().ground_speed(); + const float speed = AP::ahrs().groundspeed(); _SPort_data.speed_in_meter = speed; _SPort_data.speed_in_centimeter = (speed - _SPort_data.speed_in_meter) * 100; } else { @@ -137,7 +139,6 @@ void AP_Frsky_Backend::calc_gps_position(void) _SPort_data.speed_in_centimeter = 0; } - AP_AHRS &_ahrs = AP::ahrs(); _SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS }