/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xyz() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
void AC_PosControl::update_vel_controller_xyz()
{
update_vel_controller_xy();
// update altitude target
set_alt_target_from_climb_rate_ff(_vel_desired.z, _dt, false);
// run z-axis position controller
update_z_controller();
}
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xyz() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
{
// capture time since last iteration
uint32_t now = AP_HAL::millis();
float dt = (now - _last_update_xy_ms)*0.001f;
// call xy controller
if (dt >= get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// apply desired velocity request to position target
desired_vel_to_pos(dt);
// run position controller's position error to desired velocity step
pos_to_rate_xy(XY_MODE_POS_LIMITED_AND_VEL_FF, dt, ekfNavVelGainScaler);
// run velocity to acceleration step
rate_to_accel_xy(dt, ekfNavVelGainScaler);
// run acceleration to lean angle step
accel_to_lean_angles(dt, ekfNavVelGainScaler, false);
// update xy update time
_last_update_xy_ms = now;
}
// update altitude target
set_alt_target_from_climb_rate_ff(_vel_desired.z, _dt, false);
// run z-axis position controller
update_z_controller();
}
