From 21abe1194e34a61838387853a3117bd96150e48e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 9 Mar 2018 11:41:20 +0900 Subject: [PATCH] AC_WPNav: run horizontal position control at main loop rate --- libraries/AC_WPNav/AC_WPNav.cpp | 127 +++++++++++++++----------------- 1 file changed, 58 insertions(+), 69 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 6155700eb5..9be66fa44c 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -378,22 +378,16 @@ void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler) { // calculate dt float dt = _pos_control.time_since_last_xy_update(); + if (dt >= 0.2f) { + dt = 0.0f; + } - // 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 pos controller speed and acceleration - _pos_control.set_speed_xy(_loiter_speed_cms); - _pos_control.set_accel_xy(_loiter_accel_cmss); + // initialise pos controller speed and acceleration + _pos_control.set_speed_xy(_loiter_speed_cms); + _pos_control.set_accel_xy(_loiter_accel_cmss); - calc_loiter_desired_velocity(dt,ekfGndSpdLimit); - _pos_control.update_xy_controller(ekfNavVelGainScaler); - } + calc_loiter_desired_velocity(dt,ekfGndSpdLimit); + _pos_control.update_xy_controller(ekfNavVelGainScaler); } /// init_brake_target - initializes stop position from current position and velocity @@ -422,14 +416,13 @@ void AC_WPNav::update_brake(float ekfGndSpdLimit, float ekfNavVelGainScaler) { // calculate dt float dt = _pos_control.time_since_last_xy_update(); - - // run at poscontrol update rate. - if (dt >= _pos_control.get_dt_xy()) { - - // send adjusted feed forward velocity back to position controller - _pos_control.set_desired_velocity_xy(0.0f, 0.0f); - _pos_control.update_xy_controller(ekfNavVelGainScaler); + if (dt >= 0.2f) { + dt = 0.0f; } + + // send adjusted feed forward velocity back to position controller + _pos_control.set_desired_velocity_xy(0.0f, 0.0f); + _pos_control.update_xy_controller(ekfNavVelGainScaler); } /// @@ -794,40 +787,35 @@ int32_t AC_WPNav::get_wp_bearing_to_destination() const /// update_wpnav - run the wp controller - should be called at 100hz or higher bool AC_WPNav::update_wpnav() { + bool ret = true; + // calculate dt float dt = _pos_control.time_since_last_xy_update(); - bool ret = true; + if (dt >= 0.2f) { + dt = 0.0f; + } - // update at poscontrol update rate - if (dt >= _pos_control.get_dt_xy()) { - // allow the accel and speed values to be set without changing - // out of auto mode. This makes it easier to tune auto flight - _pos_control.set_accel_xy(_wp_accel_cms); - _pos_control.set_accel_z(_wp_accel_z_cms); - - // sanity check dt - if (dt >= 0.2f) { - dt = 0.0f; - } + // allow the accel and speed values to be set without changing + // out of auto mode. This makes it easier to tune auto flight + _pos_control.set_accel_xy(_wp_accel_cms); + _pos_control.set_accel_z(_wp_accel_z_cms); - // advance the target if necessary - if (!advance_wp_target_along_track(dt)) { - // To-Do: handle inability to advance along track (probably because of missing terrain data) - ret = false; - } + // advance the target if necessary + if (!advance_wp_target_along_track(dt)) { + // To-Do: handle inability to advance along track (probably because of missing terrain data) + ret = false; + } - // freeze feedforwards during known discontinuities - // TODO: why always consider Z axis discontinuous? - if (_flags.new_wp_destination) { - _flags.new_wp_destination = false; - } + // freeze feedforwards during known discontinuities + if (_flags.new_wp_destination) { + _flags.new_wp_destination = false; _pos_control.freeze_ff_z(); + } - _pos_control.update_xy_controller(1.0f); - check_wp_leash_length(); + _pos_control.update_xy_controller(1.0f); + check_wp_leash_length(); - _wp_last_update = AP_HAL::millis(); - } + _wp_last_update = AP_HAL::millis(); return ret; } @@ -976,7 +964,11 @@ bool AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V { // mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint bool prev_segment_exists = (_flags.reached_destination && ((AP_HAL::millis() - _wp_last_update) < 1000)); - float dt = _pos_control.get_dt_xy(); + + float dt = _pos_control.time_since_last_xy_update(); + if (dt >= 0.2f) { + dt = 0.0f; + } // check _wp_accel_cms is reasonable to avoid divide by zero if (_wp_accel_cms <= 0) { @@ -1091,34 +1083,31 @@ bool AC_WPNav::update_spline() return false; } - float dt = _pos_control.time_since_last_xy_update(); bool ret = true; - // run at poscontrol update rate - if (dt >= _pos_control.get_dt_xy()) { - // sanity check dt - if (dt >= 0.2f) { - dt = 0.0f; - } + // calculate dt + float dt = _pos_control.time_since_last_xy_update(); + if (dt >= 0.2f) { + dt = 0.0f; + } - // advance the target if necessary - if (!advance_spline_target_along_track(dt)) { - // To-Do: handle failure to advance along track (due to missing terrain data) - ret = false; - } + // advance the target if necessary + if (!advance_spline_target_along_track(dt)) { + // To-Do: handle failure to advance along track (due to missing terrain data) + ret = false; + } - // freeze feedforwards during known discontinuities - // TODO: why always consider Z axis discontinuous? - if (_flags.new_wp_destination) { - _flags.new_wp_destination = false; - } + // freeze feedforwards during known discontinuities + if (_flags.new_wp_destination) { + _flags.new_wp_destination = false; _pos_control.freeze_ff_z(); + } - // run horizontal position controller - _pos_control.update_xy_controller(1.0f); + // run horizontal position controller + _pos_control.update_xy_controller(1.0f); + + _wp_last_update = AP_HAL::millis(); - _wp_last_update = AP_HAL::millis(); - } return ret; }