diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0fa5781d3c..b3b9bfed10 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -473,6 +473,7 @@ private: // Used to exit the roll and pitch auto trim function uint8_t auto_trim_counter; + bool auto_trim_started = false; // Camera #if CAMERA == ENABLED @@ -845,9 +846,10 @@ private: void winch_init(); void winch_update(); - // switches.cpp + // RC_Channel.cpp void save_trim(); void auto_trim(); + void auto_trim_cancel(); // system.cpp void init_ardupilot() override; diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index d42a1d4d49..d03fd30132 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -580,14 +580,41 @@ void Copter::save_trim() // 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 +void Copter::auto_trim_cancel() +{ + auto_trim_counter = 0; + AP_Notify::flags.save_trim = false; + gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim cancelled"); +} + void Copter::auto_trim() { if (auto_trim_counter > 0) { - auto_trim_counter--; + if (copter.flightmode != &copter.mode_stabilize || + !copter.motors->armed()) { + auto_trim_cancel(); + return; + } // flash the leds AP_Notify::flags.save_trim = true; + if (!auto_trim_started) { + if (ap.land_complete) { + // haven't taken off yet + return; + } + auto_trim_started = true; + } + + if (ap.land_complete) { + // landed again. + auto_trim_cancel(); + return; + } + + auto_trim_counter--; + // calculate roll trim adjustment float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f); @@ -601,6 +628,7 @@ void Copter::auto_trim() // on last iteration restore leds and accel gains to normal if (auto_trim_counter == 0) { AP_Notify::flags.save_trim = false; + gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim: Trims saved"); } } } diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index c72acc1a23..3ea3f96cfe 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -52,7 +52,9 @@ void Copter::arm_motors_check() // arm the motors and configure for flight if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && control_mode == Mode::Number::STABILIZE) { + gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start"); auto_trim_counter = 250; + auto_trim_started = false; // ensure auto-disarm doesn't trigger immediately auto_disarm_begin = millis(); }