From cde7ae246e96d9f8a391707c51b3a0a75d6a8bc7 Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Tue, 12 May 2020 11:17:21 +0900 Subject: [PATCH] Copter: add ZIGZ_AUTO_ENABLE parameter --- ArduCopter/Parameters.cpp | 47 ++++-------------------- ArduCopter/Parameters.h | 10 +---- ArduCopter/config.h | 4 -- ArduCopter/mode.h | 13 +++++++ ArduCopter/mode_zigzag.cpp | 75 +++++++++++++++++++++++++++++++++++--- 5 files changed, 92 insertions(+), 57 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 703d16ff5f..751f958aa7 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -964,46 +964,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { #endif #if MODE_ZIGZAG_ENABLED == ENABLED -#if SPRAYER_ENABLED == ENABLED - // @Param: ZIGZ_SPRAYER - // @DisplayName: Auto sprayer in ZigZag - // @Description: Enable the auto sprayer in ZigZag mode. SPRAY_ENABLE = 1 and SERVOx_FUNCTION = 22(SprayerPump) / 23(SprayerSpinner) also must be set. This makes the sprayer on while moving to destination A or B. The sprayer will stop if the vehicle reaches destination or the flight mode is changed from ZigZag to other. - // @Values: 0:Disabled,1:Enabled - // @User: Advanced - AP_GROUPINFO("ZIGZ_SPRAYER", 38, ParametersG2, zigzag_spray_enabled, ZIGZAG_SPRAY_ENABLED), -#endif // SPRAYER_ENABLED == ENABLED - - // @Param: ZIGZ_WP_DELAY - // @DisplayName: The delay for zigzag waypoint - // @Description: Waiting time after reached the destination - // @Units: s - // @Range: 1 127 - // @User: Advanced - AP_GROUPINFO("ZIGZ_WP_DELAY", 39, ParametersG2, zigzag_wp_delay, ZIGZAG_WP_DELAY), - - // @Param: ZIGZ_SIDE_DIST - // @DisplayName: Sideways distance in ZigZag auto - // @Description: The distance to move sideways in ZigZag mode - // @Units: m - // @Range: 0.1 100 - // @User: Advanced - AP_GROUPINFO("ZIGZ_SIDE_DIST", 40, ParametersG2, zigzag_side_dist, 4), - - // @Param: ZIGZ_DIRECTION - // @DisplayName: Sideways direction in ZigZag auto - // @Description: The direction to move sideways in ZigZag mode - // @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 - + // @Group: ZIGZ_ + // @Path: mode_zigzag.cpp + AP_SUBGROUPPTR(mode_zigzag_ptr, "ZIGZ_", 38, ParametersG2, ModeZigZag), +#endif AP_GROUPEND }; @@ -1044,6 +1008,9 @@ ParametersG2::ParametersG2(void) ,arot(copter.inertial_nav) #endif ,button_ptr(&copter.button) +#if MODE_ZIGZAG_ENABLED == ENABLED + ,mode_zigzag_ptr(&copter.mode_zigzag) +#endif { AP_Param::setup_object_defaults(this, var_info); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e2d3e004d7..319bd4cfa2 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -612,14 +612,8 @@ public: #endif #if MODE_ZIGZAG_ENABLED == ENABLED -#if SPRAYER_ENABLED == ENABLED - // auto pump enable/disable - AP_Int8 zigzag_spray_enabled; -#endif - AP_Int8 zigzag_wp_delay; - AP_Float zigzag_side_dist; - AP_Int8 zigzag_direction; - AP_Int16 zigzag_line_num; + // we need a pointer to the mode for the G2 table + void *mode_zigzag_ptr; #endif }; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index a8f584cf1b..a6de8540d2 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -364,10 +364,6 @@ // ZigZag - allow vehicle to fly in a zigzag manner with predefined point A B #ifndef MODE_ZIGZAG_ENABLED # define MODE_ZIGZAG_ENABLED !HAL_MINIMIZE_FEATURES -# define ZIGZAG_WP_DELAY 1 -#if HAL_SPRAYER_ENABLED -# define ZIGZAG_SPRAY_ENABLED DISABLED -#endif #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 6647e023a0..155b029be9 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1383,6 +1383,7 @@ protected: class ModeZigZag : public Mode { public: + ModeZigZag(void); // Inherit constructor using Mode::Mode; @@ -1419,6 +1420,8 @@ public: // return manual control to the pilot void return_to_manual_control(bool maintain_target); + static const struct AP_Param::GroupInfo var_info[]; + protected: const char *name() const override { return "ZIGZAG"; } @@ -1439,6 +1442,16 @@ private: Vector3f current_dest; // current target destination (use for resume after suspending) bool current_terr_alt; + // parameters + AP_Int8 _auto_enabled; // top level enable/disable control +#if SPRAYER_ENABLED == ENABLED + AP_Int8 _spray_enabled; // auto spray enable/disable +#endif + AP_Int8 _wp_delay; // delay for zigzag waypoint + AP_Float _side_dist; // sideways distance + AP_Int8 _direction; // sideways direction + AP_Int16 _line_num; // total number of lines + enum ZigZagState { STORING_POINTS, // storing points A and B, pilot has manual control AUTO, // after A and B defined, pilot toggle the switch from one side to the other, vehicle flies autonomously diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 797f5c6748..2db1fcb63c 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -9,6 +9,61 @@ #define ZIGZAG_WP_RADIUS_CM 300 #define ZIGZAG_LINE_INFINITY -1 +const AP_Param::GroupInfo ModeZigZag::var_info[] = { + // @Param: AUTO_ENABLE + // @DisplayName: ZigZag auto enable/disable + // @Description: Allows you to enable (1) or disable (0) ZigZag auto feature + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO_FLAGS("AUTO_ENABLE", 1, ModeZigZag, _auto_enabled, 0, AP_PARAM_FLAG_ENABLE), + +#if SPRAYER_ENABLED == ENABLED + // @Param: SPRAYER + // @DisplayName: Auto sprayer in ZigZag + // @Description: Enable the auto sprayer in ZigZag mode. SPRAY_ENABLE = 1 and SERVOx_FUNCTION = 22(SprayerPump) / 23(SprayerSpinner) also must be set. This makes the sprayer on while moving to destination A or B. The sprayer will stop if the vehicle reaches destination or the flight mode is changed from ZigZag to other. + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("SPRAYER", 2, ModeZigZag, _spray_enabled, 0), +#endif // SPRAYER_ENABLED == ENABLED + + // @Param: WP_DELAY + // @DisplayName: The delay for zigzag waypoint + // @Description: Waiting time after reached the destination + // @Units: s + // @Range: 1 127 + // @User: Advanced + AP_GROUPINFO("WP_DELAY", 3, ModeZigZag, _wp_delay, 1), + + // @Param: SIDE_DIST + // @DisplayName: Sideways distance in ZigZag auto + // @Description: The distance to move sideways in ZigZag mode + // @Units: m + // @Range: 0.1 100 + // @User: Advanced + AP_GROUPINFO("SIDE_DIST", 4, ModeZigZag, _side_dist, 4), + + // @Param: DIRECTION + // @DisplayName: Sideways direction in ZigZag auto + // @Description: The direction to move sideways in ZigZag mode + // @Values: 0:forward, 1:right, 2:backward, 3:left + // @User: Advanced + AP_GROUPINFO("DIRECTION", 5, ModeZigZag, _direction, 0), + + // @Param: 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("LINE_NUM", 6, ModeZigZag, _line_num, 0), + + AP_GROUPEND +}; + +ModeZigZag::ModeZigZag(void) : Mode() +{ + AP_Param::setup_object_defaults(this, var_info); +} + // initialise zigzag controller bool ModeZigZag::init(bool ignore_checks) { @@ -61,8 +116,8 @@ void ModeZigZag::run() pos_control->set_max_accel_z(g.pilot_accel_z); // 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); + zigzag_direction = (Direction)constrain_int16(_direction, 0, 3); + line_num = constrain_int16(_line_num, -1, 32767); // auto control if (stage == AUTO) { @@ -356,7 +411,7 @@ bool ModeZigZag::reached_destination() if (reach_wp_time_ms == 0) { reach_wp_time_ms = now; } - return ((now - reach_wp_time_ms) > (uint16_t)constrain_int16(g2.zigzag_wp_delay, 1, 127) * 1000); + return ((now - reach_wp_time_ms) > (uint16_t)constrain_int16(_wp_delay, 1, 127) * 1000); } // calculate next destination according to vector A-B and current position @@ -446,7 +501,7 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con } // adjust AB_side length to zigzag_side_dist - float scalar = constrain_float(g2.zigzag_side_dist, 0.1f, 100.0f) * 100 / safe_sqrt(AB_side.length_squared()); + float scalar = constrain_float(_side_dist, 0.1f, 100.0f) * 100 / safe_sqrt(AB_side.length_squared()); // get distance from vehicle to start_pos const Vector3f curr_pos = inertial_nav.get_position(); @@ -470,6 +525,11 @@ 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() { + // exit immediately if we are disabled + if (!_auto_enabled) { + return; + } + // make sure both A and B point are registered and not when moving to A or B if (stage != MANUAL_REGAIN) { return; @@ -499,6 +559,11 @@ void ModeZigZag::run_auto() // suspend zigzag auto void ModeZigZag::suspend_auto() { + // exit immediately if we are disabled + if (!_auto_enabled) { + return; + } + if (auto_stage != AutoState::MANUAL) { is_suspended = true; return_to_manual_control(true); @@ -518,7 +583,7 @@ void ModeZigZag::init_auto() void ModeZigZag::spray(bool b) { #if SPRAYER_ENABLED == ENABLED - if (g2.zigzag_spray_enabled) { + if (_spray_enabled) { copter.sprayer.run(b); } #endif