mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Copter: add option to disable LANDING_GEAR
This commit is contained in:
parent
52cd042d69
commit
ed8384b45a
@ -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
|
||||||
|
@ -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),
|
||||||
|
@ -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();
|
||||||
|
@ -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: {
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
//////////////////////////////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////////////////////////////
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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();
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
Loading…
Reference in New Issue
Block a user