From 8102629dfb2fcc8857f42cc8f73e3933b6d74404 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 25 Aug 2021 14:53:56 +0900 Subject: [PATCH] Copter: refactor/rename precland methods --- ArduCopter/mode.cpp | 31 ++++++++++++++++--------------- ArduCopter/mode.h | 32 ++++++++++++++++---------------- ArduCopter/mode_auto.cpp | 2 +- ArduCopter/mode_land.cpp | 2 +- ArduCopter/mode_rtl.cpp | 2 +- 5 files changed, 35 insertions(+), 34 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 001766cc51..6758beed2f 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -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 diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 077c126514..98a3c57d25 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -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. diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 3664b786f2..4d0a158b35 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -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 diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 59504e7f36..3d59afb071 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -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); } } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 58d4d022fb..01fe719db7 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -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()