Copter: add option to disable LANDING_GEAR

This commit is contained in:
Tatsuya Yamaguchi 2021-02-07 12:29:33 +09:00 committed by Randy Mackay
parent 52cd042d69
commit ed8384b45a
13 changed files with 40 additions and 0 deletions

View File

@ -46,6 +46,7 @@
//#define MODE_ZIGZAG_ENABLED DISABLED // disable zigzag mode support //#define MODE_ZIGZAG_ENABLED DISABLED // disable zigzag mode support
//#define OSD_ENABLED DISABLED // disable on-screen-display support //#define OSD_ENABLED DISABLED // disable on-screen-display support
//#define BUTTON_ENABLED DISABLED // disable button 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 // features below are disabled by default on all boards
//#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes //#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes

View File

@ -142,7 +142,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
SCHED_TASK(ekf_check, 10, 75), SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(check_vibration, 10, 50), SCHED_TASK(check_vibration, 10, 50),
SCHED_TASK(gpsglitch_check, 10, 50), SCHED_TASK(gpsglitch_check, 10, 50),
#if LANDING_GEAR_ENABLED == ENABLED
SCHED_TASK(landinggear_update, 10, 75), SCHED_TASK(landinggear_update, 10, 75),
#endif
SCHED_TASK(standby_update, 100, 75), SCHED_TASK(standby_update, 100, 75),
SCHED_TASK(lost_vehicle_check, 10, 50), SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180), SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180),

View File

@ -515,7 +515,9 @@ private:
#endif #endif
// Landing Gear Controller // Landing Gear Controller
#if LANDING_GEAR_ENABLED == ENABLED
AP_LandingGear landinggear; AP_LandingGear landinggear;
#endif
// terrain handling // terrain handling
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN && MODE_AUTO_ENABLED == ENABLED #if AP_TERRAIN_AVAILABLE && AC_TERRAIN && MODE_AUTO_ENABLED == ENABLED
@ -770,8 +772,10 @@ private:
void set_land_complete_maybe(bool b); void set_land_complete_maybe(bool b);
void update_throttle_mix(); void update_throttle_mix();
#if LANDING_GEAR_ENABLED == ENABLED
// landing_gear.cpp // landing_gear.cpp
void landinggear_update(); void landinggear_update();
#endif
// standby.cpp // standby.cpp
void standby_update(); void standby_update();

View File

@ -865,6 +865,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
return MAV_RESULT_FAILED; return MAV_RESULT_FAILED;
#endif #endif
#if LANDING_GEAR_ENABLED == ENABLED
case MAV_CMD_AIRFRAME_CONFIGURATION: { case MAV_CMD_AIRFRAME_CONFIGURATION: {
// Param 1: Select which gear, not used in ArduPilot // Param 1: Select which gear, not used in ArduPilot
// Param 2: 0 = Deploy, 1 = Retract // 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; return MAV_RESULT_FAILED;
} }
#endif
/* Solo user presses Fly button */ /* Solo user presses Fly button */
case MAV_CMD_SOLO_BTN_FLY_CLICK: { case MAV_CMD_SOLO_BTN_FLY_CLICK: {

View File

@ -500,9 +500,11 @@ const AP_Param::Info Copter::var_info[] = {
GOBJECT(parachute, "CHUTE_", AP_Parachute), GOBJECT(parachute, "CHUTE_", AP_Parachute),
#endif #endif
#if LANDING_GEAR_ENABLED == ENABLED
// @Group: LGR_ // @Group: LGR_
// @Path: ../libraries/AP_LandingGear/AP_LandingGear.cpp // @Path: ../libraries/AP_LandingGear/AP_LandingGear.cpp
GOBJECT(landinggear, "LGR_", AP_LandingGear), GOBJECT(landinggear, "LGR_", AP_LandingGear),
#endif
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// @Group: IM_ // @Group: IM_
@ -1161,8 +1163,10 @@ void Copter::load_parameters(void)
AP_Param::load_all(); AP_Param::load_all();
AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table)); AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table));
#if LANDING_GEAR_ENABLED == ENABLED
// convert landing gear parameters // convert landing gear parameters
convert_lgr_parameters(); convert_lgr_parameters();
#endif
// convert fs_options parameters // convert fs_options parameters
convert_fs_options_params(); convert_fs_options_params();
@ -1334,6 +1338,7 @@ void Copter::convert_pid_parameters(void)
SRV_Channels::upgrade_parameters(); SRV_Channels::upgrade_parameters();
} }
#if LANDING_GEAR_ENABLED == ENABLED
/* /*
convert landing gear parameters convert landing gear parameters
*/ */
@ -1415,6 +1420,7 @@ void Copter::convert_lgr_parameters(void)
servo_reversed->set_and_save_ifchanged(1); servo_reversed->set_and_save_ifchanged(1);
} }
} }
#endif
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// handle conversion of tradheli parameters from Copter-3.6 to Copter-3.7 // handle conversion of tradheli parameters from Copter-3.6 to Copter-3.7

View File

@ -377,6 +377,12 @@
# define BUTTON_ENABLED ENABLED # define BUTTON_ENABLED ENABLED
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// Landing Gear support
#ifndef LANDING_GEAR_ENABLED
#define LANDING_GEAR_ENABLED !HAL_MINIMIZE_FEATURES
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION // RADIO CONFIGURATION
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////

View File

@ -329,8 +329,10 @@ void Copter::parachute_release()
// release parachute // release parachute
parachute.release(); parachute.release();
#if LANDING_GEAR_ENABLED == ENABLED
// deploy landing gear // deploy landing gear
landinggear.set_position(AP_LandingGear::LandingGear_Deploy); landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
#endif
} }
// parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error // parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error

View File

@ -1,5 +1,6 @@
#include "Copter.h" #include "Copter.h"
#if LANDING_GEAR_ENABLED == ENABLED
// Run landing gear controller at 10Hz // Run landing gear controller at 10Hz
void Copter::landinggear_update() void Copter::landinggear_update()
@ -33,3 +34,5 @@ void Copter::landinggear_update()
landinggear.update(height_cm * 0.01f); // convert cm->m for update call landinggear.update(height_cm * 0.01f); // convert cm->m for update call
} }
#endif // LANDING_GEAR_ENABLED

View File

@ -248,8 +248,10 @@ void ModeAuto::land_start(const Vector3f& destination)
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AUTO_YAW_HOLD);
#if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear // optionally deploy landing gear
copter.landinggear.deploy_for_landing(); copter.landinggear.deploy_for_landing();
#endif
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
// disable the fence on landing // disable the fence on landing
@ -1521,11 +1523,13 @@ bool ModeAuto::verify_takeoff()
// have we reached our target altitude? // have we reached our target altitude?
const bool reached_wp_dest = copter.wp_nav->reached_wp_destination(); const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
#if LANDING_GEAR_ENABLED == ENABLED
// if we have reached our destination // if we have reached our destination
if (reached_wp_dest) { if (reached_wp_dest) {
// retract the landing gear // retract the landing gear
copter.landinggear.retract_after_takeoff(); copter.landinggear.retract_after_takeoff();
} }
#endif
return reached_wp_dest; return reached_wp_dest;
} }

View File

@ -406,8 +406,10 @@ void ModeGuided::takeoff_run()
{ {
auto_takeoff_run(); auto_takeoff_run();
if (wp_nav->reached_wp_destination()) { if (wp_nav->reached_wp_destination()) {
#if LANDING_GEAR_ENABLED == ENABLED
// optionally retract landing gear // optionally retract landing gear
copter.landinggear.retract_after_takeoff(); copter.landinggear.retract_after_takeoff();
#endif
// switch to position control mode but maintain current target // switch to position control mode but maintain current target
const Vector3f target = wp_nav->get_wp_destination(); const Vector3f target = wp_nav->get_wp_destination();

View File

@ -37,8 +37,10 @@ bool ModeLand::init(bool ignore_checks)
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AUTO_YAW_HOLD);
#if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear // optionally deploy landing gear
copter.landinggear.deploy_for_landing(); copter.landinggear.deploy_for_landing();
#endif
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
// disable the fence on landing // disable the fence on landing

View File

@ -275,8 +275,10 @@ void ModeRTL::descent_start()
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AUTO_YAW_HOLD);
#if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear // optionally deploy landing gear
copter.landinggear.deploy_for_landing(); copter.landinggear.deploy_for_landing();
#endif
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
// disable the fence on landing // disable the fence on landing
@ -368,8 +370,10 @@ void ModeRTL::land_start()
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AUTO_YAW_HOLD);
#if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear // optionally deploy landing gear
copter.landinggear.deploy_for_landing(); copter.landinggear.deploy_for_landing();
#endif
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
// disable the fence on landing // disable the fence on landing

View File

@ -138,8 +138,10 @@ void Copter::init_ardupilot()
init_precland(); init_precland();
#endif #endif
#if LANDING_GEAR_ENABLED == ENABLED
// initialise landing gear position // initialise landing gear position
landinggear.init(); landinggear.init();
#endif
#ifdef USERHOOK_INIT #ifdef USERHOOK_INIT
USERHOOK_INIT USERHOOK_INIT