diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 056bbbc7f4..8309d8bb40 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -38,7 +38,6 @@ float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle) // called at 100hz void Copter::update_throttle_hover() { -#if FRAME_CONFIG != HELI_FRAME // if not armed or landed exit if (!motors->armed() || ap.land_complete) { return; @@ -57,13 +56,12 @@ void Copter::update_throttle_hover() // get throttle output float throttle = motors->get_throttle(); - // calc average throttle if we are in a level hover + // calc average throttle if we are in a level hover. accounts for heli hover roll trim if (throttle > 0.0f && fabsf(inertial_nav.get_velocity_z()) < 60 && - labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) { + labs(ahrs.roll_sensor-attitude_control->get_roll_trim_cd()) < 500 && labs(ahrs.pitch_sensor) < 500) { // Can we set the time constant automatically motors->update_throttle_hover(0.01f); } -#endif } // set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 3952c75f0f..dc6597d27e 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -74,7 +74,14 @@ void Copter::check_dynamic_flight(void) void Copter::update_heli_control_dynamics(void) { // Use Leaky_I if we are not moving fast - attitude_control->use_leaky_i(!heli_flags.dynamic_flight); +// attitude_control->use_leaky_i(!heli_flags.dynamic_flight); + attitude_control->use_leaky_i(false); + + if (ap.land_complete || ap.land_complete_maybe) { + motors->set_land_complete(true); + } else { + motors->set_land_complete(false); + } if (ap.land_complete || (is_zero(motors->get_desired_rotor_speed()))){ // if we are landed or there is no rotor power demanded, decrement slew scalar diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index f3c2c8bb9d..4196fa4892 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -47,7 +47,7 @@ void Copter::update_land_detector() } else if (ap.land_complete) { #if FRAME_CONFIG == HELI_FRAME // if rotor speed and collective pitch are high then clear landing flag - if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { + if (motors->get_takeoff_collective() && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { #else // if throttle output is high then clear landing flag if (motors->get_throttle() > get_non_takeoff_throttle()) { diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index eadeb4893f..36f757d080 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -49,16 +49,16 @@ void ModeAcro_Heli::run() attitude_control->reset_rate_controller_I_terms(); break; case AP_Motors::SpoolState::GROUND_IDLE: - // Landed - if (motors->init_targets_on_arming()) { + // If aircraft is landed, set target heading to current and reset the integrator + // Otherwise motors could be at ground idle for practice autorotation + if (copter.ap.land_complete) { attitude_control->set_attitude_target_to_current_attitude(); attitude_control->reset_rate_controller_I_terms(); } break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: - // clear landing flag above zero throttle - if (!motors->limit.throttle_lower) { - set_land_complete(false); + if (copter.ap.land_complete) { + attitude_control->reset_rate_controller_I_terms(); } break; case AP_Motors::SpoolState::SPOOLING_UP: diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index 633d0396c0..f92f4a3642 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -54,16 +54,16 @@ void ModeStabilize_Heli::run() attitude_control->reset_rate_controller_I_terms(); break; case AP_Motors::SpoolState::GROUND_IDLE: - // Landed - if (motors->init_targets_on_arming()) { + // If aircraft is landed, set target heading to current and reset the integrator + // Otherwise motors could be at ground idle for practice autorotation + if (copter.ap.land_complete) { attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); } break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: - // clear landing flag above zero throttle - if (!motors->limit.throttle_lower) { - set_land_complete(false); + if (copter.ap.land_complete) { + attitude_control->reset_rate_controller_I_terms(); } case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_DOWN: