Sub: Use new reset_i in relax_alt_hold_controllers

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
This commit is contained in:
Patrick José Pereira 2019-03-13 14:19:42 -03:00 committed by Jacob Walser
parent 33768cd79c
commit d2052cdd35

View File

@ -103,13 +103,13 @@ void Sub::althold_run()
// output pilot's throttle // output pilot's throttle
attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
// reset z targets to current values // reset z targets to current values
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); pos_control.relax_alt_hold_controllers();
engageStopZ = true; engageStopZ = true;
lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z()); lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z());
} else { // hold z } else { // hold z
if (ap.at_bottom) { if (ap.at_bottom) {
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator pos_control.relax_alt_hold_controllers(); // clear velocity and position targets
pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
} else if (rangefinder_alt_ok()) { } else if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking // if rangefinder is ok, use surface tracking
@ -123,7 +123,7 @@ void Sub::althold_run()
// or smaller input signals // or smaller input signals
if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z()))) { if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z()))) {
engageStopZ = false; engageStopZ = false;
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); pos_control.relax_alt_hold_controllers();
} }
pos_control.update_z_controller(); pos_control.update_z_controller();