AP_Follow: Scripted follow in Plane

ArduPilot · Oct 7, 2024 · acf2fee · acf2fee
1 parent d004abb
commit acf2fee
Showing 1 changed file with 28 additions and 5 deletions.
diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp
@@ -40,7 +40,7 @@ extern const AP_HAL::HAL& hal;
 #define AP_FOLLOW_POS_P_DEFAULT 0.1f    // position error gain default
 
 #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
-#define AP_FOLLOW_ALT_TYPE_DEFAULT       0
+#define AP_FOLLOW_ALT_TYPE_DEFAULT       0 // absolute/AMSL altitude
 #define AP_FOLLOW_ALTITUDE_TYPE_ORIGIN   2 // origin relative altitude
 #define AP_FOLLOW_ALTITUDE_TYPE_TERRAIN  3 // terrain relative altitude
 #else
@@ -347,12 +347,35 @@ bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
         _target_location.lng = packet.lon;
 
         // select altitude source based on FOLL_ALT_TYPE param 
-        if (_alt_type == AP_FOLLOW_ALT_TYPE_DEFAULT) {
-            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+        switch(_alt_type) {
+            case AP_FOLLOW_ALT_TYPE_DEFAULT:
+                _target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
+                break;
+            case AP_FOLLOW_ALTITUDE_TYPE_RELATIVE:
+            case AP_FOLLOW_ALTITUDE_TYPE_ORIGIN:
+                _target_location.set_alt_cm(packet.relative_alt * 0.1, static_cast<Location::AltFrame>((int)_alt_type));
+                break;
+            case AP_FOLLOW_ALTITUDE_TYPE_TERRAIN: {
+                /// Altitude comes in AMSL
+                int32_t terrain_altitude_cm;
+                _target_location.set_alt_cm(packet.alt * 0.1, Location::AltFrame::ABSOLUTE);
+                // convert the incoming altitude to terrain altitude
+                if(_target_location.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, terrain_altitude_cm)) {
+                    _target_location.set_alt_cm(terrain_altitude_cm, Location::AltFrame::ABOVE_TERRAIN);
+                }
+                break;
+            }
+        }
+#else
+        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+            // above home alt
+            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
         } else {
-            _target_location.set_alt_cm(packet.relative_alt / 10, static_cast<Location::AltFrame>((int)_alt_type));
+            // absolute altitude
+            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
         }
-
+#endif
         _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