5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-24 00:33:56 -04:00

Sub: run guided velocity control at main loop rate

This commit is contained in:
Randy Mackay 2018-03-09 11:42:53 +09:00
parent d7ea8f073f
commit d43341c532

View File

@ -427,24 +427,21 @@ 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;
}
// 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);
// run position controller
pos_control.update_xy_controller(ekfNavVelGainScaler);
// 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;
// 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);
float lateral_out, forward_out;
translate_pos_control_rp(lateral_out, forward_out);