diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index fbd5c4e452..0edf56ea57 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -427,23 +427,20 @@ void Sub::guided_posvel_control_run() // calculate dt float dt = pos_control.time_since_last_xy_update(); - // update at poscontrol update rate - if (dt >= pos_control.get_dt_xy()) { - // sanity check dt - if (dt >= 0.2f) { - dt = 0.0f; - } + // sanity check dt + if (dt >= 0.2f) { + dt = 0.0f; + } - // advance position target using velocity target - posvel_pos_target_cm += posvel_vel_target_cms * dt; + // advance position target using velocity target + posvel_pos_target_cm += posvel_vel_target_cms * dt; - // send position and velocity targets to position controller - pos_control.set_pos_target(posvel_pos_target_cm); - pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y); + // send position and velocity targets to position controller + pos_control.set_pos_target(posvel_pos_target_cm); + pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y); - // run position controller - pos_control.update_xy_controller(ekfNavVelGainScaler); - } + // run position controller + pos_control.update_xy_controller(ekfNavVelGainScaler); float lateral_out, forward_out; translate_pos_control_rp(lateral_out, forward_out);