Copter: Tradheli - fixes collective jump on rotor shutdown in althold and loiter

This commit is contained in:
bnsgeyer 2018-06-10 12:52:57 -04:00 committed by Randy Mackay
parent 30e0c7f746
commit b4d2406062
2 changed files with 6 additions and 0 deletions

View File

@ -66,6 +66,9 @@ void Copter::ModeAltHold::run()
// force descent rate and call position controller // force descent rate and call position controller
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
heli_flags.init_targets_on_arming=true; heli_flags.init_targets_on_arming=true;
if (ap.land_complete_maybe) {
pos_control->relax_alt_hold_controllers(0.0f);
}
#else #else
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif #endif

View File

@ -122,6 +122,9 @@ void Copter::ModeLoiter::run()
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// force descent rate and call position controller // force descent rate and call position controller
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
if (ap.land_complete_maybe) {
pos_control->relax_alt_hold_controllers(0.0f);
}
#else #else
loiter_nav->init_target(); loiter_nav->init_target();
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();