Rover: mode class gets set_desired_location_NED

APMrover2/mode.cpp

@@ -152,6 +152,20 @@ void Mode::set_desired_location(const struct Location& destination, float next_l
     }
 }
 
+// set desired location as an offset from the EKF origin in NED frame
+bool Mode::set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd)
+{
+    Location destination_ned;
+    // initialise destination to ekf origin
+    if (!ahrs.get_origin(destination_ned)) {
+        return false;
+    }
+    // apply offset
+    location_offset(destination_ned, destination.x, destination.y);
+    set_desired_location(destination_ned, next_leg_bearing_cd);
+    return true;
+}
+
 // set desired heading and speed
 void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
 {

APMrover2/mode.h

@@ -76,6 +76,9 @@ public:
     //   next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn)
     virtual void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN);
 
+    // set desired location as offset from the EKF origin, return true on success
+    bool set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN);
+
     // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
     virtual bool reached_destination() { return true; }