ArduCopter: restore auto-trim method but now use AHRS.add_trim

ArduCopter/ArduCopter.pde

@@ -819,8 +819,8 @@ static uint8_t auto_disarming_counter;
 // prevents duplicate GPS messages from entering system
 static uint32_t last_gps_time;
 
-// Used to auto exit the roll_pitch_trim saving function
-static uint8_t save_trim_counter;
+// Used to exit the roll and pitch auto trim function
+static uint8_t auto_trim_counter;
 
 // Reference to the AP relay object - APM1 only
 AP_Relay relay;
@@ -1068,8 +1068,8 @@ static void medium_loop()
         }
 #endif
 
-        // save_trim - stores roll and pitch radio inputs to ahrs
-        save_trim();
+        // auto_trim - stores roll and pitch radio inputs to ahrs
+        auto_trim();
 
         // record throttle output
         // ------------------------------

ArduCopter/control_modes.pde

@@ -86,7 +86,7 @@ static void read_trim_switch()
 
         case CH7_SAVE_TRIM:
             if(ap_system.CH7_flag && control_mode <= ACRO && g.rc_3.control_in == 0) {
-                save_trim_counter = 15;
+                save_trim();
             }
             break;
 
@@ -155,28 +155,40 @@ static void read_trim_switch()
 // save_trim - adds roll and pitch trims from the radio to ahrs
 static void save_trim()
 {
-    float roll_trim, pitch_trim;
+    // save roll and pitch trim
+    float roll_trim = ToRad((float)g.rc_1.control_in/100.0);
+    float pitch_trim = ToRad((float)g.rc_2.control_in/100.0);
+    ahrs.add_trim(roll_trim, pitch_trim);
+}
 
-    if(save_trim_counter > 0) {
-        save_trim_counter--;
+// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
+// meant to be called continuously while the pilot attempts to keep the copter level
+static void auto_trim()
+{
+    if(auto_trim_counter > 0) {
+        auto_trim_counter--;
 
-        // first few iterations we simply flash the leds
+        // flash the leds
         led_mode = SAVE_TRIM_LEDS;
 
-        if(save_trim_counter == 1) {
-            // save roll trim
-            roll_trim = ToRad((float)g.rc_1.control_in/100.0);
-            pitch_trim = ToRad((float)g.rc_2.control_in/100.0);
-            ahrs.add_trim(roll_trim, pitch_trim);
+        // calculate roll trim adjustment
+        float roll_trim_adjustment = ToRad((float)-g.rc_1.control_in / 4000.0);
 
-            reset_control_switch();
+        // calculate pitch trim adjustment
+        float pitch_trim_adjustment = ToRad((float)g.rc_2.control_in / 4000.0);
 
-            // switch back leds to normal
-            led_mode = NORMAL_LEDS;
+        // make sure accelerometer values impact attitude quickly
+        ahrs.set_fast_gains(true);
 
-            save_trim_counter = 0;
+        // add trim to ahrs object
+        // save to eeprom on last iteration
+        ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
+
+        // on last iteration restore leds and accel gains to normal
+        if(auto_trim_counter == 0) {
+            ahrs.set_fast_gains(false);
+            led_mode = NORMAL_LEDS;
         }
     }
 }
 
-

ArduCopter/motors.pde

@@ -3,6 +3,7 @@
 // 10 = 1 second
 #define ARM_DELAY 20
 #define DISARM_DELAY 20
+#define AUTO_TRIM_DELAY 100
 
 
 // called at 10hz
@@ -36,56 +37,58 @@ static void arm_motors()
 
     // full right
     if (tmp > 4000) {
-        if (arming_counter == ARM_DELAY) {
-            if(motors.armed() == false) {
-                // arm the motors and configure for flight
 
+        // increase the arming counter to a maximum of 1 beyond the auto trim counter
+        if( arming_counter <= AUTO_TRIM_DELAY ) {
+            arming_counter++;
+        }
+
+        // arm the motors and configure for flight
+        if (arming_counter == ARM_DELAY && !motors.armed()) {
 ////////////////////////////////////////////////////////////////////////////////
 // Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters
 ////////////////////////////////////////////////////////////////////////////////
 #if AP_LIMITS == ENABLED
-                if (limits.enabled() && limits.required()) {
-                    gcs_send_text_P(SEVERITY_LOW, PSTR("Limits - Running pre-arm checks"));
-
-                    // check only pre-arm required modules
-                    if (limits.check_required()) {
-                        gcs_send_text_P(SEVERITY_LOW, PSTR("ARMING PREVENTED - Limit Breached"));
-                        limits.set_state(LIMITS_TRIGGERED);
-                        gcs_send_message(MSG_LIMITS_STATUS);
-
-                        arming_counter++;                                 // restart timer by cycling
-                    }else{
-                        init_arm_motors();
-                    }
+            if (limits.enabled() && limits.required()) {
+                gcs_send_text_P(SEVERITY_LOW, PSTR("Limits - Running pre-arm checks"));
+
+                // check only pre-arm required modules
+                if (limits.check_required()) {
+                    gcs_send_text_P(SEVERITY_LOW, PSTR("ARMING PREVENTED - Limit Breached"));
+                    limits.set_state(LIMITS_TRIGGERED);
+                    gcs_send_message(MSG_LIMITS_STATUS);
+
+                    arming_counter++;                                 // restart timer by cycling
                 }else{
                     init_arm_motors();
                 }
-
-#else  // without AP_LIMITS, just arm motors
+            }else{
                 init_arm_motors();
+            }
+#else  // without AP_LIMITS, just arm motors
+            init_arm_motors();
 #endif //AP_LIMITS_ENABLED
+        }
 
-            }
-            // keep going up
-            arming_counter++;
-        } else{
-            arming_counter++;
+        // arm the motors and configure for flight
+        if (arming_counter == AUTO_TRIM_DELAY && motors.armed()) {
+            auto_trim_counter = 250;
         }
 
-        // full left
+    // full left
     }else if (tmp < -4000) {
-        if (arming_counter == DISARM_DELAY) {
-            if(motors.armed()) {
-                // arm the motors and configure for flight
-                init_disarm_motors();
-            }
-            // keep going up
-            arming_counter++;
-        }else{
+
+        // increase the counter to a maximum of 1 beyond the disarm delay
+        if( arming_counter <= DISARM_DELAY ) {
             arming_counter++;
         }
 
-        // Yaw is centered
+        // disarm the motors
+        if (arming_counter == DISARM_DELAY && motors.armed()) {
+            init_disarm_motors();
+        }
+
+    // Yaw is centered so reset arming counter
     }else{
         arming_counter = 0;
     }

libraries/AP_AHRS/AP_AHRS.cpp

@@ -97,7 +97,7 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
 }
 
 // add_trim - adjust the roll and pitch trim up to a total of 10 degrees
-void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians)
+void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
 {
     Vector3f trim = _trim.get();
 
@@ -105,6 +105,11 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians)
     trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0), ToRad(10.0));
     trim.y = constrain(trim.y + pitch_in_radians, ToRad(-10.0), ToRad(10.0));
 
-    // set and save new trim values
-    _trim.set_and_save(trim);
+    // set new trim values
+    _trim.set(trim);
+
+    // save to eeprom
+    if( save_to_eeprom ) {
+        _trim.save();
+    }
 }

libraries/AP_AHRS/AP_AHRS.h

@@ -148,7 +148,7 @@ public:
     virtual void            set_trim(Vector3f new_trim) { _trim.set_and_save(new_trim); }
 
     // add_trim - adjust the roll and pitch trim up to a total of 10 degrees
-    virtual void            add_trim(float roll_in_radians, float pitch_in_radians);
+    virtual void            add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
 
     // settable parameters
     AP_Float _kp_yaw;