mirror of https://github.com/ArduPilot/ardupilot
Copter: create and update Zigzag flight mode
This commit is contained in:
CAO MUQING
Randy Mackay
parent
09030b1b13
commit
71beab7502
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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),
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
Loading…
Reference in New Issue