Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: Auto Terain following update

This commit is contained in:
Leonard Hall 2021-07-14 23:47:46 +09:30 committed by Andrew Tridgell
parent 3fb09de378
commit fc8242db68
2 changed files with 44 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -156,11 +156,6 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
_scurve_next_leg.init();
_track_scalar_dt = 1.0f;
// reset input shaped terrain offsets
_pos_terrain_offset = 0.0f;
_vel_terrain_offset = 0.0f;
_accel_terrain_offset = 0.0f;
// set flag so get_yaw() returns current heading target
_flags.reached_destination = false;
_flags.fast_waypoint = false;
@ -171,6 +166,10 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
_origin = _destination = stopping_point;
_terrain_alt = false;
_this_leg_is_spline = false;
// initialise the terrain velocity to the current maximum velocity
_terain_vel = _wp_desired_speed_xy_cms;
_terain_accel = 0.0;
}
/// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
@ -178,7 +177,17 @@ void AC_WPNav::set_speed_xy(float speed_cms)
{
// range check target speed
if (speed_cms >= WPNAV_WP_SPEED_MIN) {
// update terrain following speed scalar
_terain_vel = speed_cms * _terain_vel / _wp_desired_speed_xy_cms;
// initialize the desired wp speed
_wp_desired_speed_xy_cms = speed_cms;
// update position controller speed and acceleration
_pos_control.set_max_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss);
_pos_control.set_correction_speed_accel_xy(_wp_desired_speed_xy_cms, _wp_accel_cmss);
// change track speed
update_track_with_speed_accel_limits();
}
}
@ -265,7 +274,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
if (_this_leg_is_spline) {
// if previous leg was a spline we can use current target velocity vector for origin velocity vector
Vector3f curr_target_vel = _pos_control.get_vel_desired_cms();
curr_target_vel.z -= _vel_terrain_offset;
curr_target_vel.z -= _pos_control.get_vel_offset_z_cms();
origin_speed = curr_target_vel.length();
} else {
// store previous leg
@ -283,11 +292,11 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
if (terrain_alt) {
// new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
_origin.z -= origin_terr_offset;
_pos_terrain_offset += origin_terr_offset;
_pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() + origin_terr_offset);
} else {
// new destination is alt-above-ekf-origin, previous destination was alt-above-terrain
_origin.z += origin_terr_offset;
_pos_terrain_offset -= origin_terr_offset;
_pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() - origin_terr_offset);
}
}
@ -421,21 +430,18 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
if (_terrain_alt && !get_terrain_offset(terr_offset)) {
return false;
}
float pos_offset_z_buffer = 1000.0;
const float offset_z_scaler = _pos_control.pos_offset_z_scaler(terr_offset, pos_offset_z_buffer);
// input shape the terrain offset
shape_pos_vel_accel(terr_offset, 0.0f, 0.0f,
_pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset,
0.0f, _pos_control.get_max_speed_down_cms(), _pos_control.get_max_speed_up_cms(),
-_pos_control.get_max_accel_z_cmss(), _pos_control.get_max_accel_z_cmss(), _pos_control.get_shaping_tc_z_s(), dt);
update_pos_vel_accel(_pos_terrain_offset, _vel_terrain_offset, _accel_terrain_offset, dt, 0.0f);
_pos_control.update_pos_offset_z(terr_offset);
// get current position and adjust altitude to origin and destination's frame (i.e. _frame)
const Vector3f &curr_pos = _inav.get_position() - Vector3f{0, 0, terr_offset};
// Use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of aircraft
Vector3f curr_target_vel = _pos_control.get_vel_desired_cms();
curr_target_vel.z -= _vel_terrain_offset;
curr_target_vel.z -= _pos_control.get_vel_offset_z_cms();
float track_scaler_dt = 1.0f;
// check target velocity is non-zero
@ -445,13 +451,18 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
const float track_velocity = _inav.get_velocity().dot(track_direction);
// set time scaler to be consistent with the achievable aircraft speed with a 5% buffer for short term variation.
track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f);
// set time scaler to not exceed the maximum vertical velocity during terrain following.
if (is_positive(curr_target_vel.z)) {
track_scaler_dt = MIN(track_scaler_dt, fabsf(_wp_speed_up_cms / curr_target_vel.z));
} else if (is_negative(curr_target_vel.z)) {
track_scaler_dt = MIN(track_scaler_dt, fabsf(_wp_speed_down_cms / curr_target_vel.z));
}
}
float vel_time_scalar = 1.0;
if (is_positive(_wp_desired_speed_xy_cms)) {
update_vel_accel(_terain_vel, _terain_accel, dt, false);
shape_vel_accel( _wp_desired_speed_xy_cms * offset_z_scaler, 0.0,
_terain_vel, _terain_accel,
0.0, _wp_desired_speed_xy_cms,
-_wp_accel_cmss, _wp_accel_cmss, _pos_control.get_shaping_tc_xy_s(), dt);
vel_time_scalar = _terain_vel / _wp_desired_speed_xy_cms;
}
// change s-curve time speed with a time constant of maximum acceleration / maximum jerk
float track_scaler_tc = 1.0f;
if (!is_zero(_wp_jerk)) {
@ -466,18 +477,21 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
if (!_this_leg_is_spline) {
// update target position, velocity and acceleration
target_pos = _origin;
s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _flags.fast_waypoint, _track_scalar_dt * dt, target_pos, target_vel, target_accel);
s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _flags.fast_waypoint, _track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel, target_accel);
} else {
// splinetarget_vel
target_vel = curr_target_vel;
_spline_this_leg.advance_target_along_track(_track_scalar_dt * dt, target_pos, target_vel);
_spline_this_leg.advance_target_along_track(_track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel);
s_finished = _spline_this_leg.reached_destination();
}
target_vel *= vel_time_scalar;
target_accel *= sq(vel_time_scalar);
// convert final_target.z to altitude above the ekf origin
target_pos.z += _pos_terrain_offset;
target_vel.z += _vel_terrain_offset;
target_accel.z += _accel_terrain_offset;
target_pos.z += _pos_control.get_pos_offset_z_cm();
target_vel.z += _pos_control.get_vel_offset_z_cms();
target_accel.z += _pos_control.get_accel_offset_z_cmss();
// pass new target to the position controller
_pos_control.set_pos_vel_accel(target_pos.topostype(), target_vel, target_accel);
@ -686,11 +700,11 @@ bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_
if (terrain_alt) {
// new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
_origin.z -= origin_terr_offset;
_pos_terrain_offset += origin_terr_offset;
_pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() + origin_terr_offset);
} else {
// new destination is alt-above-ekf-origin, previous destination was alt-above-terrain
_origin.z += origin_terr_offset;
_pos_terrain_offset -= origin_terr_offset;
_pos_control.set_pos_offset_z_cm(_pos_control.get_pos_offset_z_cm() - origin_terr_offset);
}
}

8
libraries/AC_WPNav/AC_WPNav.h
View File

@ -263,6 +263,8 @@ protected:
Vector3f _destination; // target destination in cm from ekf origin
float _track_error_xy; // horizontal error of the actual position vs the desired position
float _track_scalar_dt; // time compression multiplier to slow the progress along the track
float _terain_vel; // maximum horizontal velocity used to ensure the aircraft can maintain height above terain
float _terain_accel; // acceleration value used to change _terain_vel
// terrain following variables
bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin
@ -270,10 +272,4 @@ protected:
AP_Int8 _rangefinder_use; // parameter that specifies if the range finder should be used for terrain following commands
bool _rangefinder_healthy; // true if rangefinder distance is healthy (i.e. between min and maximum)
float _rangefinder_alt_cm; // latest distance from the rangefinder
// position, velocity and acceleration targets passed to position controller
postype_t _pos_terrain_offset;
float _vel_terrain_offset;
float _accel_terrain_offset;
};