From 78fd5fac0425a45ec3f06c62c4346277b4f26060 Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Wed, 8 Apr 2020 00:23:27 +0900 Subject: [PATCH] Copter: allow to resume in ZigZag Auto --- ArduCopter/RC_Channel.cpp | 3 ++ ArduCopter/mode.h | 9 +++++- ArduCopter/mode_zigzag.cpp | 57 ++++++++++++++++++++++++++++++++------ 3 files changed, 60 insertions(+), 9 deletions(-) diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index e67dcf9e75..6b7b6a6e4c 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -485,6 +485,8 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw case AUX_FUNC::ZIGZAG_SaveWP: #if MODE_ZIGZAG_ENABLED == ENABLED if (copter.flightmode == &copter.mode_zigzag) { + // initialize zigzag auto + copter.mode_zigzag.init_auto(); switch (ch_flag) { case LOW: copter.mode_zigzag.save_or_move_to_destination(ModeZigZag::Destination::A); @@ -570,6 +572,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw copter.mode_zigzag.run_auto(); break; default: + copter.mode_zigzag.suspend_auto(); break; } } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index aa7f021b09..6647e023a0 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1402,7 +1402,11 @@ public: bool init(bool ignore_checks) override; void exit(); void run() override; + + // auto control methods. copter flies grid pattern void run_auto(); + void suspend_auto(); + void init_auto(); bool requires_GPS() const override { return true; } bool has_manual_throttle() const override { return false; } @@ -1432,6 +1436,8 @@ private: Vector2f dest_A; // in NEU frame in cm relative to ekf origin Vector2f dest_B; // in NEU frame in cm relative to ekf origin + Vector3f current_dest; // current target destination (use for resume after suspending) + bool current_terr_alt; enum ZigZagState { STORING_POINTS, // storing points A and B, pilot has manual control @@ -1440,7 +1446,7 @@ private: } stage; enum AutoState { - MANUAL, // after initializing, before switching to ZigZag Auto + MANUAL, // not in ZigZag Auto AB_MOVING, // moving from A to B or from B to A SIDEWAYS, // moving to sideways } auto_stage; @@ -1450,6 +1456,7 @@ private: bool is_auto; // enable zigzag auto feature which is automate both AB and sideways uint16_t line_count = 0; // current line number int16_t line_num = 0; // target line number + bool is_suspended; // true if zigzag auto is suspended }; #if MODE_AUTOROTATE_ENABLED == ENABLED diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 25885ba432..797f5c6748 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -7,6 +7,7 @@ */ #define ZIGZAG_WP_RADIUS_CM 300 +#define ZIGZAG_LINE_INFINITY -1 // initialise zigzag controller bool ModeZigZag::init(bool ignore_checks) @@ -37,9 +38,9 @@ bool ModeZigZag::init(bool ignore_checks) stage = STORING_POINTS; dest_A.zero(); dest_B.zero(); - auto_stage = AutoState::MANUAL; - line_count = 0; + // initialize zigzag auto + init_auto(); return true; } @@ -72,7 +73,7 @@ void ModeZigZag::run() // if vehicle has reached destination switch to manual control or moving to A or B AP_Notify::events.waypoint_complete = 1; if (is_auto) { - if (line_num == -1 || line_count < line_num) { + if (line_num == ZIGZAG_LINE_INFINITY || line_count < line_num) { if (auto_stage == AutoState::SIDEWAYS) { save_or_move_to_destination((ab_dest_stored == Destination::A) ? Destination::B : Destination::A); } else { @@ -81,8 +82,7 @@ void ModeZigZag::run() move_to_side(); } } else { - is_auto = false; - line_count = 0; + init_auto(); return_to_manual_control(true); } } else { @@ -147,7 +147,7 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest) // spray on while moving to A or B spray(true); reach_wp_time_ms = 0; - if (is_auto == false || line_num == -1) { + if (is_auto == false || line_num == ZIGZAG_LINE_INFINITY) { gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", (ab_dest == Destination::A) ? "A" : "B"); } else { line_count++; @@ -169,6 +169,8 @@ void ModeZigZag::move_to_side() if (wp_nav->set_wp_destination(next_dest, terr_alt)) { stage = AUTO; auto_stage = AutoState::SIDEWAYS; + current_dest = next_dest; + current_terr_alt = terr_alt; reach_wp_time_ms = 0; char const *dir[] = {"forward", "right", "backward", "left"}; gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", dir[(uint8_t)zigzag_direction]); @@ -193,7 +195,6 @@ void ModeZigZag::return_to_manual_control(bool maintain_target) } else { loiter_nav->init_target(); } - auto_stage = AutoState::MANUAL; is_auto = false; gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control"); } @@ -469,8 +470,48 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con // run zigzag auto feature which is automate both AB and sideways void ModeZigZag::run_auto() { + // make sure both A and B point are registered and not when moving to A or B + if (stage != MANUAL_REGAIN) { + return; + } + is_auto = true; - move_to_side(); + // resume if zigzag auto is suspended + if (is_suspended && line_count <= line_num) { + // resume the stage when it was suspended + if (auto_stage == AutoState::AB_MOVING) { + line_count--; + save_or_move_to_destination(ab_dest_stored); + } else if (auto_stage == AutoState::SIDEWAYS) { + wp_nav->wp_and_spline_init(); + if (wp_nav->set_wp_destination(current_dest, current_terr_alt)) { + stage = AUTO; + reach_wp_time_ms = 0; + char const *dir[] = {"forward", "right", "backward", "left"}; + gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", dir[(uint8_t)zigzag_direction]); + } + } + } else { + move_to_side(); + } +} + +// suspend zigzag auto +void ModeZigZag::suspend_auto() +{ + if (auto_stage != AutoState::MANUAL) { + is_suspended = true; + return_to_manual_control(true); + } +} + +// initialize zigzag auto +void ModeZigZag::init_auto() +{ + is_auto = false; + auto_stage = AutoState::MANUAL; + line_count = 0; + is_suspended = false; } // spray on / off