// 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); } }