// check_wp_leash_length - check if waypoint leash lengths need to be recalculated
// should be called after _pos_control.update_xy_controller which may have changed the position controller leash lengths
void AC_WPNav::check_wp_leash_length()
{
// exit immediately if recalc is not required
if (_flags.recalc_wp_leash) {
calculate_wp_leash_length();
}
}
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// 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) :
_inav(inav),
_ahrs(ahrs),
_pid_pos_lat(pid_pos_lat),
_pid_pos_lon(pid_pos_lon),
_pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_rate_lon),
_loiter_last_update(0),
_wpnav_last_update(0),
_althold_kP(WPNAV_ALT_HOLD_P),
_desired_roll(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_accel_cms(WPNAV_LOITER_ACCEL_MAX),
_lean_angle_max_cd(MAX_LEAN_ANGLE),
_wp_leash_xy(WPNAV_MIN_LEASH_LENGTH),
_wp_leash_z(WPNAV_MIN_LEASH_LENGTH),
_track_accel(0),
_track_speed(0),
_track_leash_length(0),
dist_error(0,0),
desired_vel(0,0),
desired_accel(0,0)
{
AP_Param::setup_object_defaults(this, var_info);
// initialise leash lengths
calculate_wp_leash_length(true);
calculate_loiter_leash_length();
}
/// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
/// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin)
/// returns false on failure (likely caused by missing terrain data)
bool AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt)
{
// store origin and destination locations
_origin = origin;
_destination = destination;
_terrain_alt = terrain_alt;
Vector3f pos_delta = _destination - _origin;
_track_length = pos_delta.length(); // get track length
// calculate each axis' percentage of the total distance to the destination
if (is_zero(_track_length)) {
// avoid possible divide by zero
_pos_delta_unit.x = 0;
_pos_delta_unit.y = 0;
_pos_delta_unit.z = 0;
}else{
_pos_delta_unit = pos_delta/_track_length;
}
// calculate leash lengths
calculate_wp_leash_length();
// initialise yaw heading
if (_track_length >= WPNAV_YAW_DIST_MIN) {
_yaw = get_bearing_cd(_origin, _destination);
} else {
// set target yaw to current heading target
_yaw = _attitude_control.get_att_target_euler_cd().z;
}
// get origin's alt-above-terrain
float origin_terr_offset = 0.0f;
if (terrain_alt) {
if (!get_terrain_offset(origin_terr_offset)) {
return false;
}
}
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin + Vector3f(0,0,origin_terr_offset));
_track_desired = 0; // target is at beginning of track
_flags.reached_destination = false;
_flags.fast_waypoint = false; // default waypoint back to slow
_flags.slowing_down = false; // target is not slowing down yet
_flags.segment_type = SEGMENT_STRAIGHT;
_flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition
// initialise the limited speed to current speed along the track
const Vector3f &curr_vel = _inav.get_velocity();
// 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;
_limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
return true;
}
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
{
// store origin and destination locations
_origin = origin;
_destination = destination;
Vector3f pos_delta = _destination - _origin;
_track_length = pos_delta.length(); // get track length
// calculate each axis' percentage of the total distance to the destination
if (_track_length == 0.0f) {
// avoid possible divide by zero
_pos_delta_unit.x = 0;
_pos_delta_unit.y = 0;
_pos_delta_unit.z = 0;
}else{
_pos_delta_unit = pos_delta/_track_length;
}
// check _wp_accel_cms is reasonable
if (_wp_accel_cms <= 0) {
_wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
}
// initialise position controller speed and acceleration
_pos_control.set_speed_xy(_wp_speed_cms);
_pos_control.set_accel_xy(_wp_accel_cms);
_pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
_pos_control.calc_leash_length_xy();
_pos_control.calc_leash_length_z();
// calculate leash lengths
calculate_wp_leash_length();
// initialise yaw heading
_yaw = get_bearing_cd(_origin, _destination);
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin);
_track_desired = 0; // target is at beginning of track
_flags.reached_destination = false;
_flags.fast_waypoint = false; // default waypoint back to slow
_flags.segment_type = SEGMENT_STRAIGHT;
// initialise the limited speed to current speed along the track
const Vector3f &curr_vel = _inav->get_velocity();
// 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;
_limited_speed_xy_cms = constrain_float(speed_along_track,0,_wp_speed_cms);
}
/// 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)
{
// store origin and destination locations
_origin = origin;
_destination = destination;
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
// calculate each axis' percentage of the total distance to the destination
if (_track_length == 0.0f) {
// avoid possible divide by zero
_pos_delta_unit.x = 0;
_pos_delta_unit.y = 0;
_pos_delta_unit.z = 0;
}else{
_pos_delta_unit = pos_delta/_track_length;
}
calculate_wp_leash_length(climb); // update leash lengths
// initialise intermediate point to the origin
_track_desired = 0;
_target = origin;
_flags.reached_destination = false;
_wpnav_reset = 1; //reset wpnav update
// initialise the limited speed to current speed along the track
const Vector3f &curr_vel = _inav->get_velocity();
// 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;
_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;
}
/// set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
/// seg_type should be calculated by calling function based on the mission
void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination)
{
// mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint
bool prev_segment_exists = (_flags.reached_destination && ((hal.scheduler->millis() - _wp_last_update) < 1000));
// check _wp_accel_cms is reasonable to avoid divide by zero
if (_wp_accel_cms <= 0) {
_wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
}
// segment start types
// stop - vehicle is not moving at origin
// straight-fast - vehicle is moving, previous segment is straight. vehicle will fly straight through the waypoint before beginning it's spline path to the next wp
// _flag.segment_type holds whether prev segment is straight vs spline but we don't know if it has a delay
// spline-fast - vehicle is moving, previous segment is splined, vehicle will fly through waypoint but previous segment should have it flying in the correct direction (i.e. exactly parallel to position difference vector from previous segment's origin to this segment's destination)
// calculate spline velocity at origin
if (stopped_at_start || !prev_segment_exists) {
// if vehicle is stopped at the origin, set origin velocity to 0.1 * distance vector from origin to destination
_spline_origin_vel = (destination - origin) * 0.1f;
_spline_time = 0.0f;
_spline_vel_scaler = 0.0f;
}else{
// look at previous segment to determine velocity at origin
if (_flags.segment_type == SEGMENT_STRAIGHT) {
// previous segment is straight, vehicle is moving so vehicle should fly straight through the origin
// before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination
_spline_origin_vel = (_destination - _origin);
_spline_time = 0.0f; // To-Do: this should be set based on how much overrun there was from straight segment?
_spline_vel_scaler = 0.0f; // To-Do: this should be set based on speed at end of prev straight segment?
}else{
// previous segment is splined, vehicle will fly through origin
// we can use the previous segment's destination velocity as this segment's origin velocity
// Note: previous segment will leave destination velocity parallel to position difference vector
// from previous segment's origin to this segment's destination)
_spline_origin_vel = _spline_destination_vel;
if (_spline_time > 1.0f && _spline_time < 1.1f) { // To-Do: remove hard coded 1.1f
_spline_time -= 1.0f;
}else{
_spline_time = 0.0f;
}
_spline_vel_scaler = 0.0f;
}
}
// calculate spline velocity at destination
switch (seg_end_type) {
case SEGMENT_END_STOP:
// if vehicle stops at the destination set destination velocity to 0.1 * distance vector from origin to destination
_spline_destination_vel = (destination - origin) * 0.1f;
_flags.fast_waypoint = false;
break;
case SEGMENT_END_STRAIGHT:
// if next segment is straight, vehicle's final velocity should face along the next segment's position
_spline_destination_vel = (next_destination - destination);
_flags.fast_waypoint = true;
break;
case SEGMENT_END_SPLINE:
// if next segment is splined, vehicle's final velocity should face parallel to the line from the origin to the next destination
_spline_destination_vel = (next_destination - origin);
_flags.fast_waypoint = true;
break;
}
// code below ensures we don't get too much overshoot when the next segment is short
float vel_len = (_spline_origin_vel + _spline_destination_vel).length();
float pos_len = (destination - origin).length() * 4.0f;
if (vel_len > pos_len) {
// if total start+stop velocity is more than twice position difference
// use a scaled down start and stop velocityscale the start and stop velocities down
float vel_scaling = pos_len / vel_len;
// update spline calculator
update_spline_solution(origin, destination, _spline_origin_vel * vel_scaling, _spline_destination_vel * vel_scaling);
}else{
// update spline calculator
update_spline_solution(origin, destination, _spline_origin_vel, _spline_destination_vel);
}
// initialise yaw heading to current heading
_yaw = _ahrs->yaw_sensor;
// store origin and destination locations
_origin = origin;
_destination = destination;
// initialise position controller speed and acceleration
_pos_control.set_speed_xy(_wp_speed_cms);
_pos_control.set_accel_xy(_wp_accel_cms);
_pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
_pos_control.calc_leash_length_xy();
_pos_control.calc_leash_length_z();
// calculate leash lengths
calculate_wp_leash_length();
// calculate slow down distance
// To-Do: this should be used for straight segments as well
// To-Do: should we use a combination of horizontal and vertical speeds?
// To-Do: update this automatically when speed or acceleration is changed
_spline_slow_down_dist = _wp_speed_cms * _wp_speed_cms / (2.0f*_wp_accel_cms);
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin);
_flags.reached_destination = false;
_flags.segment_type = SEGMENT_SPLINE;
}
/// advance_spline_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_spline_target_along_track(float dt)
{
if (!_flags.reached_destination) {
Vector3f target_pos, target_vel;
// update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel);
_pos_delta_unit = target_vel/target_vel.length();
calculate_wp_leash_length();
// get current location
Vector3f curr_pos = _inav.get_position();
// get terrain altitude offset for origin and current position (i.e. change in terrain altitude from a position vs ekf origin)
float terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(terr_offset)) {
return false;
}
// calculate position error
Vector3f track_error = curr_pos - target_pos;
track_error.z -= terr_offset;
// calculate the horizontal error
float track_error_xy = pythagorous2(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get position control leash lengths
float leash_xy = _pos_control.get_leash_xy();
float leash_z;
if (track_error.z >= 0) {
leash_z = _pos_control.get_leash_up_z();
}else{
leash_z = _pos_control.get_leash_down_z();
}
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy);
if (track_leash_slack < 0.0f) {
track_leash_slack = 0.0f;
}
// update velocity
float spline_dist_to_wp = (_destination - target_pos).length();
float vel_limit = _wp_speed_cms;
if (!is_zero(dt)) {
vel_limit = MIN(vel_limit, track_leash_slack/dt);
}
// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
_spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cms);
}else if(_spline_vel_scaler < vel_limit) {
// increase velocity using acceleration
_spline_vel_scaler += _wp_accel_cms * dt;
}
// constrain target velocity
_spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit);
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
float target_vel_length = target_vel.length();
if (!is_zero(target_vel_length)) {
_spline_time_scale = _spline_vel_scaler/target_vel_length;
}
// update target position
target_pos.z += terr_offset;
_pos_control.set_pos_target(target_pos);
// update the yaw
_yaw = RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x));
// advance spline time to next step
_spline_time += _spline_time_scale*dt;
// we will reach the next waypoint in the next step so set reached_destination flag
// To-Do: is this one step too early?
if (_spline_time >= 1.0f) {
_flags.reached_destination = true;
}
}
return true;
}
/// advance_spline_target_along_track - move target location along track from origin to destination
bool AC_WPNav::advance_spline_target_along_track(float dt)
{
if (!_flags.reached_destination) {
Vector3f target_pos, target_vel;
// update target position and velocity from spline calculator
calc_spline_pos_vel(_spline_time, target_pos, target_vel);
// if target velocity is zero the origin and destination must be the same
// so flag reached destination (and protect against divide by zero)
float target_vel_length = target_vel.length();
if (is_zero(target_vel_length)) {
_flags.reached_destination = true;
return true;
}
_pos_delta_unit = target_vel / target_vel_length;
calculate_wp_leash_length();
// get current location
Vector3f curr_pos = _inav.get_position();
// get terrain altitude offset for origin and current position (i.e. change in terrain altitude from a position vs ekf origin)
float terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(terr_offset)) {
return false;
}
// calculate position error
Vector3f track_error = curr_pos - target_pos;
track_error.z -= terr_offset;
// calculate the horizontal error
_track_error_xy = norm(track_error.x, track_error.y);
// calculate the vertical error
float track_error_z = fabsf(track_error.z);
// get position control leash lengths
float leash_xy = _pos_control.get_leash_xy();
float leash_z;
if (track_error.z >= 0) {
leash_z = _pos_control.get_leash_up_z();
}else{
leash_z = _pos_control.get_leash_down_z();
}
// calculate how far along the track we could move the intermediate target before reaching the end of the leash
float track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-_track_error_xy)/leash_xy);
if (track_leash_slack < 0.0f) {
track_leash_slack = 0.0f;
}
// update velocity
float spline_dist_to_wp = (_destination - target_pos).length();
float vel_limit = _wp_speed_cms;
if (!is_zero(dt)) {
vel_limit = MIN(vel_limit, track_leash_slack/dt);
}
// if within the stopping distance from destination, set target velocity to sqrt of distance * 2 * acceleration
if (!_flags.fast_waypoint && spline_dist_to_wp < _slow_down_dist) {
_spline_vel_scaler = safe_sqrt(spline_dist_to_wp * 2.0f * _wp_accel_cmss);
}else if(_spline_vel_scaler < vel_limit) {
// increase velocity using acceleration
_spline_vel_scaler += _wp_accel_cmss * dt;
}
// constrain target velocity
_spline_vel_scaler = constrain_float(_spline_vel_scaler, 0.0f, vel_limit);
// scale the spline_time by the velocity we've calculated vs the velocity that came out of the spline calculator
_spline_time_scale = _spline_vel_scaler / target_vel_length;
// update target position
target_pos.z += terr_offset;
_pos_control.set_pos_target(target_pos);
// update the target yaw if origin and destination are at least 2m apart horizontally
if (_track_length_xy >= WPNAV_YAW_DIST_MIN) {
if (_pos_control.get_leash_xy() < WPNAV_YAW_DIST_MIN) {
// if the leash is very short (i.e. flying at low speed) use the target point's velocity along the track
if (!is_zero(target_vel.x) && !is_zero(target_vel.y)) {
set_yaw_cd(RadiansToCentiDegrees(atan2f(target_vel.y,target_vel.x)));
}
} else {
// point vehicle along the leash (i.e. point vehicle towards target point on the segment from origin to destination)
float track_error_xy_length = safe_sqrt(sq(track_error.x)+sq(track_error.y));
if (track_error_xy_length > MIN(WPNAV_YAW_DIST_MIN, _pos_control.get_leash_xy()*WPNAV_YAW_LEASH_PCT_MIN)) {
// To-Do: why is track_error sign reversed?
set_yaw_cd(RadiansToCentiDegrees(atan2f(-track_error.y,-track_error.x)));
}
}
}
// advance spline time to next step
_spline_time += _spline_time_scale*dt;
// we will reach the next waypoint in the next step so set reached_destination flag
// To-Do: is this one step too early?
if (_spline_time >= 1.0f) {
_flags.reached_destination = true;
}
}
return true;
}
