Line data Source code
1 : #include "Copter.h"
2 :
3 : #if MODE_ZIGZAG_ENABLED
4 :
5 : /*
6 : * Init and run calls for zigzag flight mode
7 : */
8 :
9 : #define ZIGZAG_WP_RADIUS_M 3.0
10 : #define ZIGZAG_LINE_INFINITY -1
11 :
12 : const AP_Param::GroupInfo ModeZigZag::var_info[] = {
13 : // @Param: AUTO_ENABLE
14 : // @DisplayName: ZigZag auto enable/disable
15 : // @Description: Allows you to enable (1) or disable (0) ZigZag auto feature
16 : // @Values: 0:Disabled,1:Enabled
17 : // @User: Advanced
18 : AP_GROUPINFO_FLAGS("AUTO_ENABLE", 1, ModeZigZag, _auto_enabled, 0, AP_PARAM_FLAG_ENABLE),
19 :
20 : #if HAL_SPRAYER_ENABLED
21 : // @Param: SPRAYER
22 : // @DisplayName: Auto sprayer in ZigZag
23 : // @Description: Enable the auto sprayer in ZigZag mode. SPRAY_ENABLE = 1 and SERVOx_FUNCTION = 22(SprayerPump) / 23(SprayerSpinner) also must be set. This makes the sprayer on while moving to destination A or B. The sprayer will stop if the vehicle reaches destination or the flight mode is changed from ZigZag to other.
24 : // @Values: 0:Disabled,1:Enabled
25 : // @User: Advanced
26 : AP_GROUPINFO("SPRAYER", 2, ModeZigZag, _spray_enabled, 0),
27 : #endif // HAL_SPRAYER_ENABLED
28 :
29 : // @Param: WP_DELAY
30 : // @DisplayName: The delay for zigzag waypoint
31 : // @Description: Waiting time after reached the destination
32 : // @Units: s
33 : // @Range: 0 127
34 : // @User: Advanced
35 : AP_GROUPINFO("WP_DELAY", 3, ModeZigZag, _wp_delay_s, 0),
36 :
37 : // @Param: SIDE_DIST
38 : // @DisplayName: Sideways distance in ZigZag auto
39 : // @Description: The distance to move sideways in ZigZag mode
40 : // @Units: m
41 : // @Range: 0.1 100
42 : // @User: Advanced
43 : AP_GROUPINFO("SIDE_DIST", 4, ModeZigZag, _side_dist_m, 4),
44 :
45 : // @Param: DIRECTION
46 : // @DisplayName: Sideways direction in ZigZag auto
47 : // @Description: The direction to move sideways in ZigZag mode
48 : // @Values: 0:forward, 1:right, 2:backward, 3:left
49 : // @User: Advanced
50 : AP_GROUPINFO("DIRECTION", 5, ModeZigZag, _direction, 0),
51 :
52 : // @Param: LINE_NUM
53 : // @DisplayName: Total number of lines
54 : // @Description: Total number of lines for ZigZag auto if 1 or more. -1: Infinity, 0: Just moving to sideways
55 : // @Range: -1 32767
56 : // @User: Advanced
57 : AP_GROUPINFO("LINE_NUM", 6, ModeZigZag, _line_num, 0),
58 :
59 : AP_GROUPEND
60 : };
61 :
62 1001 : ModeZigZag::ModeZigZag(void) : Mode()
63 : {
64 1001 : AP_Param::setup_object_defaults(this, var_info);
65 1001 : }
66 :
67 : // initialise zigzag controller
68 4 : bool ModeZigZag::init(bool ignore_checks)
69 : {
70 : // convert pilot input to lean angles
71 : float target_roll_rad, target_pitch_rad;
72 4 : get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, loiter_nav->get_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
73 :
74 : // process pilot's roll and pitch input
75 4 : loiter_nav->set_pilot_desired_acceleration_rad(target_roll_rad, target_pitch_rad);
76 :
77 4 : loiter_nav->init_target();
78 :
79 : // set vertical speed and acceleration limits
80 4 : pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
81 4 : pos_control->D_set_correction_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
82 :
83 : // initialise the vertical position controller
84 4 : if (!pos_control->D_is_active()) {
85 0 : pos_control->D_init_controller();
86 : }
87 :
88 : // initialise waypoint state
89 4 : stage = STORING_POINTS;
90 4 : dest_A_ne_m.zero();
91 4 : dest_B_ne_m.zero();
92 :
93 : // initialize zigzag auto
94 4 : init_auto();
95 :
96 4 : return true;
97 : }
98 :
99 : // perform cleanup required when leaving zigzag mode
100 4 : void ModeZigZag::exit()
101 : {
102 : // The sprayer will stop if the flight mode is changed from ZigZag to other
103 4 : spray(false);
104 4 : }
105 :
106 : // run the zigzag controller
107 : // should be called at 100hz or more
108 136401 : void ModeZigZag::run()
109 : {
110 : // set vertical speed and acceleration limits
111 136401 : pos_control->D_set_max_speed_accel_m(get_pilot_speed_dn_ms(), get_pilot_speed_up_ms(), get_pilot_accel_D_mss());
112 :
113 : // set the direction and the total number of lines
114 136401 : zigzag_direction = (Direction)constrain_int16(_direction, 0, 3);
115 136401 : line_num = constrain_int16(_line_num, ZIGZAG_LINE_INFINITY, INT16_MAX);
116 :
117 : // auto control
118 136401 : if (stage == AUTO) {
119 65815 : if (is_disarmed_or_landed() || !motors->get_interlock()) {
120 : // vehicle should be under manual control when disarmed or landed
121 0 : return_to_manual_control(false);
122 65815 : } else if (reached_destination()) {
123 : // if vehicle has reached destination switch to manual control or moving to A or B
124 8 : AP_Notify::events.waypoint_complete = 1;
125 8 : if (is_auto) {
126 0 : if (line_num == ZIGZAG_LINE_INFINITY || line_count < line_num) {
127 0 : if (auto_stage == AutoState::SIDEWAYS) {
128 0 : save_or_move_to_destination((ab_dest_stored == Destination::A) ? Destination::B : Destination::A);
129 : } else {
130 : // spray off
131 0 : spray(false);
132 0 : move_to_side();
133 : }
134 : } else {
135 0 : init_auto();
136 0 : return_to_manual_control(true);
137 : }
138 : } else {
139 8 : return_to_manual_control(true);
140 : }
141 : } else {
142 65807 : auto_control();
143 : }
144 : }
145 :
146 : // manual control
147 136401 : if (stage == STORING_POINTS || stage == MANUAL_REGAIN) {
148 : // receive pilot's inputs, do position and attitude control
149 70594 : manual_control();
150 : }
151 136401 : }
152 :
153 : // save current position as A or B. If both A and B have been saved move to the one specified
154 28 : void ModeZigZag::save_or_move_to_destination(Destination ab_dest)
155 : {
156 : // get current position as an offset from EKF origin
157 28 : const Vector2p curr_pos_ned_m = pos_control->get_pos_desired_NED_m().xy();
158 :
159 : // handle state machine changes
160 28 : switch (stage) {
161 :
162 8 : case STORING_POINTS:
163 8 : if (ab_dest == Destination::A) {
164 : // store point A
165 4 : dest_A_ne_m = curr_pos_ned_m;
166 4 : gcs().send_text(MAV_SEVERITY_INFO, "%s: point A stored", name());
167 4 : LOGGER_WRITE_EVENT(LogEvent::ZIGZAG_STORE_A);
168 : } else {
169 : // store point B
170 4 : dest_B_ne_m = curr_pos_ned_m;
171 4 : gcs().send_text(MAV_SEVERITY_INFO, "%s: point B stored", name());
172 4 : LOGGER_WRITE_EVENT(LogEvent::ZIGZAG_STORE_B);
173 : }
174 : // if both A and B have been stored advance state
175 8 : if (!dest_A_ne_m.is_zero() && !dest_B_ne_m.is_zero() && !is_zero((dest_B_ne_m - dest_A_ne_m).length_squared())) {
176 4 : stage = MANUAL_REGAIN;
177 4 : spray(false);
178 4 : } else if (!dest_A_ne_m.is_zero() || !dest_B_ne_m.is_zero()) {
179 : // if only A or B have been stored, spray on
180 4 : spray(true);
181 : }
182 8 : break;
183 :
184 20 : case AUTO:
185 : case MANUAL_REGAIN:
186 : // A and B have been defined, move vehicle to destination A or B
187 20 : Vector3p next_dest_ned_m;
188 : bool is_terrain_alt;
189 20 : if (calculate_next_dest_m(ab_dest, stage == AUTO, next_dest_ned_m, is_terrain_alt)) {
190 20 : wp_nav->wp_and_spline_init_m();
191 20 : if (wp_nav->set_wp_destination_NED_m(next_dest_ned_m, is_terrain_alt)) {
192 20 : stage = AUTO;
193 20 : auto_stage = AutoState::AB_MOVING;
194 20 : ab_dest_stored = ab_dest;
195 : // spray on while moving to A or B
196 20 : spray(true);
197 20 : reach_wp_time_ms = 0;
198 20 : if (is_auto == false || line_num == ZIGZAG_LINE_INFINITY) {
199 20 : gcs().send_text(MAV_SEVERITY_INFO, "%s: moving to %s", name(), (ab_dest == Destination::A) ? "A" : "B");
200 : } else {
201 0 : line_count++;
202 0 : gcs().send_text(MAV_SEVERITY_INFO, "%s: moving to %s (line %d/%d)", name(), (ab_dest == Destination::A) ? "A" : "B", line_count, line_num);
203 : }
204 : }
205 : }
206 20 : break;
207 : }
208 28 : }
209 :
210 0 : void ModeZigZag::move_to_side()
211 : {
212 0 : if (!dest_A_ne_m.is_zero() && !dest_B_ne_m.is_zero() && !is_zero((dest_B_ne_m - dest_A_ne_m).length_squared())) {
213 0 : Vector3p next_dest_ned_m;
214 : bool is_terrain_alt;
215 0 : if (calculate_side_dest_m(next_dest_ned_m, is_terrain_alt)) {
216 0 : wp_nav->wp_and_spline_init_m();
217 0 : if (wp_nav->set_wp_destination_NED_m(next_dest_ned_m, is_terrain_alt)) {
218 0 : stage = AUTO;
219 0 : auto_stage = AutoState::SIDEWAYS;
220 0 : current_dest_ned_m = next_dest_ned_m;
221 0 : current_is_terr_alt = is_terrain_alt;
222 0 : reach_wp_time_ms = 0;
223 0 : char const *dir[] = {"forward", "right", "backward", "left"};
224 0 : gcs().send_text(MAV_SEVERITY_INFO, "%s: moving to %s", name(), dir[(uint8_t)zigzag_direction]);
225 : }
226 : }
227 : }
228 0 : }
229 :
230 : // return manual control to the pilot
231 32 : void ModeZigZag::return_to_manual_control(bool maintain_target)
232 : {
233 32 : if (stage == AUTO) {
234 16 : stage = MANUAL_REGAIN;
235 16 : spray(false);
236 16 : loiter_nav->clear_pilot_desired_acceleration();
237 16 : if (maintain_target) {
238 8 : const Vector3p& wp_dest_ned_m = wp_nav->get_wp_destination_NED_m();
239 8 : loiter_nav->init_target_m(wp_dest_ned_m.xy());
240 : #if AP_RANGEFINDER_ENABLED
241 8 : if (copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy()) {
242 0 : copter.surface_tracking.external_init();
243 : }
244 : #endif
245 : } else {
246 8 : loiter_nav->init_target();
247 : }
248 16 : is_auto = false;
249 16 : gcs().send_text(MAV_SEVERITY_INFO, "%s: manual control", name());
250 : }
251 32 : }
252 :
253 : // fly the vehicle to closest point on line perpendicular to dest_A_ne_m or dest_B_ne_m
254 65807 : void ModeZigZag::auto_control()
255 : {
256 : // process pilot's yaw input
257 65807 : const float target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
258 :
259 : // set motors to full range
260 65807 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
261 :
262 : // run waypoint controller
263 65807 : const bool wpnav_ok = wp_nav->update_wpnav();
264 :
265 : // WP_Nav has set the vertical position control targets
266 : // run the vertical position controller and set output throttle
267 65807 : pos_control->D_update_controller();
268 :
269 : // call attitude controller
270 : // roll & pitch from waypoint controller, yaw rate from pilot
271 65807 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(wp_nav->get_roll_rad(), wp_nav->get_pitch_rad(), target_yaw_rate_rads);
272 :
273 : // if wpnav failed (because of lack of terrain data) switch back to pilot control for next iteration
274 65807 : if (!wpnav_ok) {
275 0 : return_to_manual_control(false);
276 : }
277 65807 : }
278 :
279 : // manual_control - process manual control
280 70594 : void ModeZigZag::manual_control()
281 : {
282 70594 : float target_yaw_rate_rads = 0.0f;
283 70594 : float target_climb_rate_ms = 0.0f;
284 :
285 : // process pilot inputs unless we are in radio failsafe
286 : float target_roll_rad, target_pitch_rad;
287 :
288 : // convert pilot input to lean angles
289 70594 : get_pilot_desired_lean_angles_rad(target_roll_rad, target_pitch_rad, loiter_nav->get_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad());
290 :
291 : // process pilot's roll and pitch input
292 70594 : loiter_nav->set_pilot_desired_acceleration_rad(target_roll_rad, target_pitch_rad);
293 :
294 : // get pilot's desired yaw rate
295 70594 : target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
296 :
297 : // get pilot desired climb rate
298 70594 : target_climb_rate_ms = get_pilot_desired_climb_rate_ms();
299 :
300 : // relax loiter target if we might be landed
301 70594 : if (copter.ap.land_complete_maybe) {
302 0 : loiter_nav->soften_for_landing();
303 : }
304 :
305 : // Loiter State Machine Determination
306 70594 : AltHoldModeState althold_state = get_alt_hold_state_D_ms(target_climb_rate_ms);
307 :
308 : // althold state machine
309 70594 : switch (althold_state) {
310 :
311 0 : case AltHoldModeState::MotorStopped:
312 0 : attitude_control->reset_rate_controller_I_terms();
313 0 : attitude_control->reset_yaw_target_and_rate();
314 0 : pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero
315 0 : loiter_nav->init_target();
316 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(loiter_nav->get_roll_rad(), loiter_nav->get_pitch_rad(), target_yaw_rate_rads);
317 0 : break;
318 :
319 0 : case AltHoldModeState::Takeoff:
320 : // initiate take-off
321 0 : if (!takeoff.running()) {
322 0 : takeoff.start_m(constrain_float(g2.pilot_takeoff_alt_m, 0.0, 10.0));
323 : }
324 :
325 : // get avoidance adjusted climb rate
326 0 : target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
327 :
328 : // run loiter controller
329 0 : loiter_nav->update();
330 :
331 : // call attitude controller
332 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(loiter_nav->get_roll_rad(), loiter_nav->get_pitch_rad(), target_yaw_rate_rads);
333 :
334 : // set position controller targets adjusted for pilot input
335 0 : takeoff.do_pilot_takeoff_ms(target_climb_rate_ms);
336 0 : break;
337 :
338 0 : case AltHoldModeState::Landed_Ground_Idle:
339 0 : attitude_control->reset_yaw_target_and_rate();
340 : FALLTHROUGH;
341 :
342 0 : case AltHoldModeState::Landed_Pre_Takeoff:
343 0 : attitude_control->reset_rate_controller_I_terms_smoothly();
344 0 : loiter_nav->init_target();
345 0 : attitude_control->input_thrust_vector_rate_heading_rads(loiter_nav->get_thrust_vector(), target_yaw_rate_rads);
346 0 : pos_control->D_relax_controller(0.0f); // forces throttle output to decay to zero
347 0 : break;
348 :
349 70594 : case AltHoldModeState::Flying:
350 : // run loiter controller
351 70594 : loiter_nav->update();
352 :
353 : // call attitude controller
354 70594 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(loiter_nav->get_roll_rad(), loiter_nav->get_pitch_rad(), target_yaw_rate_rads);
355 :
356 : // get avoidance adjusted climb rate
357 70594 : target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
358 :
359 : #if AP_RANGEFINDER_ENABLED
360 : // update the vertical offset based on the surface measurement
361 70594 : copter.surface_tracking.update_surface_offset();
362 : #endif
363 :
364 : // Send the commanded climb rate to the position controller
365 70594 : pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms);
366 70594 : break;
367 : }
368 :
369 : // run the vertical position controller and set output throttle
370 70594 : pos_control->D_update_controller();
371 70594 : }
372 :
373 : // return true if vehicle is within a small area around the destination
374 65815 : bool ModeZigZag::reached_destination()
375 : {
376 : // check if wp_nav believes it has reached the destination
377 65815 : if (!wp_nav->reached_wp_destination()) {
378 65807 : return false;
379 : }
380 :
381 : // check distance to destination
382 8 : if (wp_nav->get_wp_distance_to_destination_m() > ZIGZAG_WP_RADIUS_M) {
383 0 : return false;
384 : }
385 :
386 : // wait at time which is set in zigzag_wp_delay
387 8 : uint32_t now = AP_HAL::millis();
388 8 : if (reach_wp_time_ms == 0) {
389 8 : reach_wp_time_ms = now;
390 : }
391 8 : return ((now - reach_wp_time_ms) >= (uint16_t)constrain_int16(_wp_delay_s, 0, 127) * 1000);
392 : }
393 :
394 : // calculate next destination according to vector A-B and current position
395 : // use_wpnav_alt should be true if waypoint controller's altitude target should be used, false for position control or current altitude target
396 : // is_terrain_alt is returned as true if the next_dest_ned_m.z is relative to the terrain surface
397 20 : bool ModeZigZag::calculate_next_dest_m(Destination ab_dest, bool use_wpnav_alt, Vector3p& next_dest_ned_m, bool& is_terrain_alt) const
398 : {
399 : // define start_pos_ne_m as either destination A or B
400 20 : Vector2p start_pos_ne_m = (ab_dest == Destination::A) ? dest_A_ne_m : dest_B_ne_m;
401 :
402 : // calculate vector from A to B
403 20 : Vector2f AB_diff_ne_m = (dest_B_ne_m - dest_A_ne_m).tofloat();
404 :
405 : // check distance between A and B
406 20 : if (is_zero(AB_diff_ne_m.length_squared())) {
407 0 : return false;
408 : }
409 :
410 : // get distance from vehicle to start_pos_ne_m
411 20 : const Vector2p curr_pos_ne_m = pos_control->get_pos_desired_NED_m().xy();
412 20 : Vector2p veh_to_start_pos_ne_m = (curr_pos_ne_m - start_pos_ne_m);
413 :
414 : // lengthen AB_diff_ne_m so that it is at least as long as vehicle is from start point
415 : // we need to ensure that the lines perpendicular to AB are long enough to reach the vehicle
416 20 : float scalar = 1.0f;
417 20 : if (veh_to_start_pos_ne_m.length_squared() > AB_diff_ne_m.length_squared()) {
418 20 : scalar = veh_to_start_pos_ne_m.length() / AB_diff_ne_m.length();
419 : }
420 :
421 : // create a line perpendicular to AB but originating at start_pos_ne_m
422 20 : Vector2p perp1 = start_pos_ne_m + Vector2p(-AB_diff_ne_m[1] * scalar, AB_diff_ne_m[0] * scalar);
423 20 : Vector2p perp2 = start_pos_ne_m + Vector2p(AB_diff_ne_m[1] * scalar, -AB_diff_ne_m[0] * scalar);
424 :
425 : // find the closest point on the perpendicular line
426 20 : const Vector2p closest2d_ne_m = Vector2p::closest_point(curr_pos_ne_m, perp1, perp2);
427 20 : next_dest_ned_m.x = closest2d_ne_m.x;
428 20 : next_dest_ned_m.y = closest2d_ne_m.y;
429 :
430 20 : if (use_wpnav_alt) {
431 : // get altitude target from waypoint controller
432 0 : is_terrain_alt = wp_nav->origin_and_destination_are_terrain_alt();
433 0 : next_dest_ned_m.z = wp_nav->get_wp_destination_NED_m().z;
434 : } else {
435 20 : is_terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy();
436 20 : next_dest_ned_m.z = pos_control->get_pos_desired_NED_m().z;
437 20 : if (!is_terrain_alt) {
438 20 : next_dest_ned_m.z += pos_control->get_pos_terrain_D_m();
439 : }
440 : }
441 :
442 20 : return true;
443 : }
444 :
445 : // calculate side destination according to vertical vector A-B and current position
446 : // is_terrain_alt is returned as true if the next_dest_ned_m.z is relative to the terrain surfaces
447 0 : bool ModeZigZag::calculate_side_dest_m(Vector3p& next_dest_ned_m, bool& is_terrain_alt) const
448 : {
449 : // calculate vector from A to B
450 0 : Vector2f AB_diff_ne_m = (dest_B_ne_m - dest_A_ne_m).tofloat();
451 :
452 : // calculate a vertical right or left vector for AB from the current yaw direction
453 0 : Vector2f AB_side_ne_m;
454 0 : if (zigzag_direction == Direction::RIGHT || zigzag_direction == Direction::LEFT) {
455 0 : float yaw_ab_sign = (-ahrs.sin_yaw() * AB_diff_ne_m[1]) + (ahrs.cos_yaw() * -AB_diff_ne_m[0]);
456 0 : if (is_positive(yaw_ab_sign * (zigzag_direction == Direction::RIGHT ? 1 : -1))) {
457 0 : AB_side_ne_m = Vector2f(AB_diff_ne_m[1], -AB_diff_ne_m[0]);
458 : } else {
459 0 : AB_side_ne_m = Vector2f(-AB_diff_ne_m[1], AB_diff_ne_m[0]);
460 : }
461 0 : } else {
462 0 : float yaw_ab_sign = (ahrs.cos_yaw() * AB_diff_ne_m[1]) + (ahrs.sin_yaw() * -AB_diff_ne_m[0]);
463 0 : if (is_positive(yaw_ab_sign * (zigzag_direction == Direction::FORWARD ? 1 : -1))) {
464 0 : AB_side_ne_m = Vector2f(AB_diff_ne_m[1], -AB_diff_ne_m[0]);
465 : } else {
466 0 : AB_side_ne_m = Vector2f(-AB_diff_ne_m[1], AB_diff_ne_m[0]);
467 : }
468 : }
469 :
470 : // check distance the vertical vector between A and B
471 0 : float AB_side_ne_m_length = AB_side_ne_m.length();
472 0 : if (is_zero(AB_side_ne_m_length)) {
473 0 : return false;
474 : }
475 :
476 : // adjust AB_side_ne_m length to zigzag_side_dist
477 0 : float scalar = constrain_float(_side_dist_m, 0.1, 100.0) / AB_side_ne_m_length;
478 :
479 : // get distance from vehicle to start_pos_ne_m
480 0 : const Vector3p curr_pos_ned_m = pos_control->get_pos_desired_NED_m();
481 0 : next_dest_ned_m.xy() = curr_pos_ned_m.xy() + (AB_side_ne_m.topostype() * scalar);
482 :
483 : // if we have a downward facing range finder then use terrain altitude targets
484 0 : is_terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy();
485 0 : next_dest_ned_m.z = curr_pos_ned_m.z;
486 :
487 0 : return true;
488 : }
489 :
490 : // run zigzag auto feature which is automate both AB and sideways
491 0 : void ModeZigZag::run_auto()
492 : {
493 : // exit immediately if we are disabled
494 0 : if (!_auto_enabled) {
495 0 : return;
496 : }
497 :
498 : // make sure both A and B point are registered and not when moving to A or B
499 0 : if (stage != MANUAL_REGAIN) {
500 0 : return;
501 : }
502 :
503 0 : is_auto = true;
504 : // resume if zigzag auto is suspended
505 0 : if (is_suspended && line_count <= line_num) {
506 : // resume the stage when it was suspended
507 0 : if (auto_stage == AutoState::AB_MOVING) {
508 0 : line_count--;
509 0 : save_or_move_to_destination(ab_dest_stored);
510 0 : } else if (auto_stage == AutoState::SIDEWAYS) {
511 0 : wp_nav->wp_and_spline_init_m();
512 0 : if (wp_nav->set_wp_destination_NED_m(current_dest_ned_m, current_is_terr_alt)) {
513 0 : stage = AUTO;
514 0 : reach_wp_time_ms = 0;
515 0 : char const *dir[] = {"forward", "right", "backward", "left"};
516 0 : gcs().send_text(MAV_SEVERITY_INFO, "%s: moving to %s", name(), dir[(uint8_t)zigzag_direction]);
517 : }
518 : }
519 0 : } else {
520 0 : move_to_side();
521 : }
522 : }
523 :
524 : // suspend zigzag auto
525 0 : void ModeZigZag::suspend_auto()
526 : {
527 : // exit immediately if we are disabled
528 0 : if (!_auto_enabled) {
529 0 : return;
530 : }
531 :
532 0 : if (auto_stage != AutoState::MANUAL) {
533 0 : is_suspended = true;
534 0 : return_to_manual_control(true);
535 : }
536 : }
537 :
538 : // initialize zigzag auto
539 56 : void ModeZigZag::init_auto()
540 : {
541 56 : is_auto = false;
542 56 : auto_stage = AutoState::MANUAL;
543 56 : line_count = 0;
544 56 : is_suspended = false;
545 56 : }
546 :
547 : // spray on / off
548 48 : void ModeZigZag::spray(bool b)
549 : {
550 : #if HAL_SPRAYER_ENABLED
551 48 : if (_spray_enabled) {
552 0 : copter.sprayer.run(b);
553 : }
554 : #endif
555 48 : }
556 :
557 2045 : float ModeZigZag::wp_distance_m() const
558 : {
559 2045 : return is_auto ? wp_nav->get_wp_distance_to_destination_m() : 0.0f;
560 : }
561 2045 : float ModeZigZag::wp_bearing_deg() const
562 : {
563 2045 : return is_auto ? degrees(wp_nav->get_wp_bearing_to_destination_rad()) : 0;
564 : }
565 2045 : float ModeZigZag::crosstrack_error_m() const
566 : {
567 2045 : return is_auto ? wp_nav->crosstrack_error_m() : 0;
568 : }
569 :
570 : #endif // MODE_ZIGZAG_ENABLED
|
