Line data Source code
1 : #include "Plane.h"
2 :
3 : /********************************************************************************/
4 : // Command Event Handlers
5 : /********************************************************************************/
6 1337 : bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
7 : {
8 1329 : if (control_mode == &mode_auto &&
9 2666 : AP_Mission::is_nav_cmd(cmd) &&
10 1237 : mission.get_prev_nav_cmd_index() != cmd.index) {
11 1065 : reset_alt_offset();
12 : }
13 :
14 : // default to non-VTOL loiter
15 1337 : auto_state.vtol_loiter = false;
16 :
17 : #if AP_TERRAIN_AVAILABLE
18 1337 : plane.target_altitude.terrain_following_pending = false;
19 : #endif
20 :
21 : // special handling for nav vs non-nav commands
22 1337 : if (AP_Mission::is_nav_cmd(cmd)) {
23 : // set takeoff_complete to true so we don't add extra elevator
24 : // except in a takeoff
25 1245 : auto_state.takeoff_complete = true;
26 :
27 1245 : nav_controller->set_data_is_stale();
28 :
29 : // start non-idle
30 1245 : auto_state.idle_mode = false;
31 :
32 : // reset loiter start time. New command is a new loiter
33 1245 : loiter.start_time_ms = 0;
34 :
35 : // Mission lookahead is only valid in auto
36 1245 : if (control_mode == &mode_auto) {
37 1237 : AP_Mission::Mission_Command next_nav_cmd;
38 1237 : const uint16_t next_index = mission.get_current_nav_index() + 1;
39 1237 : const bool have_next_cmd = mission.get_next_nav_cmd(next_index, next_nav_cmd);
40 1237 : auto_state.wp_is_land_approach = have_next_cmd && (next_nav_cmd.id == MAV_CMD_NAV_LAND);
41 : #if HAL_QUADPLANE_ENABLED
42 1237 : if (have_next_cmd && quadplane.is_vtol_land(next_nav_cmd.id)) {
43 34 : auto_state.wp_is_land_approach = false;
44 : }
45 : #endif
46 : }
47 : }
48 :
49 1337 : switch(cmd.id) {
50 :
51 153 : case MAV_CMD_NAV_TAKEOFF:
52 153 : crash_state.is_crashed = false;
53 : #if HAL_QUADPLANE_ENABLED
54 153 : if (quadplane.is_vtol_takeoff(cmd.id)) {
55 13 : return quadplane.do_vtol_takeoff(cmd);
56 : }
57 : #endif
58 140 : do_takeoff(cmd);
59 140 : break;
60 :
61 876 : case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
62 876 : do_nav_wp(cmd);
63 876 : break;
64 :
65 116 : case MAV_CMD_NAV_LAND: // LAND to Waypoint
66 : #if HAL_QUADPLANE_ENABLED
67 116 : if (quadplane.is_vtol_land(cmd.id)) {
68 2 : crash_state.is_crashed = false;
69 2 : return quadplane.do_vtol_land(cmd);
70 : }
71 : #endif
72 114 : do_land(cmd);
73 114 : break;
74 :
75 7 : case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
76 7 : do_loiter_unlimited(cmd);
77 7 : break;
78 :
79 6 : case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
80 6 : do_loiter_turns(cmd);
81 6 : break;
82 :
83 1 : case MAV_CMD_NAV_LOITER_TIME:
84 1 : do_loiter_time(cmd);
85 1 : break;
86 :
87 16 : case MAV_CMD_NAV_LOITER_TO_ALT:
88 16 : do_loiter_to_alt(cmd);
89 16 : break;
90 :
91 3 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
92 3 : set_mode(mode_rtl, ModeReason::MISSION_CMD);
93 3 : break;
94 :
95 0 : case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
96 0 : do_continue_and_change_alt(cmd);
97 0 : break;
98 :
99 2 : case MAV_CMD_NAV_ALTITUDE_WAIT:
100 2 : do_altitude_wait(cmd);
101 2 : break;
102 :
103 : #if HAL_QUADPLANE_ENABLED
104 36 : case MAV_CMD_NAV_VTOL_TAKEOFF:
105 36 : crash_state.is_crashed = false;
106 36 : return quadplane.do_vtol_takeoff(cmd);
107 :
108 29 : case MAV_CMD_NAV_VTOL_LAND:
109 : case MAV_CMD_NAV_PAYLOAD_PLACE:
110 29 : if (quadplane.landing_with_fixed_wing_spiral_approach()) {
111 : // the user wants to approach the landing in a fixed wing flight mode
112 : // the waypoint will be used as a loiter_to_alt
113 : // after which point the plane will compute the optimal into the wind direction
114 : // and fly in on that direction towards the landing waypoint
115 : // it will then transition to VTOL and do a normal quadplane landing
116 2 : do_landing_vtol_approach(cmd);
117 2 : break;
118 : } else {
119 27 : return quadplane.do_vtol_land(cmd);
120 : }
121 : #endif
122 :
123 : // Conditional commands
124 :
125 0 : case MAV_CMD_CONDITION_DELAY:
126 0 : do_wait_delay(cmd);
127 0 : break;
128 :
129 0 : case MAV_CMD_CONDITION_DISTANCE:
130 0 : do_within_distance(cmd);
131 0 : break;
132 :
133 : // Do commands
134 :
135 11 : case MAV_CMD_DO_CHANGE_SPEED:
136 11 : do_change_speed(cmd);
137 11 : break;
138 :
139 0 : case MAV_CMD_DO_SET_HOME:
140 0 : do_set_home(cmd);
141 0 : break;
142 :
143 2 : case MAV_CMD_DO_INVERTED_FLIGHT:
144 2 : if (cmd.p1 == 0 || cmd.p1 == 1) {
145 2 : auto_state.inverted_flight = (bool)cmd.p1;
146 2 : gcs().send_text(MAV_SEVERITY_INFO, "Set inverted %u", cmd.p1);
147 : }
148 2 : break;
149 :
150 55 : case MAV_CMD_DO_RETURN_PATH_START:
151 : case MAV_CMD_DO_LAND_START:
152 55 : break;
153 :
154 0 : case MAV_CMD_DO_AUTOTUNE_ENABLE:
155 0 : autotune_enable(cmd.p1);
156 0 : break;
157 :
158 : #if HAL_MOUNT_ENABLED
159 : // Sets the region of interest (ROI) for a sensor set or the
160 : // vehicle itself. This can then be used by the vehicles control
161 : // system to control the vehicle attitude and the attitude of various
162 : // devices such as cameras.
163 : // |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
164 : // ROI_NONE can be handled by the regular ROI handler because lat, lon, alt are always zero
165 0 : case MAV_CMD_DO_SET_ROI_LOCATION:
166 : case MAV_CMD_DO_SET_ROI_NONE:
167 : case MAV_CMD_DO_SET_ROI:
168 0 : if (!cmd.content.location.initialised()) {
169 : // switch off the camera tracking if enabled
170 0 : if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
171 0 : camera_mount.set_mode_to_default();
172 : }
173 : } else {
174 : // set mount's target location
175 0 : camera_mount.set_roi_target(cmd.content.location);
176 : }
177 0 : break;
178 :
179 0 : case MAV_CMD_DO_MOUNT_CONTROL: // 205
180 : // point the camera to a specified angle
181 0 : camera_mount.set_angle_target(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw, false);
182 0 : break;
183 : #endif
184 :
185 : #if HAL_QUADPLANE_ENABLED
186 2 : case MAV_CMD_DO_VTOL_TRANSITION:
187 2 : plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)cmd.content.do_vtol_transition.target_state);
188 2 : break;
189 : #endif
190 :
191 : #if AP_ICENGINE_ENABLED
192 21 : case MAV_CMD_DO_ENGINE_CONTROL:
193 21 : plane.g2.ice_control.engine_control(cmd.content.do_engine_control.start_control,
194 21 : cmd.content.do_engine_control.cold_start,
195 21 : cmd.content.do_engine_control.height_delay_cm*0.01f,
196 21 : cmd.content.do_engine_control.allow_disarmed_start);
197 21 : break;
198 : #endif
199 :
200 : #if AP_SCRIPTING_ENABLED
201 0 : case MAV_CMD_NAV_SCRIPT_TIME:
202 0 : do_nav_script_time(cmd);
203 0 : break;
204 : #endif
205 :
206 0 : case MAV_CMD_NAV_DELAY:
207 0 : mode_auto.do_nav_delay(cmd);
208 0 : break;
209 :
210 1 : default:
211 : // unable to use the command, allow the vehicle to try the next command
212 1 : return false;
213 : }
214 :
215 1258 : return true;
216 : }
217 :
218 : /*******************************************************************************
219 : Verify command Handlers
220 :
221 : Each type of mission element has a "verify" operation. The verify
222 : operation returns true when the mission element has completed and we
223 : should move onto the next mission element.
224 : Return true if we do not recognize the command so that we move on to the next command
225 : *******************************************************************************/
226 :
227 246732 : bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Returns true if command complete
228 : {
229 246732 : switch(cmd.id) {
230 :
231 8792 : case MAV_CMD_NAV_TAKEOFF:
232 : #if HAL_QUADPLANE_ENABLED
233 8792 : if (quadplane.is_vtol_takeoff(cmd.id)) {
234 3725 : return quadplane.verify_vtol_takeoff(cmd);
235 : }
236 : #endif
237 5067 : return verify_takeoff();
238 :
239 145036 : case MAV_CMD_NAV_WAYPOINT:
240 145036 : return verify_nav_wp(cmd);
241 :
242 40526 : case MAV_CMD_NAV_LAND:
243 : #if HAL_QUADPLANE_ENABLED
244 40526 : if (quadplane.is_vtol_land(cmd.id)) {
245 1393 : return quadplane.verify_vtol_land();
246 : }
247 : #endif
248 39133 : if (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
249 247 : return landing.verify_abort_landing(prev_WP_loc, next_WP_loc, current_loc, auto_state.takeoff_altitude_rel_cm, throttle_suppressed);
250 :
251 : } else {
252 : // use rangefinder to correct if possible
253 38886 : bool rangefinder_active = false;
254 38886 : float height = plane.get_landing_height(rangefinder_active);
255 :
256 : // for flare calculations we don't want to use the terrain
257 : // correction as otherwise we will flare early on rising
258 : // ground
259 38886 : height -= auto_state.terrain_correction;
260 38886 : return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc,
261 38886 : height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(),
262 38886 : rangefinder_active);
263 : }
264 :
265 476 : case MAV_CMD_NAV_LOITER_UNLIM:
266 476 : return verify_loiter_unlim(cmd);
267 :
268 2951 : case MAV_CMD_NAV_LOITER_TURNS:
269 2951 : return verify_loiter_turns(cmd);
270 :
271 2880 : case MAV_CMD_NAV_LOITER_TIME:
272 2880 : return verify_loiter_time();
273 :
274 8707 : case MAV_CMD_NAV_LOITER_TO_ALT:
275 8707 : return verify_loiter_to_alt(cmd);
276 :
277 :
278 0 : case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT:
279 0 : return verify_continue_and_change_alt();
280 :
281 10478 : case MAV_CMD_NAV_ALTITUDE_WAIT:
282 10478 : return mode_auto.verify_altitude_wait(cmd);
283 :
284 : #if HAL_QUADPLANE_ENABLED
285 6001 : case MAV_CMD_NAV_VTOL_TAKEOFF:
286 6001 : return quadplane.verify_vtol_takeoff(cmd);
287 20795 : case MAV_CMD_NAV_VTOL_LAND:
288 : case MAV_CMD_NAV_PAYLOAD_PLACE:
289 20795 : if (quadplane.landing_with_fixed_wing_spiral_approach() && !verify_landing_vtol_approach(cmd)) {
290 : // verify_landing_vtol_approach will return true once we have completed the approach,
291 : // in which case we fall over to normal vtol landing code
292 1026 : return false;
293 : } else {
294 19769 : return quadplane.verify_vtol_land();
295 : }
296 : #endif // HAL_QUADPLANE_ENABLED
297 :
298 : // Conditional commands
299 :
300 0 : case MAV_CMD_CONDITION_DELAY:
301 0 : return verify_wait_delay();
302 :
303 0 : case MAV_CMD_CONDITION_DISTANCE:
304 0 : return verify_within_distance();
305 :
306 : #if AP_SCRIPTING_ENABLED
307 0 : case MAV_CMD_NAV_SCRIPT_TIME:
308 0 : return verify_nav_script_time(cmd);
309 : #endif
310 :
311 0 : case MAV_CMD_NAV_DELAY:
312 0 : return mode_auto.verify_nav_delay(cmd);
313 :
314 : // do commands (always return true)
315 90 : case MAV_CMD_DO_CHANGE_SPEED:
316 : case MAV_CMD_DO_SET_HOME:
317 : case MAV_CMD_DO_INVERTED_FLIGHT:
318 : case MAV_CMD_DO_RETURN_PATH_START:
319 : case MAV_CMD_DO_LAND_START:
320 : case MAV_CMD_DO_FENCE_ENABLE:
321 : case MAV_CMD_DO_AUTOTUNE_ENABLE:
322 : case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
323 : case MAV_CMD_DO_SET_ROI_LOCATION:
324 : case MAV_CMD_DO_SET_ROI_NONE:
325 : case MAV_CMD_DO_SET_ROI:
326 : case MAV_CMD_DO_MOUNT_CONTROL:
327 : case MAV_CMD_DO_VTOL_TRANSITION:
328 : case MAV_CMD_DO_ENGINE_CONTROL:
329 90 : return true;
330 :
331 0 : default:
332 : // error message
333 0 : gcs().send_text(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
334 : // return true if we do not recognize the command so that we move on to the next command
335 0 : return true;
336 : }
337 : }
338 :
339 : /********************************************************************************/
340 : // Nav (Must) commands
341 : /********************************************************************************/
342 :
343 87 : void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm)
344 : {
345 87 : auto_state.next_wp_crosstrack = false;
346 87 : auto_state.crosstrack = false;
347 87 : prev_WP_loc = current_loc;
348 87 : next_WP_loc = calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm);
349 :
350 87 : fix_terrain_WP(next_WP_loc, __AP_LINE__);
351 :
352 87 : setup_terrain_target_alt(next_WP_loc);
353 87 : set_target_altitude_location(next_WP_loc);
354 :
355 87 : if (aparm.loiter_radius < 0) {
356 0 : loiter.direction = -1;
357 : } else {
358 87 : loiter.direction = 1;
359 : }
360 :
361 87 : setup_alt_slope();
362 87 : setup_turn_angle();
363 87 : }
364 :
365 98 : Location Plane::calc_best_rally_or_home_location(const Location &_current_loc, float rtl_home_alt_amsl_cm) const
366 : {
367 : #if HAL_RALLY_ENABLED
368 98 : return plane.rally.calc_best_rally_or_home_location(_current_loc, rtl_home_alt_amsl_cm);
369 : #else
370 : return Location {
371 : plane.home.lat,
372 : plane.home.lng,
373 : int32_t(rtl_home_alt_amsl_cm),
374 : Location::AltFrame::ABSOLUTE
375 : };
376 : #endif
377 : }
378 :
379 : /*
380 : start a NAV_TAKEOFF command
381 : */
382 140 : void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
383 : {
384 140 : prev_WP_loc = current_loc;
385 140 : set_next_WP(cmd.content.location);
386 : // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
387 140 : auto_state.takeoff_pitch_cd = (int16_t)cmd.p1 * 100;
388 140 : if (auto_state.takeoff_pitch_cd <= 0) {
389 : // if the mission doesn't specify a pitch use 4 degrees
390 27 : auto_state.takeoff_pitch_cd = 400;
391 : }
392 140 : auto_state.takeoff_altitude_rel_cm = next_WP_loc.alt - home.alt;
393 140 : next_WP_loc.lat = home.lat + 10;
394 140 : next_WP_loc.lng = home.lng + 10;
395 140 : auto_state.takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction.
396 140 : auto_state.rotation_complete = false;
397 140 : auto_state.height_below_takeoff_to_level_off_cm = 0;
398 : // Flag also used to override "on the ground" throttle disable
399 :
400 : // zero locked course
401 140 : steer_state.locked_course_err = 0;
402 140 : steer_state.hold_course_cd = -1;
403 140 : auto_state.baro_takeoff_alt = barometer.get_altitude();
404 140 : }
405 :
406 876 : void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd)
407 : {
408 876 : set_next_WP(cmd.content.location);
409 876 : }
410 :
411 114 : void Plane::do_land(const AP_Mission::Mission_Command& cmd)
412 : {
413 114 : set_next_WP(cmd.content.location);
414 :
415 : // configure abort altitude and pitch
416 : // if NAV_LAND has an abort altitude then use it, else use last takeoff, else use 30m
417 114 : if (cmd.p1 > 0) {
418 1 : auto_state.takeoff_altitude_rel_cm = (int16_t)cmd.p1 * 100;
419 113 : } else if (auto_state.takeoff_altitude_rel_cm <= 0) {
420 1 : auto_state.takeoff_altitude_rel_cm = 3000;
421 : }
422 :
423 114 : if (auto_state.takeoff_pitch_cd <= 0) {
424 : // If no takeoff command has ever been used, default to a conservative 10deg
425 1 : auto_state.takeoff_pitch_cd = 1000;
426 : }
427 :
428 : #if AP_RANGEFINDER_ENABLED
429 : // zero rangefinder state, start to accumulate good samples now
430 114 : memset(&rangefinder_state, 0, sizeof(rangefinder_state));
431 : #endif
432 :
433 114 : landing.do_land(cmd, relative_altitude);
434 :
435 114 : if (flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
436 : // if we were in an abort we need to explicitly move out of the abort state, as it's sticky
437 0 : set_flight_stage(AP_FixedWing::FlightStage::LAND);
438 : }
439 114 : }
440 :
441 : #if HAL_QUADPLANE_ENABLED
442 2 : void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)
443 : {
444 : //set target alt
445 2 : Location loc = cmd.content.location;
446 2 : loc.sanitize(current_loc);
447 2 : set_next_WP(loc);
448 :
449 2 : vtol_approach_s.approach_stage = VTOLApproach::Stage::LOITER_TO_ALT;
450 2 : }
451 : #endif
452 :
453 30 : void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
454 : {
455 30 : if (cmd.content.location.loiter_ccw) {
456 4 : loiter.direction = -1;
457 : } else {
458 26 : loiter.direction = 1;
459 : }
460 30 : }
461 :
462 7 : void Plane::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
463 : {
464 7 : Location cmdloc = cmd.content.location;
465 7 : cmdloc.sanitize(current_loc);
466 7 : set_next_WP(cmdloc);
467 7 : loiter_set_direction_wp(cmd);
468 7 : }
469 :
470 6 : void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd)
471 : {
472 6 : Location cmdloc = cmd.content.location;
473 6 : cmdloc.sanitize(current_loc);
474 6 : set_next_WP(cmdloc);
475 6 : loiter_set_direction_wp(cmd);
476 6 : const float turns = cmd.get_loiter_turns();
477 :
478 6 : loiter.total_cd = (uint32_t)(turns * 36000UL);
479 6 : condition_value = 1; // used to signify primary turns goal not yet met
480 6 : }
481 :
482 1 : void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd)
483 : {
484 1 : Location cmdloc = cmd.content.location;
485 1 : cmdloc.sanitize(current_loc);
486 1 : set_next_WP(cmdloc);
487 1 : loiter_set_direction_wp(cmd);
488 :
489 : // we set start_time_ms when we reach the waypoint
490 1 : loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // convert sec to ms
491 1 : condition_value = 1; // used to signify primary time goal not yet met
492 1 : }
493 :
494 0 : void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
495 : {
496 : // select heading method. Either mission, gps bearing projection or yaw based
497 : // If prev_WP_loc and next_WP_loc are different then an accurate wp based bearing can
498 : // be computed. However, if we had just changed modes before this, such as an aborted landing
499 : // via mode change, the prev and next wps are the same.
500 : float bearing;
501 0 : if (!prev_WP_loc.same_latlon_as(next_WP_loc)) {
502 : // use waypoint based bearing, this is the usual case
503 0 : steer_state.hold_course_cd = -1;
504 0 : } else if (AP::gps().status() >= AP_GPS_FixType::FIX_2D) {
505 : // use gps ground course based bearing hold
506 0 : steer_state.hold_course_cd = -1;
507 0 : bearing = AP::gps().ground_course();
508 0 : next_WP_loc.offset_bearing(bearing, 1000); // push it out 1km
509 : } else {
510 : // use yaw based bearing hold
511 0 : steer_state.hold_course_cd = wrap_360_cd(ahrs.yaw_sensor);
512 0 : bearing = ahrs.get_yaw_deg();
513 0 : next_WP_loc.offset_bearing(bearing, 1000); // push it out 1km
514 : }
515 :
516 0 : if (cmd.content.location.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {
517 0 : next_WP_loc.copy_alt_from(cmd.content.location);
518 : } else {
519 : int32_t alt_abs_cm;
520 : // if this fails we don't change alt
521 0 : if (cmd.content.location.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_abs_cm)) {
522 0 : next_WP_loc.set_alt_cm(alt_abs_cm,
523 : Location::AltFrame::ABSOLUTE);
524 : }
525 : }
526 0 : condition_value = cmd.p1;
527 0 : reset_offset_altitude();
528 0 : }
529 :
530 2 : void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd)
531 : {
532 : // set all servos to trim until we reach altitude or descent speed
533 2 : auto_state.idle_mode = true;
534 : #if AP_PLANE_GLIDER_PULLUP_ENABLED
535 2 : mode_auto.pullup.reset();
536 : #endif
537 2 : }
538 :
539 16 : void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
540 : {
541 : //set target alt
542 16 : Location loc = cmd.content.location;
543 16 : loc.sanitize(current_loc);
544 16 : set_next_WP(loc);
545 16 : loiter_set_direction_wp(cmd);
546 :
547 : // init to 0, set to 1 when altitude is reached
548 16 : condition_value = 0;
549 16 : }
550 :
551 : // do_nav_delay - Delay the next navigation command
552 0 : void ModeAuto::do_nav_delay(const AP_Mission::Mission_Command& cmd)
553 : {
554 0 : nav_delay.time_start_ms = millis();
555 :
556 0 : if (cmd.content.nav_delay.seconds > 0) {
557 : // relative delay
558 0 : nav_delay.time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
559 : } else {
560 : // absolute delay to utc time
561 : #if AP_RTC_ENABLED
562 0 : nav_delay.time_max_ms = AP::rtc().get_time_utc(cmd.content.nav_delay.hour_utc, cmd.content.nav_delay.min_utc, cmd.content.nav_delay.sec_utc, 0);
563 : #else
564 : nav_delay.time_max_ms = 0;
565 : #endif
566 : }
567 0 : gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay.time_max_ms/1000));
568 0 : }
569 :
570 : /********************************************************************************/
571 : // Verify Nav (Must) commands
572 : /********************************************************************************/
573 5067 : bool Plane::verify_takeoff()
574 : {
575 5067 : bool trust_ahrs_yaw = AP::ahrs().initialised();
576 : #if AP_AHRS_DCM_ENABLED
577 5067 : trust_ahrs_yaw |= ahrs.dcm_yaw_initialised();
578 : #endif
579 5067 : if (trust_ahrs_yaw && steer_state.hold_course_cd == -1) {
580 : // once we reach sufficient speed for good GPS course
581 : // estimation we save our current GPS ground course
582 : // corrected for summed yaw to set the take off
583 : // course. This keeps wings level until we are ready to
584 : // rotate, and also allows us to cope with arbitrary
585 : // compass errors for auto takeoff
586 3924 : if (gps.status() >= AP_GPS_FixType::FIX_3D &&
587 3924 : gps.ground_speed() > GPS_GND_CRS_MIN_SPD &&
588 135 : hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
589 135 : float takeoff_course = wrap_PI(radians(gps.ground_course())) - steer_state.locked_course_err;
590 135 : takeoff_course = wrap_PI(takeoff_course);
591 135 : steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);
592 135 : gcs().send_text(MAV_SEVERITY_INFO, "Holding course %d at %.1fm/s (%.1f)",
593 135 : (int)steer_state.hold_course_cd,
594 135 : (double)gps.ground_speed(),
595 135 : (double)degrees(steer_state.locked_course_err));
596 : }
597 : }
598 :
599 5067 : if (steer_state.hold_course_cd != -1) {
600 : // call navigation controller for heading hold
601 3240 : nav_controller->update_heading_hold(steer_state.hold_course_cd);
602 : } else {
603 1827 : nav_controller->update_level_flight();
604 : }
605 :
606 : // check for optional takeoff timeout
607 5067 : if (plane.check_takeoff_timeout()) {
608 0 : mission.reset();
609 : }
610 :
611 : // see if we have reached takeoff altitude
612 5067 : int32_t relative_alt_cm = adjusted_relative_altitude_cm();
613 5067 : if (
614 10015 : relative_alt_cm > auto_state.takeoff_altitude_rel_cm || // altitude reached
615 4948 : plane.check_takeoff_timeout_level_off() // pitch level-off maneuver has timed out
616 : ) {
617 130 : gcs().send_text(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm",
618 130 : (double)(relative_alt_cm*0.01f));
619 130 : steer_state.hold_course_cd = -1;
620 130 : auto_state.takeoff_complete = true;
621 130 : next_WP_loc = prev_WP_loc = current_loc;
622 :
623 : #if AP_FENCE_ENABLED
624 130 : plane.fence.auto_enable_fence_after_takeoff();
625 : #endif
626 :
627 : // don't cross-track on completion of takeoff, as otherwise we
628 : // can end up doing too sharp a turn
629 130 : auto_state.next_wp_crosstrack = false;
630 130 : return true;
631 : } else {
632 4937 : return false;
633 : }
634 : }
635 :
636 : /*
637 : update navigation for normal mission waypoints. Return true when the
638 : waypoint is complete
639 : */
640 145036 : bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
641 : {
642 145036 : steer_state.hold_course_cd = -1;
643 :
644 : // depending on the pass by flag either go to waypoint in regular manner or
645 : // fly past it for set distance along the line of waypoints
646 145036 : Location flex_next_WP_loc = next_WP_loc;
647 :
648 145036 : uint8_t cmd_passby = HIGHBYTE(cmd.p1); // distance in meters to pass beyond the wp
649 145036 : uint8_t cmd_acceptance_distance = LOWBYTE(cmd.p1); // radius in meters to accept reaching the wp
650 :
651 145036 : if (cmd_passby > 0) {
652 0 : const float dist = prev_WP_loc.get_distance(flex_next_WP_loc);
653 0 : const float bearing_deg = degrees(prev_WP_loc.get_bearing(flex_next_WP_loc));
654 :
655 0 : if (is_positive(dist)) {
656 0 : flex_next_WP_loc.offset_bearing(bearing_deg, cmd_passby);
657 : }
658 : }
659 :
660 145036 : if (auto_state.crosstrack) {
661 92605 : nav_controller->update_waypoint(prev_WP_loc, flex_next_WP_loc);
662 : } else {
663 52431 : nav_controller->update_waypoint(current_loc, flex_next_WP_loc);
664 : }
665 :
666 : // see if the user has specified a maximum distance to waypoint
667 : // If override with p3 - then this is not used as it will overfly badly
668 145036 : if (g.waypoint_max_radius > 0 &&
669 0 : auto_state.wp_distance > (uint16_t)g.waypoint_max_radius) {
670 0 : if (current_loc.past_interval_finish_line(prev_WP_loc, flex_next_WP_loc)) {
671 : // this is needed to ensure completion of the waypoint
672 0 : if (cmd_passby == 0) {
673 0 : prev_WP_loc = current_loc;
674 : }
675 : }
676 0 : return false;
677 : }
678 :
679 145036 : float acceptance_distance_m = 0; // default to: if overflown - let it fly up to the point
680 145036 : if (cmd_acceptance_distance > 0) {
681 : // allow user to override acceptance radius
682 0 : acceptance_distance_m = cmd_acceptance_distance;
683 145036 : } else if (cmd_passby == 0) {
684 145036 : acceptance_distance_m = nav_controller->turn_distance(get_wp_radius(), auto_state.next_turn_angle);
685 : }
686 145036 : const float wp_dist = current_loc.get_distance(flex_next_WP_loc);
687 145036 : if (wp_dist <= acceptance_distance_m) {
688 704 : gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%i dist %um",
689 704 : (unsigned)mission.get_current_nav_cmd().index,
690 704 : (unsigned)current_loc.get_distance(flex_next_WP_loc));
691 704 : return true;
692 : }
693 :
694 : // have we flown past the waypoint?
695 144332 : if (current_loc.past_interval_finish_line(prev_WP_loc, flex_next_WP_loc)) {
696 48 : gcs().send_text(MAV_SEVERITY_INFO, "Passed waypoint #%i dist %um",
697 48 : (unsigned)mission.get_current_nav_cmd().index,
698 48 : (unsigned)current_loc.get_distance(flex_next_WP_loc));
699 48 : return true;
700 : }
701 :
702 144284 : return false;
703 : }
704 :
705 476 : bool Plane::verify_loiter_unlim(const AP_Mission::Mission_Command &cmd)
706 : {
707 : // else use mission radius
708 476 : update_loiter(cmd.p1);
709 476 : return false;
710 : }
711 :
712 2880 : bool Plane::verify_loiter_time()
713 : {
714 2880 : bool result = false;
715 : // mission radius is always aparm.loiter_radius
716 2880 : update_loiter(0);
717 :
718 2880 : if (loiter.start_time_ms == 0) {
719 2830 : if (reached_loiter_target() && loiter.sum_cd > 1) {
720 : // we've reached the target, start the timer
721 0 : loiter.start_time_ms = millis();
722 : }
723 50 : } else if (condition_value != 0) {
724 : // primary goal, loiter time
725 50 : if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
726 : // primary goal completed, initialize secondary heading goal
727 0 : condition_value = 0;
728 0 : result = verify_loiter_heading(true);
729 : }
730 : } else {
731 : // secondary goal, loiter to heading
732 0 : result = verify_loiter_heading(false);
733 : }
734 :
735 2880 : if (result) {
736 0 : gcs().send_text(MAV_SEVERITY_INFO,"Loiter time complete");
737 0 : auto_state.vtol_loiter = false;
738 : }
739 2880 : return result;
740 : }
741 :
742 2951 : bool Plane::verify_loiter_turns(const AP_Mission::Mission_Command &cmd)
743 : {
744 2951 : bool result = false;
745 2951 : uint16_t radius = HIGHBYTE(cmd.p1);
746 2951 : if (cmd.type_specific_bits & (1U<<0)) {
747 : // special storage handling allows for larger radii
748 479 : radius *= 10;
749 : }
750 2951 : update_loiter(radius);
751 :
752 : // LOITER_TURNS makes no sense as VTOL
753 2951 : auto_state.vtol_loiter = false;
754 :
755 2951 : if (!reached_loiter_target()) {
756 1498 : result = false;
757 1453 : } else if (condition_value != 0) {
758 : // primary goal, loiter turns
759 604 : if (loiter.sum_cd > loiter.total_cd && loiter.sum_cd > 1) {
760 : // primary goal completed, initialize secondary heading goal
761 5 : condition_value = 0;
762 5 : result = verify_loiter_heading(true);
763 : }
764 : } else {
765 : // secondary goal, loiter to heading
766 849 : result = verify_loiter_heading(false);
767 : }
768 :
769 2951 : if (result) {
770 4 : gcs().send_text(MAV_SEVERITY_INFO,"Loiter orbits complete");
771 : }
772 2951 : return result;
773 : }
774 :
775 : /*
776 : verify a LOITER_TO_ALT command. This involves checking we have
777 : reached both the desired altitude and desired heading. The desired
778 : altitude only needs to be reached once.
779 : */
780 12667 : bool Plane::verify_loiter_to_alt(const AP_Mission::Mission_Command &cmd)
781 : {
782 12667 : bool result = false;
783 :
784 12667 : update_loiter(cmd.p1);
785 :
786 : // condition_value == 0 means alt has never been reached
787 12667 : if (condition_value == 0) {
788 : // primary goal, loiter to alt
789 10655 : if (labs(loiter.sum_cd) > 1 && (loiter.reached_target_alt || loiter.unable_to_achieve_target_alt)) {
790 : // primary goal completed, initialize secondary heading goal
791 13 : if (loiter.unable_to_achieve_target_alt) {
792 0 : gcs().send_text(MAV_SEVERITY_INFO,"Loiter to alt was stuck at %d", int(current_loc.alt/100));
793 : }
794 :
795 13 : condition_value = 1;
796 13 : result = verify_loiter_heading(true);
797 : }
798 : } else {
799 : // secondary goal, loiter to heading
800 2012 : result = verify_loiter_heading(false);
801 : }
802 :
803 12667 : if (result) {
804 13 : gcs().send_text(MAV_SEVERITY_INFO,"Loiter to alt complete");
805 : }
806 12667 : return result;
807 : }
808 :
809 :
810 0 : bool Plane::verify_continue_and_change_alt()
811 : {
812 : // is waypoint info not available and heading hold is?
813 0 : if (prev_WP_loc.same_latlon_as(next_WP_loc) &&
814 0 : steer_state.hold_course_cd != -1) {
815 : //keep flying the same course with fixed steering heading computed at start if cmd
816 0 : nav_controller->update_heading_hold(steer_state.hold_course_cd);
817 : }
818 : else {
819 : // Is the next_WP less than 200 m away?
820 0 : if (current_loc.get_distance(next_WP_loc) < 200.0f) {
821 : //push another 300 m down the line
822 0 : int32_t next_wp_bearing_cd = prev_WP_loc.get_bearing_to(next_WP_loc);
823 0 : next_WP_loc.offset_bearing(next_wp_bearing_cd * 0.01f, 300.0f);
824 : }
825 :
826 : //keep flying the same course
827 0 : nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
828 : }
829 :
830 : //climbing?
831 0 : if (condition_value == 1 && adjusted_altitude_cm() >= next_WP_loc.alt) {
832 0 : return true;
833 : }
834 : //descending?
835 0 : else if (condition_value == 2 &&
836 0 : adjusted_altitude_cm() <= next_WP_loc.alt) {
837 0 : return true;
838 : }
839 : //don't care if we're climbing or descending
840 0 : else if (labs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
841 0 : return true;
842 : }
843 :
844 0 : return false;
845 : }
846 :
847 : /*
848 : see if we have reached altitude or descent speed
849 : */
850 10478 : bool ModeAuto::verify_altitude_wait(const AP_Mission::Mission_Command &cmd)
851 : {
852 : #if AP_PLANE_GLIDER_PULLUP_ENABLED
853 10478 : if (pullup.in_pullup()) {
854 81 : return pullup.verify_pullup();
855 : }
856 : #endif
857 :
858 : /*
859 : the target altitude in param1 is always AMSL
860 : */
861 10397 : const float alt_diff = plane.current_loc.alt*0.01 - cmd.content.altitude_wait.altitude;
862 10397 : bool completed = false;
863 10397 : if (alt_diff > 0) {
864 1 : gcs().send_text(MAV_SEVERITY_INFO,"Reached altitude");
865 1 : completed = true;
866 10396 : } else if (cmd.content.altitude_wait.descent_rate > 0 &&
867 10396 : plane.auto_state.sink_rate > cmd.content.altitude_wait.descent_rate) {
868 0 : gcs().send_text(MAV_SEVERITY_INFO, "Reached descent rate %.1f m/s", (double)plane.auto_state.sink_rate);
869 0 : completed = true;
870 : }
871 :
872 10397 : if (completed) {
873 : #if AP_PLANE_GLIDER_PULLUP_ENABLED
874 1 : if (pullup.pullup_start()) {
875 : // we are doing a pullup, ALTITUDE_WAIT not complete until pullup is done
876 1 : return false;
877 : }
878 : #endif
879 0 : return true;
880 : }
881 :
882 10396 : const float time_to_alt = alt_diff / MIN(plane.auto_state.sink_rate, -0.01);
883 :
884 : /*
885 : if requested, wiggle servos
886 :
887 : we don't start a wiggle if we expect to release soon as we don't
888 : want the servos to be off trim at the time of release
889 : */
890 10396 : if (cmd.content.altitude_wait.wiggle_time != 0 &&
891 10396 : (plane.auto_state.sink_rate > 0 || time_to_alt > cmd.content.altitude_wait.wiggle_time*5)) {
892 17276 : if (wiggle.stage == 0 &&
893 8380 : AP_HAL::millis() - wiggle.last_ms > cmd.content.altitude_wait.wiggle_time*1000) {
894 34 : wiggle.stage = 1;
895 34 : wiggle.last_ms = AP_HAL::millis();
896 : // idle_wiggle_stage is updated in wiggle_servos()
897 : }
898 : }
899 :
900 10396 : return false;
901 : }
902 :
903 : // verify_nav_delay - check if we have waited long enough
904 0 : bool ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
905 : {
906 0 : if (AP::arming().is_armed_and_safety_off()) {
907 : // don't delay while armed, we need a nav controller running
908 0 : return true;
909 : }
910 0 : if (millis() - nav_delay.time_start_ms > nav_delay.time_max_ms) {
911 0 : nav_delay.time_max_ms = 0;
912 0 : return true;
913 : }
914 0 : return false;
915 : }
916 :
917 : /********************************************************************************/
918 : // Condition (May) commands
919 : /********************************************************************************/
920 :
921 0 : void Plane::do_wait_delay(const AP_Mission::Mission_Command& cmd)
922 : {
923 0 : condition_start = millis();
924 0 : condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
925 0 : }
926 :
927 0 : void Plane::do_within_distance(const AP_Mission::Mission_Command& cmd)
928 : {
929 0 : condition_value = cmd.content.distance.meters;
930 0 : }
931 :
932 : /********************************************************************************/
933 : // Verify Condition (May) commands
934 : /********************************************************************************/
935 :
936 0 : bool Plane::verify_wait_delay()
937 : {
938 0 : if ((unsigned)(millis() - condition_start) > (unsigned)condition_value) {
939 0 : condition_value = 0;
940 0 : return true;
941 : }
942 0 : return false;
943 : }
944 :
945 0 : bool Plane::verify_within_distance()
946 : {
947 0 : if (auto_state.wp_distance < MAX(condition_value,0)) {
948 0 : condition_value = 0;
949 0 : return true;
950 : }
951 0 : return false;
952 : }
953 :
954 : /********************************************************************************/
955 : // Do (Now) commands
956 : /********************************************************************************/
957 :
958 40 : void Plane::do_loiter_at_location()
959 : {
960 40 : if (aparm.loiter_radius < 0) {
961 0 : loiter.direction = -1;
962 : } else {
963 40 : loiter.direction = 1;
964 : }
965 40 : next_WP_loc = current_loc;
966 40 : }
967 :
968 11 : bool Plane::do_change_speed(const AP_Mission::Mission_Command& cmd)
969 : {
970 11 : return do_change_speed(
971 11 : (SPEED_TYPE)cmd.content.speed.speed_type,
972 11 : cmd.content.speed.target_ms,
973 11 : cmd.content.speed.throttle_pct
974 11 : );
975 : }
976 :
977 39 : bool Plane::do_change_speed(SPEED_TYPE speedtype, float speed_target_ms, float throttle_pct)
978 : {
979 39 : switch (speedtype) {
980 25 : case SPEED_TYPE_AIRSPEED:
981 25 : if (is_equal(speed_target_ms, -2.0f)) {
982 2 : new_airspeed_cm = -1; // return to default airspeed
983 2 : return true;
984 46 : } else if ((speed_target_ms >= aparm.airspeed_min.get()) &&
985 23 : (speed_target_ms <= aparm.airspeed_max.get())) {
986 23 : new_airspeed_cm = speed_target_ms * 100; //new airspeed target for AUTO or GUIDED modes
987 23 : gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)speed_target_ms);
988 23 : return true;
989 : }
990 0 : break;
991 8 : case SPEED_TYPE_GROUNDSPEED:
992 8 : gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)speed_target_ms);
993 8 : aparm.min_groundspeed.set(speed_target_ms);
994 8 : return true;
995 :
996 6 : case SPEED_TYPE_CLIMB_SPEED:
997 : case SPEED_TYPE_DESCENT_SPEED:
998 : case SPEED_TYPE_ENUM_END:
999 6 : break;
1000 : }
1001 :
1002 6 : if (throttle_pct > 0 && throttle_pct <= 100) {
1003 6 : gcs().send_text(MAV_SEVERITY_INFO, "Set throttle %u", (unsigned)throttle_pct);
1004 6 : aparm.throttle_cruise.set(throttle_pct);
1005 6 : return true;
1006 : }
1007 :
1008 0 : return false;
1009 : }
1010 :
1011 0 : void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
1012 : {
1013 0 : if (cmd.p1 == 1 && gps.status() >= AP_GPS_FixType::FIX_3D) {
1014 0 : if (!set_home_persistently(gps.location())) {
1015 : // silently ignore error
1016 : }
1017 : } else {
1018 0 : if (!AP::ahrs().set_home(cmd.content.location)) {
1019 : // silently ignore failure
1020 : }
1021 : }
1022 0 : }
1023 :
1024 : // start_command_callback - callback function called from ap-mission when it begins a new mission command
1025 : // 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
1026 1359 : bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd)
1027 : {
1028 1359 : if (control_mode == &mode_auto) {
1029 1329 : return start_command(cmd);
1030 : }
1031 30 : return true;
1032 : }
1033 :
1034 : // verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
1035 : // 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
1036 245563 : bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
1037 : {
1038 245563 : if (control_mode == &mode_auto) {
1039 245563 : bool cmd_complete = verify_command(cmd);
1040 :
1041 : // send message to GCS
1042 245563 : if (cmd_complete) {
1043 1031 : gcs().send_mission_item_reached_message(cmd.index);
1044 : }
1045 :
1046 245563 : return cmd_complete;
1047 : }
1048 0 : return false;
1049 : }
1050 :
1051 : // exit_mission_callback - callback function called from ap-mission when the mission has completed
1052 : // 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
1053 5 : void Plane::exit_mission_callback()
1054 : {
1055 5 : if (control_mode == &mode_auto) {
1056 5 : set_mode(mode_rtl, ModeReason::MISSION_END);
1057 5 : gcs().send_text(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL");
1058 : }
1059 5 : }
1060 :
1061 : #if HAL_QUADPLANE_ENABLED
1062 2590 : bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
1063 : {
1064 2590 : const float radius = is_zero(quadplane.fw_land_approach_radius_m)? aparm.loiter_radius : quadplane.fw_land_approach_radius_m;
1065 2590 : const int8_t direction = is_negative(radius) ? -1 : 1;
1066 2590 : const float abs_radius = fabsf(radius);
1067 :
1068 2590 : loiter.direction = direction;
1069 :
1070 2590 : switch (vtol_approach_s.approach_stage) {
1071 0 : case VTOLApproach::Stage::RTL:
1072 : {
1073 : // fly home and loiter at RTL alt
1074 0 : nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
1075 0 : if (plane.reached_loiter_target()) {
1076 : // descend to Q RTL alt
1077 0 : plane.do_RTL(plane.home.alt + plane.quadplane.qrtl_alt_m*100UL);
1078 0 : plane.loiter_angle_reset();
1079 0 : vtol_approach_s.approach_stage = VTOLApproach::Stage::LOITER_TO_ALT;
1080 : }
1081 0 : break;
1082 : }
1083 799 : case VTOLApproach::Stage::LOITER_TO_ALT:
1084 : {
1085 799 : nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
1086 :
1087 799 : if (labs(loiter.sum_cd) > 1 && (loiter.reached_target_alt || loiter.unable_to_achieve_target_alt)) {
1088 2 : Vector3f wind;
1089 : // use the estimate even if it is not marked valid,
1090 : // to preserve existing behaviour
1091 2 : IGNORE_RETURN(ahrs.get_wind(wind));
1092 2 : vtol_approach_s.approach_direction_deg = degrees(atan2f(-wind.y, -wind.x));
1093 2 : gcs().send_text(MAV_SEVERITY_INFO, "Selected an approach path of %.1f", (double)vtol_approach_s.approach_direction_deg);
1094 2 : vtol_approach_s.approach_stage = VTOLApproach::Stage::ENSURE_RADIUS;
1095 : }
1096 799 : break;
1097 : }
1098 2 : case VTOLApproach::Stage::ENSURE_RADIUS:
1099 : {
1100 : // validate that the vehicle is at least the expected distance away from the loiter point
1101 : // require an angle total of at least 2 centidegrees, due to special casing of 1 centidegree
1102 2 : if (((fabsF(cmd.content.location.get_distance(current_loc) - abs_radius) > 5.0f) &&
1103 4 : (cmd.content.location.get_distance(current_loc) < abs_radius)) ||
1104 2 : (labs(loiter.sum_cd) < 2)) {
1105 0 : nav_controller->update_loiter(cmd.content.location, abs_radius, direction);
1106 0 : break;
1107 : }
1108 2 : vtol_approach_s.approach_stage = VTOLApproach::Stage::WAIT_FOR_BREAKOUT;
1109 : FALLTHROUGH;
1110 : }
1111 173 : case VTOLApproach::Stage::WAIT_FOR_BREAKOUT:
1112 : {
1113 173 : nav_controller->update_loiter(cmd.content.location, radius, direction);
1114 :
1115 173 : const float breakout_direction_rad = radians(vtol_approach_s.approach_direction_deg + (direction > 0 ? 270 : 90));
1116 :
1117 : // breakout when within 5 degrees of the opposite direction
1118 173 : if (fabsF(wrap_PI(ahrs.get_yaw_rad() - breakout_direction_rad)) < radians(5.0f)) {
1119 2 : gcs().send_text(MAV_SEVERITY_INFO, "Starting VTOL land approach path");
1120 2 : vtol_approach_s.approach_stage = VTOLApproach::Stage::APPROACH_LINE;
1121 2 : set_next_WP(cmd.content.location);
1122 : // fallthrough
1123 : } else {
1124 171 : break;
1125 : }
1126 : FALLTHROUGH;
1127 : }
1128 : case VTOLApproach::Stage::APPROACH_LINE:
1129 : {
1130 : // project an approach path
1131 58 : Location start = cmd.content.location;
1132 58 : Location end = cmd.content.location;
1133 :
1134 : // project a 1km waypoint to either side of the landing location
1135 58 : start.offset_bearing(vtol_approach_s.approach_direction_deg + 180, 1000);
1136 58 : end.offset_bearing(vtol_approach_s.approach_direction_deg, 1000);
1137 :
1138 58 : nav_controller->update_waypoint(start, end);
1139 :
1140 : // check if we should move on to the next waypoint
1141 58 : Location breakout_stopping_loc = cmd.content.location;
1142 58 : breakout_stopping_loc.offset_bearing(vtol_approach_s.approach_direction_deg + 180, quadplane.stopping_distance_m());
1143 58 : const bool past_finish_line = current_loc.past_interval_finish_line(start, breakout_stopping_loc);
1144 :
1145 58 : Location breakout_loc = cmd.content.location;
1146 58 : breakout_loc.offset_bearing(vtol_approach_s.approach_direction_deg + 180, abs_radius);
1147 58 : const bool half_radius = current_loc.line_path_proportion(breakout_loc, cmd.content.location) > 0.5;
1148 58 : bool lined_up = true;
1149 58 : Vector3f vel_NED;
1150 58 : if (ahrs.get_velocity_NED(vel_NED)) {
1151 58 : const Vector2f target_vec = current_loc.get_distance_NE(cmd.content.location);
1152 58 : const float angle_err = fabsf(wrap_180(degrees(vel_NED.xy().angle(target_vec))));
1153 58 : lined_up = (angle_err < 30);
1154 : }
1155 :
1156 58 : if (past_finish_line && (lined_up || half_radius)) {
1157 2 : vtol_approach_s.approach_stage = VTOLApproach::Stage::VTOL_LANDING;
1158 2 : quadplane.do_vtol_land(cmd);
1159 : // fallthrough
1160 : } else {
1161 : break;
1162 : }
1163 : FALLTHROUGH;
1164 : }
1165 : case VTOLApproach::Stage::VTOL_LANDING:
1166 : // nothing to do here, we should be into the quadplane landing code
1167 1564 : return true;
1168 : }
1169 :
1170 1026 : return false;
1171 : }
1172 : #endif // HAL_QUADPLANE_ENABLED
1173 :
1174 2879 : bool Plane::verify_loiter_heading(bool init)
1175 : {
1176 : #if HAL_QUADPLANE_ENABLED
1177 2879 : if (quadplane.in_vtol_auto()) {
1178 : // skip heading verify if in VTOL auto
1179 0 : return true;
1180 : }
1181 : #endif
1182 :
1183 : #if MODE_AUTOLAND_ENABLED
1184 2879 : if (control_mode == &mode_autoland) {
1185 : // autoland mode has its own lineup criterion
1186 434 : return mode_autoland.landing_lined_up();
1187 : }
1188 : #endif
1189 :
1190 : //Get the lat/lon of next Nav waypoint after this one:
1191 2445 : AP_Mission::Mission_Command next_nav_cmd;
1192 2445 : if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
1193 : next_nav_cmd)) {
1194 : //no next waypoint to shoot for -- go ahead and break out of loiter
1195 0 : return true;
1196 : }
1197 :
1198 2445 : if (init) {
1199 15 : loiter.sum_cd = 0;
1200 : }
1201 :
1202 2445 : return plane.mode_loiter.isHeadingLinedUp(next_WP_loc, next_nav_cmd.content.location);
1203 : }
1204 :
1205 145036 : float Plane::get_wp_radius() const
1206 : {
1207 : #if HAL_QUADPLANE_ENABLED
1208 145036 : if (plane.quadplane.in_vtol_mode()) {
1209 885 : return plane.quadplane.wp_nav->get_wp_radius_m();
1210 : }
1211 : #endif
1212 144151 : return g.waypoint_radius;
1213 : }
1214 :
1215 : #if AP_SCRIPTING_ENABLED
1216 : /*
1217 : support for scripted navigation, with verify operation for completion
1218 : */
1219 0 : void Plane::do_nav_script_time(const AP_Mission::Mission_Command& cmd)
1220 : {
1221 0 : nav_scripting.enabled = true;
1222 0 : nav_scripting.id++;
1223 0 : nav_scripting.start_ms = AP_HAL::millis();
1224 0 : nav_scripting.current_ms = nav_scripting.start_ms;
1225 :
1226 : // start with current roll rate, pitch rate and throttle
1227 0 : nav_scripting.roll_rate_dps = plane.rollController.get_pid_info().target;
1228 0 : nav_scripting.pitch_rate_dps = plane.pitchController.get_pid_info().target;
1229 0 : nav_scripting.yaw_rate_dps = degrees(ahrs.get_gyro().z);
1230 0 : nav_scripting.throttle_pct = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
1231 0 : }
1232 :
1233 : /*
1234 : wait for scripting to say that the mission item is complete
1235 : */
1236 0 : bool Plane::verify_nav_script_time(const AP_Mission::Mission_Command& cmd)
1237 : {
1238 0 : if (cmd.content.nav_script_time.timeout_s > 0) {
1239 0 : const uint32_t now = AP_HAL::millis();
1240 0 : if (now - nav_scripting.start_ms > cmd.content.nav_script_time.timeout_s*1000U) {
1241 0 : gcs().send_text(MAV_SEVERITY_INFO, "NavScriptTime timed out");
1242 0 : nav_scripting.enabled = false;
1243 0 : nav_scripting.rudder_offset_pct = 0;
1244 0 : nav_scripting.run_yaw_rate_controller = true;
1245 : }
1246 : }
1247 0 : return !nav_scripting.enabled;
1248 : }
1249 :
1250 : // check if we are in a NAV_SCRIPT_* command
1251 61007492 : bool Plane::nav_scripting_active(void)
1252 : {
1253 61007492 : if (nav_scripting.enabled && AP_HAL::millis() - nav_scripting.current_ms > 1000) {
1254 : // set_target_throttle_rate_rpy has not been called from script in last 1000ms
1255 0 : nav_scripting.enabled = false;
1256 0 : nav_scripting.current_ms = 0;
1257 0 : nav_scripting.rudder_offset_pct = 0;
1258 0 : nav_scripting.run_yaw_rate_controller = true;
1259 0 : gcs().send_text(MAV_SEVERITY_INFO, "NavScript time out");
1260 : }
1261 108837379 : if (control_mode == &mode_auto &&
1262 47829887 : mission.get_current_nav_cmd().id != MAV_CMD_NAV_SCRIPT_TIME) {
1263 47829887 : nav_scripting.enabled = false;
1264 : }
1265 61007492 : return nav_scripting.enabled;
1266 : }
1267 :
1268 : // support for NAV_SCRIPTING mission command
1269 0 : bool Plane::nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4)
1270 : {
1271 0 : if (!nav_scripting_active()) {
1272 : // not in NAV_SCRIPT_TIME
1273 0 : return false;
1274 : }
1275 0 : const auto &c = mission.get_current_nav_cmd().content.nav_script_time;
1276 0 : id = nav_scripting.id;
1277 0 : cmd = c.command;
1278 0 : arg1 = c.arg1.get();
1279 0 : arg2 = c.arg2.get();
1280 0 : arg3 = c.arg3;
1281 0 : arg4 = c.arg4;
1282 0 : return true;
1283 : }
1284 :
1285 : // called when script has completed the command
1286 0 : void Plane::nav_script_time_done(uint16_t id)
1287 : {
1288 0 : if (id == nav_scripting.id) {
1289 0 : nav_scripting.enabled = false;
1290 : }
1291 0 : }
1292 :
1293 : // support for NAV_SCRIPTING mission command and aerobatics in other allowed modes
1294 4953 : void Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps)
1295 : {
1296 4953 : nav_scripting.roll_rate_dps = constrain_float(roll_rate_dps, -g.acro_roll_rate, g.acro_roll_rate);
1297 4953 : nav_scripting.pitch_rate_dps = constrain_float(pitch_rate_dps, -g.acro_pitch_rate, g.acro_pitch_rate);
1298 4953 : nav_scripting.yaw_rate_dps = constrain_float(yaw_rate_dps, -g.acro_yaw_rate, g.acro_yaw_rate);
1299 4953 : nav_scripting.throttle_pct = constrain_float(throttle_pct, aparm.throttle_min, aparm.throttle_max);
1300 4953 : nav_scripting.current_ms = AP_HAL::millis();
1301 4953 : }
1302 :
1303 : // support for rudder offset override in aerobatic scripting
1304 4953 : void Plane::set_rudder_offset(float rudder_pct, bool run_yaw_rate_controller)
1305 : {
1306 4953 : nav_scripting.rudder_offset_pct = rudder_pct;
1307 4953 : nav_scripting.run_yaw_rate_controller = run_yaw_rate_controller;
1308 4953 : }
1309 :
1310 : // enable NAV_SCRIPTING takeover in modes other than AUTO using script time mission commands
1311 2 : bool Plane::nav_scripting_enable(uint8_t mode)
1312 : {
1313 2 : uint8_t current_control_mode = control_mode->mode_number();
1314 2 : if (current_control_mode == mode) {
1315 1 : switch (current_control_mode) {
1316 1 : case Mode::Number::CIRCLE:
1317 : case Mode::Number::STABILIZE:
1318 : case Mode::Number::ACRO:
1319 : case Mode::Number::FLY_BY_WIRE_A:
1320 : case Mode::Number::FLY_BY_WIRE_B:
1321 : case Mode::Number::CRUISE:
1322 : case Mode::Number::LOITER:
1323 1 : nav_scripting.enabled = true;
1324 1 : nav_scripting.current_ms = AP_HAL::millis();
1325 1 : break;
1326 0 : default:
1327 0 : nav_scripting.enabled = false;
1328 : }
1329 : } else {
1330 1 : nav_scripting.enabled = false;
1331 : }
1332 2 : return nav_scripting.enabled;
1333 : }
1334 : #endif // AP_SCRIPTING_ENABLED
1335 :
1336 : /*
1337 : return true if this is a LAND command
1338 : note that we consider a PAYLOAD_PLACE to be a land command as it
1339 : follows the landing logic for quadplanes
1340 : */
1341 111403 : bool Plane::is_land_command(uint16_t command) const
1342 : {
1343 : return
1344 107999 : command == MAV_CMD_NAV_VTOL_LAND ||
1345 219402 : command == MAV_CMD_NAV_LAND ||
1346 111403 : command == MAV_CMD_NAV_PAYLOAD_PLACE;
1347 : }
1348 :
1349 : /*
1350 : return true if in a specific AUTO mission command
1351 : */
1352 235485 : bool Plane::in_auto_mission_id(uint16_t command) const
1353 : {
1354 235485 : return control_mode == &mode_auto && mission.get_current_nav_id() == command;
1355 : }
1356 :
|
