diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index f887047652..0e5da7fb2f 100644 --- a/ArduSub/ArduSub.cpp +++ b/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), diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 47f9a3bc14..9179c85fb7 100644 --- a/ArduSub/Sub.cpp +++ b/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), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 4cf79b46cd..1ae5020a73 100644 --- a/ArduSub/Sub.h +++ b/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; diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index d290145ba7..a3ae9d266f 100644 --- a/ArduSub/radio.cpp +++ b/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; - } - } -} -