From 2fecc02aa60cc1466e57d6fba78f722eefee99ed Mon Sep 17 00:00:00 2001
From: olliw42 <waldmanns@gmx.de>
Date: Tue, 24 Aug 2021 16:18:45 +0200
Subject: [PATCH]  Rover/GCS_MAVLINK: add send_servo_out() method and use
 GCS_MAVLINK::receiver_rssi()

---
 Rover/GCS_Mavlink.cpp | 8 ++++----
 Rover/GCS_Mavlink.h   | 2 ++
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp
index 0f022d48ef..f0d3e116d0 100644
--- a/Rover/GCS_Mavlink.cpp
+++ b/Rover/GCS_Mavlink.cpp
@@ -117,10 +117,10 @@ void GCS_MAVLINK_Rover::send_nav_controller_output() const
         control_mode->crosstrack_error());
 }
 
-void Rover::send_servo_out(mavlink_channel_t chan)
+void GCS_MAVLINK_Rover::send_servo_out()
 {
     float motor1, motor3;
-    if (g2.motors.have_skid_steering()) {
+    if (rover.g2.motors.have_skid_steering()) {
         motor1 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttleLeft) / 1000.0f);
         motor3 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttleRight) / 1000.0f);
     } else {
@@ -139,7 +139,7 @@ void Rover::send_servo_out(mavlink_channel_t chan)
         0,
         0,
         0,
-        rssi.read_receiver_rssi_uint8());
+        receiver_rssi());
 }
 
 int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const
@@ -329,7 +329,7 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
 
     case MSG_SERVO_OUT:
         CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
-        rover.send_servo_out(chan);
+        send_servo_out();
         break;
 
     case MSG_WHEEL_DISTANCE:
diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h
index d5a2e2db21..ec0cbb9a90 100644
--- a/Rover/GCS_Mavlink.h
+++ b/Rover/GCS_Mavlink.h
@@ -44,6 +44,8 @@ private:
     void handle_set_position_target_global_int(const mavlink_message_t &msg);
     void handle_radio(const mavlink_message_t &msg);
 
+    void send_servo_out();
+
     void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override;
 
     MAV_MODE base_mode() const override;