Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Improve depth hold behavior

Browse Source
This commit is contained in:
Jacob Walser 2017-02-27 13:06:55 -05:00
parent fae6c084ac
commit 4233ebd005
1 changed files with 16 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
ArduSub/control_althold.cpp
View File

@ -60,10 +60,6 @@ void Sub::althold_run()
// get pilot's desired yaw rate // get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
// call attitude controller // call attitude controller
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
@ -86,24 +82,24 @@ void Sub::althold_run()
} }
} }
// adjust climb rate using rangefinder if (fabsf(channel_throttle->norm_input()-0.5) > 0.05) { // Throttle input above 5%
if (rangefinder_alt_ok()) { // output pilot's throttle
// if rangefinder is ok, use surface tracking attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); // reset z targets to current values
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
} else { // hold z
if (ap.at_bottom) {
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator
pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
} else if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
float target_climb_rate = get_surface_tracking_climb_rate(0, pos_control.get_alt_target(), G_Dt);
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
}
pos_control.update_z_controller();
} }
// call z axis position controller
if (ap.at_bottom) {
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator
pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
} else {
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
}
pos_control.update_z_controller();
//control_in is range 0-1000
//radio_in is raw pwm value
motors.set_forward(channel_forward->norm_input()); motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input()); motors.set_lateral(channel_lateral->norm_input());
} }