Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-09 08:04:14 -03:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: remove xy pos controller

Browse Source
This commit is contained in:
Randy Mackay 2014-01-19 19:26:29 +09:00 committed by Andrew Tridgell
parent d380e6b4d3
commit 0c8cbba644
2 changed files with 203 additions and 481 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

530
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -68,7 +68,8 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// Note that the Vector/Matrix constructors already implicitly zero // Note that the Vector/Matrix constructors already implicitly zero
// their values. // their values.
// //
AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) : AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosControl& pos_control,
APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) :
_inav(inav), _inav(inav),
_ahrs(ahrs), _ahrs(ahrs),
_pos_control(pos_control), _pos_control(pos_control),
@ -77,26 +78,20 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_
_pid_rate_lat(pid_rate_lat), _pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_rate_lon), _pid_rate_lon(pid_rate_lon),
_loiter_last_update(0), _loiter_last_update(0),
_wpnav_last_update(0), _loiter_step(0),
_althold_kP(WPNAV_ALT_HOLD_P), _pilot_accel_fwd_cms(0),
_desired_roll(0), _pilot_accel_rgt_cms(0),
_desired_pitch(0),
_target(0,0,0),
_pilot_vel_forward_cms(0),
_pilot_vel_right_cms(0),
_target_vel(0,0,0),
_vel_last(0,0,0),
_loiter_leash(WPNAV_MIN_LEASH_LENGTH), _loiter_leash(WPNAV_MIN_LEASH_LENGTH),
_loiter_accel_cms(WPNAV_LOITER_ACCEL_MAX), _loiter_accel_cms(WPNAV_LOITER_ACCEL_MAX),
_lean_angle_max_cd(MAX_LEAN_ANGLE), _wp_last_update(0),
_wp_step(0),
_track_length(0.0),
_track_desired(0.0),
_wp_leash_xy(WPNAV_MIN_LEASH_LENGTH), _wp_leash_xy(WPNAV_MIN_LEASH_LENGTH),
_wp_leash_z(WPNAV_MIN_LEASH_LENGTH), _wp_leash_z(WPNAV_MIN_LEASH_LENGTH),
_track_accel(0), _track_accel(0.0),
_track_speed(0), _track_speed(0.0),
_track_leash_length(0), _track_leash_length(0.0)
dist_error(0,0),
desired_vel(0,0),
desired_accel(0,0)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
@ -106,97 +101,74 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_
} }
/// ///
/// simple loiter controller /// loiter controller
/// ///
/// init_loiter_target - sets initial loiter target based on current position and velocity
void AC_WPNav::init_loiter_target()
{
_pos_control.init_pos_target(_inav->get_position(),_inav->get_velocity());
// initialise pilot input
_pilot_vel_forward_cms = 0;
_pilot_vel_right_cms = 0;
}
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const
{
float linear_distance; // half the distace we swap between linear and sqrt and the distace we offset sqrt.
float linear_velocity; // the velocity we swap between linear and sqrt.
float vel_total;
float target_dist;
float kP = _pid_pos_lat->kP();
// calculate current velocity
vel_total = safe_sqrt(velocity.x*velocity.x + velocity.y*velocity.y);
// avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
if (vel_total < 10.0f || kP <= 0.0f || _wp_accel_cms <= 0.0f) {
target = position;
return;
}
// calculate point at which velocity switches from linear to sqrt
linear_velocity = _wp_accel_cms/kP;
// calculate distance within which we can stop
if (vel_total < linear_velocity) {
target_dist = vel_total/kP;
} else {
linear_distance = _wp_accel_cms/(2.0f*kP*kP);
target_dist = linear_distance + (vel_total*vel_total)/(2.0f*_wp_accel_cms);
}
target_dist = constrain_float(target_dist, 0, _wp_leash_xy);
target.x = position.x + (target_dist * velocity.x / vel_total);
target.y = position.y + (target_dist * velocity.y / vel_total);
target.z = position.z;
}
/// set_loiter_target in cm from home /// set_loiter_target in cm from home
void AC_WPNav::set_loiter_target(const Vector3f& position) void AC_WPNav::set_loiter_target(const Vector3f& position)
{ {
_target = position; // set target position
_target_vel.x = 0; _pos_control.set_pos_target(_inav->get_position());
_target_vel.y = 0;
}
/// init_loiter_target - set initial loiter target based on current position and velocity // initialise feed forward velocity to zero
void AC_WPNav::init_loiter_target(const Vector3f& position, const Vector3f& velocity) _pos_control.set_desired_velocity(0,0);
{
// set target position and velocity based on current pos and velocity
_target.x = position.x;
_target.y = position.y;
_target_vel.x = velocity.x;
_target_vel.y = velocity.y;
// initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the loiter controller is first run // initialise pos controller speed, acceleration and leash length
_desired_roll = constrain_int32(_ahrs->roll_sensor,-_lean_angle_max_cd,_lean_angle_max_cd); _pos_control.set_speed_xy(_loiter_speed_cms);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-_lean_angle_max_cd,_lean_angle_max_cd); _pos_control.set_accel_xy(_loiter_accel_cms);
_pos_control.set_leash_xy(_loiter_leash);
// initialise pilot input // initialise pilot input
_pilot_vel_forward_cms = 0; _pilot_accel_fwd_cms = 0;
_pilot_vel_right_cms = 0; _pilot_accel_rgt_cms = 0;
// set last velocity to current velocity
// To-Do: remove the line below by instead forcing reset_I to be called on the first loiter_update call
_vel_last = _inav->get_velocity();
} }
/// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void AC_WPNav::move_loiter_target(float control_roll, float control_pitch, float dt) void AC_WPNav::init_loiter_target()
{ {
// convert pilot input to desired velocity in cm/s Vector3f curr_vel = _inav->get_velocity();
_pilot_vel_forward_cms = -control_pitch * _loiter_accel_cms / 4500.0f;
_pilot_vel_right_cms = control_roll * _loiter_accel_cms / 4500.0f; // set target position
_pos_control.set_pos_target(_inav->get_position());
// initialise feed forward velocities to zero
_pos_control.set_desired_velocity(curr_vel.x, curr_vel.y);
// initialise pos controller speed, acceleration and leash length
// To-Do: will this cause problems for circle which calls this continuously?
_pos_control.set_speed_xy(_loiter_speed_cms);
_pos_control.set_accel_xy(_loiter_accel_cms);
_pos_control.set_leash_xy(_loiter_leash);
// initialise pilot input
_pilot_accel_fwd_cms = 0;
_pilot_accel_rgt_cms = 0;
} }
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target /// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void AC_WPNav::translate_loiter_target_movements(float nav_dt) void AC_WPNav::calculate_loiter_leash_length()
{ {
Vector2f target_vel_adj; _loiter_leash = _pos_control.calc_leash_length_xy(_loiter_speed_cms,_loiter_accel_cms);
float vel_total; }
/// set_pilot_desired_acceleration - sets pilot desired acceleration from roll and pitch stick input
void AC_WPNav::set_pilot_desired_acceleration(float control_roll, float control_pitch)
{
// convert pilot input to desired acceleration in cm/s/s
_pilot_accel_fwd_cms = -control_pitch * _loiter_accel_cms / 4500.0f;
_pilot_accel_rgt_cms = control_roll * _loiter_accel_cms / 4500.0f;
}
/// get_loiter_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav::get_loiter_stopping_point_xy(Vector3f& stopping_point) const
{
_pos_control.get_stopping_point_xy(stopping_point);
}
/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void AC_WPNav::calc_loiter_desired_velocity(float nav_dt)
{
// range check nav_dt // range check nav_dt
if( nav_dt < 0 ) { if( nav_dt < 0 ) {
return; return;
@ -208,167 +180,102 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
} }
// rotate pilot input to lat/lon frame // rotate pilot input to lat/lon frame
target_vel_adj.x = (_pilot_vel_forward_cms*_ahrs->cos_yaw() - _pilot_vel_right_cms*_ahrs->sin_yaw()); Vector2f desired_accel;
target_vel_adj.y = (_pilot_vel_forward_cms*_ahrs->sin_yaw() + _pilot_vel_right_cms*_ahrs->cos_yaw()); desired_accel.x = (_pilot_accel_fwd_cms*_ahrs->cos_yaw() - _pilot_accel_rgt_cms*_ahrs->sin_yaw());
desired_accel.y = (_pilot_accel_fwd_cms*_ahrs->sin_yaw() + _pilot_accel_rgt_cms*_ahrs->cos_yaw());
// add desired change in velocity to current target velocit // get pos_control's feed forward velocity
_target_vel.x += target_vel_adj.x*nav_dt; Vector2f desired_vel = _pos_control.get_desired_velocity();
_target_vel.y += target_vel_adj.y*nav_dt;
if(_target_vel.x > 0 ) { // add pilot commanded acceleration
_target_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms; desired_vel += desired_accel * nav_dt;
_target_vel.x = max(_target_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}else if(_target_vel.x < 0) { // reduce velocity with fake wind resistance
_target_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms; if(desired_vel.x > 0 ) {
_target_vel.x = min(_target_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); desired_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.x/_loiter_speed_cms;
desired_vel.x = max(desired_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}else if(desired_vel.x < 0) {
desired_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.x/_loiter_speed_cms;
desired_vel.x = min(desired_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
} }
if(_target_vel.y > 0 ) { if(desired_vel.y > 0 ) {
_target_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms; desired_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.y/_loiter_speed_cms;
_target_vel.y = max(_target_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); desired_vel.y = max(desired_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
}else if(_target_vel.y < 0) { }else if(desired_vel.y < 0) {
_target_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms; desired_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*desired_vel.y/_loiter_speed_cms;
_target_vel.y = min(_target_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0); desired_vel.y = min(desired_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
} }
// constrain the velocity vector and scale if necessary // constrain and scale the feed forward velocity if necessary
vel_total = safe_sqrt(_target_vel.x*_target_vel.x + _target_vel.y*_target_vel.y); float vel_total = safe_sqrt(desired_vel.x*desired_vel.x + desired_vel.y*desired_vel.y);
if (vel_total > _loiter_speed_cms && vel_total > 0.0f) { if (vel_total > _loiter_speed_cms && vel_total > 0.0f) {
_target_vel.x = _loiter_speed_cms * _target_vel.x/vel_total; desired_vel.x = _loiter_speed_cms * desired_vel.x/vel_total;
_target_vel.y = _loiter_speed_cms * _target_vel.y/vel_total; desired_vel.y = _loiter_speed_cms * desired_vel.y/vel_total;
} }
// update target position // send adjusted feed forward velocity back to position controller
_target.x += _target_vel.x * nav_dt; _pos_control.set_desired_velocity(desired_vel.x,desired_vel.y);
_target.y += _target_vel.y * nav_dt;
// constrain target position to within reasonable distance of current location
Vector3f curr_pos = _inav->get_position();
Vector3f distance_err = _target - curr_pos;
float distance = safe_sqrt(distance_err.x*distance_err.x + distance_err.y*distance_err.y);
if (distance > _loiter_leash && distance > 0.0f) {
_target.x = curr_pos.x + _loiter_leash * distance_err.x/distance;
_target.y = curr_pos.y + _loiter_leash * distance_err.y/distance;
}
} }
/// get_bearing_to_target - get bearing to loiter target in centi-degrees /// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t AC_WPNav::get_bearing_to_target() const int32_t AC_WPNav::get_loiter_bearing_to_target() const
{ {
return get_bearing_cd(_inav->get_position(), _pos_control.get_post_target()); return get_bearing_cd(_inav->get_position(), _pos_control.get_pos_target());
} }
/// update_loiter - run the loiter controller - should be called at 10hz /// update_loiter - run the loiter controller - should be called at 100hz
void AC_WPNav::update_loiter() void AC_WPNav::update_loiter()
{ {
// calculate dt // calculate dt
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
float dt = (now - _loiter_last_update) / 1000.0f; float dt = (now - _loiter_last_update) / 1000.0f;
// catch if we've just been started
if( dt >= 1.0f ) {
dt = 0.0f;
reset_I();
_loiter_step = 0;
}
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle // 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) { if (dt > 0.095f) {
_loiter_step = 0; // double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
} }
// run loiter steps
switch (_loiter_step) {
case 0:
// capture time since last iteration // capture time since last iteration
_loiter_dt = dt;
_loiter_last_update = now; _loiter_last_update = now;
// translate any adjustments from pilot to loiter target // translate any adjustments from pilot to loiter target
translate_loiter_target_movements(_loiter_dt); calc_loiter_desired_velocity(dt);
_loiter_step++; // trigger position controller on next update
break; _pos_control.trigger_xy();
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;
}
}
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void AC_WPNav::calculate_loiter_leash_length()
{
// get loiter position P
float kP = _pid_pos_lat->kP();
// check loiter speed
if( _loiter_speed_cms < 100.0f) {
_loiter_speed_cms = 100.0f;
}
// set loiter acceleration to 1/2 loiter speed
_loiter_accel_cms = _loiter_speed_cms / 2.0f;
// avoid divide by zero
if (kP <= 0.0f || _wp_accel_cms <= 0.0f) {
_loiter_leash = WPNAV_MIN_LEASH_LENGTH;
return;
}
// calculate horizontal leash length
if(WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR <= _wp_accel_cms / kP) {
// linear leash length based on speed close in
_loiter_leash = WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR / kP;
}else{ }else{
// leash length grows at sqrt of speed further out // run loiter's position to velocity step
_loiter_leash = (_wp_accel_cms / (2.0f*kP*kP)) + (WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR*WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR / (2.0f*_wp_accel_cms)); _pos_control.update_pos_controller(true);
}
// ensure leash is at least 1m long
if( _loiter_leash < WPNAV_MIN_LEASH_LENGTH ) {
_loiter_leash = WPNAV_MIN_LEASH_LENGTH;
} }
} }
/// ///
/// waypoint navigation /// waypoint navigation
/// ///
/// set_destination - set destination using cm from home /// set_destination - set destination using cm from home
void AC_WPNav::set_destination(const Vector3f& destination) void AC_WPNav::set_wp_destination(const Vector3f& destination)
{ {
// if waypoint controlls is active and copter has reached the previous waypoint use it for the origin // if waypoint controller is active and copter has reached the previous waypoint use it for the origin
if( _flags.reached_destination && ((hal.scheduler->millis() - _wpnav_last_update) < 1000) ) { if( _flags.reached_destination && ((hal.scheduler->millis() - _wp_last_update) < 1000) ) {
_origin = _destination; _origin = _destination;
}else{ }else{
// otherwise calculate origin from the current position and velocity // otherwise calculate origin from the current position and velocity
get_stopping_point(_inav->get_position(), _inav->get_velocity(), _origin); _pos_control.get_stopping_point_xy(_origin);
} }
// set origin and destination // set origin and destination
set_origin_and_destination(_origin, destination); set_wp_origin_and_destination(_origin, destination);
} }
/// set_origin_and_destination - set origin and destination using lat/lon coordinates /// set_origin_and_destination - set origin and destination using lat/lon coordinates
void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination) void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
{ {
// store origin and destination locations // store origin and destination locations
_origin = origin; _origin = origin;
_destination = destination; _destination = destination;
Vector3f pos_delta = _destination - _origin; Vector3f pos_delta = _destination - _origin;
// calculate leash lengths
bool climb = pos_delta.z >= 0; // climbing vs descending leads to different leash lengths because speed_up_cms and speed_down_cms can be different
_track_length = pos_delta.length(); // get track length _track_length = pos_delta.length(); // get track length
// calculate each axis' percentage of the total distance to the destination // calculate each axis' percentage of the total distance to the destination
@ -380,33 +287,32 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
}else{ }else{
_pos_delta_unit = pos_delta/_track_length; _pos_delta_unit = pos_delta/_track_length;
} }
calculate_wp_leash_length(climb); // update leash lengths
// calculate leash lengths
bool climb = pos_delta.z >= 0; // climbing vs descending leads to different leash lengths because speed_up_cms and speed_down_cms can be different
calculate_wp_leash_length(climb);
// initialise intermediate point to the origin // initialise intermediate point to the origin
_track_desired = 0; _pos_control.set_pos_target(origin);
_target = origin; _track_desired = 0; // target is at beginning of track
_flags.reached_destination = false; _flags.reached_destination = false;
_flags.fast_waypoint = false; // default waypoint back to slow
// initialise the limited speed to current speed along the track // initialise the limited speed to current speed along the track
const Vector3f &curr_vel = _inav->get_velocity(); const Vector3f &curr_vel = _inav->get_velocity();
// get speed along track (note: we convert vertical speed into horizontal speed equivalent) // get speed along track (note: we convert vertical speed into horizontal speed equivalent)
float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z; float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z;
_limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms); _limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
// default waypoint back to slow
_flags.fast_waypoint = false;
// initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the wpnav controller is first run
_desired_roll = constrain_int32(_ahrs->roll_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
// reset target velocity - only used by loiter controller's interpretation of pilot input
_target_vel.x = 0;
_target_vel.y = 0;
} }
/// advance_target_along_track - move target location along track from origin to destination /// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav::advance_target_along_track(float dt) void AC_WPNav::get_wp_stopping_point_xy(Vector3f& stopping_point) const
{
_pos_control.get_stopping_point_xy(stopping_point);
}
/// advance_wp_target_along_track - move target location along track from origin to destination
void AC_WPNav::advance_wp_target_along_track(float dt)
{ {
float track_covered; float track_covered;
Vector3f track_error; Vector3f track_error;
@ -481,7 +387,7 @@ void AC_WPNav::advance_target_along_track(float dt)
_track_desired = max(_track_desired, track_desired_temp); _track_desired = max(_track_desired, track_desired_temp);
// recalculate the desired position // recalculate the desired position
_target = _origin + _pos_delta_unit * _track_desired; _pos_control.set_pos_target(_origin + _pos_delta_unit * _track_desired);
// check if we've reached the waypoint // check if we've reached the waypoint
if( !_flags.reached_destination ) { if( !_flags.reached_destination ) {
@ -500,16 +406,16 @@ void AC_WPNav::advance_target_along_track(float dt)
} }
} }
/// get_distance_to_destination - get horizontal distance to destination in cm /// get_wp_distance_to_destination - get horizontal distance to destination in cm
float AC_WPNav::get_distance_to_destination() float AC_WPNav::get_wp_distance_to_destination()
{ {
// get current location // get current location
Vector3f curr = _inav->get_position(); Vector3f curr = _inav->get_position();
return pythagorous2(_destination.x-curr.x,_destination.y-curr.y); return pythagorous2(_destination.x-curr.x,_destination.y-curr.y);
} }
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_bearing_to_destination() int32_t AC_WPNav::get_wp_bearing_to_destination()
{ {
return get_bearing_cd(_inav->get_position(), _destination); return get_bearing_cd(_inav->get_position(), _destination);
} }
@ -519,48 +425,23 @@ void AC_WPNav::update_wpnav()
{ {
// calculate dt // calculate dt
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
float dt = (now - _wpnav_last_update) / 1000.0f; float dt = (now - _wp_last_update) / 1000.0f;
// catch if we've just been started
if( dt >= 1.0f ) {
dt = 0.0;
reset_I();
_wpnav_step = 0;
}
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle // 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) { if (dt > 0.095f) {
_wpnav_step = 0; // double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
} }
// run loiter steps
switch (_wpnav_step) {
case 0:
// capture time since last iteration // capture time since last iteration
_wpnav_dt = dt; _wp_last_update = now;
_wpnav_last_update = now;
// advance the target if necessary // advance the target if necessary
if (dt > 0.0f) { advance_wp_target_along_track(dt);
advance_target_along_track(dt); _pos_control.trigger_xy();
} }else{
_wpnav_step++; // run position controller
break; _pos_control.update_pos_controller(false);
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;
} }
} }
@ -568,112 +449,6 @@ void AC_WPNav::update_wpnav()
/// shared methods /// shared methods
/// ///
/// 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;
}
}
/// 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)
{
const 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.0f ) {
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;
}
}
/// get_loiter_acceleration_to_lean_angles - loiter acceleration controller
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float accel_lon)
{
float z_accel_meas = -GRAVITY_MSS * 100; // gravity in cm/s/s
float accel_forward;
float accel_right;
// To-Do: add 1hz filter to accel_lat, accel_lon
// rotate accelerations into body forward-right frame
accel_forward = accel_lat*_ahrs->cos_yaw() + accel_lon*_ahrs->sin_yaw();
accel_right = -accel_lat*_ahrs->sin_yaw() + accel_lon*_ahrs->cos_yaw();
// update angle targets that will be passed to stabilize controller
_desired_roll = constrain_float(fast_atan(accel_right*_ahrs->cos_pitch()/(-z_accel_meas))*(18000/M_PI_F), -_lean_angle_max_cd, _lean_angle_max_cd);
_desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI_F), -_lean_angle_max_cd, _lean_angle_max_cd);
}
// get_bearing_cd - return bearing in centi-degrees between two positions // get_bearing_cd - return bearing in centi-degrees between two positions
// To-Do: move this to math library // To-Do: move this to math library
float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const
@ -685,24 +460,13 @@ float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destinati
return bearing; return bearing;
} }
/// reset_I - clears I terms from loiter PID controller
void AC_WPNav::reset_I()
{
_pid_pos_lon->reset_I();
_pid_pos_lat->reset_I();
_pid_rate_lon->reset_I();
_pid_rate_lat->reset_I();
// set last velocity to current velocity
_vel_last = _inav->get_velocity();
}
/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller /// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
void AC_WPNav::calculate_wp_leash_length(bool climb) void AC_WPNav::calculate_wp_leash_length(bool climb)
{ {
// get loiter position P // get loiter position P
float kP = _pid_pos_lat->kP(); float kP = _pid_pos_lat->kP();
float althold_kP = _pos_control.althold_kP();
// sanity check acceleration and avoid divide by zero // sanity check acceleration and avoid divide by zero
if (_wp_accel_cms <= 0.0f) { if (_wp_accel_cms <= 0.0f) {
@ -714,7 +478,7 @@ void AC_WPNav::calculate_wp_leash_length(bool climb)
_wp_leash_xy = WPNAV_MIN_LEASH_LENGTH; _wp_leash_xy = WPNAV_MIN_LEASH_LENGTH;
return; return;
} }
// calculate horiztonal leash length // calculate horizontal leash length
if(_wp_speed_cms <= _wp_accel_cms / kP) { if(_wp_speed_cms <= _wp_accel_cms / kP) {
// linear leash length based on speed close in // linear leash length based on speed close in
_wp_leash_xy = _wp_speed_cms / kP; _wp_leash_xy = _wp_speed_cms / kP;
@ -735,12 +499,12 @@ void AC_WPNav::calculate_wp_leash_length(bool climb)
}else{ }else{
speed_vert = _wp_speed_down_cms; speed_vert = _wp_speed_down_cms;
} }
if(speed_vert <= WPNAV_ALT_HOLD_ACCEL_MAX / _althold_kP) { if(speed_vert <= WPNAV_ALT_HOLD_ACCEL_MAX / althold_kP) {
// linear leash length based on speed close in // linear leash length based on speed close in
_wp_leash_z = speed_vert / _althold_kP; _wp_leash_z = speed_vert / althold_kP;
}else{ }else{
// leash length grows at sqrt of speed further out // leash length grows at sqrt of speed further out
_wp_leash_z = (WPNAV_ALT_HOLD_ACCEL_MAX / (2.0*_althold_kP*_althold_kP)) + (speed_vert*speed_vert / (2*WPNAV_ALT_HOLD_ACCEL_MAX)); _wp_leash_z = (WPNAV_ALT_HOLD_ACCEL_MAX / (2.0*althold_kP*althold_kP)) + (speed_vert*speed_vert / (2*WPNAV_ALT_HOLD_ACCEL_MAX));
} }
// ensure leash is at least 1m long // ensure leash is at least 1m long

150
libraries/AC_WPNav/AC_WPNav.h
View File

@ -2,7 +2,6 @@
#ifndef AC_WPNAV_H #ifndef AC_WPNAV_H
#define AC_WPNAV_H #define AC_WPNAV_H
#include <inttypes.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <AP_Math.h> #include <AP_Math.h>
@ -44,60 +43,64 @@ public:
AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosControl& pos_control, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon); AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosControl& pos_control, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
/// ///
/// simple loiter controller /// loiter controller
/// ///
/// get_loiter_target - get loiter target as position vector (from home in cm)
const Vector3f &get_loiter_target() const { return _target; }
/// set_loiter_target in cm from home /// set_loiter_target in cm from home
void set_loiter_target(const Vector3f& position); void set_loiter_target(const Vector3f& position);
/// init_loiter_target - sets initial loiter target based on current position and velocity /// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void init_loiter_target() { _pos_control.init_pos_target(_inav->get_position(),_inav->get_velocity()); } void init_loiter_target();
/// move_loiter_target - move destination using pilot input
void move_loiter_target(float control_roll, float control_pitch, float dt);
/// get_distance_to_target - get horizontal distance to loiter target in cm
float get_distance_to_target() const;
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t get_bearing_to_target() const;
/// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter();
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const;
/// set_loiter_velocity - allows main code to pass the maximum velocity for loiter /// set_loiter_velocity - allows main code to pass the maximum velocity for loiter
void set_loiter_velocity(float velocity_cms) { _loiter_speed_cms = velocity_cms; }; void set_loiter_velocity(float velocity_cms) { _loiter_speed_cms = velocity_cms; };
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void calculate_loiter_leash_length();
/// set_pilot_desired_acceleration - sets pilot desired acceleration from roll and pitch stick input
void set_pilot_desired_acceleration(float control_roll, float control_pitch);
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void get_loiter_stopping_point_xy(Vector3f& stopping_point) const;
/// get_loiter_distance_to_target - get horizontal distance to loiter target in cm
float get_loiter_distance_to_target() const { return _pos_control.get_distance_to_target(); }
/// get_loiter_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t get_loiter_bearing_to_target() const;
/// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter();
/// ///
/// waypoint controller /// waypoint controller
/// ///
/// get_destination waypoint using position vector (distance from home in cm) /// get_wp_destination waypoint using position vector (distance from home in cm)
const Vector3f &get_destination() const { return _destination; } const Vector3f &get_wp_destination() const { return _destination; }
/// set_destination waypoint using position vector (distance from home in cm) /// set_wp_destination waypoint using position vector (distance from home in cm)
void set_destination(const Vector3f& destination); void set_wp_destination(const Vector3f& destination);
/// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm) /// set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
void set_origin_and_destination(const Vector3f& origin, const Vector3f& destination); void set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination);
/// advance_target_along_track - move target location along track from origin to destination /// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration
void advance_target_along_track(float dt); /// results placed in stopping_position vector
void get_wp_stopping_point_xy(Vector3f& stopping_point) const;
/// get_distance_to_destination - get horizontal distance to destination in cm /// advance_wp_target_along_track - move target location along track from origin to destination
float get_distance_to_destination(); void advance_wp_target_along_track(float dt);
/// get_wp_distance_to_destination - get horizontal distance to destination in cm
float get_wp_distance_to_destination();
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees /// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t get_bearing_to_destination(); int32_t get_wp_bearing_to_destination();
/// reached_destination - true when we have come within RADIUS cm of the waypoint /// reached_destination - true when we have come within RADIUS cm of the waypoint
bool reached_destination() const { return _flags.reached_destination; } bool reached_wp_destination() const { return _flags.reached_destination; }
/// set_fast_waypoint - set to true to ignore the waypoint radius and consider the waypoint 'reached' the moment the intermediate point reaches it /// set_fast_waypoint - set to true to ignore the waypoint radius and consider the waypoint 'reached' the moment the intermediate point reaches it
void set_fast_waypoint(bool fast) { _flags.fast_waypoint = fast; } void set_fast_waypoint(bool fast) { _flags.fast_waypoint = fast; }
@ -110,17 +113,14 @@ public:
/// ///
/// get desired roll, pitch which should be fed into stabilize controllers /// get desired roll, pitch which should be fed into stabilize controllers
int32_t get_desired_roll() const { return _desired_roll; }; int32_t get_roll() const { return _pos_control.get_roll(); };
int32_t get_desired_pitch() const { return _desired_pitch; }; int32_t get_pitch() const { return _pos_control.get_pitch(); };
/// get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller /// get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller
float get_desired_alt() const { return _target.z; } float get_desired_alt() const { return _pos_control.get_alt_target(); }
/// set_desired_alt - set desired altitude (in cm above home) /// set_desired_alt - set desired altitude (in cm above home)
void set_desired_alt(float desired_alt) { _target.z = desired_alt; } void set_desired_alt(float desired_alt) { _pos_control.set_alt_target(desired_alt); }
/// set_althold_kP - pass in alt hold controller's P gain
void set_althold_kP(float kP) { if(kP>0.0f) _althold_kP = kP; }
/// set_horizontal_velocity - allows main code to pass target horizontal velocity for wp navigation /// set_horizontal_velocity - allows main code to pass target horizontal velocity for wp navigation
void set_horizontal_velocity(float velocity_cms) { _wp_speed_cms = velocity_cms; }; void set_horizontal_velocity(float velocity_cms) { _wp_speed_cms = velocity_cms; };
@ -134,14 +134,11 @@ public:
/// get_descent_velocity - returns target descent speed in cm/s during missions. Note: always positive /// get_descent_velocity - returns target descent speed in cm/s during missions. Note: always positive
float get_descent_velocity() const { return _wp_speed_down_cms; }; float get_descent_velocity() const { return _wp_speed_down_cms; };
/// get_waypoint_radius - access for waypoint radius in cm /// get_wp_radius - access for waypoint radius in cm
float get_waypoint_radius() const { return _wp_radius_cm; } float get_wp_radius() const { return _wp_radius_cm; }
/// get_waypoint_acceleration - returns acceleration in cm/s/s during missions /// get_wp_acceleration - returns acceleration in cm/s/s during missions
float get_waypoint_acceleration() const { return _wp_accel_cms.get(); } float get_wp_acceleration() const { return _wp_accel_cms.get(); }
/// set_lean_angle_max - limits maximum lean angle
void set_lean_angle_max(int16_t angle_cd) { if (angle_cd >= 1000 && angle_cd <= 8000) {_lean_angle_max_cd = angle_cd;} }
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
@ -152,30 +149,13 @@ protected:
uint8_t fast_waypoint : 1; // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint uint8_t fast_waypoint : 1; // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
} _flags; } _flags;
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target /// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
void translate_loiter_target_movements(float nav_dt); /// updated velocity sent directly to position controller
void calc_loiter_desired_velocity(float nav_dt);
/// get_loiter_position_to_velocity - loiter position controller
/// converts desired position held in _target vector to desired velocity
void get_loiter_position_to_velocity(float dt, float max_speed_cms);
/// get_loiter_velocity_to_acceleration - loiter velocity controller
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void get_loiter_velocity_to_acceleration(float vel_lat_cms, float vel_lon_cms, float dt);
/// get_loiter_acceleration_to_lean_angles - loiter acceleration controller
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void get_loiter_acceleration_to_lean_angles(float accel_lat_cmss, float accel_lon_cmss);
/// get_bearing_cd - return bearing in centi-degrees between two positions /// get_bearing_cd - return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const; float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const;
/// reset_I - clears I terms from loiter PID controller
void reset_I();
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void calculate_loiter_leash_length();
/// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller /// calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
/// set climb param to true if track climbs vertically, false if descending /// set climb param to true if track climbs vertically, false if descending
void calculate_wp_leash_length(bool climb); void calculate_wp_leash_length(bool climb);
@ -198,29 +178,19 @@ protected:
AP_Float _wp_speed_down_cms; // descent speed target in cm/s AP_Float _wp_speed_down_cms; // descent speed target in cm/s
AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
AP_Float _wp_accel_cms; // acceleration in cm/s/s during missions AP_Float _wp_accel_cms; // acceleration in cm/s/s during missions
uint8_t _loiter_step; // used to decide which portion of loiter controller to run during this iteration
uint8_t _wpnav_step; // used to decide which portion of wpnav controller to run during this iteration
uint32_t _loiter_last_update; // time of last update_loiter call
uint32_t _wpnav_last_update; // time of last update_wpnav call
float _loiter_dt; // time difference since last loiter call
float _wpnav_dt; // time difference since last loiter call
float _althold_kP; // alt hold's P gain
// output from controller
int32_t _desired_roll; // fed to stabilize controllers at 50hz
int32_t _desired_pitch; // fed to stabilize controllers at 50hz
// loiter controller internal variables // loiter controller internal variables
Vector3f _target; // loiter's target location in cm from home uint32_t _loiter_last_update; // time of last update_loiter call
int16_t _pilot_vel_forward_cms; // pilot's desired velocity forward (body-frame) uint8_t _loiter_step; // used to decide which portion of loiter controller to run during this iteration
int16_t _pilot_vel_right_cms; // pilot's desired velocity right (body-frame) int16_t _pilot_accel_fwd_cms; // pilot's desired acceleration forward (body-frame)
Vector3f _target_vel; // pilot's latest desired velocity in earth-frame int16_t _pilot_accel_rgt_cms; // pilot's desired acceleration right (body-frame)
Vector3f _vel_last; // previous iterations velocity in cm/s
float _loiter_leash; // loiter's horizontal leash length in cm. used to stop the pilot from pushing the target location too far from the current location float _loiter_leash; // loiter's horizontal leash length in cm. used to stop the pilot from pushing the target location too far from the current location
float _loiter_accel_cms; // loiter's acceleration in cm/s/s float _loiter_accel_cms; // loiter's acceleration in cm/s/s
int16_t _lean_angle_max_cd; // maximum lean angle in centi-degrees
// waypoint controller internal variables // waypoint controller internal variables
uint32_t _wp_last_update; // time of last update_wpnav call
uint8_t _wp_step; // used to decide which portion of wpnav controller to run during this iteration
Vector3f _origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP) Vector3f _origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP)
Vector3f _destination; // target destination in cm from home (equivalent to next_WP) Vector3f _destination; // target destination in cm from home (equivalent to next_WP)
Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination
@ -232,17 +202,5 @@ protected:
float _track_accel; // acceleration along track float _track_accel; // acceleration along track
float _track_speed; // speed in cm/s along track float _track_speed; // speed in cm/s along track
float _track_leash_length; // leash length along track float _track_leash_length; // leash length along track
public:
// for logging purposes
Vector2f dist_error; // distance error calculated by loiter controller
Vector2f desired_vel; // loiter controller desired velocity
Vector2f desired_accel; // the resulting desired acceleration
// To-Do: add split of fast (100hz for accel->angle) and slow (10hz for navigation)
/// update - run the loiter and wpnav controllers - should be called at 100hz
//void update_100hz(void);
/// update - run the loiter and wpnav controllers - should be called at 10hz
//void update_10hz(void);
}; };
#endif // AC_WPNAV_H #endif // AC_WPNAV_H