From 71beab750223356d4f799af001641c5e5c32ca18 Mon Sep 17 00:00:00 2001 From: CAO MUQING Date: Fri, 7 Sep 2018 12:23:33 +0800 Subject: [PATCH] Copter: create and update Zigzag flight mode --- ArduCopter/APM_Config.h | 1 + ArduCopter/Copter.h | 3 + ArduCopter/Parameters.cpp | 12 +- ArduCopter/RC_Channel.cpp | 10 ++ ArduCopter/config.h | 6 + ArduCopter/defines.h | 3 + ArduCopter/mode.cpp | 6 + ArduCopter/mode.h | 55 +++++++ ArduCopter/mode_zigzag.cpp | 317 +++++++++++++++++++++++++++++++++++++ 9 files changed, 407 insertions(+), 6 deletions(-) create mode 100644 ArduCopter/mode_zigzag.cpp diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 51c0eea680..1149263298 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -42,6 +42,7 @@ //#define MODE_SMARTRTL_ENABLED DISABLED // disable smartrtl mode support //#define MODE_SPORT_ENABLED DISABLED // disable sport mode support //#define MODE_THROW_ENABLED DISABLED // disable throw mode support +//#define MODE_ZIGZAG_ENABLED DISABLED // disable zigzag mode support //#define DEVO_TELEM_ENABLED DISABLED // disable DEVO telemetry, if you don't use Walkera RX-707 (or newer) receivers diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index e1179094cd..9777614f93 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -991,6 +991,9 @@ private: #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED ModeFlowHold mode_flowhold; #endif +#if MODE_ZIGZAG_ENABLED == ENABLED + ModeZigZag mode_zigzag; +#endif // mode.cpp Mode *mode_from_mode_num(const uint8_t mode); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 4ad20273fb..2730fc2dfd 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -270,42 +270,42 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FLTMODE1 // @DisplayName: Flight Mode 1 // @Description: Flight mode when Channel 5 pwm is <= 1230 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag // @User: Standard GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), // @Param: FLTMODE2 // @DisplayName: Flight Mode 2 // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag // @User: Standard GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2), // @Param: FLTMODE3 // @DisplayName: Flight Mode 3 // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag // @User: Standard GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3), // @Param: FLTMODE4 // @DisplayName: Flight Mode 4 // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag // @User: Standard GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4), // @Param: FLTMODE5 // @DisplayName: Flight Mode 5 // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag // @User: Standard GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5), // @Param: FLTMODE6 // @DisplayName: Flight Mode 6 // @Description: Flight mode when Channel 5 pwm is >=1750 - // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow + // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 5deb3587ce..0f68dde6d1 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -103,6 +103,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_ case USER_FUNC1: case USER_FUNC2: case USER_FUNC3: + case ZIGZAG: break; default: RC_Channel::init_aux_function(ch_option, ch_flag); @@ -524,6 +525,15 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw userhook_auxSwitch3(ch_flag); break; #endif + + case ZIGZAG: +#if MODE_ZIGZAG_ENABLED == ENABLED + if (copter.flightmode == &copter.mode_zigzag) { + copter.mode_zigzag.receive_signal_from_auxsw(ch_flag); + } +#endif + break; + default: RC_Channel::do_aux_function(ch_option, ch_flag); break; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 851a2757ca..3f333e4dcf 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -349,6 +349,12 @@ # define MODE_THROW_ENABLED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// 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 +#endif + ////////////////////////////////////////////////////////////////////////////// // Beacon support - support for local positioning systems #ifndef BEACON_ENABLED diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index ed725d5f08..392dce685e 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -53,6 +53,7 @@ enum control_mode_t { SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps FLOWHOLD = 22, // FLOWHOLD holds position with optical flow without rangefinder FOLLOW = 23, // follow attempts to follow another vehicle or ground station + ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B }; enum mode_reason_t { @@ -331,6 +332,8 @@ enum LoggingParameters { #define DATA_WINCH_RELAXED 68 #define DATA_WINCH_LENGTH_CONTROL 69 #define DATA_WINCH_RATE_CONTROL 70 +#define DATA_ZIGZAG_STORE_A 71 +#define DATA_ZIGZAG_STORE_B 72 // Error message sub systems and error codes #define ERROR_SUBSYSTEM_MAIN 1 diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 9d03cfaa8d..0cb822f51d 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -157,6 +157,12 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) break; #endif +#if MODE_ZIGZAG_ENABLED == ENABLED + case ZIGZAG: + ret = &mode_zigzag; + break; +#endif + default: break; } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index c310895ee8..12acac1a72 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1254,3 +1254,58 @@ protected: uint32_t last_log_ms; // system time of last time desired velocity was logging }; + +class ModeZigZag : public Mode { + +public: + + // inherit constructor + using Copter::Mode::Mode; + + bool init(bool ignore_checks) override; + void run() override; + + 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 is_autopilot() const override { return true; } + + void receive_signal_from_auxsw(RC_Channel::aux_switch_pos_t aux_switch_position); + +protected: + + const char *name() const override { return "ZIGZAG"; } + const char *name4() const override { return "ZIGZ"; } + +private: + + void auto_control(); + void manual_control(); + bool has_arr_at_dest(); + bool calculate_next_dest(Vector3f& next_dest, RC_Channel::aux_switch_pos_t next_A_or_B) const; + bool set_destination(const Vector3f& destination, RC_Channel::aux_switch_pos_t aux_switch_position); + + struct { + Vector2f A_pos; // in NEU frame in cm relative to home location + Vector2f B_pos; // in NEU frame in cm relative to home location + RC_Channel::aux_switch_pos_t switch_pos_A; // switch position recorded as point A + RC_Channel::aux_switch_pos_t switch_pos_B; // switch position recorded as point B + } zigzag_waypoint; + + enum zigzag_state { + REQUIRE_A, // point A is not defined yet, pilot has manual control + REQUIRE_B, // point B is not defined but A has been defined, 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 + }; + + zigzag_state stage = REQUIRE_A; + + struct { + uint32_t last_judge_pos_time; + Vector3f last_pos; + bool is_keeping_time; + } zigzag_judge_moving; + + bool zigzag_is_between_A_and_B; +}; diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp new file mode 100644 index 0000000000..958df6e521 --- /dev/null +++ b/ArduCopter/mode_zigzag.cpp @@ -0,0 +1,317 @@ +#include "Copter.h" + +#if MODE_ZIGZAG_ENABLED == ENABLED + +/* +* Init and run calls for zigzag flight mode +*/ + +#define ZIGZAG_WP_RADIUS_SQUARED 9 + +// init - initialise zigzag controller +bool Copter::ModeZigZag::init(bool ignore_checks) +{ + if (!copter.position_ok() && !ignore_checks) { + return false; + } + + // initialize's loiter position and velocity on xy-axes from current pos and velocity + loiter_nav->init_target(); + + // initialise position_z and desired velocity_z + if (!pos_control->is_active_z()) { + pos_control->set_alt_target_to_current_alt(); + pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + } + + // initialise waypoint state + zigzag_is_between_A_and_B = false; + zigzag_judge_moving.is_keeping_time = false; + stage = REQUIRE_A; + return true; +} + +// run - runs the zigzag controller +// should be called at 100hz or more +void Copter::ModeZigZag::run() +{ + // initialize vertical speed and acceleration's range + pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); + pos_control->set_max_accel_z(g.pilot_accel_z); + + // if not auto armed or motors not enabled set throttle to zero and exit immediately + if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) { + zero_throttle_and_relax_ac(); + return; + } + + // manual control activated when point A B is not defined + if (stage == REQUIRE_A || stage == REQUIRE_B || stage == MANUAL_REGAIN) { + // receive pilot's inputs, do position and attitude control + manual_control(); + } else { + // auto flight + // judge if the vehicle has arrived at the current destination + // if yes, go to the manual control stage + // else, fly to current destination + if (has_arr_at_dest()) { // if the vehicle has arrived at the current destination + stage = MANUAL_REGAIN; + loiter_nav->init_target(); + AP_Notify::events.waypoint_complete = 1; // play a tone + } else { + auto_control(); + } + } +} + +// auto_control - guide the vehicle to fly to current destination +void Copter::ModeZigZag::auto_control() +{ + // process pilot's yaw input + float target_yaw_rate = 0; + if (!copter.failsafe.radio) { + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + } + + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + + // run waypoint controller to update xy + copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); + + // call z-axis position controller (wpnav should have already updated it's alt target) + pos_control->update_z_controller(); + + // call attitude controller + // roll & pitch from waypoint controller, yaw rate from pilot + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); +} + +// manual_control - process manual control +void Copter::ModeZigZag::manual_control() +{ + float target_yaw_rate = 0.0f; + float target_climb_rate = 0.0f; + + // process pilot inputs unless we are in radio failsafe + if (!copter.failsafe.radio) { + float target_roll, target_pitch; + // apply SIMPLE mode transform to pilot inputs + update_simple_mode(); + + // convert pilot input to lean angles + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + + // process pilot's roll and pitch input + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + + // get pilot desired climb rate + target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + // make sure the climb rate is in the given range, prevent floating point errors + target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); + } else { + // clear out pilot desired acceleration in case radio failsafe event occurs and we + // do not switch to RTL for some reason + loiter_nav->clear_pilot_desired_acceleration(); + } + + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + // run loiter controller + loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler); + + // call attitude controller + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), + loiter_nav->get_pitch(), target_yaw_rate); + + // adjust climb rate using rangefinder + target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); + + // get avoidance adjusted climb rate + target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); + + // update altitude target and call position controller + pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + // adjusts target up or down using a climb rate + + pos_control->update_z_controller(); +} + +// has_arr_at_next_dest - judge if the vehicle is within a small area around the current destination +bool Copter::ModeZigZag::has_arr_at_dest() +{ + if (!zigzag_judge_moving.is_keeping_time) { + zigzag_judge_moving.is_keeping_time = true; + zigzag_judge_moving.last_judge_pos_time = AP_HAL::millis(); + zigzag_judge_moving.last_pos = inertial_nav.get_position(); + return false; + } + if ((AP_HAL::millis() - zigzag_judge_moving.last_judge_pos_time) < 1000) { + return false; + } + Vector3f cur_pos = inertial_nav.get_position(); + const float dist_x = cur_pos.x - zigzag_judge_moving.last_pos.x; + const float dist_y = cur_pos.y - zigzag_judge_moving.last_pos.y; + if ((sq(dist_x) + sq(dist_y)) < ZIGZAG_WP_RADIUS_SQUARED) { + return true; + } + zigzag_judge_moving.last_judge_pos_time = AP_HAL::millis(); + zigzag_judge_moving.last_pos = inertial_nav.get_position(); + return false; +} + +// calculate_next_dest - calculate next destination according to vector A-B and current position +bool Copter::ModeZigZag::calculate_next_dest(Vector3f& next_dest, RC_Channel::aux_switch_pos_t next_A_or_B) const +{ + // calculate difference between A and B - vector AB and its direction + Vector2f pos_diff = zigzag_waypoint.B_pos - zigzag_waypoint.A_pos; + // get current position + Vector3f cur_pos = inertial_nav.get_position(); + if (!zigzag_is_between_A_and_B) { + // if the drone's position is on the side of A or B + if (next_A_or_B != zigzag_waypoint.switch_pos_B && next_A_or_B != zigzag_waypoint.switch_pos_A) { + return false; // if next_dest not initialised, return false + } + if (next_A_or_B == zigzag_waypoint.switch_pos_B) { + next_dest.x = cur_pos.x + pos_diff.x; + next_dest.y = cur_pos.y + pos_diff.y; + next_dest.z = cur_pos.z; + return true; + } + // can only be the case when (next_A_or_B == zigzag_waypoint.switch_pos_A) + next_dest.x = cur_pos.x - pos_diff.x; + next_dest.y = cur_pos.y - pos_diff.y; + next_dest.z = cur_pos.z; + return true; + } + // used to check if the drone is outside A-B scale + int8_t next_dir = 1; + // if the drone's position is between A and B + const Vector2f cur_pos_2d{cur_pos.x, cur_pos.y}; + const Vector2f AB = zigzag_waypoint.B_pos - zigzag_waypoint.A_pos; + const Vector2f P_on_AB = Vector2f::closest_point(cur_pos_2d, zigzag_waypoint.A_pos, zigzag_waypoint.B_pos); + float dist_AB = AB.length(); + float dist_from_AB_squared = (P_on_AB - cur_pos_2d).length_squared(); + next_dest.z = cur_pos.z; + if (is_zero(dist_AB)) { // protection against division by zero + return false; + } + if (next_A_or_B != zigzag_waypoint.switch_pos_B && next_A_or_B != zigzag_waypoint.switch_pos_A) { + return false; // if next_dest not initialised, return false + } + if (next_A_or_B == zigzag_waypoint.switch_pos_B) { + // calculate next B + Vector2f pos_diff_BC = cur_pos_2d - zigzag_waypoint.B_pos; + if ((pos_diff_BC.x*pos_diff.x + pos_diff_BC.y*pos_diff.y) > 0) { + next_dir = -1; + } + float dist_CB_squared = (cur_pos_2d - zigzag_waypoint.B_pos).length_squared(); + float dist_BE = sqrtf(dist_CB_squared - dist_from_AB_squared); + float dist_ratio = dist_BE / dist_AB; + next_dest.x = cur_pos.x + next_dir*dist_ratio*pos_diff.x; + next_dest.y = cur_pos.y + next_dir*dist_ratio*pos_diff.y; + return true; + } + // can only be the case when (next_A_or_B == zigzag_waypoint.switch_pos_A) + // calculate next A + Vector2f pos_diff_AC = cur_pos_2d - zigzag_waypoint.A_pos; + if ((pos_diff_AC.x*pos_diff.x + pos_diff_AC.y*pos_diff.y) < 0) { + next_dir = -1; + } + float dist_CA_squared = (cur_pos_2d - zigzag_waypoint.A_pos).length_squared(); + float dist_AE = sqrtf(dist_CA_squared - dist_from_AB_squared); + float dist_ratio = dist_AE / dist_AB; + next_dest.x = cur_pos.x - next_dir*dist_ratio*pos_diff.x; + next_dest.y = cur_pos.y - next_dir*dist_ratio*pos_diff.y; + return true; +} + +// called by ZIGZAG case in RC_Channel.cpp +// used to record point A, B and give the signal to fly to next destination automatically +void Copter::ModeZigZag::receive_signal_from_auxsw(RC_Channel::aux_switch_pos_t aux_switch_position) +{ + // define point A and B + if (stage == REQUIRE_A || stage == REQUIRE_B) { + if (aux_switch_position != RC_Channel::aux_switch_pos_t::MIDDLE) { + Vector3f cur_pos = inertial_nav.get_position(); + set_destination(cur_pos, aux_switch_position); + return; + } + } else { + // A and B have been defined + if (aux_switch_position != RC_Channel::aux_switch_pos_t::MIDDLE) { // switch position in HIGH or LOW + // calculate next point A or B + // need to judge if the drone's position is between A and B + Vector3f next_dest; + if (calculate_next_dest(next_dest, aux_switch_position)) { + // initialise waypoint and spline controller + wp_nav->wp_and_spline_init(); + set_destination(next_dest, aux_switch_position); + // initialise yaw + auto_yaw.set_mode_to_default(false); + stage = AUTO; + zigzag_is_between_A_and_B = false; + } + } else { //switch in middle position, regain the control + if (stage == AUTO) { + stage = MANUAL_REGAIN; + loiter_nav->init_target(); + zigzag_is_between_A_and_B = true; + } else { + zigzag_is_between_A_and_B = false; + } + } + } +} + +// set_destination - sets zigzag mode's target destination +// Returns true if the fence is enabled and guided waypoint is within the fence +// else return false if the waypoint is outside the fence +bool Copter::ModeZigZag::set_destination(const Vector3f& destination, RC_Channel::aux_switch_pos_t aux_switch_position) +{ + +#if AC_FENCE == ENABLED + // reject destination if outside the fence + Location_Class dest_loc(destination); + if (!copter.fence.check_destination_within_fence(dest_loc)) { + copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE); + return false; + } +#endif + switch (stage) { + case REQUIRE_A: + // define point A + zigzag_waypoint.A_pos.x = destination.x; + zigzag_waypoint.A_pos.y = destination.y; + zigzag_waypoint.switch_pos_A = aux_switch_position; + stage = REQUIRE_B; // next need to define point B + gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point A stored"); + copter.Log_Write_Event(DATA_ZIGZAG_STORE_A); + return true; + case REQUIRE_B: + // point B will only be defined after A is defined + // if user toggle to the switch position that were previously defined as A + // exit the function and do nothing + if (aux_switch_position == zigzag_waypoint.switch_pos_A) { + return true; + } + // define point B + zigzag_waypoint.B_pos.x = destination.x; + zigzag_waypoint.B_pos.y = destination.y; + zigzag_waypoint.switch_pos_B = aux_switch_position; + stage = MANUAL_REGAIN; // user is still in manual control until he/she returns the switch again to point A position + gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point B stored"); + copter.Log_Write_Event(DATA_ZIGZAG_STORE_B); + return true; + default: + // when both A and B are defined and switch in not in middle position, set waypoint destination + // no need to check return status because terrain data is not used + wp_nav->set_wp_destination(destination, false); + return true; + } + +} +#endif // MODE_ZIGZAG_ENABLED == ENABLED