Line data Source code
1 : #include "Copter.h"
2 :
3 : #if MODE_AUTO_ENABLED
4 :
5 : /*
6 : * Init and run calls for auto flight mode
7 : *
8 : * This file contains the implementation for Land, Waypoint navigation and Takeoff from Auto mode
9 : * Command execution code (i.e. command_logic.pde) should:
10 : * a) switch to Auto flight mode with set_mode() function. This will cause auto_init to be called
11 : * b) call one of the three auto initialisation functions: auto_wp_start(), auto_takeoff_start(), auto_land_start()
12 : * c) call one of the verify functions auto_wp_verify(), auto_takeoff_verify, auto_land_verify repeated to check if the command has completed
13 : * The main loop (i.e. fast loop) will call update_flight_modes() which will in turn call auto_run() which, based upon the auto_mode variable will call
14 : * correct auto_wp_run, auto_takeoff_run or auto_land_run to actually implement the feature
15 : */
16 :
17 : /*
18 : * While in the auto flight mode, navigation or do/now commands can be run.
19 : * Code in this file implements the navigation commands
20 : */
21 :
22 : // auto_init - initialise auto controller
23 98 : bool ModeAuto::init(bool ignore_checks)
24 : {
25 98 : auto_RTL = false;
26 98 : if (mission.present() || ignore_checks) {
27 : // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
28 98 : if (motors->armed() && copter.ap.land_complete && !mission.starts_with_takeoff_cmd()) {
29 0 : gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
30 0 : return false;
31 : }
32 :
33 98 : _mode = SubMode::LOITER;
34 :
35 : // stop ROI from carrying over from previous runs of the mission
36 : // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
37 98 : if (auto_yaw.mode() == AutoYaw::Mode::ROI) {
38 0 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
39 : }
40 :
41 : // initialise waypoint and spline controller
42 98 : wp_nav->wp_and_spline_init_m();
43 :
44 : // initialise desired speed overrides
45 98 : desired_speed_override_ms = {0, 0, 0};
46 :
47 : // initialise LOITER_TURNS circle state
48 98 : circle = {};
49 :
50 : // set flag to start mission
51 98 : waiting_to_start = true;
52 :
53 : // initialise mission change check (ignore results)
54 98 : IGNORE_RETURN(mis_change_detector.check_for_mission_change());
55 :
56 : // clear guided limits
57 98 : copter.mode_guided.limit_clear();
58 :
59 : // reset flag indicating if pilot has applied roll or pitch inputs during landing
60 98 : copter.ap.land_repo_active = false;
61 :
62 : #if AC_PRECLAND_ENABLED
63 : // initialise precland state machine
64 98 : copter.precland_statemachine.init();
65 : #endif
66 :
67 98 : return true;
68 : } else {
69 0 : return false;
70 : }
71 : }
72 :
73 : // stop mission when we leave auto mode
74 80 : void ModeAuto::exit()
75 : {
76 80 : if (copter.mode_auto.mission.state() == AP_Mission::MISSION_RUNNING) {
77 43 : copter.mode_auto.mission.stop();
78 : }
79 : #if HAL_MOUNT_ENABLED
80 80 : copter.camera_mount.set_mode_to_default();
81 : #endif // HAL_MOUNT_ENABLED
82 :
83 80 : auto_RTL = false;
84 80 : }
85 :
86 : // auto_run - runs the auto controller
87 : // should be called at 100hz or more
88 3482367 : void ModeAuto::run()
89 : {
90 : // start or update mission
91 3482367 : if (waiting_to_start) {
92 : // don't start the mission until we have an origin
93 44563 : Location loc;
94 44563 : if (copter.ahrs.get_origin(loc)) {
95 : // start/resume the mission (based on MIS_RESTART parameter)
96 98 : mission.start_or_resume();
97 98 : waiting_to_start = false;
98 :
99 : // initialise mission change check (ignore results)
100 98 : IGNORE_RETURN(mis_change_detector.check_for_mission_change());
101 : }
102 : } else {
103 : // check for mission changes
104 3437804 : if (mis_change_detector.check_for_mission_change()) {
105 : // if mission is running restart the current command if it is a waypoint or spline command
106 29 : if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_mode == SubMode::WP)) {
107 1 : if (mission.restart_current_nav_cmd()) {
108 0 : gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command");
109 : } else {
110 : // failed to restart mission for some reason
111 1 : gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed but failed to restart command");
112 : }
113 : }
114 : }
115 :
116 3437804 : mission.update();
117 : }
118 :
119 : // call the correct auto controller
120 3482367 : switch (_mode) {
121 :
122 685267 : case SubMode::TAKEOFF:
123 685267 : takeoff_run();
124 685267 : break;
125 :
126 1501079 : case SubMode::WP:
127 : case SubMode::CIRCLE_MOVE_TO_EDGE:
128 : case SubMode::CIRCLE:
129 : // the circle orbit is flown as an S-curve waypoint leg
130 1501079 : wp_run();
131 1501079 : break;
132 :
133 151476 : case SubMode::LAND:
134 151476 : land_run();
135 151476 : break;
136 :
137 1014590 : case SubMode::RTL:
138 1014590 : rtl_run();
139 1014590 : break;
140 :
141 0 : case SubMode::NAVGUIDED:
142 : case SubMode::NAV_SCRIPT_TIME:
143 : #if AC_NAV_GUIDED || AP_SCRIPTING_ENABLED
144 0 : nav_guided_run();
145 : #endif
146 0 : break;
147 :
148 91648 : case SubMode::LOITER:
149 91648 : loiter_run();
150 91648 : break;
151 :
152 14519 : case SubMode::LOITER_TO_ALT:
153 14519 : loiter_to_alt_run();
154 14519 : break;
155 :
156 : #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED
157 23788 : case SubMode::NAV_PAYLOAD_PLACE:
158 23788 : payload_place.run();
159 23788 : break;
160 : #endif
161 :
162 0 : case SubMode::NAV_ATTITUDE_TIME:
163 0 : nav_attitude_time_run();
164 0 : break;
165 : }
166 :
167 : // only pretend to be in auto RTL so long as mission still thinks its in a landing sequence or the mission has completed
168 3482367 : const bool auto_rtl_active = mission.get_in_landing_sequence_flag() || mission.get_in_return_path_flag() || mission.state() == AP_Mission::mission_state::MISSION_COMPLETE;
169 3482367 : if (auto_RTL && !auto_rtl_active) {
170 4 : auto_RTL = false;
171 : // log exit from Auto RTL
172 : #if HAL_LOGGING_ENABLED
173 4 : copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), ModeReason::AUTO_RTL_EXIT);
174 : #endif
175 : }
176 3482367 : }
177 :
178 : // return true if a position estimate is required
179 175799 : bool ModeAuto::requires_position() const
180 : {
181 : // position estimate is required in all sub modes except attitude control
182 175799 : return _mode != SubMode::NAV_ATTITUDE_TIME;
183 : }
184 :
185 : // set submode. This may re-trigger the vehicle's EKF failsafe if the new submode requires a position estimate
186 416 : void ModeAuto::set_submode(SubMode new_submode)
187 : {
188 : // return immediately if the submode has not been changed
189 416 : if (new_submode == _mode) {
190 175 : return;
191 : }
192 :
193 : // backup old mode
194 241 : SubMode old_submode = _mode;
195 :
196 : // set mode
197 241 : _mode = new_submode;
198 :
199 : // if changing out of the nav-attitude-time submode, recheck the EKF failsafe
200 : // this may trigger a flight mode change if the EKF failsafe is active
201 241 : if (old_submode == SubMode::NAV_ATTITUDE_TIME) {
202 0 : copter.failsafe_ekf_recheck();
203 : }
204 : }
205 :
206 5527316 : bool ModeAuto::option_is_enabled(Option option) const
207 : {
208 5527316 : return ((copter.g2.auto_options & (uint32_t)option) != 0);
209 : }
210 :
211 53 : bool ModeAuto::allows_arming(AP_Arming::Method method) const
212 : {
213 53 : if (auto_RTL) {
214 0 : return false;
215 : }
216 53 : return option_is_enabled(Option::AllowArming);
217 : }
218 :
219 : #if WEATHERVANE_ENABLED
220 2958721 : bool ModeAuto::allows_weathervaning() const
221 : {
222 2958721 : return option_is_enabled(Option::AllowWeatherVaning);
223 : }
224 : #endif
225 :
226 : // determine EKF reset handling method based on Guide submode
227 3482367 : bool ModeAuto::move_vehicle_on_ekf_reset() const
228 : {
229 : // call the correct auto controller
230 3482367 : switch (_mode) {
231 1981320 : case SubMode::TAKEOFF:
232 : case SubMode::LAND:
233 : case SubMode::RTL:
234 : case SubMode::NAVGUIDED:
235 : case SubMode::LOITER:
236 : case SubMode::LOITER_TO_ALT:
237 : #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED
238 : case SubMode::NAV_PAYLOAD_PLACE:
239 : #endif
240 : case SubMode::NAV_SCRIPT_TIME:
241 : case SubMode::NAV_ATTITUDE_TIME:
242 : // these submodes reset their targets so the vehicle does not physically move
243 1981320 : return false;
244 1501047 : case SubMode::WP:
245 : case SubMode::CIRCLE_MOVE_TO_EDGE:
246 : case SubMode::CIRCLE:
247 : // these submodes smoothly move to maintain an absolute position
248 : // (the circle orbit and the move-to-edge leg are S-curve waypoint legs, like WP)
249 1501047 : return true;
250 : }
251 :
252 : // should never reach here but just in case
253 0 : return true;
254 : }
255 :
256 : // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
257 5 : bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
258 : {
259 5 : if (!mission.jump_to_landing_sequence(get_stopping_point())) {
260 1 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
261 : // make sad noise
262 1 : if (copter.ap.initialised) {
263 1 : AP_Notify::events.user_mode_change_failed = 1;
264 : }
265 1 : gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found");
266 1 : return false;
267 : }
268 :
269 4 : return enter_auto_rtl(reason);
270 : }
271 :
272 : // Join mission after DO_RETURN_PATH_START waypoint, if succeeds pretend to be Auto RTL mode
273 5 : bool ModeAuto::return_path_start_auto_RTL(ModeReason reason)
274 : {
275 5 : if (!mission.jump_to_closest_mission_leg(get_stopping_point())) {
276 2 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
277 : // make sad noise
278 2 : if (copter.ap.initialised) {
279 2 : AP_Notify::events.user_mode_change_failed = 1;
280 : }
281 2 : gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No return path found");
282 2 : return false;
283 : }
284 :
285 3 : return enter_auto_rtl(reason);
286 : }
287 :
288 : // Try join return path else do land start
289 13 : bool ModeAuto::return_path_or_jump_to_landing_sequence_auto_RTL(ModeReason reason)
290 : {
291 13 : const Location stopping_point = get_stopping_point();
292 13 : if (!mission.jump_to_closest_mission_leg(stopping_point) && !mission.jump_to_landing_sequence(stopping_point)) {
293 11 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
294 : // make sad noise
295 11 : if (copter.ap.initialised) {
296 11 : AP_Notify::events.user_mode_change_failed = 1;
297 : }
298 11 : gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No return path or landing sequence found");
299 11 : return false;
300 : }
301 :
302 2 : return enter_auto_rtl(reason);
303 : }
304 :
305 : // Enter auto rtl pseudo mode
306 9 : bool ModeAuto::enter_auto_rtl(ModeReason reason)
307 : {
308 9 : mission.set_force_resume(true);
309 :
310 : // if not already in auto switch to auto
311 9 : if ((copter.flightmode == this) || set_mode(Mode::Number::AUTO, reason)) {
312 9 : auto_RTL = true;
313 : #if HAL_LOGGING_ENABLED
314 : // log entry into AUTO RTL
315 9 : copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason);
316 : #endif
317 :
318 : // make happy noise
319 9 : if (copter.ap.initialised) {
320 9 : AP_Notify::events.user_mode_change = 1;
321 : }
322 9 : return true;
323 : }
324 :
325 : // mode change failed, revert force resume flag
326 0 : mission.set_force_resume(false);
327 :
328 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
329 : // make sad noise
330 0 : if (copter.ap.initialised) {
331 0 : AP_Notify::events.user_mode_change_failed = 1;
332 : }
333 0 : return false;
334 : }
335 :
336 : // lua scripts use this to retrieve the contents of the active command
337 0 : bool ModeAuto::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4)
338 : {
339 : #if AP_SCRIPTING_ENABLED
340 0 : if (_mode == SubMode::NAV_SCRIPT_TIME) {
341 0 : id = nav_scripting.id;
342 0 : cmd = nav_scripting.command;
343 0 : arg1 = nav_scripting.arg1;
344 0 : arg2 = nav_scripting.arg2;
345 0 : arg3 = nav_scripting.arg3;
346 0 : arg4 = nav_scripting.arg4;
347 0 : return true;
348 : }
349 : #endif
350 0 : return false;
351 : }
352 :
353 : // lua scripts use this to indicate when they have complete the command
354 0 : void ModeAuto::nav_script_time_done(uint16_t id)
355 : {
356 : #if AP_SCRIPTING_ENABLED
357 0 : if ((_mode == SubMode::NAV_SCRIPT_TIME) && (id == nav_scripting.id)) {
358 0 : nav_scripting.done = true;
359 : }
360 : #endif
361 0 : }
362 :
363 : // auto_loiter_start - initialises loitering in auto mode
364 : // returns success/failure because this can be called by exit_mission
365 3 : bool ModeAuto::loiter_start()
366 : {
367 : // return failure if GPS is bad
368 3 : if (!copter.position_ok()) {
369 0 : return false;
370 : }
371 3 : _mode = SubMode::LOITER;
372 :
373 : // calculate stopping point
374 3 : Vector3p stopping_point_ned_m;
375 3 : wp_nav->get_wp_stopping_point_NED_m(stopping_point_ned_m);
376 :
377 : // initialise waypoint controller target to stopping point
378 3 : wp_nav->set_wp_destination_NED_m(stopping_point_ned_m);
379 :
380 : // hold yaw at current heading
381 3 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
382 :
383 3 : return true;
384 : }
385 :
386 : // auto_rtl_start - initialises RTL in AUTO flight mode
387 46 : void ModeAuto::rtl_start()
388 : {
389 : // call regular rtl flight mode initialisation and ask it to ignore checks
390 46 : if (copter.mode_rtl.init(true)) {
391 46 : set_submode(SubMode::RTL);
392 : } else {
393 : // this should never happen because RTL never fails init if argument is true
394 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
395 : }
396 46 : }
397 :
398 : // initialise waypoint controller to implement take-off
399 92 : void ModeAuto::takeoff_start(const Location& dest_loc)
400 : {
401 92 : if (!copter.current_loc.initialised()) {
402 : // this should never happen because mission commands are not executed until
403 : // the AHRS/EKF origin is set by which time current_loc should also have been set
404 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
405 0 : return;
406 : }
407 :
408 : // calculate current and target altitudes
409 : // by default current_alt_m and alt_target_m are alt-above-EKF-origin
410 : float alt_target_m;
411 92 : bool alt_target_terrain = false;
412 92 : float current_alt_m = pos_control->get_pos_estimate_U_m();
413 : float terrain_u_m; // terrain's altitude in m above the ekf origin
414 92 : if ((dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) && wp_nav->get_terrain_U_m(terrain_u_m)) {
415 : // subtract terrain offset to convert vehicle's alt-above-ekf-origin to alt-above-terrain
416 0 : current_alt_m -= terrain_u_m;
417 :
418 : // specify alt_target_m as alt-above-terrain
419 0 : alt_target_m = dest_loc.alt * 0.01;
420 0 : alt_target_terrain = true;
421 : } else {
422 : // set horizontal target
423 92 : Location dest(dest_loc);
424 92 : dest.lat = copter.current_loc.lat;
425 92 : dest.lng = copter.current_loc.lng;
426 :
427 : // get altitude target above EKF origin
428 92 : if (!dest.get_alt_m(Location::AltFrame::ABOVE_ORIGIN, alt_target_m)) {
429 : // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
430 9 : LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
431 : // fall back to altitude above current altitude
432 9 : alt_target_m = current_alt_m + dest.alt * 0.01;
433 : }
434 : }
435 :
436 : // sanity check target
437 92 : float alt_target_min_m = current_alt_m + (copter.ap.land_complete ? 1.0 : 0.0);
438 92 : alt_target_m = MAX(alt_target_m, alt_target_min_m);
439 :
440 : // initialise yaw
441 92 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
442 :
443 : // clear i term when we're taking off
444 92 : pos_control->D_init_controller();
445 :
446 : // initialise alt for WP_NAVALT_MIN and set completion alt
447 92 : auto_takeoff.start_m(alt_target_m, alt_target_terrain);
448 :
449 : // set submode
450 92 : set_submode(SubMode::TAKEOFF);
451 : }
452 :
453 : // auto_wp_start - initialises waypoint controller to implement flying to a particular destination
454 230 : bool ModeAuto::wp_start(const Location& dest_loc)
455 : {
456 : // init wpnav and set origin if transitioning from takeoff
457 230 : if (!wp_nav->is_active()) {
458 68 : Vector3p stopping_point_ned_m;
459 68 : if (_mode == SubMode::TAKEOFF) {
460 58 : Vector3p takeoff_complete_pos_ned_m;
461 58 : if (auto_takeoff.get_completion_pos_ned_m(takeoff_complete_pos_ned_m)) {
462 57 : stopping_point_ned_m = takeoff_complete_pos_ned_m;
463 : }
464 : }
465 68 : float des_speed_xy_ms = is_positive(desired_speed_override_ms.xy) ? desired_speed_override_ms.xy : 0;
466 68 : wp_nav->wp_and_spline_init_m(des_speed_xy_ms, stopping_point_ned_m);
467 :
468 : // override speeds up and down if necessary
469 68 : if (is_positive(desired_speed_override_ms.up)) {
470 0 : wp_nav->set_speed_up_ms(desired_speed_override_ms.up);
471 : }
472 68 : if (is_positive(desired_speed_override_ms.down)) {
473 0 : wp_nav->set_speed_down_ms(desired_speed_override_ms.down);
474 : }
475 : }
476 :
477 230 : if (!wp_nav->set_wp_destination_loc(dest_loc)) {
478 0 : return false;
479 : }
480 :
481 : // initialise yaw
482 : // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
483 230 : if (auto_yaw.mode() != AutoYaw::Mode::ROI && !(auto_yaw.mode() == AutoYaw::Mode::FIXED && copter.g.wp_yaw_behavior == Copter::WPYawBehavior::NONE)) {
484 230 : auto_yaw.set_mode_to_default(false);
485 : }
486 :
487 : // set submode
488 230 : set_submode(SubMode::WP);
489 :
490 230 : return true;
491 : }
492 :
493 : // auto_land_start - initialises controller to implement a landing
494 10 : void ModeAuto::land_start()
495 : {
496 : // set horizontal speed and acceleration limits
497 10 : pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
498 10 : pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
499 :
500 : // initialise the vertical position controller
501 10 : if (!pos_control->NE_is_active()) {
502 0 : pos_control->NE_init_controller();
503 : }
504 :
505 : // set vertical speed and acceleration limits
506 10 : pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
507 10 : pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
508 :
509 : // initialise the vertical position controller
510 10 : if (!pos_control->D_is_active()) {
511 0 : pos_control->D_init_controller();
512 : }
513 :
514 : // initialise yaw
515 10 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
516 :
517 : #if AP_LANDINGGEAR_ENABLED
518 : // optionally deploy landing gear
519 10 : copter.landinggear.deploy_for_landing();
520 : #endif
521 :
522 : // reset flag indicating if pilot has applied roll or pitch inputs during landing
523 10 : copter.ap.land_repo_active = false;
524 :
525 : // this will be set true if prec land is later active
526 10 : copter.ap.prec_land_active = false;
527 :
528 : // set submode
529 10 : set_submode(SubMode::LAND);
530 10 : }
531 :
532 : // circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
533 : // we assume the caller has performed all required GPS_ok checks
534 5 : void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m)
535 : {
536 : // set circle center
537 5 : copter.circle_nav->set_center(circle_center);
538 :
539 : // set circle radius. The radius is read back so circle.radius_m carries AC_Circle's
540 : // clamp, and a zero (panorama) radius is stored explicitly because get_radius_m() falls
541 : // back to the CIRCLE_RADIUS parameter when the commanded radius is zero
542 5 : copter.circle_nav->set_radius_m(radius_m);
543 5 : circle.radius_m = is_positive(radius_m) ? copter.circle_nav->get_radius_m() : 0.0f;
544 :
545 : // Always fly to the exact edge of the circle first: circle_start() derives the S-curve
546 : // orbit's radius purely from wp_nav's current destination, with no closed-loop correction
547 : // across the orbit, so that destination must land exactly on the circle at zero speed.
548 : // If the vehicle is already there this leg is short and completes within a tick or two,
549 : // so this costs nothing in the common case.
550 5 : Vector3p circle_edge_ned_m;
551 : float dist_to_edge_m;
552 5 : copter.circle_nav->get_closest_point_on_circle_NED_m(circle_edge_ned_m, dist_to_edge_m);
553 :
554 : // initialise wpnav to move to edge of circle. set_center() has already resolved the
555 : // command's altitude and frame into the center, and the edge point carries the center's
556 : // altitude, so the point goes straight through in NED rather than round-tripping via a
557 : // Location and back
558 5 : if (!wp_nav->set_wp_destination_NED_m(circle_edge_ned_m, copter.circle_nav->center_is_terrain_alt())) {
559 : // failure to set destination can only be because of missing terrain data
560 0 : copter.failsafe_terrain_on_event();
561 : }
562 :
563 : // if we are outside the circle, point at the edge, otherwise hold yaw
564 5 : const float dist_to_center_m = get_horizontal_distance(pos_control->get_pos_estimate_NED_m().xy().tofloat(), copter.circle_nav->get_center_NED_m().xy().tofloat());
565 : // initialise yaw
566 : // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
567 5 : if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
568 5 : if (dist_to_center_m > circle.radius_m && dist_to_center_m > 5.0) {
569 2 : auto_yaw.set_mode_to_default(false);
570 : } else {
571 : // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
572 3 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
573 : }
574 : }
575 :
576 : // set the submode to move to the edge of the circle
577 5 : set_submode(SubMode::CIRCLE_MOVE_TO_EDGE);
578 5 : }
579 :
580 : // auto_circle_start - begin flying a circular orbit as an S-curve waypoint leg
581 : // circle_movetoedge_start has already configured circle_nav with the center, radius and
582 : // altitude frame; those are used here only as a parameter store. CIRCLE_RATE is read only
583 : // for the radius-0 panorama spin.
584 5 : void ModeAuto::circle_start()
585 : {
586 : // radius-0 panorama: hold the current wp_nav destination (the circle center, already
587 : // reached by the move-to-edge leg) and spin yaw at the circle rate. No orbit leg is built,
588 : // rather than relying on calculate_circle_track()'s degenerate-arc guard to reject a
589 : // zero-radius orbit, so the hold does not depend on the origin landing exactly on the center.
590 5 : if (!is_positive(circle.radius_m)) {
591 : // a zero radius cannot encode a direction in the mission item (loiter_ccw comes from
592 : // param3 < 0), so CIRCLE_RATE supplies both the rate and the spin direction
593 1 : const float rate_degs = copter.circle_nav->get_rate_degs();
594 : // use the rate set_fixed_yaw_rad() will actually fly, which is the yaw slew limit for a
595 : // zero rate and is clamped to that limit above it. One expression feeds both the yaw
596 : // command and the duration below so the two cannot disagree
597 1 : const float slew_rate_max_rads = attitude_control->get_slew_yaw_max_rads();
598 1 : const float rate_rads = is_positive(fabsf(rate_degs)) ? MIN(radians(fabsf(rate_degs)), slew_rate_max_rads)
599 1 : : slew_rate_max_rads;
600 1 : if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
601 : // slew the yaw target through the requested turns at the circle rate. FIXED is an
602 : // angle target, so unlike a rate command it cannot under-rotate against the yaw slew
603 : // limit, and it needs no teardown when the command ends
604 1 : auto_yaw.set_fixed_yaw_rad(fabsf(circle.turns_signed) * M_2PI, rate_rads,
605 1 : is_negative(rate_degs) ? -1 : 1, true);
606 : }
607 :
608 : // the panorama runs for as long as the commanded turns take at that rate, the same way
609 : // AC_Circle's angle counter measured it
610 1 : circle.panorama_start_ms = millis();
611 1 : circle.panorama_rate_rads = rate_rads;
612 :
613 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: circling %.1f turns", (double)fabsf(circle.turns_signed));
614 :
615 : // set submode to circle
616 1 : set_submode(SubMode::CIRCLE);
617 1 : return;
618 : }
619 :
620 : // no whole turn has been announced yet
621 4 : circle.turns_reported = 0;
622 :
623 4 : const Vector3p center_ned_m = copter.circle_nav->get_center_NED_m();
624 4 : const bool is_terrain_alt = copter.circle_nav->center_is_terrain_alt();
625 :
626 : // start the S-curve orbit leg (origin is the current wp_nav destination, on the circle edge).
627 : // The orbit is flown at the waypoint speed like any other leg, limited by the corner
628 : // acceleration through the arc radius
629 4 : if (!wp_nav->set_circle_destination_NED_m(center_ned_m.xy().tofloat(), circle.turns_signed, center_ned_m.z, is_terrain_alt)) {
630 : // failure to set destination can only be because of missing terrain data
631 0 : copter.failsafe_terrain_on_event();
632 0 : return;
633 : }
634 :
635 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: circling %.1f turns", (double)fabsf(circle.turns_signed));
636 :
637 4 : if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
638 4 : auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
639 : }
640 :
641 : // set submode to circle
642 4 : set_submode(SubMode::CIRCLE);
643 : }
644 :
645 : #if AC_NAV_GUIDED
646 : // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
647 0 : void ModeAuto::nav_guided_start()
648 : {
649 : // call regular guided flight mode initialisation
650 0 : if (!copter.mode_guided.init(true)) {
651 : // this should never happen because guided mode never fails to init
652 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
653 0 : return;
654 : }
655 :
656 : // initialise guided start time and position as reference for limit checking
657 0 : copter.mode_guided.limit_init_time_and_pos();
658 :
659 : // set submode
660 0 : set_submode(SubMode::NAVGUIDED);
661 : }
662 : #endif //AC_NAV_GUIDED
663 :
664 1531481 : bool ModeAuto::is_landing() const
665 : {
666 1531481 : switch(_mode) {
667 123084 : case SubMode::LAND:
668 123084 : return true;
669 358464 : case SubMode::RTL:
670 358464 : return copter.mode_rtl.is_landing();
671 1049933 : default:
672 1049933 : return false;
673 : }
674 : return false;
675 : }
676 :
677 586906 : bool ModeAuto::is_taking_off() const
678 : {
679 586906 : return ((_mode == SubMode::TAKEOFF) && !auto_takeoff.complete);
680 : }
681 :
682 : #if AC_PAYLOAD_PLACE_ENABLED
683 : // auto_payload_place_start - initialises controller to implement a placing
684 1 : void PayloadPlace::start_descent()
685 : {
686 1 : auto *pos_control = copter.pos_control;
687 1 : auto *wp_nav = copter.wp_nav;
688 :
689 : // set horizontal speed and acceleration limits
690 1 : pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
691 1 : pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
692 :
693 : // initialise the vertical position controller
694 1 : if (!pos_control->NE_is_active()) {
695 0 : pos_control->NE_init_controller();
696 : }
697 :
698 : // set vertical speed and acceleration limits
699 1 : pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
700 1 : pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
701 :
702 : // initialise the vertical position controller
703 1 : if (!pos_control->D_is_active()) {
704 0 : pos_control->D_init_controller();
705 : }
706 :
707 : // initialise yaw
708 1 : copter.flightmode->auto_yaw.set_mode(Mode::AutoYaw::Mode::HOLD);
709 :
710 1 : state = PayloadPlace::State::Descent_Start;
711 1 : }
712 : #endif
713 :
714 : // returns true if pilot's yaw input should be used to adjust vehicle's heading
715 2936403 : bool ModeAuto::use_pilot_yaw(void) const
716 : {
717 : // use option bit except during land and RTL
718 2936403 : switch (_mode) {
719 134457 : case SubMode::LAND:
720 134457 : return copter.mode_land.use_pilot_yaw();
721 918671 : case SubMode::RTL:
722 918671 : return copter.mode_rtl.use_pilot_yaw();
723 : #if AC_NAV_GUIDED
724 0 : case SubMode::NAVGUIDED:
725 0 : return copter.mode_guided.use_pilot_yaw();
726 : #endif
727 1883275 : default:
728 1883275 : return !option_is_enabled(Option::IgnorePilotYaw);
729 : }
730 : }
731 :
732 233 : bool ModeAuto::set_speed_NE_ms(float speed_ne_ms)
733 : {
734 233 : copter.wp_nav->set_speed_NE_ms(speed_ne_ms);
735 233 : desired_speed_override_ms.xy = speed_ne_ms;
736 233 : return true;
737 : }
738 :
739 0 : bool ModeAuto::set_speed_up_ms(float speed_up_ms)
740 : {
741 0 : copter.wp_nav->set_speed_up_ms(speed_up_ms);
742 0 : desired_speed_override_ms.up = speed_up_ms;
743 0 : return true;
744 : }
745 :
746 0 : bool ModeAuto::set_speed_down_ms(float speed_down_ms)
747 : {
748 0 : copter.wp_nav->set_speed_down_ms(speed_down_ms);
749 0 : desired_speed_override_ms.down = speed_down_ms;
750 0 : return true;
751 : }
752 :
753 : // start_command - this function will be called when the ap_mission lib wishes to start a new command
754 426 : bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
755 : {
756 426 : switch(cmd.id) {
757 :
758 : ///
759 : /// navigation commands
760 : ///
761 92 : case MAV_CMD_NAV_VTOL_TAKEOFF:
762 : case MAV_CMD_NAV_TAKEOFF: // 22
763 92 : do_takeoff(cmd);
764 92 : break;
765 :
766 215 : case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint
767 : case MAV_CMD_NAV_ARC_WAYPOINT: // 36 Navigate to waypoint via an arc
768 215 : do_nav_wp(cmd);
769 215 : break;
770 :
771 11 : case MAV_CMD_NAV_VTOL_LAND:
772 : case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
773 11 : do_land(cmd);
774 11 : break;
775 :
776 0 : case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
777 0 : do_loiter_unlimited(cmd);
778 0 : break;
779 :
780 5 : case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times
781 5 : do_circle(cmd);
782 5 : break;
783 :
784 5 : case MAV_CMD_NAV_LOITER_TIME: // 19
785 5 : do_loiter_time(cmd);
786 5 : break;
787 :
788 2 : case MAV_CMD_NAV_LOITER_TO_ALT:
789 2 : do_loiter_to_alt(cmd);
790 2 : break;
791 :
792 46 : case MAV_CMD_NAV_RETURN_TO_LAUNCH: //20
793 46 : do_RTL();
794 46 : break;
795 :
796 24 : case MAV_CMD_NAV_SPLINE_WAYPOINT: // 82 Navigate to Waypoint using spline
797 24 : do_spline_wp(cmd);
798 24 : break;
799 :
800 : #if AC_NAV_GUIDED
801 0 : case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer
802 0 : do_nav_guided_enable(cmd);
803 0 : break;
804 : #endif
805 :
806 5 : case MAV_CMD_NAV_DELAY: // 93 Delay the next navigation command
807 5 : do_nav_delay(cmd);
808 5 : break;
809 :
810 : #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED
811 2 : case MAV_CMD_NAV_PAYLOAD_PLACE: // 94 place at Waypoint
812 2 : do_payload_place(cmd);
813 2 : break;
814 : #endif
815 :
816 : #if AP_SCRIPTING_ENABLED
817 0 : case MAV_CMD_NAV_SCRIPT_TIME:
818 0 : do_nav_script_time(cmd);
819 0 : break;
820 : #endif
821 :
822 0 : case MAV_CMD_NAV_ATTITUDE_TIME:
823 0 : do_nav_attitude_time(cmd);
824 0 : break;
825 :
826 : //
827 : // conditional commands
828 : //
829 0 : case MAV_CMD_CONDITION_DELAY: // 112
830 0 : do_wait_delay(cmd);
831 0 : break;
832 :
833 0 : case MAV_CMD_CONDITION_DISTANCE: // 114
834 0 : do_within_distance(cmd);
835 0 : break;
836 :
837 4 : case MAV_CMD_CONDITION_YAW: // 115
838 4 : do_yaw(cmd);
839 4 : break;
840 :
841 : ///
842 : /// do commands
843 : ///
844 8 : case MAV_CMD_DO_CHANGE_SPEED: // 178
845 8 : do_change_speed(cmd);
846 8 : break;
847 :
848 0 : case MAV_CMD_DO_SET_HOME: // 179
849 0 : do_set_home(cmd);
850 0 : break;
851 :
852 0 : case MAV_CMD_DO_SET_ROI_LOCATION: // 195
853 : case MAV_CMD_DO_SET_ROI_NONE: // 197
854 : case MAV_CMD_DO_SET_ROI: // 201
855 : // point the copter and camera at a region of interest (ROI)
856 : // ROI_NONE can be handled by the regular ROI handler because lat, lon, alt are always zero
857 0 : do_roi(cmd);
858 0 : break;
859 :
860 : #if HAL_MOUNT_ENABLED
861 0 : case MAV_CMD_DO_MOUNT_CONTROL: // 205
862 : // point the camera to a specified angle
863 0 : do_mount_control(cmd);
864 0 : break;
865 : #endif
866 :
867 : #if AC_NAV_GUIDED
868 0 : case MAV_CMD_DO_GUIDED_LIMITS: // 220 accept guided mode limits
869 0 : do_guided_limits(cmd);
870 0 : break;
871 : #endif
872 :
873 : #if AP_WINCH_ENABLED
874 0 : case MAV_CMD_DO_WINCH: // Mission command to control winch
875 0 : do_winch(cmd);
876 0 : break;
877 : #endif
878 :
879 7 : case MAV_CMD_DO_RETURN_PATH_START:
880 : case MAV_CMD_DO_LAND_START:
881 7 : break;
882 :
883 0 : default:
884 : // unable to use the command, allow the vehicle to try the next command
885 0 : return false;
886 : }
887 :
888 : // always return success
889 426 : return true;
890 : }
891 :
892 : // exit_mission - function that is called once the mission completes
893 52 : void ModeAuto::exit_mission()
894 : {
895 : // play a tone
896 52 : AP_Notify::events.mission_complete = 1;
897 : // if we are not on the ground switch to loiter or land
898 52 : if (!copter.ap.land_complete) {
899 : // try to enter loiter but if that fails land
900 3 : if (!loiter_start()) {
901 0 : set_mode(Mode::Number::LAND, ModeReason::MISSION_END);
902 : }
903 : } else {
904 : // if we've landed it's safe to disarm
905 49 : copter.arming.disarm(AP_Arming::Method::MISSIONEXIT);
906 : }
907 52 : }
908 :
909 : // do_guided - start guided mode
910 0 : bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
911 : {
912 : // only process guided waypoint if we are in guided mode
913 0 : if (!copter.flightmode->in_guided_mode()) {
914 0 : return false;
915 : }
916 :
917 : // switch to handle different commands
918 0 : switch (cmd.id) {
919 :
920 0 : case MAV_CMD_NAV_WAYPOINT:
921 : {
922 : // set wp_nav's destination
923 0 : Location dest(cmd.content.location);
924 0 : return copter.mode_guided.set_destination(dest);
925 : }
926 :
927 0 : case MAV_CMD_CONDITION_YAW:
928 0 : do_yaw(cmd);
929 0 : return true;
930 :
931 0 : default:
932 : // reject unrecognised command
933 0 : return false;
934 : }
935 :
936 : return true;
937 : }
938 :
939 52149 : float ModeAuto::wp_distance_m() const
940 : {
941 : // the circle orbit is flown as an S-curve waypoint leg, so all submodes use wp_nav
942 52149 : return wp_nav->get_wp_distance_to_destination_m();
943 : }
944 :
945 52149 : float ModeAuto::wp_bearing_deg() const
946 : {
947 52149 : return degrees(wp_nav->get_wp_bearing_to_destination_rad());
948 : }
949 :
950 50107 : bool ModeAuto::get_wp(Location& destination) const
951 : {
952 50107 : switch (_mode) {
953 0 : case SubMode::NAVGUIDED:
954 0 : return copter.mode_guided.get_wp(destination);
955 20639 : case SubMode::WP:
956 : case SubMode::CIRCLE_MOVE_TO_EDGE:
957 : case SubMode::CIRCLE:
958 : // the orbit and the move-to-edge leg are wp_nav legs like WP
959 20639 : return wp_nav->get_oa_wp_destination(destination);
960 17426 : case SubMode::RTL:
961 17426 : return copter.mode_rtl.get_wp(destination);
962 12042 : default:
963 12042 : return false;
964 : }
965 : }
966 :
967 : /*******************************************************************************
968 : Verify command Handlers
969 :
970 : Each type of mission element has a "verify" operation. The verify
971 : operation returns true when the mission element has completed and we
972 : should move onto the next mission element.
973 : Return true if we do not recognize the command so that we move on to the next command
974 : *******************************************************************************/
975 :
976 : // verify_command - callback function called from ap-mission at 10hz or higher when a command is being run
977 : // 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
978 3369496 : bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
979 : {
980 3369496 : if (copter.flightmode != &copter.mode_auto) {
981 0 : return false;
982 : }
983 :
984 3369496 : bool cmd_complete = false;
985 :
986 3369496 : switch (cmd.id) {
987 : //
988 : // navigation commands
989 : //
990 681192 : case MAV_CMD_NAV_VTOL_TAKEOFF:
991 : case MAV_CMD_NAV_TAKEOFF:
992 681192 : cmd_complete = verify_takeoff();
993 681192 : break;
994 :
995 1199774 : case MAV_CMD_NAV_WAYPOINT:
996 : case MAV_CMD_NAV_ARC_WAYPOINT:
997 1199774 : cmd_complete = verify_nav_wp(cmd);
998 1199774 : break;
999 :
1000 202530 : case MAV_CMD_NAV_VTOL_LAND:
1001 : case MAV_CMD_NAV_LAND:
1002 202530 : cmd_complete = verify_land();
1003 202530 : break;
1004 :
1005 : #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED
1006 23788 : case MAV_CMD_NAV_PAYLOAD_PLACE:
1007 23788 : cmd_complete = payload_place.verify();
1008 23788 : break;
1009 : #endif
1010 :
1011 0 : case MAV_CMD_NAV_LOITER_UNLIM:
1012 0 : cmd_complete = verify_loiter_unlimited();
1013 0 : break;
1014 :
1015 77227 : case MAV_CMD_NAV_LOITER_TURNS:
1016 77227 : cmd_complete = verify_circle(cmd);
1017 77227 : break;
1018 :
1019 14710 : case MAV_CMD_NAV_LOITER_TIME:
1020 14710 : cmd_complete = verify_loiter_time(cmd);
1021 14710 : break;
1022 :
1023 14519 : case MAV_CMD_NAV_LOITER_TO_ALT:
1024 14519 : return verify_loiter_to_alt();
1025 :
1026 950728 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
1027 950728 : cmd_complete = verify_RTL();
1028 950728 : break;
1029 :
1030 107716 : case MAV_CMD_NAV_SPLINE_WAYPOINT:
1031 107716 : cmd_complete = verify_spline_wp(cmd);
1032 107716 : break;
1033 :
1034 : #if AC_NAV_GUIDED
1035 0 : case MAV_CMD_NAV_GUIDED_ENABLE:
1036 0 : cmd_complete = verify_nav_guided_enable(cmd);
1037 0 : break;
1038 : #endif
1039 :
1040 87074 : case MAV_CMD_NAV_DELAY:
1041 87074 : cmd_complete = verify_nav_delay(cmd);
1042 87074 : break;
1043 :
1044 : #if AP_SCRIPTING_ENABLED
1045 0 : case MAV_CMD_NAV_SCRIPT_TIME:
1046 0 : cmd_complete = verify_nav_script_time();
1047 0 : break;
1048 : #endif
1049 :
1050 0 : case MAV_CMD_NAV_ATTITUDE_TIME:
1051 0 : cmd_complete = verify_nav_attitude_time(cmd);
1052 0 : break;
1053 :
1054 : ///
1055 : /// conditional commands
1056 : ///
1057 0 : case MAV_CMD_CONDITION_DELAY:
1058 0 : cmd_complete = verify_wait_delay();
1059 0 : break;
1060 :
1061 0 : case MAV_CMD_CONDITION_DISTANCE:
1062 0 : cmd_complete = verify_within_distance();
1063 0 : break;
1064 :
1065 10225 : case MAV_CMD_CONDITION_YAW:
1066 10225 : cmd_complete = verify_yaw();
1067 10225 : break;
1068 :
1069 : // do commands (always return true)
1070 13 : case MAV_CMD_DO_CHANGE_SPEED:
1071 : case MAV_CMD_DO_SET_HOME:
1072 : case MAV_CMD_DO_SET_ROI_LOCATION:
1073 : case MAV_CMD_DO_SET_ROI_NONE:
1074 : case MAV_CMD_DO_SET_ROI:
1075 : #if HAL_MOUNT_ENABLED
1076 : case MAV_CMD_DO_MOUNT_CONTROL:
1077 : #endif
1078 : #if AC_NAV_GUIDED
1079 : case MAV_CMD_DO_GUIDED_LIMITS:
1080 : #endif
1081 : #if AP_FENCE_ENABLED
1082 : case MAV_CMD_DO_FENCE_ENABLE:
1083 : #endif
1084 : #if AP_WINCH_ENABLED
1085 : case MAV_CMD_DO_WINCH:
1086 : #endif
1087 : case MAV_CMD_DO_RETURN_PATH_START:
1088 : case MAV_CMD_DO_LAND_START:
1089 13 : cmd_complete = true;
1090 13 : break;
1091 :
1092 0 : default:
1093 : // error message
1094 0 : gcs().send_text(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
1095 : // return true if we do not recognize the command so that we move on to the next command
1096 0 : cmd_complete = true;
1097 0 : break;
1098 : }
1099 :
1100 :
1101 : // send message to GCS
1102 3354977 : if (cmd_complete) {
1103 348 : gcs().send_mission_item_reached_message(cmd.index);
1104 : }
1105 :
1106 3354977 : return cmd_complete;
1107 : }
1108 :
1109 : // takeoff_run - takeoff in auto mode
1110 : // called by auto_run at 100hz or more
1111 685267 : void ModeAuto::takeoff_run()
1112 : {
1113 : // if the user doesn't want to raise the throttle we can set it automatically
1114 : // note that this can defeat the disarm check on takeoff
1115 685267 : if (option_is_enabled(Option::AllowTakeOffWithoutRaisingThrottle)) {
1116 548278 : copter.set_auto_armed(true);
1117 : }
1118 685267 : auto_takeoff.run();
1119 685267 : }
1120 :
1121 : // auto_wp_run - runs the auto waypoint controller
1122 : // called by auto_run at 100hz or more
1123 1508159 : void ModeAuto::wp_run()
1124 : {
1125 : // if not armed set throttle to zero and exit immediately
1126 1508159 : if (is_disarmed_or_landed()) {
1127 17764 : make_safe_ground_handling();
1128 17764 : return;
1129 : }
1130 :
1131 : // set motors to full range
1132 1490395 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1133 :
1134 : // run waypoint controller
1135 1490395 : copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
1136 :
1137 : // WP_Nav has set the vertical position control targets
1138 : // run the vertical position controller and set output throttle
1139 1490395 : pos_control->D_update_controller();
1140 :
1141 : // call attitude controller with auto yaw
1142 1490395 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
1143 : }
1144 :
1145 : // auto_land_run - lands in auto mode
1146 : // called by auto_run at 100hz or more
1147 151476 : void ModeAuto::land_run()
1148 : {
1149 :
1150 : // if not armed set throttle to zero and exit immediately
1151 151476 : if (is_disarmed_or_landed()) {
1152 17019 : make_safe_ground_handling();
1153 17019 : return;
1154 : }
1155 :
1156 : // set motors to full range
1157 134457 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1158 :
1159 : // run normal landing or precision landing (if enabled)
1160 134457 : land_run_normal_or_precland();
1161 : }
1162 :
1163 : // auto_rtl_run - rtl in AUTO flight mode
1164 : // called by auto_run at 100hz or more
1165 1014590 : void ModeAuto::rtl_run()
1166 : {
1167 : // call regular rtl flight mode run function
1168 1014590 : copter.mode_rtl.run(false);
1169 1014590 : }
1170 :
1171 : #if AC_NAV_GUIDED || AP_SCRIPTING_ENABLED
1172 : // auto_nav_guided_run - allows control by external navigation controller
1173 : // called by auto_run at 100hz or more
1174 0 : void ModeAuto::nav_guided_run()
1175 : {
1176 : // call regular guided flight mode run function
1177 0 : copter.mode_guided.run();
1178 0 : }
1179 : #endif // AC_NAV_GUIDED || AP_SCRIPTING_ENABLED
1180 :
1181 : // auto_loiter_run - loiter in AUTO flight mode
1182 : // called by auto_run at 100hz or more
1183 91648 : void ModeAuto::loiter_run()
1184 : {
1185 : // if not armed set throttle to zero and exit immediately
1186 91648 : if (is_disarmed_or_landed()) {
1187 75110 : make_safe_ground_handling();
1188 75110 : return;
1189 : }
1190 :
1191 : // set motors to full range
1192 16538 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1193 :
1194 : // run waypoint and z-axis position controller
1195 16538 : copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
1196 :
1197 16538 : pos_control->D_update_controller();
1198 :
1199 : // call attitude controller with auto yaw
1200 16538 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
1201 : }
1202 :
1203 : // auto_loiter_run - loiter to altitude in AUTO flight mode
1204 : // called by auto_run at 100hz or more
1205 14519 : void ModeAuto::loiter_to_alt_run()
1206 : {
1207 : // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
1208 14519 : if (is_disarmed_or_landed() || !motors->get_interlock()) {
1209 0 : make_safe_ground_handling();
1210 0 : return;
1211 : }
1212 :
1213 : // possibly just run the waypoint controller:
1214 14519 : if (!loiter_to_alt.reached_destination_xy) {
1215 3642 : loiter_to_alt.reached_destination_xy = wp_nav->reached_wp_destination_NE();
1216 3642 : if (!loiter_to_alt.reached_destination_xy) {
1217 3640 : wp_run();
1218 3640 : return;
1219 : }
1220 : }
1221 :
1222 10879 : if (!loiter_to_alt.loiter_start_done) {
1223 : // set horizontal speed and acceleration limits
1224 2 : pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
1225 2 : pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
1226 :
1227 2 : if (!pos_control->NE_is_active()) {
1228 0 : pos_control->NE_init_controller();
1229 : }
1230 :
1231 2 : loiter_to_alt.loiter_start_done = true;
1232 : }
1233 10879 : const float alt_error_m = copter.current_loc.alt * 0.01 - loiter_to_alt.alt_m;
1234 10879 : if (fabsf(alt_error_m) < 0.05) { // random numbers R US
1235 2 : loiter_to_alt.reached_alt = true;
1236 10877 : } else if (alt_error_m * loiter_to_alt.alt_error_m < 0) {
1237 : // we were above and are now below, or vice-versa
1238 0 : loiter_to_alt.reached_alt = true;
1239 : }
1240 10879 : loiter_to_alt.alt_error_m = alt_error_m;
1241 :
1242 : // loiter...
1243 :
1244 10879 : land_run_horizontal_control();
1245 :
1246 : // Compute a vertical velocity demand such that the vehicle
1247 : // approaches the desired altitude.
1248 10879 : float target_climb_rate_ms = sqrt_controller(
1249 : -alt_error_m,
1250 10879 : pos_control->D_get_pos_p().kP(),
1251 10879 : pos_control->D_get_max_accel_mss(),
1252 10879 : G_Dt);
1253 10879 : target_climb_rate_ms = constrain_float(target_climb_rate_ms, -pos_control->get_max_speed_down_ms(), pos_control->get_max_speed_up_ms());
1254 :
1255 : // get avoidance adjusted climb rate
1256 10879 : target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
1257 :
1258 : #if AP_RANGEFINDER_ENABLED
1259 : // update the vertical offset based on the surface measurement
1260 10879 : copter.surface_tracking.update_surface_offset();
1261 : #endif
1262 :
1263 : // Send the commanded climb rate to the position controller
1264 10879 : pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms);
1265 :
1266 10879 : pos_control->D_update_controller();
1267 : }
1268 :
1269 : // maintain an attitude for a specified time
1270 0 : void ModeAuto::nav_attitude_time_run()
1271 : {
1272 : // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
1273 0 : if (is_disarmed_or_landed() || !motors->get_interlock()) {
1274 0 : make_safe_ground_handling();
1275 0 : return;
1276 : }
1277 :
1278 : // constrain climb rate
1279 0 : float target_climb_rate_ms = constrain_float(nav_attitude_time.climb_rate_ms, -pos_control->get_max_speed_down_ms(), pos_control->get_max_speed_up_ms());
1280 :
1281 : // get avoidance adjusted climb rate
1282 0 : target_climb_rate_ms = get_avoidance_adjusted_climbrate_ms(target_climb_rate_ms);
1283 :
1284 : // limit and scale lean angles
1285 : // todo: change euler magnitiude limit to lean angle limit
1286 0 : const float angle_limit_rad = MAX(radians(10.0f), MIN(attitude_control->lean_angle_max_rad(), attitude_control->get_althold_lean_angle_max_rad()));
1287 0 : Vector2f target_rp_rad(radians(nav_attitude_time.roll_deg), radians(nav_attitude_time.pitch_deg));
1288 0 : target_rp_rad.limit_length(angle_limit_rad);
1289 :
1290 : // send targets to attitude controller
1291 0 : attitude_control->input_euler_angle_roll_pitch_yaw_rad(target_rp_rad.x, target_rp_rad.y, radians(nav_attitude_time.yaw_deg), true);
1292 :
1293 : // Send the commanded climb rate to the position controller
1294 0 : pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms);
1295 :
1296 0 : pos_control->D_update_controller();
1297 : }
1298 :
1299 : #if AC_PAYLOAD_PLACE_ENABLED
1300 : // auto_payload_place_run - places an object in auto mode
1301 : // called by auto_run at 100hz or more
1302 23788 : void PayloadPlace::run()
1303 : {
1304 23788 : const char* prefix_str = "PayloadPlace:";
1305 :
1306 23788 : if (copter.flightmode->is_disarmed_or_landed()) {
1307 0 : copter.flightmode->make_safe_ground_handling();
1308 3440 : return;
1309 : }
1310 :
1311 : // set motors to full range
1312 23788 : copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1313 :
1314 23788 : const uint32_t descent_thrust_cal_duration_ms = 2000; // milliseconds
1315 23788 : const uint32_t placed_check_duration_ms = 500; // how long we have to be below a throttle threshold before considering placed
1316 :
1317 23788 : auto &g2 = copter.g2;
1318 23788 : auto *attitude_control = copter.attitude_control;
1319 23788 : const auto &pos_control = copter.pos_control;
1320 23788 : const auto &wp_nav = copter.wp_nav;
1321 :
1322 : // Vertical thrust is taken from the attitude controller before angle boost is added
1323 23788 : const float thrust_level = attitude_control->get_throttle_in();
1324 23788 : const uint32_t now_ms = AP_HAL::millis();
1325 :
1326 : // relax position target if we might be landed
1327 : // if we discover we've landed then immediately release the load:
1328 23788 : if (copter.ap.land_complete || copter.ap.land_complete_maybe) {
1329 0 : pos_control->NE_soften_for_landing();
1330 0 : switch (state) {
1331 0 : case State::FlyToLocation:
1332 : // this is handled in wp_run()
1333 0 : break;
1334 0 : case State::Descent_Start:
1335 : // do nothing on this loop
1336 0 : break;
1337 0 : case State::Descent:
1338 0 : gcs().send_text(MAV_SEVERITY_INFO, "%s landed", prefix_str);
1339 0 : state = State::Release;
1340 0 : break;
1341 0 : case State::Release:
1342 : case State::Releasing:
1343 : case State::Delay:
1344 : case State::Ascent_Start:
1345 : case State::Ascent:
1346 : case State::Done:
1347 0 : break;
1348 : }
1349 : }
1350 :
1351 : #if AP_GRIPPER_ENABLED
1352 : // if pilot releases load manually:
1353 23788 : if (AP::gripper().valid() && AP::gripper().released()) {
1354 4118 : switch (state) {
1355 1 : case State::FlyToLocation:
1356 : case State::Descent_Start:
1357 1 : gcs().send_text(MAV_SEVERITY_INFO, "%s Abort: Gripper Open", prefix_str);
1358 : // Descent_Start has not run so we must also initialise descent_start_altitude_m
1359 1 : descent_start_altitude_m = pos_control->get_pos_desired_U_m();
1360 1 : state = State::Done;
1361 1 : break;
1362 0 : case State::Descent:
1363 0 : gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str);
1364 0 : state = State::Release;
1365 0 : break;
1366 4117 : case State::Release:
1367 : case State::Releasing:
1368 : case State::Delay:
1369 : case State::Ascent_Start:
1370 : case State::Ascent:
1371 : case State::Done:
1372 4117 : break;
1373 : }
1374 : }
1375 : #endif
1376 :
1377 23788 : switch (state) {
1378 3441 : case State::FlyToLocation:
1379 3441 : if (copter.wp_nav->reached_wp_destination()) {
1380 1 : start_descent();
1381 : }
1382 3441 : break;
1383 :
1384 1 : case State::Descent_Start:
1385 1 : descent_established_time_ms = now_ms;
1386 1 : descent_start_altitude_m = pos_control->get_pos_desired_U_m();
1387 : // limiting the decent rate to the limit set in wp_nav is not necessary but done for safety
1388 1 : descent_speed_ms = MIN((is_positive(g2.pldp_descent_speed_ms)) ? g2.pldp_descent_speed_ms : copter.mode_land.get_land_speed_ms(), wp_nav->get_default_speed_down_ms());
1389 1 : descent_thrust_level = 1.0;
1390 1 : state = State::Descent;
1391 : FALLTHROUGH;
1392 :
1393 16014 : case State::Descent:
1394 : // check maximum decent distance
1395 16014 : if (!is_zero(descent_max_m) &&
1396 0 : descent_start_altitude_m - pos_control->get_pos_desired_U_m() > descent_max_m) {
1397 0 : state = State::Ascent_Start;
1398 0 : gcs().send_text(MAV_SEVERITY_WARNING, "%s Reached maximum descent", prefix_str);
1399 0 : break;
1400 : }
1401 : // calibrate the decent thrust after aircraft has reached constant decent rate and release if threshold is reached
1402 16014 : if (pos_control->get_vel_desired_U_ms() > -0.95 * descent_speed_ms) {
1403 : // decent rate has not reached descent_speed_ms
1404 326 : descent_established_time_ms = now_ms;
1405 326 : break;
1406 15688 : } else if (now_ms - descent_established_time_ms < descent_thrust_cal_duration_ms) {
1407 : // record minimum thrust for descent_thrust_cal_duration_ms
1408 799 : descent_thrust_level = MIN(descent_thrust_level, thrust_level);
1409 799 : place_start_time_ms = now_ms;
1410 799 : break;
1411 14889 : } else if (thrust_level > g2.pldp_thrust_placed_fraction * descent_thrust_level) {
1412 : // thrust is above minimum threshold
1413 14688 : place_start_time_ms = now_ms;
1414 14688 : break;
1415 201 : } else if (is_positive(g2.pldp_range_finder_maximum_m)) {
1416 0 : if (!copter.rangefinder_state.enabled) {
1417 : // abort payload place because rangefinder is not enabled
1418 0 : state = State::Ascent_Start;
1419 0 : gcs().send_text(MAV_SEVERITY_WARNING, "%s PLDP_RNG_MAX set and rangefinder not enabled", prefix_str);
1420 0 : break;
1421 0 : } else if (copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0) && (copter.rangefinder_state.alt_m > g2.pldp_range_finder_maximum_m)) {
1422 : // range finder altitude is above maximum
1423 0 : place_start_time_ms = now_ms;
1424 0 : break;
1425 : }
1426 : }
1427 :
1428 : // If we get here:
1429 : // 1. we have reached decent velocity
1430 : // 2. measured the thrust level required for decent
1431 : // 3. detected that our thrust requirements have reduced
1432 : // 4. rangefinder range has dropped below minimum if set
1433 : // 5. place_start_time_ms has been initialised
1434 :
1435 : // payload touchdown must be detected for 0.5 seconds
1436 :
1437 201 : if (now_ms - place_start_time_ms > placed_check_duration_ms) {
1438 1 : state = State::Release;
1439 1 : gcs().send_text(MAV_SEVERITY_INFO, "%s payload release thrust threshold: %f", prefix_str, static_cast<double>(g2.pldp_thrust_placed_fraction * descent_thrust_level));
1440 : }
1441 201 : break;
1442 :
1443 1 : case State::Release:
1444 : // Reinitialise vertical position controller to remove discontinuity due to touch down of payload
1445 1 : pos_control->D_init_controller_no_descent();
1446 : #if AP_GRIPPER_ENABLED
1447 1 : if (AP::gripper().valid()) {
1448 1 : gcs().send_text(MAV_SEVERITY_INFO, "%s Releasing the gripper", prefix_str);
1449 1 : AP::gripper().release();
1450 1 : state = State::Releasing;
1451 : } else {
1452 0 : state = State::Delay;
1453 : }
1454 : #else
1455 : state = State::Delay;
1456 : #endif
1457 1 : break;
1458 :
1459 215 : case State::Releasing:
1460 : #if AP_GRIPPER_ENABLED
1461 215 : if (AP::gripper().valid() && !AP::gripper().released()) {
1462 214 : break;
1463 : }
1464 : #endif
1465 1 : state = State::Delay;
1466 : FALLTHROUGH;
1467 :
1468 1 : case State::Delay:
1469 : // If we get here we have finished releasing the gripper
1470 1 : if (now_ms - place_start_time_ms < placed_check_duration_ms + g2.pldp_delay_s * 1000.0) {
1471 0 : break;
1472 : }
1473 : FALLTHROUGH;
1474 :
1475 : case State::Ascent_Start:
1476 1 : state = State::Ascent;
1477 : FALLTHROUGH;
1478 :
1479 4117 : case State::Ascent: {
1480 : // Ascent complete when we are less than 10% of the stopping
1481 : // distance from the target altitude stopping distance from
1482 : // vel_threshold_fraction * max velocity
1483 4117 : const float vel_threshold_fraction = 0.1;
1484 4117 : const float stop_distance_m = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_ms()) / copter.pos_control->D_get_max_accel_mss();
1485 4117 : bool reached_altitude = pos_control->get_pos_desired_U_m() >= descent_start_altitude_m - stop_distance_m;
1486 4117 : if (reached_altitude) {
1487 1 : state = State::Done;
1488 : }
1489 4117 : break;
1490 : }
1491 1 : case State::Done:
1492 1 : break;
1493 0 : default:
1494 : // this should never happen
1495 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1496 0 : break;
1497 : }
1498 :
1499 23788 : switch (state) {
1500 3440 : case State::FlyToLocation:
1501 : // this should never happen
1502 3440 : return copter.mode_auto.wp_run();
1503 16014 : case State::Descent_Start:
1504 : case State::Descent:
1505 16014 : copter.flightmode->land_run_horizontal_control();
1506 : // update altitude target and call position controller
1507 16014 : pos_control->D_set_pos_target_from_climb_rate_ms(-descent_speed_ms, true);
1508 20348 : break;
1509 216 : case State::Release:
1510 : case State::Releasing:
1511 : case State::Delay:
1512 : case State::Ascent_Start:
1513 216 : copter.flightmode->land_run_horizontal_control();
1514 : // update altitude target and call position controller
1515 216 : pos_control->D_set_pos_target_from_climb_rate_ms(0.0);
1516 216 : break;
1517 4118 : case State::Ascent:
1518 : case State::Done:
1519 4118 : float vel_d_zero = 0.0;
1520 4118 : copter.flightmode->land_run_horizontal_control();
1521 4118 : float pos_d_m = -descent_start_altitude_m;
1522 4118 : pos_control->input_pos_vel_accel_D_m(pos_d_m, vel_d_zero, 0.0);
1523 4118 : break;
1524 : }
1525 20348 : pos_control->D_update_controller();
1526 : }
1527 : #endif
1528 :
1529 : // subtract position controller offsets from target location
1530 : // should be used when the location will be used as a target for the position controller
1531 260 : void ModeAuto::subtract_pos_offsets(Location& target_loc) const
1532 : {
1533 : // subtract position controller offsets from target location
1534 260 : const Vector3p& pos_ofs_ned_m = pos_control->get_pos_offset_NED_m();
1535 260 : target_loc.offset(-pos_ofs_ned_m);
1536 260 : }
1537 :
1538 : /********************************************************************************/
1539 : // Nav (Must) commands
1540 : /********************************************************************************/
1541 :
1542 : // do_takeoff - initiate takeoff navigation command
1543 92 : void ModeAuto::do_takeoff(const AP_Mission::Mission_Command& cmd)
1544 : {
1545 : // Set wp navigation target to safe altitude above current position
1546 92 : takeoff_start(cmd.content.location);
1547 92 : }
1548 :
1549 : // get the Location portion of a command. If the command's lat and lon and/or alt are zero the default_loc's lat,lon and/or alt are returned instead
1550 : // returns false if the location cannot be determined which only happens if the terrain data is unavailable
1551 605 : bool ModeAuto::get_loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& loc) const
1552 : {
1553 605 : loc = cmd.content.location;
1554 :
1555 : // use current lat, lon if zero
1556 605 : if (loc.lat == 0 && loc.lng == 0) {
1557 72 : loc.lat = default_loc.lat;
1558 72 : loc.lng = default_loc.lng;
1559 : }
1560 :
1561 : // use default altitude if not provided in cmd
1562 605 : if (loc.alt == 0) {
1563 : // set to default_loc's altitude but in command's alt frame
1564 : // note that this may use the terrain database
1565 : float default_alt_m;
1566 76 : if (!default_loc.get_alt_m(loc.get_alt_frame(), default_alt_m)) {
1567 1 : return false;
1568 : }
1569 75 : loc.set_alt_m(default_alt_m, loc.get_alt_frame());
1570 : }
1571 :
1572 604 : return true;
1573 : }
1574 :
1575 : // do_nav_wp - initiate move to next waypoint
1576 215 : void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
1577 : {
1578 : // calculate default location used when lat, lon or alt is zero
1579 215 : Location default_loc = copter.current_loc;
1580 :
1581 : // subtract position offsets
1582 215 : subtract_pos_offsets(default_loc);
1583 :
1584 215 : if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
1585 146 : if (!wp_nav->get_wp_destination_loc(default_loc)) {
1586 : // this should never happen
1587 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1588 : }
1589 : }
1590 :
1591 : // get waypoint's location from command and send to wp_nav
1592 215 : Location target_loc;
1593 215 : if (!get_loc_from_cmd(cmd, default_loc, target_loc) || !wp_start(target_loc)) {
1594 : // failure to get the location or set next destination can only be because of missing terrain data or unhealthy rangefinder
1595 1 : copter.failsafe_terrain_on_event();
1596 1 : return;
1597 : }
1598 :
1599 : // this will be used to remember the time in millis after we reach or pass the WP.
1600 214 : loiter_time = 0;
1601 : // this is the delay, stored in seconds
1602 214 : if (cmd.id == MAV_CMD_NAV_ARC_WAYPOINT) {
1603 37 : loiter_time_max = 0;
1604 : } else {
1605 177 : loiter_time_max = cmd.p1;
1606 : }
1607 :
1608 : // set next destination if necessary
1609 214 : if (!set_next_wp(cmd, target_loc)) {
1610 : // failure to set next destination can only be because of missing terrain data
1611 0 : copter.failsafe_terrain_on_event();
1612 0 : return;
1613 : }
1614 : }
1615 :
1616 : // checks the next mission command and adds it as a destination if necessary
1617 : // supports both straight line and spline waypoints
1618 : // cmd should be the current command
1619 : // default_loc should be the destination from the current_cmd but corrected for cases where user set lat, lon or alt to zero
1620 : // returns true on success, false on failure which should only happen due to a failure to retrieve terrain data
1621 238 : bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc)
1622 : {
1623 : // Do not add the next WP if the current command includes a delay (p1 > 0).
1624 : // Only MAV_CMD_NAV_WAYPOINT and MAV_CMD_NAV_SPLINE_WAYPOINT use p1 as a delay.
1625 238 : if (current_cmd.p1 > 0 && (current_cmd.id == MAV_CMD_NAV_WAYPOINT || current_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT)) {
1626 23 : return true;
1627 : }
1628 :
1629 : // do not add next wp if there are no more navigation commands
1630 215 : AP_Mission::Mission_Command next_cmd;
1631 215 : if (!mission.get_next_nav_cmd(current_cmd.index+1, next_cmd)) {
1632 11 : return true;
1633 : }
1634 :
1635 : // whether vehicle should stop at the target position depends upon the next command
1636 204 : switch (next_cmd.id) {
1637 11 : case MAV_CMD_NAV_VTOL_LAND:
1638 : case MAV_CMD_NAV_LAND:
1639 : // ensure landing alt is zero so it is populated from the current altitude
1640 11 : next_cmd.content.location.alt = 0;
1641 : FALLTHROUGH;
1642 119 : case MAV_CMD_NAV_WAYPOINT:
1643 : case MAV_CMD_NAV_LOITER_UNLIM:
1644 : #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED
1645 : case MAV_CMD_NAV_PAYLOAD_PLACE:
1646 : #endif
1647 : case MAV_CMD_NAV_LOITER_TIME: {
1648 119 : Location dest_loc;
1649 119 : Location next_dest_loc;
1650 238 : if (!get_loc_from_cmd(current_cmd, default_loc, dest_loc) ||
1651 119 : !get_loc_from_cmd(next_cmd, dest_loc, next_dest_loc)) {
1652 0 : return false;
1653 : }
1654 119 : return wp_nav->set_wp_destination_next_loc(next_dest_loc);
1655 : }
1656 14 : case MAV_CMD_NAV_SPLINE_WAYPOINT: {
1657 : // get spline's location and next location from command and send to wp_nav
1658 14 : Location next_dest_loc, next_next_dest_loc;
1659 : bool next_next_dest_loc_is_spline;
1660 14 : if (!get_spline_from_cmd(next_cmd, default_loc, next_dest_loc, next_next_dest_loc, next_next_dest_loc_is_spline)) {
1661 0 : return false;
1662 : }
1663 14 : return wp_nav->set_spline_destination_next_loc(next_dest_loc, next_next_dest_loc, next_next_dest_loc_is_spline);
1664 : }
1665 31 : case MAV_CMD_NAV_ARC_WAYPOINT: {
1666 31 : Location dest_loc;
1667 31 : Location next_dest_loc;
1668 62 : if (!get_loc_from_cmd(current_cmd, default_loc, dest_loc) ||
1669 31 : !get_loc_from_cmd(next_cmd, dest_loc, next_dest_loc)) {
1670 0 : return false;
1671 : }
1672 31 : const float arc_angle_rad = next_cmd.get_arc_angle_rad();
1673 31 : return wp_nav->set_wp_destination_next_loc(next_dest_loc, arc_angle_rad);
1674 : }
1675 40 : case MAV_CMD_NAV_LOITER_TURNS:
1676 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
1677 : case MAV_CMD_NAV_VTOL_TAKEOFF:
1678 : case MAV_CMD_NAV_TAKEOFF:
1679 : // always stop for RTL and takeoff commands
1680 : default:
1681 : // for unsupported commands it is safer to stop
1682 40 : break;
1683 : }
1684 :
1685 40 : return true;
1686 : }
1687 :
1688 : // do_land - initiate landing procedure
1689 11 : void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
1690 : {
1691 : // To-Do: check if we have already landed
1692 :
1693 : // if location provided we fly to that location at current altitude
1694 11 : if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
1695 : // set state to fly to location
1696 7 : state = State::FlyToLocation;
1697 :
1698 : // calculate default location used when alt is zero
1699 7 : Location default_loc = copter.current_loc;
1700 7 : subtract_pos_offsets(default_loc);
1701 :
1702 : // ensure landing alt is zero so it is populated from the current altitude
1703 7 : AP_Mission::Mission_Command cmd_alt_zero = cmd;
1704 7 : cmd_alt_zero.content.location.alt = 0;
1705 :
1706 : // get location from command and send to wp_nav
1707 7 : Location target_loc;
1708 7 : if (!get_loc_from_cmd(cmd_alt_zero, default_loc, target_loc) || !wp_start(target_loc)) {
1709 : // failure to get location or set next destination can only be because of missing terrain data
1710 0 : copter.failsafe_terrain_on_event();
1711 0 : return;
1712 : }
1713 7 : } else {
1714 : // set landing state
1715 4 : state = State::Descending;
1716 :
1717 : // initialise landing controller
1718 4 : land_start();
1719 : }
1720 : }
1721 :
1722 : // do_loiter_unlimited - start loitering with no end conditions
1723 : // note: caller should set yaw_mode
1724 7 : bool ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
1725 : {
1726 : // calculate default location used when lat, lon or alt is zero
1727 7 : Location default_loc = copter.current_loc;
1728 :
1729 : // subtract position offsets
1730 7 : subtract_pos_offsets(default_loc);
1731 :
1732 : // use previous waypoint destination as default if available
1733 7 : if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
1734 4 : if (!wp_nav->get_wp_destination_loc(default_loc)) {
1735 : // this should never happen
1736 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1737 : }
1738 : }
1739 :
1740 : // get location from command and send to wp_nav
1741 7 : Location target_loc;
1742 7 : if (!get_loc_from_cmd(cmd, default_loc, target_loc) || !wp_start(target_loc)) {
1743 : // failure to get location or set next destination can only be because of missing terrain data
1744 0 : copter.failsafe_terrain_on_event();
1745 0 : return false;
1746 : }
1747 :
1748 7 : return true;
1749 : }
1750 :
1751 : // do_circle - initiate moving in a circle
1752 5 : void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
1753 : {
1754 : // calculate default location used when lat, lon or alt is zero
1755 5 : Location default_loc = copter.current_loc;
1756 :
1757 : // subtract position offsets
1758 5 : subtract_pos_offsets(default_loc);
1759 :
1760 5 : Location circle_center;
1761 5 : if (!get_loc_from_cmd(cmd, default_loc, circle_center)) {
1762 : // failure to get location can only be because of missing terrain data
1763 0 : copter.failsafe_terrain_on_event();
1764 0 : return;
1765 : }
1766 :
1767 : // calculate radius
1768 5 : uint16_t radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
1769 5 : if (cmd.id == MAV_CMD_NAV_LOITER_TURNS &&
1770 5 : cmd.type_specific_bits & (1U << 0)) {
1771 : // special storage handling allows for larger radii
1772 0 : radius_m *= 10;
1773 : }
1774 :
1775 : // true if circle should be ccw
1776 5 : const bool circle_direction_ccw = cmd.content.location.loiter_ccw;
1777 :
1778 : // signed number of turns for the S-curve orbit (sign selects direction, matching arc waypoints)
1779 5 : circle.turns_signed = (circle_direction_ccw ? -1.0f : 1.0f) * cmd.get_loiter_turns();
1780 :
1781 : // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
1782 5 : circle_movetoedge_start(circle_center, radius_m);
1783 : }
1784 :
1785 : // do_loiter_time - initiate loitering at a point for a given time period
1786 : // note: caller should set yaw_mode
1787 5 : void ModeAuto::do_loiter_time(const AP_Mission::Mission_Command& cmd)
1788 : {
1789 : // re-use loiter unlimited
1790 5 : if (!do_loiter_unlimited(cmd)) {
1791 0 : return;
1792 : }
1793 :
1794 : // setup loiter timer
1795 5 : loiter_time = 0;
1796 5 : loiter_time_max = cmd.p1; // units are (seconds)
1797 : }
1798 :
1799 : // do_loiter_alt - initiate loitering at a point until a given altitude is reached
1800 : // note: caller should set yaw_mode
1801 2 : void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
1802 : {
1803 : // re-use loiter unlimited
1804 2 : if (!do_loiter_unlimited(cmd)) {
1805 0 : return;
1806 : }
1807 :
1808 : // if we aren't navigating to a location then we have to adjust
1809 : // altitude for current location
1810 2 : Location target_loc(cmd.content.location);
1811 2 : if (target_loc.lat == 0 && target_loc.lng == 0) {
1812 1 : target_loc.lat = copter.current_loc.lat;
1813 1 : target_loc.lng = copter.current_loc.lng;
1814 : }
1815 :
1816 2 : if (!target_loc.get_alt_m(Location::AltFrame::ABOVE_HOME, loiter_to_alt.alt_m)) {
1817 0 : loiter_to_alt.reached_destination_xy = true;
1818 0 : loiter_to_alt.reached_alt = true;
1819 0 : gcs().send_text(MAV_SEVERITY_INFO, "bad do_loiter_to_alt");
1820 0 : return;
1821 : }
1822 2 : loiter_to_alt.reached_destination_xy = false;
1823 2 : loiter_to_alt.loiter_start_done = false;
1824 2 : loiter_to_alt.reached_alt = false;
1825 2 : loiter_to_alt.alt_error_m = 0;
1826 :
1827 : // set vertical speed and acceleration limits
1828 2 : pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
1829 2 : pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
1830 :
1831 : // set submode
1832 2 : set_submode(SubMode::LOITER_TO_ALT);
1833 : }
1834 :
1835 : // do_spline_wp - initiate move to next waypoint
1836 24 : void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
1837 : {
1838 : // calculate default location used when lat, lon or alt is zero
1839 24 : Location default_loc = copter.current_loc;
1840 :
1841 : // subtract position offsets
1842 24 : subtract_pos_offsets(default_loc);
1843 :
1844 24 : if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
1845 22 : if (!wp_nav->get_wp_destination_loc(default_loc)) {
1846 : // this should never happen
1847 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1848 : }
1849 : }
1850 :
1851 : // get spline's location and next location from command and send to wp_nav
1852 24 : Location dest_loc, next_dest_loc;
1853 : bool next_dest_loc_is_spline;
1854 24 : if (!get_spline_from_cmd(cmd, default_loc, dest_loc, next_dest_loc, next_dest_loc_is_spline)) {
1855 0 : copter.failsafe_terrain_on_event();
1856 0 : return;
1857 : }
1858 24 : if (!wp_nav->set_spline_destination_loc(dest_loc, next_dest_loc, next_dest_loc_is_spline)) {
1859 : // failure to set destination can only be because of missing terrain data
1860 0 : copter.failsafe_terrain_on_event();
1861 0 : return;
1862 : }
1863 :
1864 : // this will be used to remember the time in millis after we reach or pass the WP.
1865 24 : loiter_time = 0;
1866 : // this is the delay, stored in seconds
1867 24 : loiter_time_max = cmd.p1;
1868 :
1869 : // set next destination if necessary
1870 24 : if (!set_next_wp(cmd, dest_loc)) {
1871 : // failure to set next destination can only be because of missing terrain data
1872 0 : copter.failsafe_terrain_on_event();
1873 0 : return;
1874 : }
1875 :
1876 : // initialise yaw
1877 : // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
1878 24 : if (auto_yaw.mode() != AutoYaw::Mode::ROI && !(auto_yaw.mode() == AutoYaw::Mode::FIXED && copter.g.wp_yaw_behavior == Copter::WPYawBehavior::NONE)) {
1879 24 : auto_yaw.set_mode_to_default(false);
1880 : }
1881 :
1882 : // set submode
1883 24 : set_submode(SubMode::WP);
1884 : }
1885 :
1886 : // calculate locations required to build a spline curve from a mission command
1887 : // dest_loc is populated from cmd's location using default_loc in cases where the lat and lon or altitude is zero
1888 : // next_dest_loc and nest_dest_loc_is_spline is filled in with the following navigation command's location if it exists. If it does not exist it is set to the dest_loc and false
1889 38 : bool ModeAuto::get_spline_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc, Location& dest_loc, Location& next_dest_loc, bool& next_dest_loc_is_spline)
1890 : {
1891 38 : if (!get_loc_from_cmd(cmd, default_loc, dest_loc)) {
1892 0 : return false;
1893 : }
1894 :
1895 : // if there is no delay at the end of this segment get next nav command
1896 38 : AP_Mission::Mission_Command temp_cmd;
1897 38 : if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
1898 31 : if (!get_loc_from_cmd(temp_cmd, dest_loc, next_dest_loc)) {
1899 0 : return false;
1900 : }
1901 31 : next_dest_loc_is_spline = temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT;
1902 : } else {
1903 7 : next_dest_loc = dest_loc;
1904 7 : next_dest_loc_is_spline = false;
1905 : }
1906 :
1907 38 : return true;
1908 : }
1909 :
1910 : #if AC_NAV_GUIDED
1911 : // do_nav_guided_enable - initiate accepting commands from external nav computer
1912 0 : void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
1913 : {
1914 0 : if (cmd.p1 > 0) {
1915 : // start guided within auto
1916 0 : nav_guided_start();
1917 : }
1918 0 : }
1919 :
1920 : // do_guided_limits - pass guided limits to guided controller
1921 0 : void ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd)
1922 : {
1923 0 : copter.mode_guided.limit_set(
1924 0 : cmd.p1 * 1000, // convert seconds to ms
1925 0 : cmd.content.guided_limits.alt_min,
1926 0 : cmd.content.guided_limits.alt_max,
1927 0 : cmd.content.guided_limits.horiz_max);
1928 0 : }
1929 : #endif // AC_NAV_GUIDED
1930 :
1931 : // do_nav_delay - Delay the next navigation command
1932 5 : void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
1933 : {
1934 5 : nav_delay_time_start_ms = millis();
1935 :
1936 5 : if (cmd.content.nav_delay.seconds > 0) {
1937 : // relative delay
1938 3 : nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
1939 : } else {
1940 : // absolute delay to utc time
1941 : #if AP_RTC_ENABLED
1942 2 : 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);
1943 : #else
1944 : nav_delay_time_max_ms = 0;
1945 : #endif
1946 : }
1947 5 : gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000));
1948 5 : }
1949 :
1950 : #if AP_SCRIPTING_ENABLED
1951 : // start accepting position, velocity and acceleration targets from lua scripts
1952 0 : void ModeAuto::do_nav_script_time(const AP_Mission::Mission_Command& cmd)
1953 : {
1954 : // call regular guided flight mode initialisation
1955 0 : if (copter.mode_guided.init(true)) {
1956 0 : nav_scripting.done = false;
1957 0 : nav_scripting.id++;
1958 0 : nav_scripting.start_ms = millis();
1959 0 : nav_scripting.command = cmd.content.nav_script_time.command;
1960 0 : nav_scripting.timeout_s = cmd.content.nav_script_time.timeout_s;
1961 0 : nav_scripting.arg1 = cmd.content.nav_script_time.arg1.get();
1962 0 : nav_scripting.arg2 = cmd.content.nav_script_time.arg2.get();
1963 0 : nav_scripting.arg3 = cmd.content.nav_script_time.arg3;
1964 0 : nav_scripting.arg4 = cmd.content.nav_script_time.arg4;
1965 0 : set_submode(SubMode::NAV_SCRIPT_TIME);
1966 : } else {
1967 : // for safety we set nav_scripting to done to protect against the mission getting stuck
1968 0 : nav_scripting.done = true;
1969 : }
1970 0 : }
1971 : #endif
1972 :
1973 : // start maintaining an attitude for a specified time
1974 0 : void ModeAuto::do_nav_attitude_time(const AP_Mission::Mission_Command& cmd)
1975 : {
1976 : // copy command arguments into local structure
1977 0 : nav_attitude_time.roll_deg = cmd.content.nav_attitude_time.roll_deg;
1978 0 : nav_attitude_time.pitch_deg = cmd.content.nav_attitude_time.pitch_deg;
1979 0 : nav_attitude_time.yaw_deg = cmd.content.nav_attitude_time.yaw_deg;
1980 0 : nav_attitude_time.climb_rate_ms = cmd.content.nav_attitude_time.climb_rate;
1981 0 : nav_attitude_time.start_ms = AP_HAL::millis();
1982 0 : set_submode(SubMode::NAV_ATTITUDE_TIME);
1983 0 : }
1984 :
1985 : /********************************************************************************/
1986 : // Condition (May) commands
1987 : /********************************************************************************/
1988 :
1989 0 : void ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd)
1990 : {
1991 0 : condition_start = millis();
1992 0 : condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
1993 0 : }
1994 :
1995 0 : void ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd)
1996 : {
1997 0 : condition_value = cmd.content.distance.meters;
1998 0 : }
1999 :
2000 4 : void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
2001 : {
2002 4 : auto_yaw.set_fixed_yaw_rad(
2003 4 : radians(cmd.content.yaw.angle_deg),
2004 4 : radians(cmd.content.yaw.turn_rate_dps),
2005 4 : cmd.content.yaw.direction,
2006 4 : cmd.content.yaw.relative_angle > 0);
2007 4 : }
2008 :
2009 : /********************************************************************************/
2010 : // Do (Now) commands
2011 : /********************************************************************************/
2012 :
2013 8 : void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
2014 : {
2015 8 : if (cmd.content.speed.target_ms > 0) {
2016 8 : switch (cmd.content.speed.speed_type) {
2017 0 : case SPEED_TYPE_CLIMB_SPEED:
2018 0 : copter.wp_nav->set_speed_up_ms(cmd.content.speed.target_ms);
2019 0 : desired_speed_override_ms.up = cmd.content.speed.target_ms;
2020 0 : break;
2021 0 : case SPEED_TYPE_DESCENT_SPEED:
2022 0 : copter.wp_nav->set_speed_down_ms(cmd.content.speed.target_ms);
2023 0 : desired_speed_override_ms.down = cmd.content.speed.target_ms;
2024 0 : break;
2025 8 : case SPEED_TYPE_AIRSPEED:
2026 : case SPEED_TYPE_GROUNDSPEED:
2027 8 : copter.wp_nav->set_speed_NE_ms(cmd.content.speed.target_ms);
2028 8 : desired_speed_override_ms.xy = cmd.content.speed.target_ms;
2029 8 : break;
2030 : }
2031 : }
2032 8 : }
2033 :
2034 0 : void ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
2035 : {
2036 0 : if (cmd.p1 == 1 || !cmd.content.location.initialised()) {
2037 0 : if (!copter.set_home_to_current_location(false)) {
2038 : // ignore failure
2039 : }
2040 : } else {
2041 0 : if (!copter.set_home(cmd.content.location, false)) {
2042 : // ignore failure
2043 : }
2044 : }
2045 0 : }
2046 :
2047 : // do_roi - starts actions required by MAV_CMD_DO_SET_ROI
2048 : // this involves either moving the camera to point at the ROI (region of interest)
2049 : // and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
2050 : // TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
2051 0 : void ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
2052 : {
2053 0 : auto_yaw.set_roi(cmd.content.location);
2054 0 : }
2055 :
2056 : #if HAL_MOUNT_ENABLED
2057 : // point the camera to a specified angle
2058 0 : void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
2059 : {
2060 : // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
2061 0 : if ((copter.camera_mount.get_mount_type() != AP_Mount::Type::None) &&
2062 0 : !copter.camera_mount.has_pan_control()) {
2063 : // Per the handler in AP_Mount, DO_MOUNT_CONTROL yaw angle is in body frame, which is
2064 : // equivalent to an offset to the current yaw demand.
2065 0 : auto_yaw.set_yaw_angle_offset_deg(cmd.content.mount_control.yaw);
2066 : }
2067 : // pass the target angles to the camera mount
2068 0 : copter.camera_mount.set_angle_target(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw, false);
2069 0 : }
2070 : #endif // HAL_MOUNT_ENABLED
2071 :
2072 : #if AP_WINCH_ENABLED
2073 : // control winch based on mission command
2074 0 : void ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
2075 : {
2076 : // Note: we ignore the gripper num parameter because we only support one gripper
2077 0 : switch (cmd.content.winch.action) {
2078 0 : case WINCH_RELAXED:
2079 0 : g2.winch.relax();
2080 0 : break;
2081 0 : case WINCH_RELATIVE_LENGTH_CONTROL:
2082 0 : g2.winch.release_length(cmd.content.winch.release_length);
2083 0 : break;
2084 0 : case WINCH_RATE_CONTROL:
2085 0 : g2.winch.set_desired_rate(cmd.content.winch.release_rate);
2086 0 : break;
2087 0 : default:
2088 : // do nothing
2089 0 : break;
2090 : }
2091 0 : }
2092 : #endif
2093 :
2094 : #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED && AC_PAYLOAD_PLACE_ENABLED
2095 : // do_payload_place - initiate placing procedure
2096 2 : void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd)
2097 : {
2098 : // if location provided we fly to that location first
2099 2 : if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0 || cmd.content.location.alt != 0) {
2100 : // set state to fly to location
2101 2 : payload_place.state = PayloadPlace::State::FlyToLocation;
2102 :
2103 : // calculate default location used when lat, lon or alt is zero
2104 2 : Location default_loc = copter.current_loc;
2105 2 : subtract_pos_offsets(default_loc);
2106 :
2107 : // get location from command and send to wp_nav
2108 2 : Location target_loc;
2109 2 : if (!get_loc_from_cmd(cmd, default_loc, target_loc) || !wp_start(target_loc)) {
2110 : // failure to get location or set next destination can only be because of missing terrain data
2111 0 : copter.failsafe_terrain_on_event();
2112 0 : return;
2113 : }
2114 2 : } else {
2115 : // initialise placing controller
2116 0 : payload_place.start_descent();
2117 : }
2118 2 : payload_place.descent_max_m = cmd.p1 * 0.01;
2119 :
2120 : // set submode
2121 2 : set_submode(SubMode::NAV_PAYLOAD_PLACE);
2122 : }
2123 : #endif // AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED
2124 :
2125 : // do_RTL - start Return-to-Launch
2126 46 : void ModeAuto::do_RTL(void)
2127 : {
2128 : // start rtl in auto flight mode
2129 46 : rtl_start();
2130 46 : }
2131 :
2132 : /********************************************************************************/
2133 : // Verify Nav (Must) commands
2134 : /********************************************************************************/
2135 :
2136 : // verify_takeoff - check if we have completed the takeoff
2137 681192 : bool ModeAuto::verify_takeoff()
2138 : {
2139 : #if AP_LANDINGGEAR_ENABLED
2140 : // if we have reached our destination
2141 681192 : if (auto_takeoff.complete) {
2142 : // retract the landing gear
2143 78 : copter.landinggear.retract_after_takeoff();
2144 : }
2145 : #endif
2146 :
2147 681192 : return auto_takeoff.complete;
2148 : }
2149 :
2150 : // verify_land - returns true if landing has been completed
2151 202530 : bool ModeAuto::verify_land()
2152 : {
2153 202530 : bool retval = false;
2154 :
2155 202530 : switch (state) {
2156 51064 : case State::FlyToLocation:
2157 : // check if we've reached the location
2158 51064 : if (copter.wp_nav->reached_wp_destination()) {
2159 : // initialise landing controller
2160 6 : land_start();
2161 :
2162 : // advance to next state
2163 6 : state = State::Descending;
2164 : }
2165 51064 : break;
2166 :
2167 151466 : case State::Descending:
2168 : // rely on THROTTLE_LAND mode to correctly update landing status
2169 151466 : retval = copter.ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE);
2170 151466 : if (retval && !mission.continue_after_land_check_for_takeoff() && copter.motors->armed()) {
2171 : /*
2172 : we want to stop mission processing on land
2173 : completion. Disarm now, then return false. This
2174 : leaves mission state machine in the current NAV_LAND
2175 : mission item. After disarming the mission will reset
2176 : */
2177 9 : copter.arming.disarm(AP_Arming::Method::LANDED);
2178 9 : retval = false;
2179 : }
2180 151466 : break;
2181 :
2182 0 : default:
2183 : // this should never happen
2184 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
2185 0 : retval = true;
2186 0 : break;
2187 : }
2188 :
2189 : // true is returned if we've successfully landed
2190 202530 : return retval;
2191 : }
2192 :
2193 : #if AC_PAYLOAD_PLACE_ENABLED
2194 : // verify_payload_place - returns true if placing has been completed
2195 23788 : bool PayloadPlace::verify()
2196 : {
2197 23788 : switch (state) {
2198 23786 : case State::FlyToLocation:
2199 : case State::Descent_Start:
2200 : case State::Descent:
2201 : case State::Release:
2202 : case State::Releasing:
2203 : case State::Delay:
2204 : case State::Ascent_Start:
2205 : case State::Ascent:
2206 23786 : return false;
2207 2 : case State::Done:
2208 2 : return true;
2209 : }
2210 : // should never get here
2211 0 : return true;
2212 : }
2213 : #endif
2214 :
2215 0 : bool ModeAuto::verify_loiter_unlimited()
2216 : {
2217 0 : return false;
2218 : }
2219 :
2220 : // verify_loiter_time - check if we have loitered long enough
2221 14710 : bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
2222 : {
2223 : // return immediately if we haven't reached our destination
2224 14710 : if (!copter.wp_nav->reached_wp_destination()) {
2225 2705 : return false;
2226 : }
2227 :
2228 : // start our loiter timer
2229 12005 : if ( loiter_time == 0 ) {
2230 5 : loiter_time = millis();
2231 : }
2232 :
2233 : // check if loiter timer has run out
2234 12005 : if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
2235 5 : gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
2236 5 : return true;
2237 : }
2238 :
2239 12000 : return false;
2240 : }
2241 :
2242 : // verify_loiter_to_alt - check if we have reached both destination
2243 : // (roughly) and altitude (precisely)
2244 14519 : bool ModeAuto::verify_loiter_to_alt() const
2245 : {
2246 14519 : if (loiter_to_alt.reached_destination_xy &&
2247 10879 : loiter_to_alt.reached_alt) {
2248 2 : return true;
2249 : }
2250 14517 : return false;
2251 : }
2252 :
2253 : // verify_RTL - return true once RTL has completed successfully
2254 : // do_RTL should have been called once first to initialise all variables
2255 950728 : bool ModeAuto::verify_RTL()
2256 : {
2257 : // return immediately if RTL's current state has not completed
2258 950728 : if (!copter.mode_rtl.state_complete()) {
2259 933316 : return false;
2260 : }
2261 :
2262 17412 : switch (copter.mode_rtl.state()) {
2263 1 : case ModeRTL::SubMode::FINAL_DESCENT:
2264 1 : return true;
2265 17273 : case ModeRTL::SubMode::LAND:
2266 : // if landing ensure motors have spooled down
2267 17273 : return motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE;
2268 138 : default:
2269 138 : break;
2270 : }
2271 :
2272 : // RTL has not completed
2273 138 : return false;
2274 : }
2275 :
2276 : /********************************************************************************/
2277 : // Verify Condition (May) commands
2278 : /********************************************************************************/
2279 :
2280 0 : bool ModeAuto::verify_wait_delay()
2281 : {
2282 0 : if (millis() - condition_start > (uint32_t)MAX(condition_value,0)) {
2283 0 : condition_value = 0;
2284 0 : return true;
2285 : }
2286 0 : return false;
2287 : }
2288 :
2289 0 : bool ModeAuto::verify_within_distance()
2290 : {
2291 0 : if (wp_distance_m() < MAX(condition_value,0)) {
2292 0 : condition_value = 0;
2293 0 : return true;
2294 : }
2295 0 : return false;
2296 : }
2297 :
2298 : // verify_yaw - return true if we have reached the desired heading
2299 10225 : bool ModeAuto::verify_yaw()
2300 : {
2301 : // make sure still in fixed yaw mode, the waypoint controller often retakes control of yaw as it executes a new waypoint command
2302 10225 : auto_yaw.set_mode(AutoYaw::Mode::FIXED);
2303 :
2304 : // check if we have reached the target heading
2305 10225 : return auto_yaw.reached_fixed_yaw_target();
2306 : }
2307 :
2308 : // verify_nav_wp - check if we have reached the next way point
2309 1199774 : bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
2310 : {
2311 : // check if we have reached the waypoint
2312 1199774 : if ( !copter.wp_nav->reached_wp_destination() ) {
2313 1183199 : return false;
2314 : }
2315 :
2316 : // start timer if necessary
2317 16575 : if (loiter_time == 0) {
2318 172 : loiter_time = millis();
2319 172 : if (loiter_time_max > 0) {
2320 : // play a tone
2321 17 : AP_Notify::events.waypoint_complete = 1;
2322 : }
2323 : }
2324 :
2325 : // check if timer has run out
2326 16575 : if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
2327 172 : if (loiter_time_max == 0) {
2328 : // play a tone
2329 155 : AP_Notify::events.waypoint_complete = 1;
2330 : }
2331 172 : gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
2332 172 : return true;
2333 : }
2334 16403 : return false;
2335 : }
2336 :
2337 : // verify_circle - check if we have circled the point enough
2338 77227 : bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
2339 : {
2340 : // check if we've reached the edge
2341 77227 : if (_mode == SubMode::CIRCLE_MOVE_TO_EDGE) {
2342 16157 : if (copter.wp_nav->reached_wp_destination()) {
2343 : // start circling
2344 5 : circle_start();
2345 : }
2346 16157 : return false;
2347 : }
2348 :
2349 61070 : if (!is_positive(circle.radius_m)) {
2350 : // radius-0 panorama: position is held while the yaw target slews through the requested
2351 : // turns. The elapsed angle is counted here rather than read back from the yaw
2352 : // controller, so anything that takes the heading away, such as an ROI, pilot yaw or
2353 : // weathervaning, changes where the aircraft points without changing how long the
2354 : // command runs. AC_Circle's angle counter behaved the same way
2355 14400 : const float angle_swept_rad = circle.panorama_rate_rads * (millis() - circle.panorama_start_ms) * 0.001;
2356 14400 : return angle_swept_rad >= fabsf(circle.turns_signed) * M_2PI;
2357 : }
2358 :
2359 : // report each whole turn as it is completed. The angle comes from the orbit leg itself, so
2360 : // it tracks the path actually flown rather than an independently integrated rate
2361 46670 : const uint16_t turns_done = copter.wp_nav->get_circle_angle_covered_rad() / M_2PI;
2362 46670 : if (turns_done > circle.turns_reported) {
2363 4 : circle.turns_reported = turns_done;
2364 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mission: circled %u/%.1f turns",
2365 : (unsigned)turns_done, (double)fabsf(circle.turns_signed));
2366 : }
2367 :
2368 : // the orbit is a single S-curve leg spanning all requested turns; it is complete
2369 : // once that leg finishes
2370 46670 : return copter.wp_nav->reached_wp_destination();
2371 : }
2372 :
2373 : // verify_spline_wp - check if we have reached the next way point using spline
2374 107716 : bool ModeAuto::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
2375 : {
2376 : // check if we have reached the waypoint
2377 107716 : if ( !copter.wp_nav->reached_wp_destination() ) {
2378 100492 : return false;
2379 : }
2380 :
2381 : // start timer if necessary
2382 7224 : if (loiter_time == 0) {
2383 23 : loiter_time = millis();
2384 : }
2385 :
2386 : // check if timer has run out
2387 7224 : if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
2388 23 : gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
2389 23 : return true;
2390 : }
2391 7201 : return false;
2392 : }
2393 :
2394 : #if AC_NAV_GUIDED
2395 : // verify_nav_guided - check if we have breached any limits
2396 0 : bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
2397 : {
2398 : // if disabling guided mode then immediately return true so we move to next command
2399 0 : if (cmd.p1 == 0) {
2400 0 : return true;
2401 : }
2402 :
2403 : // check time and position limits
2404 0 : return copter.mode_guided.limit_check();
2405 : }
2406 : #endif // AC_NAV_GUIDED
2407 :
2408 : // verify_nav_delay - check if we have waited long enough
2409 87074 : bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
2410 : {
2411 87074 : if (millis() - nav_delay_time_start_ms > nav_delay_time_max_ms) {
2412 4 : nav_delay_time_max_ms = 0;
2413 4 : return true;
2414 : }
2415 87070 : return false;
2416 : }
2417 :
2418 : #if AP_SCRIPTING_ENABLED
2419 : // check if verify_nav_script_time command has completed
2420 0 : bool ModeAuto::verify_nav_script_time()
2421 : {
2422 : // if done or timeout then return true
2423 0 : if (nav_scripting.done ||
2424 0 : ((nav_scripting.timeout_s > 0) &&
2425 0 : (AP_HAL::millis() - nav_scripting.start_ms) > (nav_scripting.timeout_s * 1000))) {
2426 0 : return true;
2427 : }
2428 0 : return false;
2429 : }
2430 : #endif
2431 :
2432 : // check if nav_attitude_time command has completed
2433 0 : bool ModeAuto::verify_nav_attitude_time(const AP_Mission::Mission_Command& cmd)
2434 : {
2435 0 : return ((AP_HAL::millis() - nav_attitude_time.start_ms) > (cmd.content.nav_attitude_time.time_sec * 1000));
2436 : }
2437 :
2438 : // pause - Prevent aircraft from progressing along the track
2439 4 : bool ModeAuto::pause()
2440 : {
2441 : // do not pause unless progressing along a wp_nav leg: the circle orbit and the move-to-edge
2442 : // leg are S-curve legs like WP, so they pause the same way
2443 4 : const bool wp_leg_active = (_mode == SubMode::WP) || (_mode == SubMode::CIRCLE_MOVE_TO_EDGE) || (_mode == SubMode::CIRCLE);
2444 4 : if (!wp_leg_active || wp_nav->reached_wp_destination()) {
2445 0 : return false;
2446 : }
2447 :
2448 4 : wp_nav->set_pause();
2449 4 : return true;
2450 : }
2451 :
2452 : // resume - Allow aircraft to progress along the track
2453 4 : bool ModeAuto::resume()
2454 : {
2455 4 : wp_nav->set_resume();
2456 4 : return true;
2457 : }
2458 :
2459 386004 : bool ModeAuto::paused() const
2460 : {
2461 386004 : return (wp_nav != nullptr) && wp_nav->paused();
2462 : }
2463 :
2464 : /*
2465 : get a height above ground estimate for landing
2466 : */
2467 273575 : float ModeAuto::get_alt_above_ground_m() const
2468 : {
2469 : // Only override if in landing submode
2470 273575 : if (_mode == SubMode::LAND) {
2471 : // Rangefinder takes priority
2472 : float alt_above_ground_m;
2473 268914 : if (copter.get_rangefinder_height_interpolated_m(alt_above_ground_m)) {
2474 195426 : return alt_above_ground_m;
2475 : }
2476 :
2477 : // Take land altitude from command
2478 206600 : const AP_Mission::Mission_Command& cmd = mission.get_current_nav_cmd();
2479 206600 : switch (cmd.id) {
2480 206600 : case MAV_CMD_NAV_VTOL_LAND:
2481 : case MAV_CMD_NAV_LAND: {
2482 206600 : if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
2483 : // If land location is valid return height above it
2484 : ftype dist_m;
2485 133112 : if (copter.current_loc.get_height_above(cmd.content.location, dist_m)) {
2486 133112 : return dist_m;
2487 : }
2488 : }
2489 73488 : break;
2490 : }
2491 :
2492 0 : default:
2493 : // Really should not end up here as were in SubMode land
2494 0 : break;
2495 : }
2496 : }
2497 :
2498 : // Use default method
2499 78149 : return Mode::get_alt_above_ground_m();
2500 : }
2501 :
2502 : #endif
|
