From 1e2621466c8e6ec0e2803d4f409b6600d5cc95af Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Sat, 27 Apr 2024 14:58:54 +0100
Subject: [PATCH] AP_Frsky_Telem: factor out vspeed and nav alt

---
 libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp | 37 ++-----------------
 .../AP_Frsky_SPort_Passthrough.cpp            |  2 +-
 2 files changed, 4 insertions(+), 35 deletions(-)

diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp
index 9db8f890c3..e78b576c27 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp
@@ -5,6 +5,7 @@
 #include <AP_Baro/AP_Baro.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_RPM/AP_RPM.h>
+#include <AP_RCTelemetry/AP_RCTelemetry.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -44,47 +45,15 @@ void AP_Frsky_Backend::loop(void)
     }
 }
 
-/*
- * get vertical speed from ahrs, if not available fall back to baro climbrate, units is m/s
- * for FrSky D and SPort protocols
- */
-float AP_Frsky_Backend::get_vspeed_ms(void)
-{
-
-    {
-        // release semaphore as soon as possible
-        AP_AHRS &_ahrs = AP::ahrs();
-        Vector3f v;
-        WITH_SEMAPHORE(_ahrs.get_semaphore());
-        if (_ahrs.get_velocity_NED(v)) {
-            return -v.z;
-        }
-    }
-
-    auto &_baro = AP::baro();
-    WITH_SEMAPHORE(_baro.get_semaphore());
-    return _baro.get_climb_rate();
-}
-
 /*
  * prepare altitude between vehicle and home location data
  * for FrSky D and SPort protocols
  */
 void AP_Frsky_Backend::calc_nav_alt(void)
 {
-    _SPort_data.vario_vspd = (int32_t)(get_vspeed_ms()*100); //convert to cm/s
-
-    Location loc;
-    float current_height = 0;
-
-    AP_AHRS &_ahrs = AP::ahrs();
-    WITH_SEMAPHORE(_ahrs.get_semaphore());
-    if (_ahrs.get_location(loc)) {
-        if (!loc.get_alt_m(Location::AltFrame::ABSOLUTE, current_height)) {
-            // ignore this error
-        }
-    }
+    _SPort_data.vario_vspd = (int32_t)(AP_RCTelemetry::get_vspeed_ms()*100); //convert to cm/s
 
+    float current_height = AP_RCTelemetry::get_nav_alt_m();
     _SPort_data.alt_nav_meters = float_to_uint16(current_height);
     _SPort_data.alt_nav_cm = float_to_uint16((current_height - _SPort_data.alt_nav_meters) * 100);
 }
diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
index 2f3acb0fb3..db60fa4048 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
@@ -646,7 +646,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_home(void)
  */
 uint32_t AP_Frsky_SPort_Passthrough::calc_velandyaw(void)
 {
-    float vspd = get_vspeed_ms();
+    float vspd = AP_RCTelemetry::get_vspeed_ms();
     // vertical velocity in dm/s
     uint32_t velandyaw = prep_number(roundf(vspd * 10), 2, 1);
     float airspeed_m;       // m/s