AC_WPNav: straight line waypoints accept terrain

libraries/AC_WPNav/AC_WPNav.cpp

@@ -426,8 +426,27 @@ void AC_WPNav::set_speed_xy(float speed_cms)
     }
 }
 
-/// set_destination - set destination using cm from home
+/// set_wp_destination waypoint using location class
-void AC_WPNav::set_wp_destination(const Vector3f& destination)
+///     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

libraries/AC_WPNav/AC_WPNav.h

@@ -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;
 };