Sub: Remove auto-trim

Sub vehicles usually have some static stability

ArduSub/ArduSub.cpp

@@ -32,7 +32,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
 #endif
     SCHED_TASK(update_batt_compass,   10,    120),
     SCHED_TASK(auto_disarm_check,     10,     50),
-    SCHED_TASK(auto_trim,             10,     75),
     SCHED_TASK(read_rangefinder,      20,    100),
     SCHED_TASK(update_altitude,       10,    100),
     SCHED_TASK(run_nav_updates,       50,    100),

ArduSub/Sub.cpp

@@ -67,7 +67,6 @@ Sub::Sub(void) :
           pmTest1(0),
           fast_loopTimer(0),
           mainLoop_count(0),
-          auto_trim_counter(0),
           ServoRelayEvents(relay),
 #if CAMERA == ENABLED
           camera(&relay),

ArduSub/Sub.h

@@ -410,9 +410,6 @@ private:
     // Counter of main loop executions.  Used for performance monitoring and failsafe processing
     uint16_t mainLoop_count;
 
-    // Used to exit the roll and pitch auto trim function
-    uint8_t auto_trim_counter;
-
     // Reference to the relay object
     AP_Relay relay;
 

ArduSub/radio.cpp

@@ -118,31 +118,3 @@ void Sub::save_trim()
     Log_Write_Event(DATA_SAVE_TRIM);
     gcs_send_text(MAV_SEVERITY_INFO, "Trim saved");
 }
-
-// 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 vehicle level
-void Sub::auto_trim()
-{
-    if (auto_trim_counter > 0) {
-        auto_trim_counter--;
-
-        // flash the leds
-        AP_Notify::flags.save_trim = true;
-
-        // calculate roll trim adjustment
-        float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f);
-
-        // calculate pitch trim adjustment
-        float pitch_trim_adjustment = ToRad((float)channel_pitch->get_control_in() / 4000.0f);
-
-        // 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) {
-            AP_Notify::flags.save_trim = false;
-        }
-    }
-}
-