aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_WPNav: spline handles terrain altitudes

Browse Source
This commit is contained in:
Randy Mackay 2016-03-11 16:43:44 +09:00
parent 8b2c479d62
commit 9fbfea951a
2 changed files with 96 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -822,11 +822,43 @@ void AC_WPNav::calculate_wp_leash_length()
/// spline methods
///
/// set_spline_destination waypoint using position vector (distance from home in cm)
/// set_spline_destination waypoint using location class
/// returns false if conversion from location to vector from ekf origin cannot be calculated
/// stopped_at_start should be set to true if vehicle is stopped at the origin
/// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type
/// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE
void AC_WPNav::set_spline_destination(const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination)
bool AC_WPNav::set_spline_destination(const Location_Class& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, Location_Class next_destination)
{
// convert destination location to vector
Vector3f dest_neu;
bool dest_terr_alt;
if (!get_vector_NEU(destination, dest_neu, dest_terr_alt)) {
return false;
}
// make altitude frames consistent
if (!next_destination.change_alt_frame(destination.get_alt_frame())) {
return false;
}
// convert next destination to vector
Vector3f next_dest_neu;
bool next_dest_terr_alt;
if (!get_vector_NEU(next_destination, next_dest_neu, next_dest_terr_alt)) {
return false;
}
// set target as vector from EKF origin
return set_spline_destination(dest_neu, dest_terr_alt, stopped_at_start, seg_end_type, next_dest_neu);
}
/// set_spline_destination waypoint using position vector (distance from home in cm)
/// returns false if conversion from location to vector from ekf origin cannot be calculated
/// terrain_alt should be true if destination.z is a desired altitudes above terrain (false if its desired altitudes above ekf origin)
/// stopped_at_start should be set to true if vehicle is stopped at the origin
/// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type
/// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE
bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination)
{
Vector3f origin;
@ -839,13 +871,23 @@ void AC_WPNav::set_spline_destination(const Vector3f& destination, bool stopped_
_pos_control.get_stopping_point_z(origin);
}
// convert origin to alt-above-terrain
if (terrain_alt) {
float origin_terr_offset;
if (!get_terrain_offset(origin, origin_terr_offset)) {
return false;
}
origin.z -= origin_terr_offset;
}
// set origin and destination
set_spline_origin_and_destination(origin, destination, stopped_at_start, seg_end_type, next_destination);
return set_spline_origin_and_destination(origin, destination, terrain_alt, stopped_at_start, seg_end_type, next_destination);
}
/// set_spline_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 desired altitudes above ekf origin)
/// 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)
bool AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt, 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 && ((AP_HAL::millis() - _wp_last_update) < 1000));
@ -933,26 +975,38 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
// store origin and destination locations
_origin = origin;
_destination = destination;
_terrain_alt = terrain_alt;
// calculate slow down distance
calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms);
// get origin's alt-above-terrain
float origin_terr_offset = 0.0f;
if (terrain_alt) {
if (!get_terrain_offset(origin, origin_terr_offset)) {
return false;
}
}
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin);
_pos_control.set_pos_target(origin + Vector3f(0,0,origin_terr_offset));
_flags.reached_destination = false;
_flags.segment_type = SEGMENT_SPLINE;
_flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition
return true;
}
/// update_spline - update spline controller
void AC_WPNav::update_spline()
bool AC_WPNav::update_spline()
{
// exit immediately if this is not a spline segment
if (_flags.segment_type != SEGMENT_SPLINE) {
return;
return false;
}
float dt = _pos_control.time_since_last_xy_update();
bool ret = true;
// run at poscontrol update rate
if (dt >= _pos_control.get_dt_xy()) {
@ -962,7 +1016,10 @@ void AC_WPNav::update_spline()
}
// advance the target if necessary
advance_spline_target_along_track(dt);
if (!advance_spline_target_along_track(dt)) {
// To-Do: handle failure to advance along track (due to missing terrain data)
ret = false;
}
// freeze feedforwards during known discontinuities
// TODO: why always consider Z axis discontinuous?
@ -977,6 +1034,7 @@ void AC_WPNav::update_spline()
_wp_last_update = AP_HAL::millis();
}
return ret;
}
/// update_spline_solution - recalculates hermite_spline_solution grid
@ -990,7 +1048,7 @@ void AC_WPNav::update_spline_solution(const Vector3f& origin, const Vector3f& de
}
/// advance_spline_target_along_track - move target location along track from origin to destination
void AC_WPNav::advance_spline_target_along_track(float dt)
bool AC_WPNav::advance_spline_target_along_track(float dt)
{
if (!_flags.reached_destination) {
Vector3f target_pos, target_vel;
@ -1003,7 +1061,16 @@ void AC_WPNav::advance_spline_target_along_track(float dt)
// 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 curr_terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(curr_pos, curr_terr_offset)) {
return false;
}
// calculate position error
Vector3f track_error = curr_pos - target_pos;
track_error.z -= curr_terr_offset;
// calculate the horizontal error
float track_error_xy = pythagorous2(track_error.x, track_error.y);
@ -1051,6 +1118,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt)
}
// update target position
target_pos.z += curr_terr_offset;
_pos_control.set_pos_target(target_pos);
// update the yaw
@ -1065,6 +1133,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt)
_flags.reached_destination = true;
}
}
return true;
}
// calc_spline_pos_vel_accel - calculates target position, velocity and acceleration for the given "spline_time"

23
libraries/AC_WPNav/AC_WPNav.h
View File

@ -196,23 +196,35 @@ public:
// get_yaw - returns target yaw in centi-degrees (used for wp and spline navigation)
float get_yaw() const { return _yaw; }
/// set_spline_destination waypoint using position vector (distance from home in cm)
/// set_spline_destination waypoint using location class
/// returns false if conversion from location to vector from ekf origin cannot be calculated
/// terrain_alt should be true if destination.z is a desired altitude above terrain (false if its desired altitude above ekf origin)
/// stopped_at_start should be set to true if vehicle is stopped at the origin
/// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type
/// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE
void set_spline_destination(const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination);
bool set_spline_destination(const Location_Class& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, Location_Class next_destination);
/// set_spline_destination waypoint using position vector (distance from home in cm)
/// returns false if conversion from location to vector from ekf origin cannot be calculated
/// terrain_alt should be true if destination.z is a desired altitudes above terrain (false if its desired altitudes above ekf origin)
/// stopped_at_start should be set to true if vehicle is stopped at the origin
/// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type
/// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE
/// next_destination.z must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination should be too)
bool set_spline_destination(const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination);
/// set_spline_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 desired altitudes above ekf origin)
/// stopped_at_start should be set to true if vehicle is stopped at the origin
/// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type
/// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE
void 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);
bool set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination);
/// reached_spline_destination - true when we have come within RADIUS cm of the waypoint
bool reached_spline_destination() const { return _flags.reached_destination; }
/// update_spline - update spline controller
void update_spline();
bool update_spline();
///
/// shared methods
@ -274,7 +286,8 @@ protected:
void update_spline_solution(const Vector3f& origin, const Vector3f& dest, const Vector3f& origin_vel, const Vector3f& dest_vel);
/// advance_spline_target_along_track - move target location along track from origin to destination
void advance_spline_target_along_track(float dt);
/// returns false if it is unable to advance (most likely because of missing terrain data)
bool advance_spline_target_along_track(float dt);
/// calc_spline_pos_vel - update position and velocity from given spline time
/// relies on update_spline_solution being called since the previous