AP_Follow: Add FOLL_ALT_SOURCE parameter, 0: absolute, 1: relative

AP_Follow: Fill _target_location.alt with packet.relative_alt when FOLL_ALT_SOURCE param is set to 1

AP_Follow: Change current_loc to have altitude above home if relative_alt flag is set

AP_Follow: Fill _target_location.alt with packet.relative_alt when FOLL_ALT_SOURCE param is set to 1

AP_Follow: Change current_loc to have altitude above home if relative_alt flag is set

AP_Follow: Fix things up before merging
- Rebase code on master
- Change all the commits to start with AP_Follow
- Change _ALT_SOURCE to _ALT_TYPE, bottom of param list, value to 10
- Minor formatting

AP_Follow: Tiny formatting issue

libraries/AP_Follow/AP_Follow.cpp

@@ -28,6 +28,8 @@ extern const AP_HAL::HAL& hal;
 #define AP_FOLLOW_OFFSET_TYPE_NED       0   // offsets are in north-east-down frame
 #define AP_FOLLOW_OFFSET_TYPE_RELATIVE  0   // offsets are relative to lead vehicle's heading
 
+#define AP_FOLLOW_ALTITUDE_TYPE_RELATIVE  1 // relative altitude is used by default   
+
 #define AP_FOLLOW_POS_P_DEFAULT 0.1f    // position error gain default
 
 // table of user settable parameters
@@ -106,6 +108,13 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     // @User: Standard
     AP_SUBGROUPINFO(_p_pos, "_POS_", 9, AP_Follow, AC_P),
 
+    // @Param: _ALT_TYPE
+    // @DisplayName: Follow altitude type
+    // @Description: Follow altitude type
+    // @Values: 0:absolute, 1: relative
+    // @User: Standard
+    AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALTITUDE_TYPE_RELATIVE),
+
     AP_GROUPEND
 };
 
@@ -159,7 +168,7 @@ bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_w
     Location current_loc;
     if (!AP::ahrs().get_position(current_loc)) {
          return false;
-     }
+    }
 
     // get target location and velocity
     Location target_loc;
@@ -168,6 +177,11 @@ bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_w
         return false;
     }
 
+    // change to altitude above home if relative altitude is being used
+    if (target_loc.flags.relative_alt == 1) {
+        current_loc.alt -= AP::ahrs().get_home().alt;
+    }
+
     // calculate difference
     const Vector3f dist_vec = location_3d_diff_NED(current_loc, target_loc);
 
@@ -255,7 +269,18 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
 
         _target_location.lat = packet.lat;
         _target_location.lng = packet.lon;
-        _target_location.alt = packet.alt / 10;     // convert millimeters to cm
+        
+        // select altitude source based on FOLL_ALT_TYPE param 
+        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+            // relative altitude
+            _target_location.alt = packet.relative_alt / 10;        // convert millimeters to cm
+            _target_location.flags.relative_alt = 1;                // set relative_alt flag
+        } else {
+            // absolute altitude
+            _target_location.alt = packet.alt / 10;                 // convert millimeters to cm
+            _target_location.flags.relative_alt = 0;                // reset relative_alt flag
+        }
+
         _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
         _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
         _target_velocity_ned.z = packet.vz * 0.01f; // velocity down

libraries/AP_Follow/AP_Follow.h

@@ -94,6 +94,7 @@ private:
     AP_Int8     _offset_type;       // offset frame type (0:North-East-Down, 1:RelativeToLeadVehicleHeading)
     AP_Vector3f _offset;            // offset from lead vehicle in meters
     AP_Int8     _yaw_behave;        // following vehicle's yaw/heading behaviour
+    AP_Int8     _alt_type;          // altitude source for follow mode
     AC_P        _p_pos;             // position error P controller
 
     // local variables