From dc2ba8236ac2075ecc2c62f02442d6dfee461d8a Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Tue, 21 Sep 2021 22:41:35 +0100
Subject: [PATCH] Plane:  trim_radio: check manual mode and rotation rates and
 add mesages

---
 ArduPlane/Plane.h   |  2 +-
 ArduPlane/radio.cpp | 32 ++++++++++++++++++++------------
 2 files changed, 21 insertions(+), 13 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index c19ee9d764..23b26ec42c 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -1024,7 +1024,7 @@ private:
     void read_radio();
     int16_t rudder_input(void);
     void control_failsafe();
-    bool trim_radio();
+    void trim_radio();
     bool rc_throttle_value_ok(void) const;
     bool rc_failsafe_active(void) const;
 
diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp
index 48ce2c7da1..75cc9b4ece 100644
--- a/ArduPlane/radio.cpp
+++ b/ArduPlane/radio.cpp
@@ -299,24 +299,32 @@ void Plane::control_failsafe()
     }
 }
 
-bool Plane::trim_radio()
+void Plane::trim_radio()
 {
     if (failsafe.rc_failsafe) {
         // can't trim if we don't have valid input
-        return false;
+        return;
     }
 
-    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 ||
-        channel_roll->get_radio_in() > channel_roll->get_radio_max()-trim_roll_range ||
-        channel_pitch->get_radio_in() < channel_pitch->get_radio_min()+trim_pitch_range ||
-        channel_pitch->get_radio_in() > channel_pitch->get_radio_max()-trim_pitch_range) {
-        // don't trim for extreme values - if we attempt to trim so
-        // there is less than 20 percent range left then assume the
+    if (plane.control_mode != &mode_manual) {
+        gcs().send_text(MAV_SEVERITY_ERROR, "trim failed, not in manual mode");
+        return;
+    }
+
+    if (labs(channel_roll->get_control_in()) > (channel_roll->get_range() * 0.2) ||
+            labs(channel_pitch->get_control_in()) > (channel_pitch->get_range() * 0.2)) {
+        // don't trim for extreme values - if we attempt to trim
+        // more 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 false;
+        gcs().send_text(MAV_SEVERITY_ERROR, "trim failed, large roll and pitch input");
+        return;
+    }
+
+    if (degrees(ahrs.get_gyro().length()) > 30.0) {
+        // rotating more than 30 deg/second
+        gcs().send_text(MAV_SEVERITY_ERROR, "trim failed, large movement");
+        return;
     }
 
     // trim main surfaces
@@ -352,7 +360,7 @@ bool Plane::trim_radio()
     channel_pitch->set_and_save_trim();
     channel_rudder->set_and_save_trim();
 
-    return true;
+    gcs().send_text(MAV_SEVERITY_NOTICE, "trim complete");
 }
 
 /*