Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: run guided velocity control at main loop rate

This commit is contained in:
Randy Mackay 2018-03-09 11:42:17 +09:00
parent 21abe1194e
commit d7ea8f073f
1 changed files with 12 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopter/mode_guided.cpp
View File

@ -536,24 +536,20 @@ void Copter::ModeGuided::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
guided_pos_target_cm += guided_vel_target_cms * dt;
// send position and velocity targets to position controller
pos_control->set_pos_target(guided_pos_target_cm);
pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_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
guided_pos_target_cm += guided_vel_target_cms * dt;
// send position and velocity targets to position controller
pos_control->set_pos_target(guided_pos_target_cm);
pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y);
// run position controllers
pos_control->update_xy_controller(ekfNavVelGainScaler);
pos_control->update_z_controller();
// call attitude controller