From 4b16271b3df99991259ffd314acab7bf7635c1a7 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 2a95aab69d..e0eef9ede5 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -112,10 +112,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 7f2248bbde..dbe0023110 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -82,11 +82,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); } - // 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 4ad303d3e7..c939892fa8 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -116,12 +116,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, -get_pilot_speed_dn(), g.pilot_speed_up); - // 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