diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index f3c5536847..ccf8498b27 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -36,6 +36,7 @@ // features below are disabled by default on all boards //#define SPRAYER ENABLED // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity) +//#define PRECISION_LANDING ENABLED // enable precision landing using companion computer or IRLock sensor // other settings //#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 55ddf0304d..2637ac7c94 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -110,6 +110,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] PROGMEM = { { SCHED_TASK(three_hz_loop), 133, 75 }, { SCHED_TASK(compass_accumulate), 8, 100 }, { SCHED_TASK(barometer_accumulate), 8, 90 }, +#if PRECISION_LANDING == ENABLED + { SCHED_TASK(update_precland), 8, 50 }, +#endif #if FRAME_CONFIG == HELI_FRAME { SCHED_TASK(check_dynamic_flight), 8, 75 }, #endif diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index c1b4db777f..0ef6474da0 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -119,6 +119,9 @@ Copter::Copter(void) : #endif #if AP_TERRAIN_AVAILABLE terrain(ahrs, mission, rally), +#endif +#if PRECISION_LANDING == ENABLED + precland(ahrs, inertial_nav, g.pi_precland, MAIN_LOOP_SECONDS), #endif in_mavlink_delay(false), gcs_out_of_time(false), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 01d118c6b6..dca5d61062 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -108,6 +108,10 @@ #include // Landing Gear library #include #include +#if PRECISION_LANDING == ENABLED +#include +#include +#endif // AP_HAL to Arduino compatibility layer // Configuration @@ -494,6 +498,11 @@ private: AP_Terrain terrain; #endif + // Precision Landing +#if PRECISION_LANDING == ENABLED + AC_PrecLand precland; +#endif + // use this to prevent recursion during sensor init bool in_mavlink_delay; @@ -867,6 +876,8 @@ private: void init_compass(); void init_optflow(); void update_optical_flow(void); + void init_precland(); + void update_precland(); void read_battery(void); void read_receiver_rssi(void); void epm_update(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index f513aeb146..1b07118904 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1058,6 +1058,12 @@ const AP_Param::Info Copter::var_info[] PROGMEM = { GOBJECT(optflow, "FLOW", OpticalFlow), #endif +#if PRECISION_LANDING == ENABLED + // @Group: PRECLAND_ + // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp + GOBJECT(precland, "PLAND_", AC_PrecLand), +#endif + // @Group: RPM // @Path: ../libraries/AP_RPM/AP_RPM.cpp GOBJECT(rpm_sensor, "RPM", AP_RPM), diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 20bd38c87c..b71edc6ff8 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -82,6 +82,9 @@ public: // Landing gear object k_param_landinggear, // 18 + // precision landing object + k_param_precland, // 19 + // Misc // k_param_log_bitmask_old = 20, // Deprecated diff --git a/ArduCopter/config.h b/ArduCopter/config.h index b87878fe9b..607e4766af 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -330,6 +330,12 @@ # define SPRAYER DISABLED #endif +////////////////////////////////////////////////////////////////////////////// +// Precision Landing with companion computer or IRLock sensor +#ifndef PRECISION_LANDING + # define PRECISION_LANDING DISABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // EPM cargo gripper #ifndef EPM_ENABLED diff --git a/ArduCopter/precision_landing.cpp b/ArduCopter/precision_landing.cpp new file mode 100644 index 0000000000..c793e2e76a --- /dev/null +++ b/ArduCopter/precision_landing.cpp @@ -0,0 +1,24 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +// +// functions to support precision landing +// + +#include "Copter.h" + +#if PRECISION_LANDING == ENABLED + +void Copter::init_precland() +{ + copter.precland.init(); +} + +void Copter::update_precland() +{ + copter.precland.update(current_loc.alt); + + // log output + Log_Write_Precland(); +} + +#endif diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 3b8480fa95..b8acf826f2 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -203,6 +203,11 @@ void Copter::init_ardupilot() camera_mount.init(serial_manager); #endif +#if PRECISION_LANDING == ENABLED + // initialise precision landing + init_precland(); +#endif + #ifdef USERHOOK_INIT USERHOOK_INIT #endif