Plane: Simplify radio trimming, improve MAVLink report

ArduPlane/GCS_Mavlink.cpp

@@ -735,11 +735,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink
 MAV_RESULT GCS_MAVLINK_Plane::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
 {
     if (is_equal(packet.param4,1.0f)) {
-        /*
-          radio trim
-        */
-        plane.trim_radio();
+        if (plane.trim_radio()) {
             return MAV_RESULT_ACCEPTED;
+        } else {
+            return MAV_RESULT_FAILED;
+        }
     }
 
     return GCS_MAVLINK::_handle_command_preflight_calibration(packet);

ArduPlane/Plane.h

@@ -906,8 +906,7 @@ private:
     void rudder_arm_disarm_check();
     void read_radio();
     void control_failsafe();
-    void trim_control_surfaces();
-    void trim_radio();
+    bool trim_radio();
     bool rc_failsafe_active(void);
     void init_rangefinder(void);
     void read_rangefinder(void);

ArduPlane/radio.cpp

@@ -282,9 +282,13 @@ void Plane::control_failsafe()
     }
 }
 
-void Plane::trim_control_surfaces()
+bool Plane::trim_radio()
 {
-    read_radio();
+    if (failsafe.rc_failsafe) {
+        // can't trim if we don't have valid input
+        return false;
+    }
+
     int16_t trim_roll_range = (channel_roll->get_radio_max() - channel_roll->get_radio_min())/5;
     int16_t trim_pitch_range = (channel_pitch->get_radio_max() - channel_pitch->get_radio_min())/5;
     if (channel_roll->get_radio_in() < channel_roll->get_radio_min()+trim_roll_range ||
@@ -295,7 +299,7 @@ void Plane::trim_control_surfaces()
         // there is less than 20 percent range left then assume the
         // sticks are not properly centered. This also prevents
         // problems with starting APM with the TX off
-        return;
+        return false;
     }
 
     // trim main surfaces
@@ -330,15 +334,8 @@ void Plane::trim_control_surfaces()
     channel_roll->set_and_save_trim();
     channel_pitch->set_and_save_trim();
     channel_rudder->set_and_save_trim();
-}
 
-void Plane::trim_radio()
-{
-    for (uint8_t y = 0; y < 30; y++) {
-        read_radio();
-    }
-
-    trim_control_surfaces();
+    return true;
 }
 
 /*

ArduPlane/system.cpp

@@ -271,8 +271,9 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
         return;
     }
 
-    if(g.auto_trim > 0 && control_mode == MANUAL)
-        trim_control_surfaces();
+    if(g.auto_trim > 0 && control_mode == MANUAL) {
+        trim_radio();
+    }
 
     // perform any cleanup required for prev flight mode
     exit_mode(control_mode);