Rover: use mode class's get_distance_to_destination and speed_error

used for reporting to GCS and logging

APMrover2/GCS_Mavlink.cpp

@@ -134,9 +134,9 @@ void Rover::send_nav_controller_output(mavlink_channel_t chan)
         ahrs.groundspeed() * ins.get_gyro().z,  // use nav_pitch to hold actual Y accel
         nav_controller->nav_bearing_cd() * 0.01f,
         nav_controller->target_bearing_cd() * 0.01f,
-        MIN(wp_distance, UINT16_MAX),
+        MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
         0,
-        groundspeed_error,
+        control_mode->speed_error(),
         nav_controller->crosstrack_error());
 }
 

APMrover2/Log.cpp

@@ -275,7 +275,7 @@ void Rover::Log_Write_Nav_Tuning()
         LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
         time_us             : AP_HAL::micros64(),
         yaw                 : static_cast<uint16_t>(ahrs.yaw_sensor),
-        wp_distance         : wp_distance,
+        wp_distance         : control_mode->get_distance_to_destination(),
         target_bearing_cd   : static_cast<uint16_t>(fabsf(nav_controller->target_bearing_cd())),
         nav_bearing_cd      : static_cast<uint16_t>(fabsf(nav_controller->nav_bearing_cd())),
         throttle            : int8_t(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)),

APMrover2/Rover.h

@@ -269,9 +269,6 @@ private:
     // angle of our next navigation waypoint
     int32_t next_navigation_leg_cd;
 
-    // ground speed error in m/s
-    float groundspeed_error;
-
     // receiver RSSI
     uint8_t receiver_rssi;