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

AC_WPNav: simplify use of terrain to just current location

Browse Source
This commit is contained in:
Randy Mackay 2016-04-23 16:29:53 +09:00
parent c5a3781507
commit 1c4b2be16a
2 changed files with 25 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -460,7 +460,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
// convert origin to alt-above-terrain
if (terrain_alt) {
float origin_terr_offset;
if (!get_terrain_offset(origin, origin_terr_offset)) {
if (!get_terrain_offset(origin_terr_offset)) {
return false;
}
origin.z -= origin_terr_offset;
@ -507,7 +507,7 @@ bool AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
// get origin's alt-above-terrain
float origin_terr_offset = 0.0f;
if (terrain_alt) {
if (!get_terrain_offset(origin, origin_terr_offset)) {
if (!get_terrain_offset(origin_terr_offset)) {
return false;
}
}
@ -576,13 +576,13 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
Vector3f curr_pos = _inav.get_position();
// calculate terrain adjustments
float curr_terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(curr_pos, curr_terr_offset)) {
float terr_offset = 0.0f;
if (_terrain_alt && !get_terrain_offset(terr_offset)) {
return false;
}
// calculate 3d vector from segment's origin
Vector3f curr_delta = (curr_pos - Vector3f(0,0,curr_terr_offset)) - _origin;
Vector3f curr_delta = (curr_pos - Vector3f(0,0,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;
@ -683,7 +683,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
// recalculate the desired position
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;
final_target.z += terr_offset;
_pos_control.set_pos_target(final_target);
// check if we've reached the waypoint
@ -694,7 +694,7 @@ bool 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 - Vector3f(0,0,curr_terr_offset)) - _destination;
Vector3f dist_to_dest = (curr_pos - Vector3f(0,0,terr_offset)) - _destination;
if( dist_to_dest.length() <= _wp_radius_cm ) {
_flags.reached_destination = true;
}
@ -871,11 +871,11 @@ bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_
// convert origin to alt-above-terrain
if (terrain_alt) {
float origin_terr_offset;
if (!get_terrain_offset(origin, origin_terr_offset)) {
float terr_offset;
if (!get_terrain_offset(terr_offset)) {
return false;
}
origin.z -= origin_terr_offset;
origin.z -= terr_offset;
}
// set origin and destination
@ -978,16 +978,16 @@ bool AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
// 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;
// get alt-above-terrain
float terr_offset = 0.0f;
if (terrain_alt) {
if (!get_terrain_offset(origin, origin_terr_offset)) {
if (!get_terrain_offset(terr_offset)) {
return false;
}
}
// initialise intermediate point to the origin
_pos_control.set_pos_target(origin + Vector3f(0,0,origin_terr_offset));
_pos_control.set_pos_target(origin + Vector3f(0,0,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
@ -1061,14 +1061,14 @@ bool AC_WPNav::advance_spline_target_along_track(float dt)
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)) {
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 -= curr_terr_offset;
track_error.z -= terr_offset;
// calculate the horizontal error
float track_error_xy = pythagorous2(track_error.x, track_error.y);
@ -1116,7 +1116,7 @@ bool AC_WPNav::advance_spline_target_along_track(float dt)
}
// update target position
target_pos.z += curr_terr_offset;
target_pos.z += terr_offset;
_pos_control.set_pos_target(target_pos);
// update the yaw
@ -1151,26 +1151,12 @@ 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)
// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
bool AC_WPNav::get_terrain_offset(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;
float terr_alt = 0.0f;
if (_terrain != NULL && _terrain->height_above_terrain(terr_alt, true)) {
offset_cm = _inav.get_altitude() - (terr_alt * 100.0f);
return true;
}

4
libraries/AC_WPNav/AC_WPNav.h
View File

@ -297,8 +297,8 @@ 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);
// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
bool get_terrain_offset(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)