diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 4a690cc1af..c2f71fc4eb 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -980,6 +980,21 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @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), #endif // MODE_ZIGZAG_ENABLED == ENABLED diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 71bbd8feb9..4903e5f802 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -617,6 +617,8 @@ public: AP_Int8 zigzag_spray_enabled; #endif AP_Int8 zigzag_wp_delay; + AP_Float zigzag_side_dist; + AP_Int8 zigzag_direction; #endif }; diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 62f7e56af7..e67dcf9e75 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -94,6 +94,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_ case AUX_FUNC::USER_FUNC3: case AUX_FUNC::WINCH_CONTROL: case AUX_FUNC::ZIGZAG: + case AUX_FUNC::ZIGZAG_Auto: case AUX_FUNC::ZIGZAG_SaveWP: break; case AUX_FUNC::ACRO_TRAINER: @@ -560,6 +561,20 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw break; } break; + + case AUX_FUNC::ZIGZAG_Auto: +#if MODE_ZIGZAG_ENABLED == ENABLED + if (copter.flightmode == &copter.mode_zigzag) { + switch (ch_flag) { + case HIGH: + copter.mode_zigzag.run_auto(); + break; + default: + break; + } + } +#endif + break; default: RC_Channel::do_aux_function(ch_option, ch_flag); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index c1ae0dc2a6..5e12ec7095 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1392,9 +1392,17 @@ public: B, // Destination B }; + enum class Direction : uint8_t { + FORWARD, // moving forward from the yaw direction + RIGHT, // moving right from the yaw direction + BACKWARD, // moving backward from the yaw direction + LEFT, // moving left from the yaw direction + } zigzag_direction; + bool init(bool ignore_checks) override; void exit(); void run() override; + void run_auto(); bool requires_GPS() const override { return true; } bool has_manual_throttle() const override { return false; } @@ -1419,17 +1427,27 @@ private: bool reached_destination(); bool calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Vector3f& next_dest, bool& terrain_alt) const; void spray(bool b); + bool calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) const; + void move_to_side(); Vector2f dest_A; // in NEU frame in cm relative to ekf origin Vector2f dest_B; // in NEU frame in cm relative to ekf origin - enum zigzag_state { + 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 MANUAL_REGAIN // pilot toggle the switch to middle position, has manual control } stage; + enum AutoState { + MANUAL, // after initializing, before switching to ZigZag Auto + AB_MOVING, // moving from A to B or from B to A + SIDEWAYS, // moving to sideways + } auto_stage; + 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 }; #if MODE_AUTOROTATE_ENABLED == ENABLED diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 2f6f9a033c..88c409b981 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -37,6 +37,7 @@ bool ModeZigZag::init(bool ignore_checks) stage = STORING_POINTS; dest_A.zero(); dest_B.zero(); + auto_stage = AutoState::MANUAL; return true; } @@ -56,15 +57,28 @@ 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 + zigzag_direction = (Direction)constrain_int16(g2.zigzag_direction, 0, 3); + // auto control if (stage == AUTO) { if (is_disarmed_or_landed() || !motors->get_interlock()) { // vehicle should be under manual control when disarmed or landed return_to_manual_control(false); } else if (reached_destination()) { - // if vehicle has reached destination switch to manual control + // if vehicle has reached destination switch to manual control or moving to A or B AP_Notify::events.waypoint_complete = 1; - return_to_manual_control(true); + if (is_auto) { + 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 { + return_to_manual_control(true); + } } else { auto_control(); } @@ -101,10 +115,10 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest) AP::logger().Write_Event(LogEvent::ZIGZAG_STORE_B); } // if both A and B have been stored advance state - if (!dest_A.is_zero() && !dest_B.is_zero() && is_positive((dest_B - dest_A).length_squared())) { + if (!dest_A.is_zero() && !dest_B.is_zero() && !is_zero((dest_B - dest_A).length_squared())) { stage = MANUAL_REGAIN; spray(false); - } else { + } else if (!dest_A.is_zero() || !dest_B.is_zero()) { // if only A or B have been stored, spray on spray(true); } @@ -119,20 +133,36 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest) wp_nav->wp_and_spline_init(); if (wp_nav->set_wp_destination(next_dest, terr_alt)) { stage = AUTO; + auto_stage = AutoState::AB_MOVING; + ab_dest_stored = ab_dest; // spray on while moving to A or B spray(true); reach_wp_time_ms = 0; - if (ab_dest == Destination::A) { - gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to A"); - } else { - gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to B"); - } + gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", (ab_dest == Destination::A) ? "A" : "B"); } } break; } } +void ModeZigZag::move_to_side() +{ + if (!dest_A.is_zero() && !dest_B.is_zero() && !is_zero((dest_B - dest_A).length_squared())) { + Vector3f next_dest; + bool terr_alt; + if (calculate_side_dest(next_dest, terr_alt)) { + wp_nav->wp_and_spline_init(); + if (wp_nav->set_wp_destination(next_dest, terr_alt)) { + stage = AUTO; + auto_stage = AutoState::SIDEWAYS; + 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]); + } + } + } +} + // return manual control to the pilot void ModeZigZag::return_to_manual_control(bool maintain_target) { @@ -149,6 +179,7 @@ void ModeZigZag::return_to_manual_control(bool maintain_target) } else { loiter_nav->init_target(); } + auto_stage = AutoState::MANUAL; gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control"); } } @@ -324,7 +355,7 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve Vector2f AB_diff = dest_B - dest_A; // check distance between A and B - if (!is_positive(AB_diff.length_squared())) { + if (is_zero(AB_diff.length_squared())) { return false; } @@ -368,6 +399,65 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve return true; } +// calculate side destination according to vertical vector A-B and current position +// terrain_alt is returned as true if the next_dest should be considered a terrain alt +bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) const +{ + // calculate vector from A to B + Vector2f AB_diff = dest_B - dest_A; + + // calculate a vertical right or left vector for AB from the current yaw direction + Vector2f AB_side; + if (zigzag_direction == Direction::RIGHT || zigzag_direction == Direction::LEFT) { + float yaw_ab_sign = (-ahrs.sin_yaw() * AB_diff[1]) + (ahrs.cos_yaw() * -AB_diff[0]); + if (is_positive(yaw_ab_sign * (zigzag_direction == Direction::RIGHT ? 1 : -1))) { + AB_side = Vector2f(AB_diff[1], -AB_diff[0]); + } else { + AB_side = Vector2f(-AB_diff[1], AB_diff[0]); + } + } else { + float yaw_ab_sign = (ahrs.cos_yaw() * AB_diff[1]) + (ahrs.sin_yaw() * -AB_diff[0]); + if (is_positive(yaw_ab_sign * (zigzag_direction == Direction::FORWARD ? 1 : -1))) { + AB_side = Vector2f(AB_diff[1], -AB_diff[0]); + } else { + AB_side = Vector2f(-AB_diff[1], AB_diff[0]); + } + } + + // check distance the vertical vector between A and B + if (is_zero(AB_side.length_squared())) { + return false; + } + + // 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()); + + // get distance from vehicle to start_pos + const Vector3f curr_pos = inertial_nav.get_position(); + const Vector2f curr_pos2d = Vector2f(curr_pos.x, curr_pos.y); + next_dest.x = curr_pos2d.x + (AB_side.x * scalar); + next_dest.y = curr_pos2d.y + (AB_side.y * scalar); + + // if we have a downward facing range finder then use terrain altitude targets + terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy(); + if (terrain_alt) { + if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) { + next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); + } + } else { + next_dest.z = pos_control->is_active_z() ? pos_control->get_alt_target() : curr_pos.z; + } + + return true; +} + +// run zigzag auto feature which is automate both AB and sideways +void ModeZigZag::run_auto() +{ + is_auto = true; + move_to_side(); +} + // spray on / off void ModeZigZag::spray(bool b) {