// update_loiter - run the loiter controller - gets called at 100hz (APM) or 400hz (PX4)
void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler)
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
// run at poscontrol update rate.
// TODO: run on user input to reduce latency, maybe if (user_input || dt >= _pos_control.get_dt_xy())
if (dt >= _pos_control.get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
calc_loiter_desired_velocity(dt,ekfGndSpdLimit);
_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF, ekfNavVelGainScaler);
}
}
/// update_loiter - run the loiter controller - should be called at 100hz
void AC_WPNav::update_loiter()
{
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _loiter_last_update) / 1000.0f;
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt > 0.095f) {
// double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
}
// capture time since last iteration
_loiter_last_update = now;
// translate any adjustments from pilot to loiter target
calc_loiter_desired_velocity(dt);
// trigger position controller on next update
_pos_control.trigger_xy();
}else{
// run loiter's position to velocity step
_pos_control.update_pos_controller(true);
}
}
// update_loiter - run the loiter controller - gets called at 100hz (APM) or 400hz (PX4)
void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler)
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
// run at poscontrol update rate.
// TODO: run on user input to reduce latency, maybe if (user_input || dt >= _pos_control.get_dt_xy())
if (dt >= _pos_control.get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// initialise ekf position reset check
check_for_ekf_position_reset();
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(_loiter_speed_cms);
_pos_control.set_accel_xy(_loiter_accel_cmss);
_pos_control.set_jerk_xy(_loiter_jerk_max_cmsss);
calc_loiter_desired_velocity(dt,ekfGndSpdLimit);
_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF, ekfNavVelGainScaler, true);
}
}
