aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: create and update Zigzag flight mode

Browse Source
This commit is contained in:
CAO MUQING 2018-09-07 12:23:33 +08:00 committed by Randy Mackay
parent 09030b1b13
commit 71beab7502
9 changed files with 407 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/APM_Config.h
View File

@ -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

3
ArduCopter/Copter.h
View File

@ -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);

12
ArduCopter/Parameters.cpp
View File

@ -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),

10
ArduCopter/RC_Channel.cpp
View File

@ -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;

6
ArduCopter/config.h
View File

@ -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

3
ArduCopter/defines.h
View File

@ -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

6
ArduCopter/mode.cpp
View File

@ -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;
}

55
ArduCopter/mode.h
View File

@ -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;
};

317
ArduCopter/mode_zigzag.cpp Normal file
View File

@ -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