From cee7e94ebc10c3acc5e31526ad14585fae069588 Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Fri, 3 Apr 2020 16:51:59 +0900 Subject: [PATCH] Copter: add zigzag_line_num parameter --- ArduCopter/Parameters.cpp | 7 +++++++ ArduCopter/Parameters.h | 1 + ArduCopter/mode.h | 2 ++ ArduCopter/mode_zigzag.cpp | 29 ++++++++++++++++++++++------- 4 files changed, 32 insertions(+), 7 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index c2f71fc4eb..703d16ff5f 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -995,6 +995,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Values: 0: forward, 1:right, 2: backward, 3: left // @User: Advanced AP_GROUPINFO("ZIGZ_DIRECTION", 41, ParametersG2, zigzag_direction, 0), + + // @Param: ZIGZ_LINE_NUM + // @DisplayName: Total number of lines + // @Description: Total number of lines for ZigZag auto if 1 or more. -1: Infinity, 0: Just moving to sideways + // @Range: -1 32767 + // @User: Advanced + AP_GROUPINFO("ZIGZ_LINE_NUM", 42, ParametersG2, zigzag_line_num, 0), #endif // MODE_ZIGZAG_ENABLED == ENABLED diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4903e5f802..e2d3e004d7 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -619,6 +619,7 @@ public: AP_Int8 zigzag_wp_delay; AP_Float zigzag_side_dist; AP_Int8 zigzag_direction; + AP_Int16 zigzag_line_num; #endif }; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 5e12ec7095..aa7f021b09 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1448,6 +1448,8 @@ private: uint32_t reach_wp_time_ms = 0; // time since vehicle reached destination (or zero if not yet reached) Destination ab_dest_stored; // store the current destination 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 }; #if MODE_AUTOROTATE_ENABLED == ENABLED diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 88c409b981..25885ba432 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -39,6 +39,8 @@ bool ModeZigZag::init(bool ignore_checks) dest_B.zero(); auto_stage = AutoState::MANUAL; + line_count = 0; + return true; } @@ -57,8 +59,9 @@ void ModeZigZag::run() pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_max_accel_z(g.pilot_accel_z); - // set the direction to move sideways + // set the direction and the total number of lines zigzag_direction = (Direction)constrain_int16(g2.zigzag_direction, 0, 3); + line_num = constrain_int16(g2.zigzag_line_num, -1, 32767); // auto control if (stage == AUTO) { @@ -69,12 +72,18 @@ 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 (auto_stage == AutoState::SIDEWAYS) { - save_or_move_to_destination((ab_dest_stored == Destination::A) ? Destination::B : Destination::A); + if (line_num == -1 || line_count < line_num) { + if (auto_stage == AutoState::SIDEWAYS) { + save_or_move_to_destination((ab_dest_stored == Destination::A) ? Destination::B : Destination::A); + } else { + // spray off + spray(false); + move_to_side(); + } } else { - // spray off - spray(false); - move_to_side(); + is_auto = false; + line_count = 0; + return_to_manual_control(true); } } else { return_to_manual_control(true); @@ -138,7 +147,12 @@ 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; - gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", (ab_dest == Destination::A) ? "A" : "B"); + if (is_auto == false || line_num == -1) { + gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", (ab_dest == Destination::A) ? "A" : "B"); + } else { + line_count++; + gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s (line %d/%d)", (ab_dest == Destination::A) ? "A" : "B", line_count, line_num); + } } } break; @@ -180,6 +194,7 @@ void ModeZigZag::return_to_manual_control(bool maintain_target) loiter_nav->init_target(); } auto_stage = AutoState::MANUAL; + is_auto = false; gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control"); } }