Line data Source code
1 : #include "Rover.h"
2 :
3 : #define AUTO_GUIDED_SEND_TARGET_MS 1000
4 :
5 40 : bool ModeAuto::_enter()
6 : {
7 : // fail to enter auto if no mission commands
8 40 : if (!mission.present()) {
9 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "No Mission. Can't set AUTO.");
10 0 : return false;
11 : }
12 :
13 : // initialise waypoint navigation library
14 40 : g2.wp_nav.init();
15 :
16 : // other initialisation
17 40 : auto_triggered = false;
18 :
19 : // clear guided limits
20 40 : rover.mode_guided.limit_clear();
21 :
22 : // initialise submode to stop or loiter
23 40 : if (rover.is_boat()) {
24 7 : if (!start_loiter()) {
25 0 : start_stop();
26 : }
27 : } else {
28 33 : start_stop();
29 : }
30 :
31 : // set flag to start mission
32 40 : waiting_to_start = true;
33 :
34 40 : return true;
35 : }
36 :
37 34 : void ModeAuto::_exit()
38 : {
39 : // stop running the mission
40 34 : if (mission.state() == AP_Mission::MISSION_RUNNING) {
41 14 : mission.stop();
42 : }
43 34 : }
44 :
45 138859 : void ModeAuto::update()
46 : {
47 : // check if mission exists (due to being cleared while disarmed in AUTO,
48 : // if no mission, then stop...needs mode change out of AUTO, mission load,
49 : // and change back to AUTO to run a mission at this point
50 138859 : if (!rover.arming.is_armed_and_safety_off() && !mission.present()) {
51 477 : start_stop();
52 : }
53 : // start or update mission
54 138859 : if (waiting_to_start) {
55 : // don't start the mission until we have an origin
56 2735 : Location loc;
57 2735 : if (ahrs.get_origin(loc)) {
58 : // start/resume the mission (based on MIS_RESTART parameter)
59 40 : mission.start_or_resume();
60 40 : waiting_to_start = false;
61 :
62 : // initialise mission change check
63 40 : IGNORE_RETURN(mis_change_detector.check_for_mission_change());
64 : }
65 : } else {
66 : // check for mission changes
67 136124 : if (mis_change_detector.check_for_mission_change()) {
68 : // if mission is running restart the current command if it is a waypoint command
69 11 : if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_submode == SubMode::WP)) {
70 0 : if (mission.restart_current_nav_cmd()) {
71 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command");
72 : } else {
73 : // failed to restart mission for some reason
74 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Auto mission changed but failed to restart command");
75 : }
76 : }
77 : }
78 :
79 136124 : mission.update();
80 : }
81 :
82 138859 : switch (_submode) {
83 91192 : case SubMode::WP:
84 : {
85 : // boats loiter once the waypoint is reached
86 91192 : bool keep_navigating = true;
87 91192 : if (rover.is_boat() && g2.wp_nav.reached_destination() && !g2.wp_nav.is_fast_waypoint()) {
88 0 : keep_navigating = !start_loiter();
89 : }
90 :
91 : // update navigation controller
92 91192 : if (keep_navigating) {
93 91192 : navigate_to_waypoint();
94 : }
95 91192 : break;
96 : }
97 :
98 0 : case SubMode::HeadingAndSpeed:
99 : {
100 0 : if (!_reached_heading) {
101 : // run steering and throttle controllers
102 0 : calc_steering_to_heading(_desired_yaw_cd);
103 0 : calc_throttle(calc_speed_nudge(_desired_speed, is_negative(_desired_speed)), true);
104 : // check if we have reached within 5 degrees of target
105 0 : _reached_heading = (fabsf(_desired_yaw_cd - ahrs.yaw_sensor) < 500);
106 : } else {
107 : // we have reached the destination so stay here
108 0 : if (rover.is_boat()) {
109 0 : if (!start_loiter()) {
110 0 : stop_vehicle();
111 : }
112 : } else {
113 0 : stop_vehicle();
114 : }
115 : }
116 0 : break;
117 : }
118 :
119 22909 : case SubMode::RTL:
120 22909 : rover.mode_rtl.update();
121 22909 : break;
122 :
123 1611 : case SubMode::Loiter:
124 1611 : rover.mode_loiter.update();
125 1611 : break;
126 :
127 500 : case SubMode::Guided:
128 : {
129 : // send location target to offboard navigation system
130 500 : send_guided_position_target();
131 500 : rover.mode_guided.update();
132 500 : break;
133 : }
134 :
135 20669 : case SubMode::Stop:
136 20669 : stop_vehicle();
137 20669 : break;
138 :
139 0 : case SubMode::NavScriptTime:
140 0 : rover.mode_guided.update();
141 0 : break;
142 :
143 1978 : case SubMode::Circle:
144 1978 : g2.mode_circle.update();
145 1978 : break;
146 : }
147 138859 : }
148 :
149 91192 : void ModeAuto::calc_throttle(float target_speed, bool avoidance_enabled)
150 : {
151 : // If not autostarting set the throttle to minimum
152 91192 : if (!check_trigger()) {
153 0 : stop_vehicle();
154 0 : return;
155 : }
156 91192 : Mode::calc_throttle(target_speed, avoidance_enabled);
157 : }
158 :
159 : // return heading (in degrees) to target destination (aka waypoint)
160 58310 : float ModeAuto::wp_bearing() const
161 : {
162 58310 : switch (_submode) {
163 38304 : case SubMode::WP:
164 38304 : return g2.wp_nav.wp_bearing_cd() * 0.01f;
165 8667 : case SubMode::HeadingAndSpeed:
166 : case SubMode::Stop:
167 8667 : return 0.0f;
168 9633 : case SubMode::RTL:
169 9633 : return rover.mode_rtl.wp_bearing();
170 668 : case SubMode::Loiter:
171 668 : return rover.mode_loiter.wp_bearing();
172 210 : case SubMode::Guided:
173 : case SubMode::NavScriptTime:
174 210 : return rover.mode_guided.wp_bearing();
175 828 : case SubMode::Circle:
176 828 : return g2.mode_circle.wp_bearing();
177 : }
178 :
179 : // this line should never be reached
180 0 : return 0.0f;
181 : }
182 :
183 : // return short-term target heading in degrees (i.e. target heading back to line between waypoints)
184 55525 : float ModeAuto::nav_bearing() const
185 : {
186 55525 : switch (_submode) {
187 36476 : case SubMode::WP:
188 36476 : return g2.wp_nav.nav_bearing_cd() * 0.01f;
189 8248 : case SubMode::HeadingAndSpeed:
190 : case SubMode::Stop:
191 8248 : return 0.0f;
192 9175 : case SubMode::RTL:
193 9175 : return rover.mode_rtl.nav_bearing();
194 637 : case SubMode::Loiter:
195 637 : return rover.mode_loiter.nav_bearing();
196 200 : case SubMode::Guided:
197 : case SubMode::NavScriptTime:
198 200 : return rover.mode_guided.nav_bearing();
199 789 : case SubMode::Circle:
200 789 : return g2.mode_circle.nav_bearing();
201 : }
202 :
203 : // this line should never be reached
204 0 : return 0.0f;
205 : }
206 :
207 : // return cross track error (i.e. vehicle's distance from the line between waypoints)
208 58310 : float ModeAuto::crosstrack_error_m() const
209 : {
210 58310 : switch (_submode) {
211 38304 : case SubMode::WP:
212 38304 : return g2.wp_nav.crosstrack_error_m();
213 8667 : case SubMode::HeadingAndSpeed:
214 : case SubMode::Stop:
215 8667 : return 0.0f;
216 9633 : case SubMode::RTL:
217 9633 : return rover.mode_rtl.crosstrack_error_m();
218 668 : case SubMode::Loiter:
219 668 : return rover.mode_loiter.crosstrack_error_m();
220 210 : case SubMode::Guided:
221 : case SubMode::NavScriptTime:
222 210 : return rover.mode_guided.crosstrack_error_m();
223 828 : case SubMode::Circle:
224 828 : return g2.mode_circle.crosstrack_error_m();
225 : }
226 :
227 : // this line should never be reached
228 0 : return 0.0f;
229 : }
230 :
231 : // return desired lateral acceleration
232 27773 : float ModeAuto::get_desired_lat_accel() const
233 : {
234 27773 : switch (_submode) {
235 18242 : case SubMode::WP:
236 18242 : return g2.wp_nav.get_lat_accel();
237 4132 : case SubMode::HeadingAndSpeed:
238 : case SubMode::Stop:
239 4132 : return 0.0f;
240 4581 : case SubMode::RTL:
241 4581 : return rover.mode_rtl.get_desired_lat_accel();
242 322 : case SubMode::Loiter:
243 322 : return rover.mode_loiter.get_desired_lat_accel();
244 100 : case SubMode::Guided:
245 : case SubMode::NavScriptTime:
246 100 : return rover.mode_guided.get_desired_lat_accel();
247 396 : case SubMode::Circle:
248 396 : return g2.mode_circle.get_desired_lat_accel();
249 : }
250 :
251 : // this line should never be reached
252 0 : return 0.0f;
253 : }
254 :
255 : // return straight-line distance (in meters) to destination
256 58310 : float ModeAuto::get_distance_to_destination() const
257 : {
258 58310 : switch (_submode) {
259 38304 : case SubMode::WP:
260 38304 : return _distance_to_destination;
261 8667 : case SubMode::HeadingAndSpeed:
262 : case SubMode::Stop:
263 : // no valid distance so return zero
264 8667 : return 0.0f;
265 9633 : case SubMode::RTL:
266 9633 : return rover.mode_rtl.get_distance_to_destination();
267 668 : case SubMode::Loiter:
268 668 : return rover.mode_loiter.get_distance_to_destination();
269 210 : case SubMode::Guided:
270 : case SubMode::NavScriptTime:
271 210 : return rover.mode_guided.get_distance_to_destination();
272 828 : case SubMode::Circle:
273 828 : return g2.mode_circle.get_distance_to_destination();
274 : }
275 :
276 : // this line should never be reached
277 0 : return 0.0f;
278 : }
279 :
280 : // get desired location
281 27753 : bool ModeAuto::get_desired_location(Location& destination) const
282 : {
283 27753 : switch (_submode) {
284 18235 : case SubMode::WP:
285 18235 : if (g2.wp_nav.is_destination_valid()) {
286 18235 : destination = g2.wp_nav.get_oa_destination();
287 18235 : return true;
288 : }
289 0 : return false;
290 4116 : case SubMode::HeadingAndSpeed:
291 : case SubMode::Stop:
292 : // no desired location for this submode
293 4116 : return false;
294 4594 : case SubMode::RTL:
295 4594 : return rover.mode_rtl.get_desired_location(destination);
296 315 : case SubMode::Loiter:
297 315 : return rover.mode_loiter.get_desired_location(destination);
298 100 : case SubMode::Guided:
299 : case SubMode::NavScriptTime:
300 100 : return rover.mode_guided.get_desired_location(destination);
301 393 : case SubMode::Circle:
302 393 : return g2.mode_circle.get_desired_location(destination);
303 : }
304 :
305 : // we should never reach here but just in case
306 0 : return false;
307 : }
308 :
309 : // set desired location to drive to
310 125 : bool ModeAuto::set_desired_location(const Location &destination, Location next_destination)
311 : {
312 : // call parent
313 125 : if (!Mode::set_desired_location(destination, next_destination)) {
314 0 : return false;
315 : }
316 :
317 125 : _submode = SubMode::WP;
318 :
319 125 : return true;
320 : }
321 :
322 : // return true if vehicle has reached or even passed destination
323 114074 : bool ModeAuto::reached_destination() const
324 : {
325 114074 : switch (_submode) {
326 91176 : case SubMode::WP:
327 91176 : return g2.wp_nav.reached_destination();
328 : break;
329 2 : case SubMode::HeadingAndSpeed:
330 : case SubMode::Stop:
331 : // always return true because this is the safer option to allow missions to continue
332 2 : return true;
333 : break;
334 22896 : case SubMode::RTL:
335 22896 : return rover.mode_rtl.reached_destination();
336 : break;
337 0 : case SubMode::Loiter:
338 0 : return rover.mode_loiter.reached_destination();
339 : break;
340 0 : case SubMode::Guided:
341 : case SubMode::NavScriptTime:
342 0 : return rover.mode_guided.reached_destination();
343 0 : case SubMode::Circle:
344 0 : return g2.mode_circle.reached_destination();
345 : }
346 :
347 : // we should never reach here but just in case, return true to allow missions to continue
348 0 : return true;
349 : }
350 :
351 : // set desired speed in m/s
352 9 : bool ModeAuto::set_desired_speed(float speed_ms)
353 : {
354 9 : switch (_submode) {
355 9 : case SubMode::WP:
356 : case SubMode::Stop:
357 9 : return g2.wp_nav.set_speed_max(speed_ms);
358 0 : case SubMode::HeadingAndSpeed:
359 0 : _desired_speed = speed_ms;
360 0 : return true;
361 0 : case SubMode::RTL:
362 0 : return rover.mode_rtl.set_desired_speed(speed_ms);
363 0 : case SubMode::Loiter:
364 0 : return rover.mode_loiter.set_desired_speed(speed_ms);
365 0 : case SubMode::Guided:
366 : case SubMode::NavScriptTime:
367 0 : return rover.mode_guided.set_desired_speed(speed_ms);
368 0 : case SubMode::Circle:
369 0 : return g2.mode_circle.set_desired_speed(speed_ms);
370 : }
371 0 : return false;
372 : }
373 :
374 : // start RTL (within auto)
375 18 : void ModeAuto::start_RTL()
376 : {
377 18 : if (rover.mode_rtl.enter()) {
378 18 : _submode = SubMode::RTL;
379 : }
380 18 : }
381 :
382 : // lua scripts use this to retrieve the contents of the active command
383 0 : bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4)
384 : {
385 : #if AP_SCRIPTING_ENABLED
386 0 : if (_submode == SubMode::NavScriptTime) {
387 0 : id = nav_scripting.id;
388 0 : cmd = nav_scripting.command;
389 0 : arg1 = nav_scripting.arg1;
390 0 : arg2 = nav_scripting.arg2;
391 0 : arg3 = nav_scripting.arg3;
392 0 : arg4 = nav_scripting.arg4;
393 0 : return true;
394 : }
395 : #endif
396 0 : return false;
397 : }
398 :
399 : // lua scripts use this to indicate when they have complete the command
400 0 : void ModeAuto::nav_script_time_done(uint16_t id)
401 : {
402 : #if AP_SCRIPTING_ENABLED
403 0 : if ((_submode == SubMode::NavScriptTime) && (id == nav_scripting.id)) {
404 0 : nav_scripting.done = true;
405 : }
406 : #endif
407 0 : }
408 :
409 : // check for triggering of start of auto mode
410 91192 : bool ModeAuto::check_trigger(void)
411 : {
412 : // check for user pressing the auto trigger to off
413 91192 : if (auto_triggered && g.auto_trigger_pin != -1 && rover.check_digital_pin(g.auto_trigger_pin) == 1) {
414 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AUTO triggered off");
415 0 : auto_triggered = false;
416 0 : return false;
417 : }
418 :
419 : // if already triggered, then return true, so you don't
420 : // need to hold the switch down
421 91192 : if (auto_triggered) {
422 91154 : return true;
423 : }
424 :
425 : // return true if auto trigger and kickstart are disabled
426 38 : if (g.auto_trigger_pin == -1 && is_zero(g.auto_kickstart)) {
427 : // no trigger configured - let's go!
428 38 : auto_triggered = true;
429 38 : return true;
430 : }
431 :
432 : // check if trigger pin has been pushed
433 0 : if (g.auto_trigger_pin != -1 && rover.check_digital_pin(g.auto_trigger_pin) == 0) {
434 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Triggered AUTO with pin");
435 0 : auto_triggered = true;
436 0 : return true;
437 : }
438 :
439 : // check if mission is started by giving vehicle a kick with acceleration > AUTO_KICKSTART
440 0 : if (!is_zero(g.auto_kickstart)) {
441 0 : const float xaccel = rover.ins.get_accel().x;
442 0 : if (xaccel >= g.auto_kickstart) {
443 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Triggered AUTO xaccel=%.1f", static_cast<double>(xaccel));
444 0 : auto_triggered = true;
445 0 : return true;
446 : }
447 : }
448 :
449 0 : return false;
450 : }
451 :
452 10 : bool ModeAuto::start_loiter()
453 : {
454 10 : if (rover.mode_loiter.enter()) {
455 10 : _submode = SubMode::Loiter;
456 10 : return true;
457 : }
458 0 : return false;
459 : }
460 :
461 : // hand over control to external navigation controller in AUTO mode
462 1 : void ModeAuto::start_guided(const Location& loc)
463 : {
464 1 : if (rover.mode_guided.enter()) {
465 1 : _submode = SubMode::Guided;
466 :
467 : // initialise guided start time and position as reference for limit checking
468 1 : rover.mode_guided.limit_init_time_and_location();
469 :
470 : // sanity check target location
471 1 : if ((loc.lat != 0) || (loc.lng != 0)) {
472 1 : guided_target.loc = loc;
473 1 : guided_target.loc.sanitize(rover.current_loc);
474 1 : guided_target.valid = true;
475 : } else {
476 0 : guided_target.valid = false;
477 : }
478 : }
479 1 : }
480 :
481 : // start stopping vehicle as quickly as possible
482 519 : void ModeAuto::start_stop()
483 : {
484 519 : _submode = SubMode::Stop;
485 519 : }
486 :
487 : // send latest position target to offboard navigation system
488 500 : void ModeAuto::send_guided_position_target()
489 : {
490 500 : if (!guided_target.valid) {
491 0 : return;
492 : }
493 :
494 : // send at maximum of 1hz
495 500 : const uint32_t now_ms = AP_HAL::millis();
496 500 : if ((guided_target.last_sent_ms == 0) || (now_ms - guided_target.last_sent_ms > AUTO_GUIDED_SEND_TARGET_MS)) {
497 10 : guided_target.last_sent_ms = now_ms;
498 :
499 : // get system id and component id of offboard navigation system
500 : uint8_t sysid;
501 : uint8_t compid;
502 : mavlink_channel_t chan;
503 10 : if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_ONBOARD_CONTROLLER, sysid, compid, chan)) {
504 0 : gcs().chan(chan-MAVLINK_COMM_0)->send_set_position_target_global_int(sysid, compid, guided_target.loc);
505 : }
506 : }
507 :
508 : }
509 :
510 : /********************************************************************************/
511 : // Command Event Handlers
512 : /********************************************************************************/
513 156 : bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
514 : {
515 156 : switch (cmd.id) {
516 123 : case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
517 123 : return do_nav_wp(cmd, false);
518 :
519 18 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
520 18 : do_RTL();
521 18 : break;
522 :
523 2 : case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
524 : case MAV_CMD_NAV_LOITER_TIME: // Loiter for specified time
525 2 : return do_nav_wp(cmd, true);
526 :
527 2 : case MAV_CMD_NAV_LOITER_TURNS:
528 2 : return do_circle(cmd);
529 :
530 1 : case MAV_CMD_NAV_GUIDED_ENABLE: // accept navigation commands from external nav computer
531 1 : do_nav_guided_enable(cmd);
532 1 : break;
533 :
534 0 : case MAV_CMD_NAV_SET_YAW_SPEED:
535 0 : do_nav_set_yaw_speed(cmd);
536 0 : break;
537 :
538 0 : case MAV_CMD_NAV_DELAY: // 93 Delay the next navigation command
539 0 : do_nav_delay(cmd);
540 0 : break;
541 :
542 : #if AP_SCRIPTING_ENABLED
543 0 : case MAV_CMD_NAV_SCRIPT_TIME:
544 0 : do_nav_script_time(cmd);
545 0 : break;
546 : #endif
547 :
548 : // Conditional commands
549 0 : case MAV_CMD_CONDITION_DELAY:
550 0 : do_wait_delay(cmd);
551 0 : break;
552 :
553 0 : case MAV_CMD_CONDITION_DISTANCE:
554 0 : do_within_distance(cmd);
555 0 : break;
556 :
557 : // Do commands
558 9 : case MAV_CMD_DO_CHANGE_SPEED:
559 9 : do_change_speed(cmd);
560 9 : break;
561 :
562 0 : case MAV_CMD_DO_SET_HOME:
563 0 : do_set_home(cmd);
564 0 : break;
565 :
566 : #if HAL_MOUNT_ENABLED
567 : // Sets the region of interest (ROI) for a sensor set or the
568 : // vehicle itself. This can then be used by the vehicles control
569 : // system to control the vehicle attitude and the attitude of various
570 : // devices such as cameras.
571 : // |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
572 : // ROI_NONE can be handled by the regular ROI handler because lat, lon, alt are always zero
573 0 : case MAV_CMD_DO_SET_ROI_LOCATION:
574 : case MAV_CMD_DO_SET_ROI_NONE:
575 : case MAV_CMD_DO_SET_ROI:
576 0 : if (!cmd.content.location.initialised()) {
577 : // switch off the camera tracking if enabled
578 0 : if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
579 0 : rover.camera_mount.set_mode_to_default();
580 : }
581 : } else {
582 : // send the command to the camera mount
583 0 : rover.camera_mount.set_roi_target(cmd.content.location);
584 : }
585 0 : break;
586 : #endif
587 :
588 0 : case MAV_CMD_DO_SET_REVERSE:
589 0 : do_set_reverse(cmd);
590 0 : break;
591 :
592 1 : case MAV_CMD_DO_GUIDED_LIMITS:
593 1 : do_guided_limits(cmd);
594 1 : break;
595 :
596 0 : default:
597 : // return false for unhandled commands
598 0 : return false;
599 : }
600 :
601 : // if we got this far we must have been successful
602 29 : return true;
603 : }
604 :
605 : // exit_mission - callback function called from ap-mission when the mission has completed
606 23 : void ModeAuto::exit_mission()
607 : {
608 : // play a tone
609 23 : AP_Notify::events.mission_complete = 1;
610 : // send message
611 23 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Mission Complete");
612 :
613 23 : switch ((DoneBehaviour)g2.mis_done_behave) {
614 9 : case DoneBehaviour::HOLD:
615 : // the default "start_stop" behaviour is used
616 9 : break;
617 3 : case DoneBehaviour::LOITER:
618 3 : if (start_loiter()) {
619 3 : return;
620 : }
621 0 : break;
622 2 : case DoneBehaviour::ACRO:
623 2 : if (rover.set_mode(rover.mode_acro, ModeReason::MISSION_END)) {
624 2 : return;
625 : }
626 0 : break;
627 9 : case DoneBehaviour::MANUAL:
628 9 : if (rover.set_mode(rover.mode_manual, ModeReason::MISSION_END)) {
629 9 : return;
630 : }
631 0 : break;
632 : }
633 :
634 9 : start_stop();
635 : }
636 :
637 : // verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
638 : // we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
639 116561 : bool ModeAuto::verify_command_callback(const AP_Mission::Mission_Command& cmd)
640 : {
641 116561 : const bool cmd_complete = verify_command(cmd);
642 :
643 : // send message to GCS
644 116561 : if (cmd_complete) {
645 139 : gcs().send_mission_item_reached_message(cmd.index);
646 : }
647 :
648 116561 : return cmd_complete;
649 : }
650 :
651 : /*******************************************************************************
652 : Verify command Handlers
653 :
654 : Each type of mission element has a "verify" operation. The verify
655 : operation returns true when the mission element has completed and we
656 : should move onto the next mission element.
657 : Return true if we do not recognize the command so that we move on to the next command
658 : *******************************************************************************/
659 :
660 116561 : bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
661 : {
662 116561 : switch (cmd.id) {
663 90268 : case MAV_CMD_NAV_WAYPOINT:
664 90268 : return verify_nav_wp(cmd);
665 :
666 22896 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
667 22896 : return verify_RTL();
668 :
669 910 : case MAV_CMD_NAV_LOITER_UNLIM:
670 910 : return verify_loiter_unlimited(cmd);
671 :
672 1977 : case MAV_CMD_NAV_LOITER_TURNS:
673 1977 : return verify_circle(cmd);
674 :
675 0 : case MAV_CMD_NAV_LOITER_TIME:
676 0 : return verify_loiter_time(cmd);
677 :
678 500 : case MAV_CMD_NAV_GUIDED_ENABLE:
679 500 : return verify_nav_guided_enable(cmd);
680 :
681 0 : case MAV_CMD_NAV_DELAY:
682 0 : return verify_nav_delay(cmd);
683 :
684 : #if AP_SCRIPTING_ENABLED
685 0 : case MAV_CMD_NAV_SCRIPT_TIME:
686 0 : return verify_nav_script_time();
687 : #endif
688 :
689 0 : case MAV_CMD_CONDITION_DELAY:
690 0 : return verify_wait_delay();
691 :
692 0 : case MAV_CMD_CONDITION_DISTANCE:
693 0 : return verify_within_distance();
694 :
695 0 : case MAV_CMD_NAV_SET_YAW_SPEED:
696 0 : return verify_nav_set_yaw_speed();
697 :
698 : // do commands (always return true)
699 10 : case MAV_CMD_DO_CHANGE_SPEED:
700 : case MAV_CMD_DO_SET_HOME:
701 : case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
702 : case MAV_CMD_DO_SET_ROI_LOCATION:
703 : case MAV_CMD_DO_SET_ROI_NONE:
704 : case MAV_CMD_DO_SET_ROI:
705 : case MAV_CMD_DO_SET_REVERSE:
706 : case MAV_CMD_DO_FENCE_ENABLE:
707 : case MAV_CMD_DO_GUIDED_LIMITS:
708 10 : return true;
709 :
710 0 : default:
711 : // error message
712 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Skipping invalid cmd #%i", cmd.id);
713 : // return true if we do not recognize the command so that we move on to the next command
714 0 : return true;
715 : }
716 : }
717 :
718 : /********************************************************************************/
719 : // Nav (Must) commands
720 : /********************************************************************************/
721 :
722 18 : void ModeAuto::do_RTL(void)
723 : {
724 : // start rtl in auto mode
725 18 : start_RTL();
726 18 : }
727 :
728 125 : bool ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
729 : {
730 : // retrieve and sanitize target location
731 125 : Location cmdloc = cmd.content.location;
732 125 : cmdloc.sanitize(rover.current_loc);
733 :
734 : // delayed stored in p1 in seconds
735 125 : loiter_duration = ((int16_t) cmd.p1 < 0) ? 0 : cmd.p1;
736 125 : loiter_start_time = 0;
737 125 : if (loiter_duration > 0) {
738 0 : always_stop_at_destination = true;
739 : }
740 :
741 : // do not add next wp if there are no more navigation commands
742 125 : AP_Mission::Mission_Command next_cmd;
743 125 : if (always_stop_at_destination || !mission.get_next_nav_cmd(cmd.index+1, next_cmd)) {
744 : // single destination
745 18 : if (!set_desired_location(cmdloc)) {
746 0 : return false;
747 : }
748 : } else {
749 : // retrieve and sanitize next destination location
750 107 : Location next_cmdloc = next_cmd.content.location;
751 107 : next_cmdloc.sanitize(cmdloc);
752 107 : if (!set_desired_location(cmdloc, next_cmdloc)) {
753 0 : return false;
754 : }
755 : }
756 :
757 : // just starting so we haven't previously reached the waypoint
758 125 : previously_reached_wp = false;
759 :
760 125 : return true;
761 : }
762 :
763 : // do_nav_delay - Delay the next navigation command
764 0 : void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
765 : {
766 0 : nav_delay_time_start_ms = millis();
767 :
768 : // boats loiter, cars and balancebots stop
769 0 : if (rover.is_boat()) {
770 0 : if (!start_loiter()) {
771 0 : start_stop();
772 : }
773 : } else {
774 0 : start_stop();
775 : }
776 :
777 0 : if (cmd.content.nav_delay.seconds > 0) {
778 : // relative delay
779 0 : nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
780 : } else {
781 : // absolute delay to utc time
782 : #if AP_RTC_ENABLED
783 0 : nav_delay_time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0);
784 : #else
785 : nav_delay_time_max_ms = 0;
786 : #endif
787 : }
788 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000));
789 0 : }
790 :
791 : // start guided within auto to allow external navigation system to control vehicle
792 1 : void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
793 : {
794 1 : if (cmd.p1 > 0) {
795 1 : start_guided(cmd.content.location);
796 : }
797 1 : }
798 :
799 : // do_set_yaw_speed - turn to a specified heading and achieve a given speed
800 0 : void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
801 : {
802 : float desired_heading_cd;
803 :
804 : // get final angle, 1 = Relative, 0 = Absolute
805 0 : if (cmd.content.set_yaw_speed.relative_angle > 0) {
806 : // relative angle
807 0 : desired_heading_cd = wrap_180_cd(ahrs.yaw_sensor + cmd.content.set_yaw_speed.angle_deg * 100.0f);
808 : } else {
809 : // absolute angle
810 0 : desired_heading_cd = cmd.content.set_yaw_speed.angle_deg * 100.0f;
811 : }
812 :
813 : // set targets
814 0 : const float speed_max = g2.wp_nav.get_default_speed();
815 0 : _desired_speed = constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max);
816 0 : _desired_yaw_cd = desired_heading_cd;
817 0 : _reached_heading = false;
818 0 : _submode = SubMode::HeadingAndSpeed;
819 0 : }
820 :
821 : /********************************************************************************/
822 : // Verify Nav (Must) commands
823 : /********************************************************************************/
824 91178 : bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
825 : {
826 : // exit immediately if we haven't reached the destination
827 91178 : if (!reached_destination()) {
828 90155 : return false;
829 : }
830 :
831 : // Check if this is the first time we have noticed reaching the waypoint
832 1023 : if (!previously_reached_wp) {
833 115 : previously_reached_wp = true;
834 :
835 : // check if we are loitering at this waypoint - the message sent to the GCS is different
836 115 : if (loiter_duration > 0) {
837 : // send message including loiter time
838 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
839 : (unsigned int)cmd.index,
840 : (unsigned int)loiter_duration);
841 : // record the current time i.e. start timer
842 0 : loiter_start_time = millis();
843 : } else {
844 : // send simpler message to GCS
845 115 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Reached waypoint #%u", (unsigned int)cmd.index);
846 : }
847 : }
848 :
849 : // Check if we have loitered long enough
850 1023 : if (loiter_duration == 0) {
851 1023 : return true;
852 : } else {
853 0 : return (((millis() - loiter_start_time) / 1000) >= loiter_duration);
854 : }
855 : }
856 :
857 : // verify_nav_delay - check if we have waited long enough
858 0 : bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
859 : {
860 0 : if (millis() - nav_delay_time_start_ms > nav_delay_time_max_ms) {
861 0 : nav_delay_time_max_ms = 0;
862 0 : return true;
863 : }
864 :
865 0 : return false;
866 : }
867 :
868 22896 : bool ModeAuto::verify_RTL() const
869 : {
870 22896 : return reached_destination();
871 : }
872 :
873 910 : bool ModeAuto::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
874 : {
875 910 : verify_nav_wp(cmd);
876 910 : return false;
877 : }
878 :
879 : // verify_loiter_time - check if we have loitered long enough
880 0 : bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
881 : {
882 0 : const bool result = verify_nav_wp(cmd);
883 0 : if (result) {
884 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Finished active loiter");
885 : }
886 0 : return result;
887 : }
888 :
889 : // check if guided has completed
890 500 : bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
891 : {
892 : // if we failed to enter guided or this command disables guided
893 : // return true so we move to next command
894 500 : if (_submode != SubMode::Guided || cmd.p1 == 0) {
895 0 : return true;
896 : }
897 :
898 : // if a location target was set, return true once vehicle is close
899 500 : if (guided_target.valid) {
900 500 : if (rover.current_loc.get_distance(guided_target.loc) <= g2.wp_nav.get_radius()) {
901 0 : return true;
902 : }
903 : }
904 :
905 : // guided command complete once a limit is breached
906 500 : return rover.mode_guided.limit_breached();
907 : }
908 :
909 : // verify_yaw - return true if we have reached the desired heading
910 0 : bool ModeAuto::verify_nav_set_yaw_speed()
911 : {
912 0 : if (_submode == SubMode::HeadingAndSpeed) {
913 0 : return _reached_heading;
914 : }
915 : // we should never reach here but just in case, return true to allow missions to continue
916 0 : return true;
917 : }
918 :
919 2 : bool ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
920 : {
921 : // retrieve and sanitize target location
922 2 : Location circle_center = cmd.content.location;
923 2 : circle_center.sanitize(rover.current_loc);
924 :
925 : // calculate radius
926 2 : uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
927 2 : if (cmd.id == MAV_CMD_NAV_LOITER_TURNS &&
928 2 : cmd.type_specific_bits & (1U << 0)) {
929 : // special storage handling allows for larger radii
930 1 : circle_radius_m *= 10;
931 : }
932 :
933 : // initialise circle mode
934 2 : if (g2.mode_circle.set_center(circle_center, circle_radius_m, cmd.content.location.loiter_ccw)) {
935 2 : _submode = SubMode::Circle;
936 2 : return true;
937 : }
938 0 : return false;
939 : }
940 :
941 1977 : bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
942 : {
943 1977 : const float turns = cmd.get_loiter_turns();
944 : // check if we have completed circling
945 1977 : return ((g2.mode_circle.get_angle_total_rad() / M_2PI) >= turns);
946 : }
947 :
948 : /********************************************************************************/
949 : // Condition (May) commands
950 : /********************************************************************************/
951 :
952 0 : void ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd)
953 : {
954 0 : condition_start = millis();
955 0 : condition_value = static_cast<int32_t>(cmd.content.delay.seconds * 1000); // convert seconds to milliseconds
956 0 : }
957 :
958 0 : void ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd)
959 : {
960 0 : condition_value = cmd.content.distance.meters;
961 0 : }
962 :
963 : /********************************************************************************/
964 : // Verify Condition (May) commands
965 : /********************************************************************************/
966 :
967 0 : bool ModeAuto::verify_wait_delay()
968 : {
969 0 : if (static_cast<uint32_t>(millis() - condition_start) > static_cast<uint32_t>(condition_value)) {
970 0 : condition_value = 0;
971 0 : return true;
972 : }
973 0 : return false;
974 : }
975 :
976 0 : bool ModeAuto::verify_within_distance()
977 : {
978 0 : if (get_distance_to_destination() < condition_value) {
979 0 : condition_value = 0;
980 0 : return true;
981 : }
982 0 : return false;
983 : }
984 :
985 :
986 : /********************************************************************************/
987 : // Do (Now) commands
988 : /********************************************************************************/
989 :
990 9 : void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
991 : {
992 : // set speed for active mode
993 9 : if (set_desired_speed(cmd.content.speed.target_ms)) {
994 9 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "speed: %.1f m/s", static_cast<double>(cmd.content.speed.target_ms));
995 : }
996 9 : }
997 :
998 0 : void ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
999 : {
1000 0 : if (cmd.p1 == 1 && rover.have_position) {
1001 0 : if (!rover.set_home_to_current_location(false)) {
1002 : // ignored...
1003 : }
1004 : } else {
1005 0 : if (!rover.set_home(cmd.content.location, false)) {
1006 : // ignored...
1007 : }
1008 : }
1009 0 : }
1010 :
1011 0 : void ModeAuto::do_set_reverse(const AP_Mission::Mission_Command& cmd)
1012 : {
1013 0 : set_reversed(cmd.p1 == 1);
1014 0 : }
1015 :
1016 : // set timeout and position limits for guided within auto
1017 1 : void ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd)
1018 : {
1019 1 : rover.mode_guided.limit_set(
1020 1 : cmd.p1 * 1000, // convert seconds to ms
1021 1 : cmd.content.guided_limits.horiz_max);
1022 1 : }
1023 :
1024 : #if AP_SCRIPTING_ENABLED
1025 : // start accepting position, velocity and acceleration targets from lua scripts
1026 0 : void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd)
1027 : {
1028 : // call regular guided flight mode initialisation
1029 0 : if (rover.mode_guided.enter()) {
1030 0 : _submode = SubMode::NavScriptTime;
1031 0 : nav_scripting.done = false;
1032 0 : nav_scripting.id++;
1033 0 : nav_scripting.start_ms = millis();
1034 0 : nav_scripting.command = cmd.content.nav_script_time.command;
1035 0 : nav_scripting.timeout_s = cmd.content.nav_script_time.timeout_s;
1036 0 : nav_scripting.arg1 = cmd.content.nav_script_time.arg1.get();
1037 0 : nav_scripting.arg2 = cmd.content.nav_script_time.arg2.get();
1038 0 : nav_scripting.arg3 = cmd.content.nav_script_time.arg3;
1039 0 : nav_scripting.arg4 = cmd.content.nav_script_time.arg4;
1040 : } else {
1041 : // for safety we set nav_scripting to done to protect against the mission getting stuck
1042 0 : nav_scripting.done = true;
1043 : }
1044 0 : }
1045 :
1046 : // check if verify_nav_script_time command has completed
1047 0 : bool ModeAuto::verify_nav_script_time()
1048 : {
1049 : // if done or timeout then return true
1050 0 : if (nav_scripting.done ||
1051 0 : ((nav_scripting.timeout_s > 0) &&
1052 0 : (AP_HAL::millis() - nav_scripting.start_ms) > (nav_scripting.timeout_s * 1000))) {
1053 0 : return true;
1054 : }
1055 0 : return false;
1056 : }
1057 : #endif
|
