From e5dded2444b17966453f4ad0668a995d73f15c93 Mon Sep 17 00:00:00 2001 From: matthewgan Date: Thu, 5 Aug 2021 14:32:09 +0200 Subject: [PATCH 1/4] add files to enable precland --- ArduPlane/Parameters.cpp | 6 ++++++ ArduPlane/Plane.h | 10 ++++++++++ ArduPlane/wscript | 1 + 3 files changed, 17 insertions(+) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 25e83a3db82d28..42613b1c9106fe 100644 --- a/ArduPlane/Parameters.cpp +++ b/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 diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index e07b27b916ae0c..fb0dec70dcc2cd 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -86,6 +86,11 @@ #include #include // Landing Gear library +#if PRECISION_LANDING == ENABLED + # include + # include +#endif + #include "GCS_Mavlink.h" #include "GCS_Plane.h" #include "quadplane.h" @@ -351,6 +356,11 @@ class Plane : public AP_Vehicle { return failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb; } + // Precision Landing + #if PRECISION_LANDING == ENABLED + AC_PrecLand precland; + #endif + // A counter used to count down valid gps fixes to allow the gps estimate to settle // before recording our home position (and executing a ground start if we booted with an air start) uint8_t ground_start_count = 5; diff --git a/ArduPlane/wscript b/ArduPlane/wscript index dde7456ca2535f..b2966b8545c475 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -30,6 +30,7 @@ def build(bld): 'AP_OSD', 'AC_AutoTune', 'AP_KDECAN', + 'AC_PrecLand', ], ) From d5c6cc3dbc5aee16aa57a1a4c02e853c731b75b4 Mon Sep 17 00:00:00 2001 From: matthewgan Date: Mon, 4 Oct 2021 10:40:58 +0200 Subject: [PATCH 2/4] add precland to config for compiling --- ArduPlane/config.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduPlane/config.h b/ArduPlane/config.h index f64eeebbe18206..bcaeec166f01f7 100644 --- a/ArduPlane/config.h +++ b/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 From 3765e158f0785741da984dc08a44c787074009a2 Mon Sep 17 00:00:00 2001 From: matthewgan Date: Mon, 4 Oct 2021 11:55:44 +0200 Subject: [PATCH 3/4] add precland support for plane --- ArduPlane/GCS_Mavlink.cpp | 6 ++++++ ArduPlane/Parameters.h | 3 +++ ArduPlane/Plane.h | 24 ++++++++++++++---------- ArduPlane/precision_landing.cpp | 26 ++++++++++++++++++++++++++ 4 files changed, 49 insertions(+), 10 deletions(-) create mode 100644 ArduPlane/precision_landing.cpp diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index af052e5c0f91de..2e46ae05bdc3f5 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/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 diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index c40307d2baf5e6..c2ff5628d322b6 100644 --- a/ArduPlane/Parameters.h +++ b/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; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index fb0dec70dcc2cd..4fa4ed3debb54e 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -86,11 +86,6 @@ #include #include // Landing Gear library -#if PRECISION_LANDING == ENABLED - # include - # include -#endif - #include "GCS_Mavlink.h" #include "GCS_Plane.h" #include "quadplane.h" @@ -99,6 +94,11 @@ // Configuration #include "config.h" +#if PRECISION_LANDING == ENABLED + # include + # include +#endif + #if ADVANCED_FAILSAFE == ENABLED #include "afs_plane.h" #endif @@ -356,11 +356,6 @@ class Plane : public AP_Vehicle { return failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb; } - // Precision Landing - #if PRECISION_LANDING == ENABLED - AC_PrecLand precland; - #endif - // A counter used to count down valid gps fixes to allow the gps estimate to settle // before recording our home position (and executing a ground start if we booted with an air start) uint8_t ground_start_count = 5; @@ -645,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&), @@ -874,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); diff --git a/ArduPlane/precision_landing.cpp b/ArduPlane/precision_landing.cpp new file mode 100644 index 00000000000000..86bd8e45d16b32 --- /dev/null +++ b/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 From 8e0128dc7c2a170d35bb81f1eabe6be17d6e864a Mon Sep 17 00:00:00 2001 From: matthewgan Date: Mon, 4 Oct 2021 12:22:28 +0200 Subject: [PATCH 4/4] add update precland alt in sched task --- ArduPlane/ArduPlane.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 3cac6592a12bac..7d52155c73aeb5 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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),