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

Copter: allow to resume in ZigZag Auto

Browse Source
This commit is contained in:
Tatsuya Yamaguchi 2020-04-08 00:23:27 +09:00 committed by Randy Mackay
parent cee7e94ebc
commit 78fd5fac04
3 changed files with 60 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/RC_Channel.cpp
View File

@ -485,6 +485,8 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
case AUX_FUNC::ZIGZAG_SaveWP:
#if MODE_ZIGZAG_ENABLED == ENABLED
if (copter.flightmode == &copter.mode_zigzag) {
// initialize zigzag auto
copter.mode_zigzag.init_auto();
switch (ch_flag) {
case LOW:
copter.mode_zigzag.save_or_move_to_destination(ModeZigZag::Destination::A);
@ -570,6 +572,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
copter.mode_zigzag.run_auto();
break;
default:
copter.mode_zigzag.suspend_auto();
break;
}
}

9
ArduCopter/mode.h
View File

@ -1402,7 +1402,11 @@ public:
bool init(bool ignore_checks) override;
void exit();
void run() override;
// auto control methods. copter flies grid pattern
void run_auto();
void suspend_auto();
void init_auto();
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
@ -1432,6 +1436,8 @@ private:
Vector2f dest_A; // in NEU frame in cm relative to ekf origin
Vector2f dest_B; // in NEU frame in cm relative to ekf origin
Vector3f current_dest; // current target destination (use for resume after suspending)
bool current_terr_alt;
enum ZigZagState {
STORING_POINTS, // storing points A and B, pilot has manual control
@ -1440,7 +1446,7 @@ private:
} stage;
enum AutoState {
MANUAL, // after initializing, before switching to ZigZag Auto
MANUAL, // not in ZigZag Auto
AB_MOVING, // moving from A to B or from B to A
SIDEWAYS, // moving to sideways
} auto_stage;
@ -1450,6 +1456,7 @@ private:
bool is_auto; // enable zigzag auto feature which is automate both AB and sideways
uint16_t line_count = 0; // current line number
int16_t line_num = 0; // target line number
bool is_suspended; // true if zigzag auto is suspended
};
#if MODE_AUTOROTATE_ENABLED == ENABLED

55
ArduCopter/mode_zigzag.cpp
View File

@ -7,6 +7,7 @@
*/
#define ZIGZAG_WP_RADIUS_CM 300
#define ZIGZAG_LINE_INFINITY -1
// initialise zigzag controller
bool ModeZigZag::init(bool ignore_checks)
@ -37,9 +38,9 @@ bool ModeZigZag::init(bool ignore_checks)
stage = STORING_POINTS;
dest_A.zero();
dest_B.zero();
auto_stage = AutoState::MANUAL;
line_count = 0;
// initialize zigzag auto
init_auto();
return true;
}
@ -72,7 +73,7 @@ void ModeZigZag::run()
// if vehicle has reached destination switch to manual control or moving to A or B
AP_Notify::events.waypoint_complete = 1;
if (is_auto) {
if (line_num == -1 || line_count < line_num) {
if (line_num == ZIGZAG_LINE_INFINITY || line_count < line_num) {
if (auto_stage == AutoState::SIDEWAYS) {
save_or_move_to_destination((ab_dest_stored == Destination::A) ? Destination::B : Destination::A);
} else {
@ -81,8 +82,7 @@ void ModeZigZag::run()
move_to_side();
}
} else {
is_auto = false;
line_count = 0;
init_auto();
return_to_manual_control(true);
}
} else {
@ -147,7 +147,7 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest)
// spray on while moving to A or B
spray(true);
reach_wp_time_ms = 0;
if (is_auto == false || line_num == -1) {
if (is_auto == false || line_num == ZIGZAG_LINE_INFINITY) {
gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", (ab_dest == Destination::A) ? "A" : "B");
} else {
line_count++;
@ -169,6 +169,8 @@ void ModeZigZag::move_to_side()
if (wp_nav->set_wp_destination(next_dest, terr_alt)) {
stage = AUTO;
auto_stage = AutoState::SIDEWAYS;
current_dest = next_dest;
current_terr_alt = terr_alt;
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]);
@ -193,7 +195,6 @@ void ModeZigZag::return_to_manual_control(bool maintain_target)
} else {
loiter_nav->init_target();
}
auto_stage = AutoState::MANUAL;
is_auto = false;
gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control");
}
@ -469,8 +470,48 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con
// run zigzag auto feature which is automate both AB and sideways
void ModeZigZag::run_auto()
{
// make sure both A and B point are registered and not when moving to A or B
if (stage != MANUAL_REGAIN) {
return;
}
is_auto = true;
// resume if zigzag auto is suspended
if (is_suspended && line_count <= line_num) {
// resume the stage when it was suspended
if (auto_stage == AutoState::AB_MOVING) {
line_count--;
save_or_move_to_destination(ab_dest_stored);
} else if (auto_stage == AutoState::SIDEWAYS) {
wp_nav->wp_and_spline_init();
if (wp_nav->set_wp_destination(current_dest, current_terr_alt)) {
stage = AUTO;
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]);
}
}
} else {
move_to_side();
}
}
// suspend zigzag auto
void ModeZigZag::suspend_auto()
{
if (auto_stage != AutoState::MANUAL) {
is_suspended = true;
return_to_manual_control(true);
}
}
// initialize zigzag auto
void ModeZigZag::init_auto()
{
is_auto = false;
auto_stage = AutoState::MANUAL;
line_count = 0;
is_suspended = false;
}
// spray on / off