diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 7d232e3af1..8ebef864e5 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -46,6 +46,7 @@ //#define MODE_ZIGZAG_ENABLED DISABLED // disable zigzag mode support //#define OSD_ENABLED DISABLED // disable on-screen-display support //#define BUTTON_ENABLED DISABLED // disable button support +//#define LANDING_GEAR_ENABLED DISABLED // disable landing gear support // features below are disabled by default on all boards //#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 0093ba7263..5c09af1f37 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -142,7 +142,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(ekf_check, 10, 75), SCHED_TASK(check_vibration, 10, 50), SCHED_TASK(gpsglitch_check, 10, 50), +#if LANDING_GEAR_ENABLED == ENABLED SCHED_TASK(landinggear_update, 10, 75), +#endif SCHED_TASK(standby_update, 100, 75), SCHED_TASK(lost_vehicle_check, 10, 50), SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7985cd87b9..0aac425824 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -515,7 +515,9 @@ private: #endif // Landing Gear Controller +#if LANDING_GEAR_ENABLED == ENABLED AP_LandingGear landinggear; +#endif // terrain handling #if AP_TERRAIN_AVAILABLE && AC_TERRAIN && MODE_AUTO_ENABLED == ENABLED @@ -770,8 +772,10 @@ private: void set_land_complete_maybe(bool b); void update_throttle_mix(); +#if LANDING_GEAR_ENABLED == ENABLED // landing_gear.cpp void landinggear_update(); +#endif // standby.cpp void standby_update(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 2668608615..180a5298fd 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -865,6 +865,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ return MAV_RESULT_FAILED; #endif +#if LANDING_GEAR_ENABLED == ENABLED case MAV_CMD_AIRFRAME_CONFIGURATION: { // Param 1: Select which gear, not used in ArduPilot // Param 2: 0 = Deploy, 1 = Retract @@ -879,6 +880,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ } return MAV_RESULT_FAILED; } +#endif /* Solo user presses Fly button */ case MAV_CMD_SOLO_BTN_FLY_CLICK: { diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index fe9607f525..abaae2f8be 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -500,9 +500,11 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(parachute, "CHUTE_", AP_Parachute), #endif +#if LANDING_GEAR_ENABLED == ENABLED // @Group: LGR_ // @Path: ../libraries/AP_LandingGear/AP_LandingGear.cpp GOBJECT(landinggear, "LGR_", AP_LandingGear), +#endif #if FRAME_CONFIG == HELI_FRAME // @Group: IM_ @@ -1161,8 +1163,10 @@ void Copter::load_parameters(void) AP_Param::load_all(); AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); +#if LANDING_GEAR_ENABLED == ENABLED // convert landing gear parameters convert_lgr_parameters(); +#endif // convert fs_options parameters convert_fs_options_params(); @@ -1334,6 +1338,7 @@ void Copter::convert_pid_parameters(void) SRV_Channels::upgrade_parameters(); } +#if LANDING_GEAR_ENABLED == ENABLED /* convert landing gear parameters */ @@ -1415,6 +1420,7 @@ void Copter::convert_lgr_parameters(void) servo_reversed->set_and_save_ifchanged(1); } } +#endif #if FRAME_CONFIG == HELI_FRAME // handle conversion of tradheli parameters from Copter-3.6 to Copter-3.7 diff --git a/ArduCopter/config.h b/ArduCopter/config.h index d677d1bba9..8c1789be6f 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -377,6 +377,12 @@ # define BUTTON_ENABLED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// Landing Gear support +#ifndef LANDING_GEAR_ENABLED + #define LANDING_GEAR_ENABLED !HAL_MINIMIZE_FEATURES +#endif + ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index b68890c3d7..c1fe3e7a2c 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -329,8 +329,10 @@ void Copter::parachute_release() // release parachute parachute.release(); +#if LANDING_GEAR_ENABLED == ENABLED // deploy landing gear landinggear.set_position(AP_LandingGear::LandingGear_Deploy); +#endif } // parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error diff --git a/ArduCopter/landing_gear.cpp b/ArduCopter/landing_gear.cpp index b61127513d..c93657df55 100644 --- a/ArduCopter/landing_gear.cpp +++ b/ArduCopter/landing_gear.cpp @@ -1,5 +1,6 @@ #include "Copter.h" +#if LANDING_GEAR_ENABLED == ENABLED // Run landing gear controller at 10Hz void Copter::landinggear_update() @@ -33,3 +34,5 @@ void Copter::landinggear_update() landinggear.update(height_cm * 0.01f); // convert cm->m for update call } + +#endif // LANDING_GEAR_ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 0352c2a41b..9d5be6d650 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -248,8 +248,10 @@ void ModeAuto::land_start(const Vector3f& destination) // initialise yaw auto_yaw.set_mode(AUTO_YAW_HOLD); +#if LANDING_GEAR_ENABLED == ENABLED // optionally deploy landing gear copter.landinggear.deploy_for_landing(); +#endif #if AC_FENCE == ENABLED // disable the fence on landing @@ -1521,11 +1523,13 @@ bool ModeAuto::verify_takeoff() // have we reached our target altitude? const bool reached_wp_dest = copter.wp_nav->reached_wp_destination(); +#if LANDING_GEAR_ENABLED == ENABLED // if we have reached our destination if (reached_wp_dest) { // retract the landing gear copter.landinggear.retract_after_takeoff(); } +#endif return reached_wp_dest; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 0f79ee3d2d..aca72cf9fd 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -406,8 +406,10 @@ void ModeGuided::takeoff_run() { auto_takeoff_run(); if (wp_nav->reached_wp_destination()) { +#if LANDING_GEAR_ENABLED == ENABLED // optionally retract landing gear copter.landinggear.retract_after_takeoff(); +#endif // switch to position control mode but maintain current target const Vector3f target = wp_nav->get_wp_destination(); diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 6768a179f1..7e6bcd8edc 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -37,8 +37,10 @@ bool ModeLand::init(bool ignore_checks) // initialise yaw auto_yaw.set_mode(AUTO_YAW_HOLD); +#if LANDING_GEAR_ENABLED == ENABLED // optionally deploy landing gear copter.landinggear.deploy_for_landing(); +#endif #if AC_FENCE == ENABLED // disable the fence on landing diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index ed597a059e..b929c2e577 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -275,8 +275,10 @@ void ModeRTL::descent_start() // initialise yaw auto_yaw.set_mode(AUTO_YAW_HOLD); +#if LANDING_GEAR_ENABLED == ENABLED // optionally deploy landing gear copter.landinggear.deploy_for_landing(); +#endif #if AC_FENCE == ENABLED // disable the fence on landing @@ -368,8 +370,10 @@ void ModeRTL::land_start() // initialise yaw auto_yaw.set_mode(AUTO_YAW_HOLD); +#if LANDING_GEAR_ENABLED == ENABLED // optionally deploy landing gear copter.landinggear.deploy_for_landing(); +#endif #if AC_FENCE == ENABLED // disable the fence on landing diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index a6d4fc4377..c4876a738f 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -138,8 +138,10 @@ void Copter::init_ardupilot() init_precland(); #endif +#if LANDING_GEAR_ENABLED == ENABLED // initialise landing gear position landinggear.init(); +#endif #ifdef USERHOOK_INIT USERHOOK_INIT