/// 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_loiter - run the loiter controller - should be called at 10hz
void AC_WPNav::update_loiter()
{
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _loiter_last_update) / 1000.0f;
// catch if we've just been started
if ((dt>=1.0f)||loiter_reset) { // ST-JD : add "or loiter_reset"
dt = 0.0f;
loiter_reset=false; // ST-JD
reset_I();
_loiter_step = 0;
}
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt > 0.095f && _loiter_step > 3) {
_loiter_step = 0;
}
// run loiter steps
switch (_loiter_step) {
case 0:
// capture time since last iteration
_loiter_dt = dt;
_loiter_last_update = now;
// translate any adjustments from pilot to loiter target
translate_loiter_target_movements(_loiter_dt);
_loiter_step++;
break;
case 1:
// run loiter's position to velocity step
get_loiter_position_to_velocity(_loiter_dt, WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR);
_loiter_step++;
break;
case 2:
// run loiter's velocity to acceleration step
get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, _loiter_dt);
_loiter_step++;
break;
case 3:
// run loiter's acceleration to lean angle step
get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y);
_loiter_step++;
break;
}
}
/// get_loiter_velocity_to_acceleration - loiter velocity controller
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void AC_WPNav::get_loiter_velocity_to_acceleration(float vel_lat, float vel_lon, float dt)
{
Vector3f vel_curr = _inav->get_velocity(); // current velocity in cm/s
Vector3f vel_error; // The velocity error in cm/s.
float accel_total; // total acceleration in cm/s/s
// reset last velocity if this controller has just been engaged or dt is zero
if( dt == 0.0 ) {
desired_accel.x = 0;
desired_accel.y = 0;
} else {
// feed forward desired acceleration calculation
desired_accel.x = (vel_lat - _vel_last.x)/dt;
desired_accel.y = (vel_lon - _vel_last.y)/dt;
}
// store this iteration's velocities for the next iteration
_vel_last.x = vel_lat;
_vel_last.y = vel_lon;
// calculate velocity error
vel_error.x = vel_lat - vel_curr.x;
vel_error.y = vel_lon - vel_curr.y;
// combine feed foward accel with PID outpu from velocity error
desired_accel.x += _pid_rate_lat->get_pid(vel_error.x, dt);
desired_accel.y += _pid_rate_lon->get_pid(vel_error.y, dt);
// scale desired acceleration if it's beyond acceptable limit
accel_total = safe_sqrt(desired_accel.x*desired_accel.x + desired_accel.y*desired_accel.y);
if( accel_total > WPNAV_ACCEL_MAX ) {
desired_accel.x = WPNAV_ACCEL_MAX * desired_accel.x/accel_total;
desired_accel.y = WPNAV_ACCEL_MAX * desired_accel.y/accel_total;
}
// call accel based controller with desired acceleration
get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y);
}
