AP_Follow: change to using position-from-origin internally

Author: peterbarker

Diff for: libraries/AP_Follow/AP_Follow.cpp

@@ -167,6 +167,26 @@ void AP_Follow::clear_offsets_if_required()
 
 // get target's estimated location
 bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
+{
+    Vector3p pos_ned;
+    Vector3f local_vel_ned;  // so we either change both return parameters or neither
+    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
+        return false;
+    }
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned * 0.01)) {
+        return false;
+    }
+    vel_ned = local_vel_ned;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
+
+    return true;
+}
+
+// get target's estimated location and velocity, both NED SI from origin
+bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
 {
     // exit immediately if not enabled
     if (!_enabled) {
@@ -187,16 +207,14 @@ bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ne
     }
 
     // project the vehicle position
-    Location last_loc = _target_location;
-    last_loc.offset(vel_ned.x * dt, vel_ned.y * dt);
-    last_loc.alt -= vel_ned.z * 100.0f * dt; // convert m/s to cm/s, multiply by dt.  minus because NED
+    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
+    pos_ned = _target_position_ned + vel_ned_p * dt;
 
-    // return latest position estimate
-    loc = last_loc;
     return true;
 }
 
 // get distance vector to target (in meters) and target's velocity all in NED frame
+// FIXME: use the target_position_ned vector!
 bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
 {
     // get our location
@@ -341,6 +359,7 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
             return false;
         }
 
+        Location _target_location;
         _target_location.lat = packet.lat;
         _target_location.lng = packet.lon;
 
@@ -352,6 +371,10 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
             // absolute altitude
             _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
         }
+        if (!_target_location.get_vector_from_origin_NEU(_target_position_ned)) {
+            return false;
+        }
+        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
 
         _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
         _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
@@ -386,18 +409,15 @@ bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
             return false;
         }
 
-        Location new_loc = _target_location;
+        Location new_loc;
         new_loc.lat = packet.lat;
         new_loc.lng = packet.lon;
         new_loc.set_alt_m(packet.alt, Location::AltFrame::ABSOLUTE);
 
-        // FOLLOW_TARGET is always AMSL, change the provided alt to
-        // above home if we are configured for relative alt
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE &&
-            !new_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
+        if (!new_loc.get_vector_from_origin_NEU(_target_position_ned)) {
             return false;
         }
-        _target_location = new_loc;
+        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
 
         if (packet.est_capabilities & (1<<1)) {
             _target_velocity_ned.x = packet.vel[0]; // velocity north
@@ -436,6 +456,12 @@ void AP_Follow::Log_Write_FOLL()
         Vector3f vel_estimate;
         UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
 
+        Location _target_location;
+        UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned * 0.01));
+        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+            _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+        }
+
         // log lead's estimated vs reported position
 // @LoggerMessage: FOLL
 // @Description: Follow library diagnostic data
@@ -537,17 +563,40 @@ void AP_Follow::clear_dist_and_bearing_to_target()
     _bearing_to_target = 0.0f;
 }
 
+// get target's estimated origin-relative-position and velocity (both
+// in SI NED), with offsets added
+bool AP_Follow::get_target_position_and_velocity_ofs(Vector3p &pos_ned, Vector3f &vel_ned) const
+{
+    Vector3f ofs_ned;
+    if (!get_offsets_ned(ofs_ned)) {
+        return false;
+    }
+    if (!get_target_position_and_velocity_ofs(pos_ned, vel_ned)) {
+        return false;
+    }
+    // can't add Vector3p and Vector3f directly:
+    pos_ned.x += ofs_ned.x;
+    pos_ned.y += ofs_ned.y;
+    pos_ned.z += ofs_ned.z;
+    return true;
+}
+
 // get target's estimated location and velocity (in NED), with offsets added
 bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const
 {
-    Vector3f ofs;
-    if (!get_offsets_ned(ofs) ||
-        !get_target_location_and_velocity(loc, vel_ned)) {
+    Vector3p pos_ned;
+    Vector3f local_vel_ned;  // so we either change both return parameters or neither
+    if (!get_target_position_and_velocity_ofs(pos_ned, local_vel_ned)) {
         return false;
     }
-    // apply offsets
-    loc.offset(ofs.x, ofs.y);
-    loc.alt -= ofs.z*100;
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned * 0.01)) {
+        return false;
+    }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
+
+    vel_ned = local_vel_ned;
     return true;
 }
 

Diff for: libraries/AP_Follow/AP_Follow.h

@@ -68,10 +68,20 @@ class AP_Follow
     // true if we have a valid target location estimate
     bool have_target() const;
 
-    // get target's estimated location and velocity (in NED)
+    // get target's estimated position (NED-from-origin) and velocity (in NED)
+    // from position-from-origin.  metres and metres/second
+    bool get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const;
+
+    // get target's estimated position (NED-from-origin) and velocity (in NED)
+    // from position-from-origin with offsets added.  metres and metres/second
+    bool get_target_position_and_velocity_ofs(Vector3p &pos_ned, Vector3f &vel_ned) const;
+
+    // get target's estimated location and velocity (in NED).  Derived
+    // from position-from-origin.
     bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const;
 
-    // get target's estimated location and velocity (in NED), with offsets added
+    // get target's estimated location and velocity (in NED), with
+    // offsets added.  Derived from position-from-origin.
     bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const;
 
     // get distance vector to target (in meters), target plus offsets, and target's velocity all in NED frame
@@ -156,7 +166,7 @@ class AP_Follow
 
     // local variables
     uint32_t _last_location_update_ms;  // system time of last position update
-    Location _target_location;      // last known location of target
+    Vector3p _target_position_ned;  // last known position of target
     Vector3f _target_velocity_ned;  // last known velocity of target in NED frame in m/s
     Vector3f _target_accel_ned;     // last known acceleration of target in NED frame in m/s/s
     uint32_t _last_heading_update_ms;   // system time of last heading update