Copter: refactor/rename precland methods

This commit is contained in:
Randy Mackay 2021-08-25 14:53:56 +09:00
parent ee176d26e9
commit 8102629dfb
5 changed files with 35 additions and 34 deletions

View File

@ -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
void Mode::execute_landing(bool pause_descent)
// run normal or precision landing (if enabled)
// pause_descent is true if vehicle should not descend
void Mode::land_run_normal_or_precland(bool pause_descent)
{
#if PRECISION_LANDING == ENABLED
if (pause_descent || !copter.precland.enabled()) {
// we don't want to start descending immediately or prec land is disabled
// in both cases just run simple land controllers
run_land_controllers(pause_descent);
land_run_horiz_and_vert_control(pause_descent);
} else {
// prec land is enabled and we have not paused descent
// the state machine takes care of the entire prec landing procedure
run_precland();
precland_run();
}
#else
run_land_controllers(pause_descent);
land_run_horiz_and_vert_control(pause_descent);
#endif
}
#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 m
void Mode::land_retry_position(const Vector3f &retry_loc)
void Mode::precland_retry_position(const Vector3f &retry_pos)
{
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){
@ -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
retry_loc_NEU = retry_loc_NEU * 100.0f;
pos_control->input_pos_xyz(retry_loc_NEU, 0.0f, 1000.0f);
retry_pos_NEU = retry_pos_NEU * 100.0f;
pos_control->input_pos_xyz(retry_pos_NEU, 0.0f, 1000.0f);
// run position controllers
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.
// 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 (!copter.ap.land_repo_active) {
@ -777,7 +778,7 @@ void Mode::run_precland()
switch (copter.precland_statemachine.update(retry_pos)) {
case AC_PrecLand_StateMachine::Status::RETRYING:
// we want to retry landing by going to another position
land_retry_position(retry_pos);
precland_retry_position(retry_pos);
break;
case AC_PrecLand_StateMachine::Status::FAILSAFE: {
@ -785,11 +786,11 @@ void Mode::run_precland()
switch (copter.precland_statemachine.get_failsafe_actions()) {
case AC_PrecLand_StateMachine::FailSafeAction::DESCEND:
// descend normally, prec land target is definitely not in sight
run_land_controllers();
land_run_horiz_and_vert_control();
break;
case AC_PrecLand_StateMachine::FailSafeAction::HOLD_POS:
// sending "true" in this argument will stop the descend
run_land_controllers(true);
land_run_horiz_and_vert_control(true);
break;
}
break;
@ -801,12 +802,12 @@ void Mode::run_precland()
case AC_PrecLand_StateMachine::Status::DESCEND:
// run land controller. This will descend towards the target if prec land target is in sight
// else it will just descend vertically
run_land_controllers();
land_run_horiz_and_vert_control();
break;
}
} else {
// 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

View File

@ -111,16 +111,27 @@ protected:
void zero_throttle_and_hold_attitude();
void make_safe_ground_handling(bool force_throttle_unlimited = false);
// functions to control landing
// in modes that support landing
// functions to control normal landing. pause_descent is true if vehicle should not descend
void land_run_horizontal_control();
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_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:
virtual float throttle_hover() const;
@ -246,17 +257,6 @@ public:
};
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;
// these are candidates for moving into the Mode base
// class.

View File

@ -890,7 +890,7 @@ void ModeAuto::land_run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run normal landing or precision landing (if enabled)
execute_landing();
land_run_normal_or_precland();
}
// auto_rtl_run - rtl in AUTO flight mode

View File

@ -85,7 +85,7 @@ void ModeLand::gps_run()
}
// run normal landing or precision landing (if enabled)
execute_landing(land_pause);
land_run_normal_or_precland(land_pause);
}
}

View File

@ -414,7 +414,7 @@ void ModeRTL::land_run(bool disarm_on_land)
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run normal landing or precision landing (if enabled)
execute_landing();
land_run_normal_or_precland();
}
void ModeRTL::build_path()