/// 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;
}
}
/// get_loiter_position_to_velocity - loiter position controller
/// converts desired position held in _target vector to desired velocity
void AC_WPNav::get_loiter_position_to_velocity(float dt, float max_speed_cms)
{
Vector3f curr = _inav->get_position();
float dist_error_total;
float vel_sqrt;
float vel_total;
float linear_distance; // the distace we swap between linear and sqrt.
float kP = _pid_pos_lat->kP();
// avoid divide by zero
if (kP <= 0.0f) {
desired_vel.x = 0.0;
desired_vel.y = 0.0;
}else{
// calculate distance error
dist_error.x = _target.x - curr.x;
dist_error.y = _target.y - curr.y;
linear_distance = _wp_accel_cms/(2.0f*kP*kP);
dist_error_total = safe_sqrt(dist_error.x*dist_error.x + dist_error.y*dist_error.y);
_distance_to_target = dist_error_total; // for reporting purposes
if( dist_error_total > 2.0f*linear_distance ) {
vel_sqrt = safe_sqrt(2.0f*_wp_accel_cms*(dist_error_total-linear_distance));
desired_vel.x = vel_sqrt * dist_error.x/dist_error_total;
desired_vel.y = vel_sqrt * dist_error.y/dist_error_total;
}else{
desired_vel.x = _pid_pos_lat->kP() * dist_error.x;
desired_vel.y = _pid_pos_lon->kP() * dist_error.y;
}
// ensure velocity stays within limits
vel_total = safe_sqrt(desired_vel.x*desired_vel.x + desired_vel.y*desired_vel.y);
if( vel_total > max_speed_cms ) {
desired_vel.x = max_speed_cms * desired_vel.x/vel_total;
desired_vel.y = max_speed_cms * desired_vel.y/vel_total;
}
// feed forward velocity request
desired_vel.x += _target_vel.x;
desired_vel.y += _target_vel.y;
}
// call velocity to acceleration controller
get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, dt);
}
/// 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;
}
}
