From 54bfd1f751d3fe4ee3521fb87328aaa42fd311f9 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Thu, 30 May 2019 12:33:22 -0300 Subject: [PATCH] Sub: Remove rangefinder support of control loops The rangefinder handling doesn't handle sonar glitches like locking on to reflections very well. We will remove the rangefinder as an input to the controllers until we can do a more robust implementation. --- ArduSub/control_althold.cpp | 4 ---- ArduSub/control_circle.cpp | 5 ----- ArduSub/control_poshold.cpp | 6 ------ 3 files changed, 15 deletions(-) diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 863a6c4ad4..50e4a9d037 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -94,10 +94,6 @@ void Sub::althold_run() if (ap.at_bottom) { 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 - } 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); } // Detects a zero derivative diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index eabf2488e1..0961fba021 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -83,11 +83,6 @@ void Sub::circle_run() attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true, get_smoothing_gain()); } - // adjust climb rate using rangefinder - if (rangefinder_alt_ok()) { - // if rangefinder is ok, use surface tracking - target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); - } // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index 78a9048774..b0beec706b 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -112,12 +112,6 @@ void Sub::poshold_run() 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); - // adjust climb rate using rangefinder - if (rangefinder_alt_ok()) { - // if rangefinder is ok, use surface tracking - target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); - } - // 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