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

Copter: support zigzag auto feature

Browse Source
This commit is contained in:
Tatsuya Yamaguchi 2020-04-03 13:48:23 +09:00 committed by Randy Mackay
parent ea3c11030c
commit 7bc528097d
5 changed files with 151 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/Parameters.cpp
View File

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

2
ArduCopter/Parameters.h
View File

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

15
ArduCopter/RC_Channel.cpp
View File

@ -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:
@ -561,6 +562,20 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
}
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);
break;

20
ArduCopter/mode.h
View File

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

110
ArduCopter/mode_zigzag.cpp
View File

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