Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add zigzag_line_num parameter

Browse Source
This commit is contained in:
Tatsuya Yamaguchi 2020-04-03 16:51:59 +09:00 committed by Randy Mackay
parent 7bc528097d
commit cee7e94ebc
4 changed files with 32 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/Parameters.cpp
View File

@ -995,6 +995,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Values: 0: forward, 1:right, 2: backward, 3: left // @Values: 0: forward, 1:right, 2: backward, 3: left
// @User: Advanced // @User: Advanced
AP_GROUPINFO("ZIGZ_DIRECTION", 41, ParametersG2, zigzag_direction, 0), AP_GROUPINFO("ZIGZ_DIRECTION", 41, ParametersG2, zigzag_direction, 0),
// @Param: ZIGZ_LINE_NUM
// @DisplayName: Total number of lines
// @Description: Total number of lines for ZigZag auto if 1 or more. -1: Infinity, 0: Just moving to sideways
// @Range: -1 32767
// @User: Advanced
AP_GROUPINFO("ZIGZ_LINE_NUM", 42, ParametersG2, zigzag_line_num, 0),
#endif // MODE_ZIGZAG_ENABLED == ENABLED #endif // MODE_ZIGZAG_ENABLED == ENABLED

1
ArduCopter/Parameters.h
View File

@ -619,6 +619,7 @@ public:
AP_Int8 zigzag_wp_delay; AP_Int8 zigzag_wp_delay;
AP_Float zigzag_side_dist; AP_Float zigzag_side_dist;
AP_Int8 zigzag_direction; AP_Int8 zigzag_direction;
AP_Int16 zigzag_line_num;
#endif #endif
}; };

2
ArduCopter/mode.h
View File

@ -1448,6 +1448,8 @@ private:
uint32_t reach_wp_time_ms = 0; // time since vehicle reached destination (or zero if not yet reached) 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 Destination ab_dest_stored; // store the current destination
bool is_auto; // enable zigzag auto feature which is automate both AB and sideways 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
}; };
#if MODE_AUTOROTATE_ENABLED == ENABLED #if MODE_AUTOROTATE_ENABLED == ENABLED

29
ArduCopter/mode_zigzag.cpp
View File

@ -39,6 +39,8 @@ bool ModeZigZag::init(bool ignore_checks)
dest_B.zero(); dest_B.zero();
auto_stage = AutoState::MANUAL; auto_stage = AutoState::MANUAL;
line_count = 0;
return true; return true;
} }
@ -57,8 +59,9 @@ void ModeZigZag::run()
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z); pos_control->set_max_accel_z(g.pilot_accel_z);
// set the direction to move sideways // set the direction and the total number of lines
zigzag_direction = (Direction)constrain_int16(g2.zigzag_direction, 0, 3); zigzag_direction = (Direction)constrain_int16(g2.zigzag_direction, 0, 3);
line_num = constrain_int16(g2.zigzag_line_num, -1, 32767);
// auto control // auto control
if (stage == AUTO) { if (stage == AUTO) {
@ -69,12 +72,18 @@ void ModeZigZag::run()
// if vehicle has reached destination switch to manual control or moving to A or B // if vehicle has reached destination switch to manual control or moving to A or B
AP_Notify::events.waypoint_complete = 1; AP_Notify::events.waypoint_complete = 1;
if (is_auto) { if (is_auto) {
if (auto_stage == AutoState::SIDEWAYS) { if (line_num == -1 || line_count < line_num) {
save_or_move_to_destination((ab_dest_stored == Destination::A) ? Destination::B : Destination::A); 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 { } else {
// spray off is_auto = false;
spray(false); line_count = 0;
move_to_side(); return_to_manual_control(true);
} }
} else { } else {
return_to_manual_control(true); return_to_manual_control(true);
@ -138,7 +147,12 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest)
// spray on while moving to A or B // spray on while moving to A or B
spray(true); spray(true);
reach_wp_time_ms = 0; reach_wp_time_ms = 0;
gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", (ab_dest == Destination::A) ? "A" : "B"); if (is_auto == false || line_num == -1) {
gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s", (ab_dest == Destination::A) ? "A" : "B");
} else {
line_count++;
gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: moving to %s (line %d/%d)", (ab_dest == Destination::A) ? "A" : "B", line_count, line_num);
}
} }
} }
break; break;
@ -180,6 +194,7 @@ void ModeZigZag::return_to_manual_control(bool maintain_target)
loiter_nav->init_target(); loiter_nav->init_target();
} }
auto_stage = AutoState::MANUAL; auto_stage = AutoState::MANUAL;
is_auto = false;
gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control"); gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control");
} }
} }