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

AC_WPNav: straight line waypoints accept terrain

This commit is contained in:
Randy Mackay 2015-08-27 11:54:16 +09:00
parent cd999a2091
commit 8b2c479d62
2 changed files with 156 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -426,8 +426,27 @@ void AC_WPNav::set_speed_xy(float speed_cms)
}
}
/// set_destination - set destination using cm from home
void AC_WPNav::set_wp_destination(const Vector3f& destination)
/// set_wp_destination waypoint using location class
/// returns false if conversion from location to vector from ekf origin cannot be calculated
bool AC_WPNav::set_wp_destination(const Location_Class& destination)
{
bool terr_alt;
Vector3f dest_neu;
// convert destination location to vector
if (!get_vector_NEU(destination, dest_neu, terr_alt)) {
return false;
}
// set target as vector from EKF origin
set_wp_destination(dest_neu, terr_alt);
return true;
}
/// set_wp_destination waypoint using position vector (distance from home in cm)
/// terrain_alt should be true if destination.z is a desired altitude above terrain
bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
{
Vector3f origin;
@ -440,16 +459,28 @@ void AC_WPNav::set_wp_destination(const Vector3f& destination)
_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_wp_origin_and_destination(origin, destination);
return set_wp_origin_and_destination(origin, destination, terrain_alt);
}
/// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
/// 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
@ -471,12 +502,20 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
if (_track_length >= WPNAV_YAW_DIST_MIN) {
_yaw = get_bearing_cd(_origin, _destination);
} else {
// set target yaw to current heading. Alternatively we could pull this from the attitude controller if we had access to it
// 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, 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));
_track_desired = 0; // target is at beginning of track
_flags.reached_destination = false;
_flags.fast_waypoint = false; // default waypoint back to slow
@ -489,6 +528,8 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
// 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;
}
/// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position
@ -525,7 +566,7 @@ void AC_WPNav::get_wp_stopping_point_xy(Vector3f& stopping_point) const
}
/// advance_wp_target_along_track - move target location along track from origin to destination
void AC_WPNav::advance_wp_target_along_track(float dt)
bool AC_WPNav::advance_wp_target_along_track(float dt)
{
float track_covered; // distance (in cm) along the track that the vehicle has traveled. Measured by drawing a perpendicular line from the track to the vehicle.
Vector3f track_error; // distance error (in cm) from the track_covered position (i.e. closest point on the line to the vehicle) and the vehicle
@ -535,7 +576,15 @@ void AC_WPNav::advance_wp_target_along_track(float dt)
// get current location
Vector3f curr_pos = _inav.get_position();
Vector3f curr_delta = curr_pos - _origin;
// calculate terrain adjustments
float curr_terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(curr_pos, curr_terr_offset)) {
return false;
}
// calculate 3d vector from segment's origin
Vector3f curr_delta = (curr_pos - Vector3f(0,0,curr_terr_offset)) - _origin;
// calculate how far along the track we are
track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;
@ -634,7 +683,10 @@ void AC_WPNav::advance_wp_target_along_track(float dt)
}
// recalculate the desired position
_pos_control.set_pos_target(_origin + _pos_delta_unit * _track_desired);
Vector3f final_target = _origin + _pos_delta_unit * _track_desired;
// convert final_target.z to altitude above the ekf origin
final_target.z += curr_terr_offset;
_pos_control.set_pos_target(final_target);
// check if we've reached the waypoint
if( !_flags.reached_destination ) {
@ -644,13 +696,16 @@ void AC_WPNav::advance_wp_target_along_track(float dt)
_flags.reached_destination = true;
}else{
// regular waypoints also require the copter to be within the waypoint radius
Vector3f dist_to_dest = curr_pos - _destination;
Vector3f dist_to_dest = (curr_pos - Vector3f(0,0,curr_terr_offset)) - _destination;
if( dist_to_dest.length() <= _wp_radius_cm ) {
_flags.reached_destination = true;
}
}
}
}
// successfully advanced along track
return true;
}
/// get_wp_distance_to_destination - get horizontal distance to destination in cm
@ -668,10 +723,11 @@ int32_t AC_WPNav::get_wp_bearing_to_destination() const
}
/// update_wpnav - run the wp controller - should be called at 100hz or higher
void AC_WPNav::update_wpnav()
bool AC_WPNav::update_wpnav()
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
bool ret = true;
// update at poscontrol update rate
if (dt >= _pos_control.get_dt_xy()) {
@ -687,7 +743,10 @@ void AC_WPNav::update_wpnav()
}
// advance the target if necessary
advance_wp_target_along_track(dt);
if (!advance_wp_target_along_track(dt)) {
// To-Do: handle inability to advance along track (probably because of missing terrain data)
ret = false;
}
// freeze feedforwards during known discontinuities
// TODO: why always consider Z axis discontinuous?
@ -702,6 +761,8 @@ void AC_WPNav::update_wpnav()
_wp_last_update = AP_HAL::millis();
}
return ret;
}
// check_wp_leash_length - check if waypoint leash lengths need to be recalculated
@ -1023,6 +1084,66 @@ void AC_WPNav::calc_spline_pos_vel(float spline_time, Vector3f& position, Vector
_hermite_spline_solution[3] * 3.0f * spline_time_sqrd;
}
// get height of terrain (in cm) above the ekf origin (+ve means current terrain higher than EKF origin's altitude)
bool AC_WPNav::get_terrain_offset(const Vector3f &pos, float& offset_cm)
{
// check we have terrain alt for ekf origin at least
if (!_ekf_origin_terrain_alt_set) {
Location_Class ekforigin = _inav.get_origin();
int32_t talt_cm;
if (ekforigin.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, talt_cm)) {
_ekf_origin_terrain_alt_set = true;
} else {
return false;
}
}
// calculate current position's terrain altitude above EKF origin's altitude
Location_Class curr_loc(pos);
curr_loc.set_alt(0,Location_Class::ALT_FRAME_ABOVE_ORIGIN);
int32_t pos_terr_alt_cm;
if (curr_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, pos_terr_alt_cm)) {
offset_cm = -pos_terr_alt_cm;
return true;
}
return false;
}
// convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain
// returns false if conversion failed (likely because terrain data was not available)
bool AC_WPNav::get_vector_NEU(const Location_Class &loc, Vector3f &vec, bool &terrain_alt)
{
// convert location to NEU vector3f
Vector3f res_vec;
if (!loc.get_vector_xy_from_origin_NEU(res_vec)) {
return false;
}
// convert altitude
if (loc.get_alt_frame() == Location_Class::ALT_FRAME_ABOVE_TERRAIN) {
int32_t terr_alt;
if (!loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, terr_alt)) {
return false;
}
vec.z = terr_alt;
terrain_alt = true;
} else {
terrain_alt = false;
int32_t temp_alt;
if (!loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_ORIGIN, temp_alt)) {
return false;
}
vec.z = temp_alt;
terrain_alt = false;
}
// copy xy (we do this to ensure we do not adjust vector unless the overall conversion is successful
vec.x = res_vec.x;
vec.y = res_vec.y;
return true;
}
///
/// shared methods

27
libraries/AC_WPNav/AC_WPNav.h
View File

@ -4,6 +4,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <AP_Common/Location.h>
#include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
#include <AC_AttitudeControl/AC_PosControl.h> // Position control library
#include <AC_AttitudeControl/AC_AttitudeControl.h> // Attitude control library
@ -133,11 +134,18 @@ public:
/// get_wp_destination waypoint using position vector (distance from home in cm)
const Vector3f &get_wp_destination() const { return _destination; }
/// set_wp_destination waypoint using location class
/// returns false if conversion from location to vector from ekf origin cannot be calculated
bool set_wp_destination(const Location_Class& destination);
/// set_wp_destination waypoint using position vector (distance from home in cm)
void set_wp_destination(const Vector3f& destination);
/// terrain_alt should be true if destination.z is a desired altitude above terrain
bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false);
/// set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
void set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination);
/// 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 set_wp_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt = false);
/// shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position
/// used to reset the position just before takeoff
@ -161,7 +169,7 @@ public:
void set_fast_waypoint(bool fast) { _flags.fast_waypoint = fast; }
/// update_wpnav - run the wp controller - should be called at 100hz or higher
void update_wpnav();
bool update_wpnav();
// check_wp_leash_length - check recalc_wp_leash flag and calls calculate_wp_leash_length() if necessary
// should be called after _pos_control.update_xy_controller which may have changed the position controller leash lengths
@ -221,7 +229,7 @@ public:
void set_desired_alt(float desired_alt) { _pos_control.set_alt_target(desired_alt); }
/// advance_wp_target_along_track - move target location along track from origin to destination
void advance_wp_target_along_track(float dt);
bool advance_wp_target_along_track(float dt);
static const struct AP_Param::GroupInfo var_info[];
@ -272,6 +280,13 @@ protected:
/// relies on update_spline_solution being called since the previous
void calc_spline_pos_vel(float spline_time, Vector3f& position, Vector3f& velocity);
// get terrain altitude difference (in cm) between at current position and ekf origin (+ve means current terrain higher than at origin)
bool get_terrain_offset(const Vector3f &pos, float& offset_cm);
// convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain
// returns false if conversion failed (likely because terrain data was not available)
bool get_vector_NEU(const Location_Class &loc, Vector3f &vec, bool &terrain_alt);
// references to inertial nav and ahrs libraries
const AP_InertialNav& _inav;
const AP_AHRS& _ahrs;
@ -319,4 +334,8 @@ protected:
Vector3f _hermite_spline_solution[4]; // array describing spline path between origin and destination
float _spline_vel_scaler; //
float _yaw; // heading according to yaw
// terrain following variables
bool _terrain_alt = false; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin
bool _ekf_origin_terrain_alt_set = false;
};