Plane: make threshold to consider trim finished a parameter

ArduCustom · Dec 10, 2023 · c1a47e0 · c1a47e0
1 parent a8966e8
commit c1a47e0
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 4 deletions.
diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
@@ -1411,6 +1411,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(follow, "FOLL", 33, ParametersG2, AP_Follow),
 #endif
 
+    // @Param: SAT_FINISHTHRESH
+    // @DisplayName: Number of microseconds of servo trim adjustment during 10s under which the auto trim is considered finished
+    // @Description: Number of microseconds of servo trim adjustment during 10s under which the auto trim is considered finished
+    // @Range: 0 255
+    // @User: Advanced
+    AP_GROUPINFO("SAT_FINISHTHRESH", 43, ParametersG2, servos_auto_trim_finished_threshold, 20),
+
     // @Param: ARMING_MODE_SW
     // @DisplayName: Selects what mode to switch to after arming
     // @Description: Selects what mode to switch to after arming

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
@@ -600,6 +600,7 @@ class ParametersG2 {
     AP_Int8 flight_mode12;
 
     AP_Int8 arming_mode_sw;
+    AP_Int8 servos_auto_trim_finished_threshold;
 
     // plane throws diff
     AP_Float ailerons_diff;

diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
@@ -1400,22 +1400,23 @@ void Plane::servos_auto_trim(void)
     } else if (now - auto_trim.last_trim_save > 10000) {
 
         if (g2.servo_channels.auto_trim_mode() == SRV_Channels::SERVO_AUTO_TRIM_ONCE) {
-            if (!SERVOS_TRIM_SET_STATUS(aileron).finished && abs(SERVOS_TRIM_SET_STATUS(aileron).adjustment) < 4) {
+            if (!SERVOS_TRIM_SET_STATUS(aileron).finished && abs(SERVOS_TRIM_SET_STATUS(aileron).adjustment) < g2.servos_auto_trim_finished_threshold) {
                 SERVOS_TRIM_SET_STATUS(aileron).finished = true;
                 GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Ailerons trim finished");
             }
 
-            if (!SERVOS_TRIM_SET_STATUS(elevator).finished && abs(SERVOS_TRIM_SET_STATUS(elevator).adjustment) < 4) {
+            if (!SERVOS_TRIM_SET_STATUS(elevator).finished && abs(SERVOS_TRIM_SET_STATUS(elevator).adjustment) < g2.servos_auto_trim_finished_threshold) {
                 SERVOS_TRIM_SET_STATUS(elevator).finished = true;
                 GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Elevator trim finished");
             }
 
-            if (!SERVOS_TRIM_SET_STATUS(elevon).finished && abs(SERVOS_TRIM_SET_STATUS(elevon).adjustment) < 4) {
+            if (!SERVOS_TRIM_SET_STATUS(elevon).finished && abs(SERVOS_TRIM_SET_STATUS(elevon).adjustment) < g2.servos_auto_trim_finished_threshold) {
                 SERVOS_TRIM_SET_STATUS(elevon).finished = true;
                 GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Elevons trim finished");
             }
 
-            if (!SERVOS_TRIM_SET_STATUS(dspoiler_finished) && abs(SERVOS_TRIM_SET_STATUS(dspoiler_outer).adjustment) < 4 && abs(SERVOS_TRIM_SET_STATUS(dspoiler_inner).adjustment) < 4) {
+            if (!SERVOS_TRIM_SET_STATUS(dspoiler_finished) && abs(SERVOS_TRIM_SET_STATUS(dspoiler_outer).adjustment) < g2.servos_auto_trim_finished_threshold &&
+                                                                abs(SERVOS_TRIM_SET_STATUS(dspoiler_inner).adjustment) < g2.servos_auto_trim_finished_threshold) {
                 SERVOS_TRIM_SET_STATUS(dspoiler_inner).finished = true;
                 SERVOS_TRIM_SET_STATUS(dspoiler_outer).finished = true;
                 SERVOS_TRIM_SET_STATUS(dspoiler_finished) = true;