Feature/precland

ArduPlane/ArduPlane.cpp

@@ -75,6 +75,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
 #if CAMERA == ENABLED
     SCHED_TASK_CLASS(AP_Camera, &plane.camera, update,      50, 100),
 #endif // CAMERA == ENABLED
+#if PRECISION_LANDING == ENABLED
+    SCHED_TASK(update_precland,      400,     50),
+#endif
     SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging,         0.2,    100),
     SCHED_TASK(compass_save,          0.1,    200),
     SCHED_TASK(Log_Write_Fast,         25,    300),

ArduPlane/GCS_Mavlink.cpp

@@ -1302,6 +1302,12 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg)
         break;
     }
 
+#if PRECISION_LANDING == ENABLED
+    case MAVLINK_MSG_ID_LANDING_TARGET:
+        plane.precland.handle_msg(msg);
+        break;
+#endif
+
     case MAVLINK_MSG_ID_TERRAIN_DATA:
     case MAVLINK_MSG_ID_TERRAIN_CHECK:
 #if AP_TERRAIN_AVAILABLE

ArduPlane/Parameters.cpp

@@ -1035,6 +1035,12 @@ const AP_Param::Info Plane::var_info[] = {
     GOBJECT(afs,  "AFS_", AP_AdvancedFailsafe),
 #endif
 
+#if PRECISION_LANDING == ENABLED
+    // @Group: PLND_
+    // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp
+    GOBJECT(precland, "PLND_", AC_PrecLand),
+#endif
+
 #if OPTFLOW == ENABLED
     // @Group: FLOW
     // @Path: ../libraries/AP_OpticalFlow/OpticalFlow.cpp

ArduPlane/Parameters.h

@@ -347,6 +347,9 @@ class Parameters {
         // 254,255: reserved
 
         k_param_vehicle = 257, // vehicle common block of parameters
+
+        // 258: precision landing object
+        k_param_precland = 258,
     };
 
     AP_Int16 format_version;

ArduPlane/Plane.h

@@ -94,6 +94,11 @@
 // Configuration
 #include "config.h"
 
+#if PRECISION_LANDING == ENABLED
+ # include <AC_PrecLand/AC_PrecLand.h>
+ # include <AP_IRLock/AP_IRLock.h>
+#endif
+
 #if ADVANCED_FAILSAFE == ENABLED
 #include "afs_plane.h"
 #endif
@@ -635,6 +640,11 @@ class Plane : public AP_Vehicle {
     AP_Terrain terrain{mission};
 #endif
 
+    // Precision Landing
+#if PRECISION_LANDING == ENABLED
+    AC_PrecLand precland;
+#endif
+
     AP_Landing landing{mission,ahrs,SpdHgt_Controller,nav_controller,aparm,
             FUNCTOR_BIND_MEMBER(&Plane::set_target_altitude_proportion, void, const Location&, float),
             FUNCTOR_BIND_MEMBER(&Plane::constrain_target_altitude_location, void, const Location&, const Location&),
@@ -864,6 +874,10 @@ class Plane : public AP_Vehicle {
     void load_parameters(void) override;
     void convert_mixers(void);
 
+    // precision_landing.cpp
+    void init_precland();
+    void update_precland();
+
     // commands_logic.cpp
     void set_next_WP(const struct Location &loc);
     void do_RTL(int32_t alt);

ArduPlane/config.h

@@ -287,6 +287,12 @@
 #define PARACHUTE HAL_PARACHUTE_ENABLED
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// Precision landing using irlock
+#ifndef PRECISION_LANDING
+#define PRECISION_LANDING ENABLED
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // Payload Gripper
 #ifndef GRIPPER_ENABLED

ArduPlane/precision_landing.cpp

@@ -0,0 +1,26 @@
+//
+// functions to support precision landing
+//
+
+#include "Plane.h"
+
+#if PRECISION_LANDING == ENABLED
+
+void Plane::init_precland()
+{
+    plane.precland.init(400);
+}
+
+void Plane::update_precland()
+{
+    int32_t height_above_ground_cm = current_loc.alt;
+
+    // use range finder altitude if it is valid, otherwise use home alt
+    // if (rangefinder_alt_ok()) {
+    //     height_above_ground_cm = rangefinder_state.alt_cm;
+    // }
+
+    // precland.update(height_above_ground_cm, rangefinder_alt_ok());
+    precland.update(height_above_ground_cm, false);
+}
+#endif

ArduPlane/wscript

@@ -30,6 +30,7 @@ def build(bld):
             'AP_OSD',
             'AC_AutoTune',
             'AP_KDECAN',
+            'AC_PrecLand',
         ],
     )