Copter: integrate landing gear option and make edge based

also guided mode now retracts landing gear after takeoff
previously landing gear deployment was "level based" meaning the pilot could not override the gear's position
This commit is contained in:
Randy Mackay 2020-02-21 12:08:26 +09:00
parent d0ff724b46
commit 24dbba59f2
6 changed files with 17 additions and 37 deletions

View File

@ -12,12 +12,6 @@ void Copter::landinggear_update()
// last status (deployed or retracted) used to check for changes, initialised to startup state of landing gear
static bool last_deploy_status = landinggear.deployed();
// if we are doing an automatic landing procedure, force the landing gear to deploy.
// To-Do: should we pause the auto-land procedure to give time for gear to come down?
if (flightmode->landing_gear_should_be_deployed()) {
landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
}
// send event message to datalog if status has changed
if (landinggear.deployed() != last_deploy_status) {
if (landinggear.deployed()) {

View File

@ -66,7 +66,6 @@ public:
virtual bool is_taking_off() const;
static void takeoff_stop() { takeoff.stop(); }
virtual bool landing_gear_should_be_deployed() const { return false; }
virtual bool is_landing() const { return false; }
// functions for reporting to GCS
@ -354,7 +353,6 @@ public:
void nav_guided_start();
bool is_landing() const override;
bool landing_gear_should_be_deployed() const override;
bool is_taking_off() const override;
@ -856,7 +854,6 @@ public:
bool is_autopilot() const override { return true; }
bool is_landing() const override { return true; };
bool landing_gear_should_be_deployed() const override { return true; };
void do_not_use_GPS();
@ -1018,7 +1015,6 @@ public:
bool state_complete() { return _state_complete; }
bool is_landing() const override;
bool landing_gear_should_be_deployed() const override;
void restart_without_terrain();

View File

@ -249,6 +249,9 @@ void ModeAuto::land_start(const Vector3f& destination)
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
// optionally deploy landing gear
copter.landinggear.deploy_for_landing();
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
@ -373,19 +376,6 @@ bool ModeAuto::is_taking_off() const
return ((_mode == Auto_TakeOff) && !wp_nav->reached_wp_destination());
}
bool ModeAuto::landing_gear_should_be_deployed() const
{
switch(_mode) {
case Auto_Land:
return true;
case Auto_RTL:
return copter.mode_rtl.landing_gear_should_be_deployed();
default:
return false;
}
return false;
}
// auto_payload_place_start - initialises controller to implement a placing
void ModeAuto::payload_place_start()
{
@ -1508,7 +1498,7 @@ bool ModeAuto::verify_takeoff()
// retract the landing gear
if (reached_wp_dest) {
copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract);
copter.landinggear.retract_after_takeoff();
}
return reached_wp_dest;

View File

@ -370,6 +370,10 @@ void ModeGuided::takeoff_run()
{
auto_takeoff_run();
if (wp_nav->reached_wp_destination()) {
// optionally retract landing gear
copter.landinggear.retract_after_takeoff();
// switch to position control mode but maintain current target
const Vector3f& target = wp_nav->get_wp_destination();
set_destination(target);
}

View File

@ -37,6 +37,9 @@ bool ModeLand::init(bool ignore_checks)
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
// optionally deploy landing gear
copter.landinggear.deploy_for_landing();
return true;
}

View File

@ -266,6 +266,9 @@ void ModeRTL::descent_start()
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
// optionally deploy landing gear
copter.landinggear.deploy_for_landing();
}
// rtl_descent_run - implements the final descent to the RTL_ALT
@ -349,6 +352,9 @@ void ModeRTL::land_start()
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
// optionally deploy landing gear
copter.landinggear.deploy_for_landing();
}
bool ModeRTL::is_landing() const
@ -356,19 +362,6 @@ bool ModeRTL::is_landing() const
return _state == RTL_Land;
}
bool ModeRTL::landing_gear_should_be_deployed() const
{
switch(_state) {
case RTL_LoiterAtHome:
case RTL_Land:
case RTL_FinalDescent:
return true;
default:
return false;
}
return false;
}
// rtl_returnhome_run - return home
// called by rtl_run at 100hz or more
void ModeRTL::land_run(bool disarm_on_land)