/// 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 - _wpnav_last_update) / 1000.0f;
// catch if we've just been started
if( dt >= 1.0f ) {
dt = 0.0;
reset_I();
_wpnav_step = 0;
} else if (_wpnav_reset > 0) {
_wpnav_step = 0;
_wpnav_reset = 0;
_accel_reset = 1;
}
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt > 0.095f && _wpnav_step > 3) {
_wpnav_step = 0;
}
// run loiter steps
switch (_wpnav_step) {
case 0:
// capture time since last iteration
_wpnav_dt = dt;
_wpnav_last_update = now;
// advance the target if necessary
if (dt > 0.0f) {
advance_target_along_track(dt);
}
_wpnav_step++;
break;
case 1:
// run loiter's position to velocity step
get_loiter_position_to_velocity(_wpnav_dt, _wp_speed_cms);
_wpnav_step++;
break;
case 2:
// run loiter's velocity to acceleration step
get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, _wpnav_dt);
_wpnav_step++;
break;
case 3:
// run loiter's acceleration to lean angle step
get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y);
_wpnav_step++;
break;
}
}
/// update_wpnav - run the wp controller - should be called at 10hz
void AC_WPNav::update_wpnav()
{
uint32_t now = hal.scheduler->millis();
float dt = (now - _wpnav_last_update) / 1000.0f;
_wpnav_last_update = now;
// catch if we've just been started
if( dt >= 1.0 ) {
dt = 0.0;
reset_I();
}else{
// advance the target if necessary
advance_target_along_track(dt);
}
// run loiter position controller
get_loiter_position_to_velocity(dt, _wp_speed_cms);
}
