/// update_velocity_controller_xy - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xy() 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_xy()
{
// capture time since last iteration
uint32_t now = AP_HAL::millis();
float dt = (now - _last_update_xy_ms)*0.001f;
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// check for ekf xy position reset
check_for_ekf_xy_reset();
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// apply desired velocity request to position target
// TODO: this will need to be removed and added to the calling function.
desired_vel_to_pos(dt);
// run position controller
run_xy_controller(dt);
// update xy update time
_last_update_xy_ms = now;
}
/// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home
void AC_PosControl::set_target_to_stopping_point_xy()
{
// check if xy leash needs to be recalculated
calc_leash_length_xy();
get_stopping_point_xy(_pos_target);
}
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
void AC_PosControl::update_xy_controller()
{
// compute dt
uint32_t now = AP_HAL::millis();
float dt = (now - _last_update_xy_ms)*0.001f;
// sanity check dt
if (dt >= POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f) {
dt = 0.0f;
}
// check for ekf xy position reset
check_for_ekf_xy_reset();
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// translate any adjustments from pilot to loiter target
desired_vel_to_pos(dt);
// run horizontal position controller
run_xy_controller(dt);
// update xy update time
_last_update_xy_ms = now;
}
/// update_velocity_controller_xy - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xy() 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_xy(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 for ekf xy position reset
check_for_ekf_xy_reset();
// 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_xy_controller - run the horizontal position controller - should be called at 100hz or higher
void AC_PosControl::update_xy_controller(xy_mode mode, float ekfNavVelGainScaler, bool use_althold_lean_angle)
{
// compute dt
uint32_t now = AP_HAL::millis();
float dt = (now - _last_update_xy_ms)*0.001f;
_last_update_xy_ms = now;
// sanity check dt - expect to be called faster than ~5hz
if (dt > POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f) {
dt = 0.0f;
}
// check for ekf xy position reset
check_for_ekf_xy_reset();
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// translate any adjustments from pilot to loiter target
desired_vel_to_pos(dt);
// run position controller's position error to desired velocity step
pos_to_rate_xy(mode, dt, ekfNavVelGainScaler);
// run position controller's velocity to acceleration step
rate_to_accel_xy(dt, ekfNavVelGainScaler);
// run position controller's acceleration to lean angle step
accel_to_lean_angles(dt, ekfNavVelGainScaler, use_althold_lean_angle);
}
/// 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 = hal.scheduler->millis();
float dt = (now - _last_update_xy_ms) / 1000.0f;
// sanity check dt - expect to be called faster than ~5hz
if (dt >= POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f) {
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 altitude target
set_alt_target_from_climb_rate(_vel_desired.z, dt, false);
// run z-axis position controller
update_z_controller();
// record update time
_last_update_xy_ms = now;
}
/// set_max_speed_xy - set the maximum horizontal speed maximum in cm/s
void AC_PosControl::set_max_speed_xy(float speed_cms)
{
if (fabsf(_speed_cms-speed_cms) > 1.0f) {
_speed_cms = speed_cms;
_flags.recalc_leash_xy = true;
calc_leash_length_xy();
}
}
/// set_max_accel_xy - set the maximum horizontal acceleration in cm/s/s
void AC_PosControl::set_max_accel_xy(float accel_cmss)
{
if (fabsf(_accel_cms-accel_cmss) > 1.0f) {
_accel_cms = accel_cmss;
_flags.recalc_leash_xy = true;
calc_leash_length_xy();
}
}
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
void AC_PosControl::update_xy_controller(bool use_desired_velocity)
{
// catch if we've just been started
uint32_t now = hal.scheduler->millis();
if ((now - _last_update_ms) >= 1000) {
_last_update_ms = now;
if (!_flags.keep_xy_I_terms) {
reset_I_xy();
}
_xy_step = 0;
}
// reset keep_xy_I_term flag in case it has been set
_flags.keep_xy_I_terms = false;
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// reset step back to 0 if loiter or waypoint parents have triggered an update and we completed the last full cycle
if (_flags.force_recalc_xy && _xy_step > 3) {
_flags.force_recalc_xy = false;
_xy_step = 0;
}
// run loiter steps
switch (_xy_step) {
case 0:
// capture time since last iteration
_dt_xy = (now - _last_update_ms) / 1000.0f;
_last_update_ms = now;
// translate any adjustments from pilot to loiter target
desired_vel_to_pos(_dt_xy);
_xy_step++;
break;
case 1:
// run position controller's position error to desired velocity step
pos_to_rate_xy(use_desired_velocity,_dt_xy);
_xy_step++;
break;
case 2:
// run position controller's velocity to acceleration step
rate_to_accel_xy(_dt_xy);
_xy_step++;
break;
case 3:
// run position controller's acceleration to lean angle step
accel_to_lean_angles();
_xy_step++;
break;
}
}
/// 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()
{
// capture time since last iteration
uint32_t now = hal.scheduler->millis();
float dt_xy = (now - _last_update_vel_xyz_ms) / 1000.0f;
// check if xy leash needs to be recalculated
calc_leash_length_xy();
// we will run the horizontal component every 4th iteration (i.e. 50hz on Pixhawk, 20hz on APM)
if (dt_xy >= POSCONTROL_VEL_UPDATE_TIME) {
// record update time
_last_update_vel_xyz_ms = now;
// sanity check dt
if (dt_xy >= POSCONTROL_ACTIVE_TIMEOUT_MS) {
dt_xy = 0.0f;
}
// apply desired velocity request to position target
desired_vel_to_pos(dt_xy);
// run position controller's position error to desired velocity step
pos_to_rate_xy(true, dt_xy);
// run velocity to acceleration step
rate_to_accel_xy(dt_xy);
// run acceleration to lean angle step
accel_to_lean_angles();
}
// update altitude target
set_alt_target_from_climb_rate(_vel_desired.z, _dt);
// run z-axis position controller
update_z_controller();
}
