From cc799d3d7e616347bb1d2fd776aac64cbd315a98 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 28 Sep 2023 15:05:51 +1000 Subject: [PATCH] Copter: encapsulate auto takeoff into an ojbect similar to the encapsulation of "user takeoff" into an object --- ArduCopter/Copter.h | 2 ++ ArduCopter/mode.h | 39 ++++++++++++--------- ArduCopter/mode_auto.cpp | 16 ++++----- ArduCopter/mode_guided.cpp | 6 ++-- ArduCopter/takeoff.cpp | 69 ++++++++++++++++++++------------------ 5 files changed, 73 insertions(+), 59 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index bb3942a8f4..5af9342850 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -235,6 +235,8 @@ public: friend class ModeAutorotate; friend class ModeTurtle; + friend class _AutoTakeoff; + Copter(void); private: diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 28c7841179..11cbf3afed 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -8,6 +8,27 @@ class ParametersG2; class GCS_Copter; +// object shared by both Guided and Auto for takeoff. +// position controller controls vehicle but the user can control the yaw. +class _AutoTakeoff { +public: + void run(); + void start(float complete_alt_cm, bool terrain_alt); + bool get_position(Vector3p& completion_pos); + + bool complete; // true when takeoff is complete + +private: + // altitude above-ekf-origin below which auto takeoff does not control horizontal position + bool no_nav_active; + float no_nav_alt_cm; + + // auto takeoff variables + float complete_alt_cm; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt) + bool terrain_alt; // true if altitudes are above terrain + Vector3p complete_pos; // target takeoff position as offset from ekf origin in cm +}; + class Mode { public: @@ -51,6 +72,8 @@ public: // do not allow copying CLASS_NO_COPY(Mode); + friend class _AutoTakeoff; + // returns a unique number specific to this mode virtual Number mode_number() const = 0; @@ -215,21 +238,7 @@ protected: virtual bool do_user_takeoff_start(float takeoff_alt_cm); - // method shared by both Guided and Auto for takeoff. - // position controller controls vehicle but the user can control the yaw. - void auto_takeoff_run(); - void auto_takeoff_start(float complete_alt_cm, bool terrain_alt); - bool auto_takeoff_get_position(Vector3p& completion_pos); - - // altitude above-ekf-origin below which auto takeoff does not control horizontal position - static bool auto_takeoff_no_nav_active; - static float auto_takeoff_no_nav_alt_cm; - - // auto takeoff variables - static float auto_takeoff_complete_alt_cm; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt) - static bool auto_takeoff_terrain_alt; // true if altitudes are above terrain - static bool auto_takeoff_complete; // true when takeoff is complete - static Vector3p auto_takeoff_complete_pos; // target takeoff position as offset from ekf origin in cm + static _AutoTakeoff auto_takeoff; public: // Navigation Yaw control diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index c1972dbf00..f60a512e81 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -350,7 +350,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc) pos_control->init_z_controller(); // initialise alt for WP_NAVALT_MIN and set completion alt - auto_takeoff_start(alt_target_cm, alt_target_terrain); + auto_takeoff.start(alt_target_cm, alt_target_terrain); // set submode set_submode(SubMode::TAKEOFF); @@ -364,7 +364,7 @@ bool ModeAuto::wp_start(const Location& dest_loc) Vector3f stopping_point; if (_mode == SubMode::TAKEOFF) { Vector3p takeoff_complete_pos; - if (auto_takeoff_get_position(takeoff_complete_pos)) { + if (auto_takeoff.get_position(takeoff_complete_pos)) { stopping_point = takeoff_complete_pos.tofloat(); } } @@ -536,7 +536,7 @@ bool ModeAuto::is_landing() const bool ModeAuto::is_taking_off() const { - return ((_mode == SubMode::TAKEOFF) && !auto_takeoff_complete); + return ((_mode == SubMode::TAKEOFF) && !auto_takeoff.complete); } // auto_payload_place_start - initialises controller to implement a placing @@ -965,7 +965,7 @@ void ModeAuto::takeoff_run() if ((copter.g2.auto_options & (int32_t)Options::AllowTakeOffWithoutRaisingThrottle) != 0) { copter.set_auto_armed(true); } - auto_takeoff_run(); + auto_takeoff.run(); } // auto_wp_run - runs the auto waypoint controller @@ -1326,13 +1326,13 @@ void ModeAuto::payload_place_run() FALLTHROUGH; case PayloadPlaceStateType_Ascent_Start: { - auto_takeoff_start(nav_payload_place.descent_start_altitude_cm, false); + auto_takeoff.start(nav_payload_place.descent_start_altitude_cm, false); nav_payload_place.state = PayloadPlaceStateType_Ascent; } break; case PayloadPlaceStateType_Ascent: - if (auto_takeoff_complete) { + if (auto_takeoff.complete) { nav_payload_place.state = PayloadPlaceStateType_Done; } break; @@ -1974,13 +1974,13 @@ bool ModeAuto::verify_takeoff() { #if AP_LANDINGGEAR_ENABLED // if we have reached our destination - if (auto_takeoff_complete) { + if (auto_takeoff.complete) { // retract the landing gear copter.landinggear.retract_after_takeoff(); } #endif - return auto_takeoff_complete; + return auto_takeoff.complete; } // verify_land - returns true if landing has been completed diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 4d0f9e3154..9c2914de8b 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -153,7 +153,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm) pos_control->init_z_controller(); // initialise alt for WP_NAVALT_MIN and set completion alt - auto_takeoff_start(alt_target_cm, alt_target_terrain); + auto_takeoff.start(alt_target_cm, alt_target_terrain); // record takeoff has not completed takeoff_complete = false; @@ -656,8 +656,8 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_ // called by guided_run at 100hz or more void ModeGuided::takeoff_run() { - auto_takeoff_run(); - if (auto_takeoff_complete && !takeoff_complete) { + auto_takeoff.run(); + if (auto_takeoff.complete && !takeoff_complete) { takeoff_complete = true; #if AP_LANDINGGEAR_ENABLED // optionally retract landing gear diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 790936ac6b..14fb7d39da 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -1,13 +1,7 @@ #include "Copter.h" Mode::_TakeOff Mode::takeoff; - -bool Mode::auto_takeoff_no_nav_active = false; -float Mode::auto_takeoff_no_nav_alt_cm = 0; -float Mode::auto_takeoff_complete_alt_cm = 0; -bool Mode::auto_takeoff_terrain_alt = false; -bool Mode::auto_takeoff_complete = false; -Vector3p Mode::auto_takeoff_complete_pos; +_AutoTakeoff Mode::auto_takeoff; // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes. // The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude @@ -118,20 +112,27 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) // auto_takeoff_run - controls the vertical position controller during the process of taking off in auto modes // auto_takeoff_complete set to true when target altitude is within 10% of the take off altitude and less than 50% max climb rate -void Mode::auto_takeoff_run() +void _AutoTakeoff::run() { + const auto &g2 = copter.g2; + const auto &inertial_nav = copter.inertial_nav; + const auto &wp_nav = copter.wp_nav; + auto *motors = copter.motors; + auto *pos_control = copter.pos_control; + auto *attitude_control = copter.attitude_control; + // if not armed set throttle to zero and exit immediately if (!motors->armed() || !copter.ap.auto_armed) { // do not spool down tradheli when on the ground with motor interlock enabled - make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); + copter.flightmode->make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); // update auto_takeoff_no_nav_alt_cm - auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; + no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; return; } // get terrain offset float terr_offset = 0.0f; - if (auto_takeoff_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) { + if (terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) { // trigger terrain failsafe copter.failsafe_terrain_on_event(); return; @@ -151,7 +152,7 @@ void Mode::auto_takeoff_run() attitude_control->reset_rate_controller_I_terms(); attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0); // update auto_takeoff_no_nav_alt_cm - auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; + no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; return; } @@ -169,7 +170,7 @@ void Mode::auto_takeoff_run() if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || (copter.pos_control->get_z_accel_cmss() >= 0.5 * copter.pos_control->get_max_accel_z_cmss()) || (copter.pos_control->get_vel_desired_cms().z >= 0.1 * copter.pos_control->get_max_speed_up_cms()) || - ( auto_takeoff_no_nav_active && (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm))) { + ( no_nav_active && (inertial_nav.get_position_z_up_cm() >= no_nav_alt_cm))) { // throttle > 90% // acceleration > 50% maximum acceleration // velocity > 10% maximum velocity @@ -180,10 +181,10 @@ void Mode::auto_takeoff_run() } // check if we are not navigating because of low altitude - if (auto_takeoff_no_nav_active) { + if (no_nav_active) { // check if vehicle has reached no_nav_alt threshold - if (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm) { - auto_takeoff_no_nav_active = false; + if (inertial_nav.get_position_z_up_cm() >= no_nav_alt_cm) { + no_nav_active = false; } pos_control->relax_velocity_controller_xy(); } else { @@ -194,7 +195,7 @@ void Mode::auto_takeoff_run() pos_control->update_xy_controller(); // command the aircraft to the take off altitude - float pos_z = auto_takeoff_complete_alt_cm + terr_offset; + float pos_z = complete_alt_cm + terr_offset; float vel_z = 0.0; copter.pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0.0); @@ -202,7 +203,7 @@ void Mode::auto_takeoff_run() pos_control->update_z_controller(); // call attitude controller with auto yaw - attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), copter.flightmode->auto_yaw.get_heading()); // takeoff complete when we are less than 1% of the stopping distance from the target altitude // and 10% our maximum climb rate @@ -211,40 +212,42 @@ void Mode::auto_takeoff_run() const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss(); bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= pos_z - stop_distance; bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * vel_threshold_fraction; - auto_takeoff_complete = reached_altitude && reached_climb_rate; + complete = reached_altitude && reached_climb_rate; // calculate completion for location in case it is needed for a smooth transition to wp_nav - if (auto_takeoff_complete) { - const Vector3p& complete_pos = copter.pos_control->get_pos_target_cm(); - auto_takeoff_complete_pos = Vector3p{complete_pos.x, complete_pos.y, pos_z}; + if (complete) { + const Vector3p& _complete_pos = copter.pos_control->get_pos_target_cm(); + complete_pos = Vector3p{_complete_pos.x, _complete_pos.y, pos_z}; } } -void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt) +void _AutoTakeoff::start(float _complete_alt_cm, bool _terrain_alt) { // auto_takeoff_complete_alt_cm is a problem if equal to auto_takeoff_start_alt_cm - auto_takeoff_complete_alt_cm = complete_alt_cm; - auto_takeoff_terrain_alt = terrain_alt; - auto_takeoff_complete = false; + complete_alt_cm = _complete_alt_cm; + terrain_alt = _terrain_alt; + complete = false; // initialise auto_takeoff_no_nav_alt_cm - auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; - if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) { + const auto &g2 = copter.g2; + const auto &inertial_nav = copter.inertial_nav; + no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; + if ((g2.wp_navalt_min > 0) && (copter.flightmode->is_disarmed_or_landed() || !copter.motors->get_interlock())) { // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min - auto_takeoff_no_nav_active = true; + no_nav_active = true; } else { - auto_takeoff_no_nav_active = false; + no_nav_active = false; } } // return takeoff final position if takeoff has completed successfully -bool Mode::auto_takeoff_get_position(Vector3p& complete_pos) +bool _AutoTakeoff::get_position(Vector3p& _complete_pos) { // only provide location if takeoff has completed - if (!auto_takeoff_complete) { + if (!complete) { return false; } - complete_pos = auto_takeoff_complete_pos; + complete_pos = _complete_pos; return true; }