/// update_wpnav - run the wp controller - should be called at 100hz or higher
void AC_WPNav::update_wpnav()
{
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _wp_last_update) / 1000.0f;
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt >= WPNAV_WP_UPDATE_TIME) {
// double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
}
// capture time since last iteration
_wp_last_update = now;
// advance the target if necessary
advance_wp_target_along_track(dt);
_pos_control.trigger_xy();
if (_flags.new_wp_destination) {
_flags.new_wp_destination = false;
_pos_control.freeze_ff_xy();
}
_pos_control.freeze_ff_z();
} else {
// run horizontal position controller
_pos_control.update_xy_controller(false);
// check if leash lengths need updating
check_wp_leash_length();
}
}
/// 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();
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_max_accel_xy(_wp_accel_cmss);
_pos_control.set_max_accel_z(_wp_accel_z_cmss);
// 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
if (_flags.new_wp_destination) {
_flags.new_wp_destination = false;
_pos_control.freeze_ff_z();
}
_pos_control.update_xy_controller();
check_wp_leash_length();
_wp_last_update = AP_HAL::millis();
return ret;
}
/// update_wpnav - run the wp controller - should be called at 100hz or higher
void AC_WPNav::update_wpnav()
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
// 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;
}
// advance the target if necessary
advance_wp_target_along_track(dt);
// freeze feedforwards during known discontinuities
// TODO: why always consider Z axis discontinuous?
if (_flags.new_wp_destination) {
_flags.new_wp_destination = false;
_pos_control.freeze_ff_xy();
}
_pos_control.freeze_ff_z();
_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f);
check_wp_leash_length();
_wp_last_update = hal.scheduler->millis();
}
}
/// update_wpnav - run the wp controller - should be called at 100hz or higher
void AC_WPNav::update_wpnav()
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
// update at poscontrol update rate
if (dt >= _pos_control.get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// advance the target if necessary
advance_wp_target_along_track(dt);
// freeze feedforwards during known discontinuities
// TODO: why always consider Z axis discontinuous?
if (_flags.new_wp_destination) {
_flags.new_wp_destination = false;
_pos_control.freeze_ff_xy();
}
_pos_control.freeze_ff_z();
_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f);
check_wp_leash_length();
_wp_last_update = hal.scheduler->millis();
}
}
/// update_wpnav - run the wp controller - should be called at 100hz or higher
bool AC_WPNav::update_wpnav()
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
bool ret = true;
// 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_jerk_xy_to_default();
_pos_control.set_accel_z(_wp_accel_z_cms);
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// 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;
_pos_control.freeze_ff_xy();
}
_pos_control.freeze_ff_z();
_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f, false);
check_wp_leash_length();
_wp_last_update = AP_HAL::millis();
}
return ret;
}
/// update_wpnav - run the wp controller - should be called at 10hz
void AC_WPNav::update_wpnav()
{
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _wp_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
_wp_last_update = now;
// advance the target if necessary
advance_wp_target_along_track(dt);
_pos_control.trigger_xy();
}else{
// run position controller
_pos_control.update_pos_controller(false);
}
}
