From ab77fdfa027350fd5fcca0e9989d07f893d29c88 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 10 Aug 2016 08:14:34 +1000
Subject: [PATCH] AP_FrSky_Telem: removed dependency on inertialnav

the AP_InertialNav library is deprecated in favor of AP_AHRS. We
should not introduce a new dependency on it
---
 libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 16 +++++++++++-----
 libraries/AP_Frsky_Telem/AP_Frsky_Telem.h   |  4 +---
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
index c06590bb69..b5e56de139 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
@@ -26,11 +26,10 @@ extern const AP_HAL::HAL& hal;
 AP_Frsky_Telem::msg_t _msg;
 
 //constructor
-AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng, const AP_InertialNav_NavEKF &inav) :
+AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng) :
     _ahrs(ahrs),
     _battery(battery),
     _rng(rng),
-    _inav(inav),
     _port(NULL),
     _protocol(),
     _initialised_uart(),
@@ -179,7 +178,10 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
                 }
                 return;
             } else if ((now - _passthrough.timer_vario) > 500) {
-                send_uint32(VARIO_FIRST_ID, (int32_t)roundf(_inav.get_velocity_z())); // vertical velocity in cm/s
+                Vector3f velNED;
+                if (_ahrs.get_velocity_NED(velNED)) {
+                    send_uint32(VARIO_FIRST_ID, (int32_t)roundf(-velNED.z*100)); // vertical velocity in cm/s, +ve up
+                }
                 _passthrough.timer_vario = AP_HAL::millis();
                 return;
             } else if ((now - _passthrough.timer_alt) > 1000) {
@@ -706,11 +708,15 @@ uint32_t AP_Frsky_Telem::calc_home(void)
 uint32_t AP_Frsky_Telem::calc_velandyaw(void)
 {
     uint32_t velandyaw;
+    Vector3f velNED {};
+
+    // if we can't get velocity then we use zero for vertical velocity
+    _ahrs.get_velocity_NED(velNED);
 
     // vertical velocity in dm/s
-    velandyaw = prep_number(roundf(_inav.get_velocity_z() * 0.1f), 2, 1);
+    velandyaw = prep_number(roundf(velNED.z * 10), 2, 1);
     // horizontal velocity in dm/s
-    velandyaw |= prep_number(roundf(_inav.get_velocity_xy() * 0.1f), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
+    velandyaw |= prep_number(roundf(_ahrs.groundspeed_vector().length() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
     // yaw from [0;36000] centidegrees to .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits)
     velandyaw |= ((uint16_t)roundf(_ahrs.yaw_sensor * 0.05f) & VELANDYAW_YAW_LIMIT)<<VELANDYAW_YAW_OFFSET;
     return velandyaw;
diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
index 5b5627c054..4564273a31 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
@@ -18,7 +18,6 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_BattMonitor/AP_BattMonitor.h>
-#include <AP_InertialNav/AP_InertialNav.h>
 #include <AP_Notify/AP_Notify.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include <AP_SerialManager/AP_SerialManager.h>
@@ -115,7 +114,7 @@ class AP_Frsky_Telem
 {
 public:
     //constructor
-    AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng, const AP_InertialNav_NavEKF &inav);
+    AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng);
 
     // init - perform required initialisation
     void init(const AP_SerialManager &serial_manager, const char *firmware_str, const char *frame_config_str, const uint8_t mav_type, AP_Float *fs_batt_voltage, AP_Float *fs_batt_mah, uint8_t *control_mode, uint32_t *ap_value, uint32_t *control_sensors_present, uint32_t *control_sensors_enabled, uint32_t *control_sensors_health, int32_t *home_distance, int32_t *home_bearing);
@@ -134,7 +133,6 @@ private:
     AP_AHRS &_ahrs;
     const AP_BattMonitor &_battery;
     const RangeFinder &_rng;
-    const AP_InertialNav &_inav;
     AP_HAL::UARTDriver *_port;                  // UART used to send data to FrSky receiver
     AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter
     bool _initialised_uart;