mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
Copter: refactor/rename precland methods
This commit is contained in:
parent
ee176d26e9
commit
8102629dfb
@ -699,28 +699,29 @@ void Mode::land_run_horizontal_control()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Do normal landing or precision landing if enabled. pause_descent is true if vehicle should not descend
|
// run normal or precision landing (if enabled)
|
||||||
void Mode::execute_landing(bool pause_descent)
|
// pause_descent is true if vehicle should not descend
|
||||||
|
void Mode::land_run_normal_or_precland(bool pause_descent)
|
||||||
{
|
{
|
||||||
#if PRECISION_LANDING == ENABLED
|
#if PRECISION_LANDING == ENABLED
|
||||||
if (pause_descent || !copter.precland.enabled()) {
|
if (pause_descent || !copter.precland.enabled()) {
|
||||||
// we don't want to start descending immediately or prec land is disabled
|
// we don't want to start descending immediately or prec land is disabled
|
||||||
// in both cases just run simple land controllers
|
// in both cases just run simple land controllers
|
||||||
run_land_controllers(pause_descent);
|
land_run_horiz_and_vert_control(pause_descent);
|
||||||
} else {
|
} else {
|
||||||
// prec land is enabled and we have not paused descent
|
// prec land is enabled and we have not paused descent
|
||||||
// the state machine takes care of the entire prec landing procedure
|
// the state machine takes care of the entire prec landing procedure
|
||||||
run_precland();
|
precland_run();
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
run_land_controllers(pause_descent);
|
land_run_horiz_and_vert_control(pause_descent);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
#if PRECISION_LANDING == ENABLED
|
#if PRECISION_LANDING == ENABLED
|
||||||
// Go towards a position commanded by prec land state machine in order to retry landing
|
// Go towards a position commanded by prec land state machine in order to retry landing
|
||||||
// The passed in location is expected to be NED and in m
|
// The passed in location is expected to be NED and in m
|
||||||
void Mode::land_retry_position(const Vector3f &retry_loc)
|
void Mode::precland_retry_position(const Vector3f &retry_pos)
|
||||||
{
|
{
|
||||||
if (!copter.failsafe.radio) {
|
if (!copter.failsafe.radio) {
|
||||||
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
|
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
|
||||||
@ -750,10 +751,10 @@ void Mode::land_retry_position(const Vector3f &retry_loc)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector3p retry_loc_NEU{retry_loc.x, retry_loc.y, retry_loc.z * -1.0f};
|
Vector3p retry_pos_NEU{retry_pos.x, retry_pos.y, retry_pos.z * -1.0f};
|
||||||
//pos contoller expects input in NEU cm's
|
//pos contoller expects input in NEU cm's
|
||||||
retry_loc_NEU = retry_loc_NEU * 100.0f;
|
retry_pos_NEU = retry_pos_NEU * 100.0f;
|
||||||
pos_control->input_pos_xyz(retry_loc_NEU, 0.0f, 1000.0f);
|
pos_control->input_pos_xyz(retry_pos_NEU, 0.0f, 1000.0f);
|
||||||
|
|
||||||
// run position controllers
|
// run position controllers
|
||||||
pos_control->update_xy_controller();
|
pos_control->update_xy_controller();
|
||||||
@ -767,7 +768,7 @@ void Mode::land_retry_position(const Vector3f &retry_loc)
|
|||||||
|
|
||||||
// Run precland statemachine. This function should be called from any mode that wants to do precision landing.
|
// Run precland statemachine. This function should be called from any mode that wants to do precision landing.
|
||||||
// This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
|
// This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
|
||||||
void Mode::run_precland()
|
void Mode::precland_run()
|
||||||
{
|
{
|
||||||
// if user is taking control, we will not run the statemachine, and simply land (may or may not be on target)
|
// if user is taking control, we will not run the statemachine, and simply land (may or may not be on target)
|
||||||
if (!copter.ap.land_repo_active) {
|
if (!copter.ap.land_repo_active) {
|
||||||
@ -777,7 +778,7 @@ void Mode::run_precland()
|
|||||||
switch (copter.precland_statemachine.update(retry_pos)) {
|
switch (copter.precland_statemachine.update(retry_pos)) {
|
||||||
case AC_PrecLand_StateMachine::Status::RETRYING:
|
case AC_PrecLand_StateMachine::Status::RETRYING:
|
||||||
// we want to retry landing by going to another position
|
// we want to retry landing by going to another position
|
||||||
land_retry_position(retry_pos);
|
precland_retry_position(retry_pos);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case AC_PrecLand_StateMachine::Status::FAILSAFE: {
|
case AC_PrecLand_StateMachine::Status::FAILSAFE: {
|
||||||
@ -785,11 +786,11 @@ void Mode::run_precland()
|
|||||||
switch (copter.precland_statemachine.get_failsafe_actions()) {
|
switch (copter.precland_statemachine.get_failsafe_actions()) {
|
||||||
case AC_PrecLand_StateMachine::FailSafeAction::DESCEND:
|
case AC_PrecLand_StateMachine::FailSafeAction::DESCEND:
|
||||||
// descend normally, prec land target is definitely not in sight
|
// descend normally, prec land target is definitely not in sight
|
||||||
run_land_controllers();
|
land_run_horiz_and_vert_control();
|
||||||
break;
|
break;
|
||||||
case AC_PrecLand_StateMachine::FailSafeAction::HOLD_POS:
|
case AC_PrecLand_StateMachine::FailSafeAction::HOLD_POS:
|
||||||
// sending "true" in this argument will stop the descend
|
// sending "true" in this argument will stop the descend
|
||||||
run_land_controllers(true);
|
land_run_horiz_and_vert_control(true);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
@ -801,12 +802,12 @@ void Mode::run_precland()
|
|||||||
case AC_PrecLand_StateMachine::Status::DESCEND:
|
case AC_PrecLand_StateMachine::Status::DESCEND:
|
||||||
// run land controller. This will descend towards the target if prec land target is in sight
|
// run land controller. This will descend towards the target if prec land target is in sight
|
||||||
// else it will just descend vertically
|
// else it will just descend vertically
|
||||||
run_land_controllers();
|
land_run_horiz_and_vert_control();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// just land, since user has taken over controls, it does not make sense to run any retries or failsafe measures
|
// just land, since user has taken over controls, it does not make sense to run any retries or failsafe measures
|
||||||
run_land_controllers();
|
land_run_horiz_and_vert_control();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
@ -111,16 +111,27 @@ protected:
|
|||||||
void zero_throttle_and_hold_attitude();
|
void zero_throttle_and_hold_attitude();
|
||||||
void make_safe_ground_handling(bool force_throttle_unlimited = false);
|
void make_safe_ground_handling(bool force_throttle_unlimited = false);
|
||||||
|
|
||||||
// functions to control landing
|
// functions to control normal landing. pause_descent is true if vehicle should not descend
|
||||||
// in modes that support landing
|
|
||||||
void land_run_horizontal_control();
|
void land_run_horizontal_control();
|
||||||
void land_run_vertical_control(bool pause_descent = false);
|
void land_run_vertical_control(bool pause_descent = false);
|
||||||
void run_land_controllers(bool pause_descent = false) {
|
void land_run_horiz_and_vert_control(bool pause_descent = false) {
|
||||||
land_run_horizontal_control();
|
land_run_horizontal_control();
|
||||||
land_run_vertical_control(pause_descent);
|
land_run_vertical_control(pause_descent);
|
||||||
}
|
}
|
||||||
// Do normal landing or precision landing if enabled. pause_descent is true if vehicle should not descend
|
|
||||||
void execute_landing(bool pause_descent = false);
|
// run normal or precision landing (if enabled)
|
||||||
|
// pause_descent is true if vehicle should not descend
|
||||||
|
void land_run_normal_or_precland(bool pause_descent = false);
|
||||||
|
|
||||||
|
#if PRECISION_LANDING == ENABLED
|
||||||
|
// Go towards a position commanded by prec land state machine in order to retry landing
|
||||||
|
// The passed in location is expected to be NED and in meters
|
||||||
|
void precland_retry_position(const Vector3f &retry_pos);
|
||||||
|
|
||||||
|
// Run precland statemachine. This function should be called from any mode that wants to do precision landing.
|
||||||
|
// This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
|
||||||
|
void precland_run();
|
||||||
|
#endif
|
||||||
|
|
||||||
// return expected input throttle setting to hover:
|
// return expected input throttle setting to hover:
|
||||||
virtual float throttle_hover() const;
|
virtual float throttle_hover() const;
|
||||||
@ -246,17 +257,6 @@ public:
|
|||||||
};
|
};
|
||||||
static AutoYaw auto_yaw;
|
static AutoYaw auto_yaw;
|
||||||
|
|
||||||
#if PRECISION_LANDING == ENABLED
|
|
||||||
// Go towards a position commanded by prec land state machine in order to retry landing
|
|
||||||
// The passed in location is expected to be NED and in meters
|
|
||||||
void land_retry_position(const Vector3f &retry_loc);
|
|
||||||
|
|
||||||
// Run precland statemachine. This function should be called from any mode that wants to do precision landing.
|
|
||||||
// This handles everything from prec landing, to prec landing failures, to retries and failsafe measures
|
|
||||||
void run_precland();
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// pass-through functions to reduce code churn on conversion;
|
// pass-through functions to reduce code churn on conversion;
|
||||||
// these are candidates for moving into the Mode base
|
// these are candidates for moving into the Mode base
|
||||||
// class.
|
// class.
|
||||||
|
@ -890,7 +890,7 @@ void ModeAuto::land_run()
|
|||||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
||||||
|
|
||||||
// run normal landing or precision landing (if enabled)
|
// run normal landing or precision landing (if enabled)
|
||||||
execute_landing();
|
land_run_normal_or_precland();
|
||||||
}
|
}
|
||||||
|
|
||||||
// auto_rtl_run - rtl in AUTO flight mode
|
// auto_rtl_run - rtl in AUTO flight mode
|
||||||
|
@ -85,7 +85,7 @@ void ModeLand::gps_run()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// run normal landing or precision landing (if enabled)
|
// run normal landing or precision landing (if enabled)
|
||||||
execute_landing(land_pause);
|
land_run_normal_or_precland(land_pause);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -414,7 +414,7 @@ void ModeRTL::land_run(bool disarm_on_land)
|
|||||||
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
||||||
|
|
||||||
// run normal landing or precision landing (if enabled)
|
// run normal landing or precision landing (if enabled)
|
||||||
execute_landing();
|
land_run_normal_or_precland();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ModeRTL::build_path()
|
void ModeRTL::build_path()
|
||||||
|
Loading…
Reference in New Issue
Block a user