From 50dfe3f19702a2d7bb8b971410d2a61923dbead9 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Thu, 24 Sep 2020 16:42:34 -0700 Subject: [PATCH] Copter: Add AUTO_OPTIONS and support arming and throttle bypass This allows you to arm the copter without any extra GCS commands while in auto, and can be done from both the GCS, or the RC Tx. This is useful for creating a simpler workflow. This also allows you to set the auto_armed flag internally, which bypasses the need to raise the throttle stick for the copter to start a takeoff. This exposed a problem where we would start running the controllers before the EKF was at all initialized, if you switched into auto to early. This now has a check that prevents us from running the mission state machine until after the origin has been set. This was a suggestion from @rmackay9. When combined these options allow you to have the vehicle on the ground, disarmed in auto with a takeoff waypoint loaded, then just arm the aircraft and watch it takeoff. This is a feature we've had on quadplanes for quite awhile now, and it has proven to be very nice for operators. --- ArduCopter/Parameters.cpp | 9 +++++++++ ArduCopter/Parameters.h | 4 ++++ ArduCopter/mode.h | 9 ++++++++- ArduCopter/mode_auto.cpp | 37 ++++++++++++++++++++++++++++++++++--- 4 files changed, 55 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index f62b953106..a5ac9705ed 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -976,6 +976,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_GROUPINFO("ACRO_OPTIONS", 39, ParametersG2, acro_options, 0), #endif +#if MODE_AUTO_ENABLED == ENABLED + // @Param: AUTO_OPTIONS + // @DisplayName: Auto mode options + // @Description: A range of options that can be applied to change auto mode behaviour. Allow arming allows the copter to be armed while in auto. Allow taking off without throttle to takeoff allows the copter to takeoff without having to raise the throttle. + // @Bitmask: 0:Allow arming,1:Allow taking off without throttle + // @User: Advanced + AP_GROUPINFO("AUTO_OPTIONS", 40, ParametersG2, auto_options, 0), +#endif + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index b8f54a4fea..0f584e5a69 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -619,6 +619,10 @@ public: AP_Int8 acro_options; #endif +#if MODE_AUTO_ENABLED == ENABLED + AP_Int32 auto_options; +#endif + }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index aa906edefb..4f24b4e691 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -353,7 +353,7 @@ public: bool requires_GPS() const override { return true; } bool has_manual_throttle() const override { return false; } - bool allows_arming(bool from_gcs) const override { return false; }; + bool allows_arming(bool from_gcs) const override; bool is_autopilot() const override { return true; } bool in_guided_mode() const override { return mode() == Auto_NavGuided; } @@ -405,6 +405,11 @@ protected: private: + enum class Options : int32_t { + AllowArming = (1 << 0U), + BypassThrottle = (1 << 1U), + }; + bool start_command(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd); void exit_mission(); @@ -515,6 +520,8 @@ private: float descend_start_altitude; float descend_max; // centimetres } nav_payload_place; + + bool waiting_for_origin; }; #if AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index f53927a699..e4c68dd225 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -43,14 +43,31 @@ bool ModeAuto::init(bool ignore_checks) // clear guided limits copter.mode_guided.limit_clear(); - // start/resume the mission (based on MIS_RESTART parameter) - mission.start_or_resume(); + // don't start the mission until we have an origin + Location loc; + if (copter.ahrs.get_origin(loc)) { + // start/resume the mission (based on MIS_RESTART parameter) + mission.start_or_resume(); + waiting_for_origin = false; + } else { + waiting_for_origin = true; + } + + // if the user doesn't want to raise the throttle we can set it automatically + if ((copter.g2.auto_options & (int32_t)Options::BypassThrottle) != 0) { + copter.set_auto_armed(true); + } return true; } else { return false; } } +bool ModeAuto::allows_arming(bool from_gcs) const +{ + return (copter.g2.auto_options & (int32_t)Options::AllowArming) != 0; +}; + // auto_run - runs the auto controller // should be called at 100hz or more // relies on run_autopilot being called at 10hz which handles decision making and non-navigation related commands @@ -592,7 +609,16 @@ bool ModeAuto::get_wp(Location& destination) // update mission void ModeAuto::run_autopilot() { - mission.update(); + Location loc; + if (waiting_for_origin) { + if (copter.ahrs.get_origin(loc)) { + // start/resume the mission (based on MIS_RESTART parameter) + mission.start_or_resume(); + waiting_for_origin = false; + } + } else { + mission.update(); + } } /******************************************************************************* @@ -714,6 +740,11 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) // called by auto_run at 100hz or more void ModeAuto::takeoff_run() { + // if the user doesn't want to raise the throttle we can set it automatically + // note that this can defeat the disarm check on takeoff + if ((copter.g2.auto_options & (int32_t)Options::BypassThrottle) != 0) { + copter.set_auto_armed(true); + } auto_takeoff_run(); }