mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 07:28:29 -04:00
Sub: Use new reset_i in relax_alt_hold_controllers
Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
This commit is contained in:
parent
33768cd79c
commit
d2052cdd35
@ -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();
|
||||||
|
Loading…
Reference in New Issue
Block a user