Line data Source code
1 : #include "Sub.h"
2 :
3 : #include <AP_RTC/AP_RTC.h>
4 :
5 : static enum AutoSurfaceState auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION;
6 :
7 : // start_command - this function will be called when the ap_mission lib wishes to start a new command
8 48 : bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
9 : {
10 48 : const Location &target_loc = cmd.content.location;
11 48 : auto alt_frame = target_loc.get_alt_frame();
12 :
13 48 : if (alt_frame == Location::AltFrame::ABOVE_HOME) {
14 29 : if (target_loc.alt > 0) {
15 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Alt above home must be negative");
16 0 : return false;
17 : }
18 19 : } else if (alt_frame == Location::AltFrame::ABOVE_TERRAIN) {
19 16 : if (target_loc.alt < 0) {
20 2 : gcs().send_text(MAV_SEVERITY_WARNING, "Alt above terrain must be positive");
21 2 : return false;
22 : }
23 : } else {
24 3 : gcs().send_text(MAV_SEVERITY_WARNING, "Bad alt frame");
25 3 : return false;
26 : }
27 :
28 43 : switch (cmd.id) {
29 :
30 : ///
31 : /// navigation commands
32 : ///
33 38 : case MAV_CMD_NAV_WAYPOINT: // 16 Navigate to Waypoint
34 38 : do_nav_wp(cmd);
35 38 : break;
36 :
37 1 : case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
38 1 : do_surface(cmd);
39 1 : break;
40 :
41 0 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
42 0 : do_RTL();
43 0 : break;
44 :
45 0 : case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
46 0 : do_loiter_unlimited(cmd);
47 0 : break;
48 :
49 2 : case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times
50 2 : do_circle(cmd);
51 2 : break;
52 :
53 0 : case MAV_CMD_NAV_LOITER_TIME: // 19
54 0 : do_loiter_time(cmd);
55 0 : break;
56 :
57 : #if NAV_GUIDED
58 0 : case MAV_CMD_NAV_GUIDED_ENABLE: // 92 accept navigation commands from external nav computer
59 0 : do_nav_guided_enable(cmd);
60 0 : break;
61 : #endif
62 :
63 0 : case MAV_CMD_NAV_DELAY: // 93 Delay the next navigation command
64 0 : do_nav_delay(cmd);
65 0 : break;
66 :
67 : //
68 : // conditional commands
69 : //
70 0 : case MAV_CMD_CONDITION_DELAY: // 112
71 0 : do_wait_delay(cmd);
72 0 : break;
73 :
74 0 : case MAV_CMD_CONDITION_DISTANCE: // 114
75 0 : do_within_distance(cmd);
76 0 : break;
77 :
78 0 : case MAV_CMD_CONDITION_YAW: // 115
79 0 : do_yaw(cmd);
80 0 : break;
81 :
82 : ///
83 : /// do commands
84 : ///
85 0 : case MAV_CMD_DO_CHANGE_SPEED: // 178
86 0 : do_change_speed(cmd);
87 0 : break;
88 :
89 0 : case MAV_CMD_DO_SET_HOME: // 179
90 0 : do_set_home(cmd);
91 0 : break;
92 :
93 0 : case MAV_CMD_DO_SET_ROI_LOCATION: // 195
94 : case MAV_CMD_DO_SET_ROI_NONE: // 197
95 : case MAV_CMD_DO_SET_ROI: // 201
96 : // point the vehicle and camera at a region of interest (ROI)
97 : // ROI_NONE can be handled by the regular ROI handler because lat, lon, alt are always zero
98 0 : do_roi(cmd);
99 0 : break;
100 :
101 0 : case MAV_CMD_DO_MOUNT_CONTROL: // 205
102 : // point the camera to a specified angle
103 0 : do_mount_control(cmd);
104 0 : break;
105 :
106 : #if NAV_GUIDED
107 0 : case MAV_CMD_DO_GUIDED_LIMITS: // 222 accept guided mode limits
108 0 : do_guided_limits(cmd);
109 0 : break;
110 : #endif
111 :
112 2 : default:
113 : // unable to use the command, allow the vehicle to try the next command
114 2 : gcs().send_text(MAV_SEVERITY_WARNING, "Ignoring command %d", cmd.id);
115 2 : return false;
116 : }
117 :
118 : // always return success
119 41 : return true;
120 : }
121 :
122 : /********************************************************************************/
123 : // Verify command Handlers
124 : /********************************************************************************/
125 :
126 : // check to see if current command goal has been achieved
127 : // called by mission library in mission.update()
128 525939 : bool Sub::verify_command_callback(const AP_Mission::Mission_Command& cmd)
129 : {
130 525939 : if (control_mode == Mode::Number::AUTO) {
131 525939 : bool cmd_complete = verify_command(cmd);
132 :
133 : // send message to GCS
134 525939 : if (cmd_complete) {
135 29 : gcs().send_mission_item_reached_message(cmd.index);
136 : }
137 :
138 525939 : return cmd_complete;
139 : }
140 0 : return false;
141 : }
142 :
143 :
144 : // check if current mission command has completed
145 525939 : bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
146 : {
147 525939 : switch (cmd.id) {
148 : //
149 : // navigation commands
150 : //
151 321347 : case MAV_CMD_NAV_WAYPOINT:
152 321347 : return verify_nav_wp(cmd);
153 :
154 68208 : case MAV_CMD_NAV_LAND:
155 68208 : return verify_surface(cmd);
156 :
157 0 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
158 0 : return verify_RTL();
159 :
160 0 : case MAV_CMD_NAV_LOITER_UNLIM:
161 0 : return verify_loiter_unlimited();
162 :
163 136384 : case MAV_CMD_NAV_LOITER_TURNS:
164 136384 : return verify_circle(cmd);
165 :
166 0 : case MAV_CMD_NAV_LOITER_TIME:
167 0 : return verify_loiter_time();
168 :
169 : #if NAV_GUIDED
170 0 : case MAV_CMD_NAV_GUIDED_ENABLE:
171 0 : return verify_nav_guided_enable(cmd);
172 : #endif
173 :
174 0 : case MAV_CMD_NAV_DELAY:
175 0 : return verify_nav_delay(cmd);
176 :
177 : ///
178 : /// conditional commands
179 : ///
180 0 : case MAV_CMD_CONDITION_DELAY:
181 0 : return verify_wait_delay();
182 :
183 0 : case MAV_CMD_CONDITION_DISTANCE:
184 0 : return verify_within_distance();
185 :
186 0 : case MAV_CMD_CONDITION_YAW:
187 0 : return verify_yaw();
188 :
189 : // do commands (always return true)
190 0 : case MAV_CMD_DO_CHANGE_SPEED:
191 : case MAV_CMD_DO_SET_HOME:
192 : case MAV_CMD_DO_SET_ROI_LOCATION:
193 : case MAV_CMD_DO_SET_ROI_NONE:
194 : case MAV_CMD_DO_SET_ROI:
195 : case MAV_CMD_DO_MOUNT_CONTROL:
196 : case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
197 : case MAV_CMD_DO_GUIDED_LIMITS:
198 0 : return true;
199 :
200 0 : default:
201 : // error message
202 0 : gcs().send_text(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd.id);
203 : // return true if we do not recognize the command so that we move on to the next command
204 0 : return true;
205 : }
206 : }
207 :
208 : // exit_mission - function that is called once the mission completes
209 6 : void Sub::exit_mission()
210 : {
211 : // play a tone
212 6 : AP_Notify::events.mission_complete = 1;
213 :
214 : // Try to enter loiter, if that fails, go to depth hold
215 6 : if (!mode_auto.auto_loiter_start()) {
216 0 : set_mode(Mode::Number::ALT_HOLD, ModeReason::MISSION_END);
217 : }
218 6 : }
219 :
220 : /********************************************************************************/
221 : // Nav (Must) commands
222 : /********************************************************************************/
223 :
224 : // do_nav_wp - initiate move to next waypoint
225 38 : void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
226 : {
227 38 : Location target_loc(cmd.content.location);
228 : // use current lat, lon if zero
229 38 : if (target_loc.lat == 0 && target_loc.lng == 0) {
230 2 : target_loc.lat = current_loc.lat;
231 2 : target_loc.lng = current_loc.lng;
232 : }
233 :
234 : // this will be used to remember the time in millis after we reach or pass the WP.
235 38 : loiter_time = 0;
236 : // this is the delay, stored in seconds
237 38 : loiter_time_max = cmd.p1;
238 :
239 : // Set wp navigation target
240 38 : mode_auto.auto_wp_start(target_loc);
241 38 : }
242 :
243 : // do_surface - initiate surface procedure
244 1 : void Sub::do_surface(const AP_Mission::Mission_Command& cmd)
245 : {
246 1 : Location target_location;
247 :
248 : // if location provided we fly to that location at current altitude
249 1 : if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
250 : // set state to go to location
251 1 : auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCATION;
252 :
253 : // calculate and set desired location below surface target
254 : // convert to location class
255 1 : target_location = Location(cmd.content.location);
256 :
257 : // decide if we will use terrain following
258 : int32_t curr_terr_alt_cm, target_terr_alt_cm;
259 1 : if (current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, curr_terr_alt_cm) &&
260 0 : target_location.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, target_terr_alt_cm)) {
261 : // if using terrain, set target altitude to current altitude above terrain
262 0 : target_location.set_alt_cm(curr_terr_alt_cm, Location::AltFrame::ABOVE_TERRAIN);
263 : } else {
264 : // set target altitude to current altitude above home
265 1 : target_location.set_alt_cm(current_loc.alt, Location::AltFrame::ABOVE_HOME);
266 : }
267 1 : } else {
268 : // set surface state to ascend
269 0 : auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
270 :
271 : // Set waypoint destination to current location at zero depth
272 0 : target_location = Location(current_loc.lat, current_loc.lng, 0, Location::AltFrame::ABOVE_HOME);
273 : }
274 :
275 : // Go to wp location
276 1 : mode_auto.auto_wp_start(target_location);
277 1 : }
278 :
279 0 : void Sub::do_RTL()
280 : {
281 0 : mode_auto.auto_wp_start(ahrs.get_home());
282 0 : }
283 :
284 : // do_loiter_unlimited - start loitering with no end conditions
285 : // note: caller should set yaw_mode
286 0 : void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
287 : {
288 : // convert back to location
289 0 : Location target_loc(cmd.content.location);
290 :
291 : // use current location if not provided
292 0 : if (target_loc.lat == 0 && target_loc.lng == 0) {
293 : // To-Do: make this simpler
294 0 : Vector3f temp_pos_neu_cm;
295 0 : wp_nav.get_wp_stopping_point_NE_cm(temp_pos_neu_cm.xy());
296 0 : const Location temp_loc(temp_pos_neu_cm, Location::AltFrame::ABOVE_ORIGIN);
297 0 : target_loc.lat = temp_loc.lat;
298 0 : target_loc.lng = temp_loc.lng;
299 : }
300 :
301 : // start way point navigator and provide it the desired location
302 0 : mode_auto.auto_wp_start(target_loc);
303 0 : }
304 :
305 : // do_circle - initiate moving in a circle
306 2 : void Sub::do_circle(const AP_Mission::Mission_Command& cmd)
307 : {
308 2 : Location circle_center(cmd.content.location);
309 :
310 : // default lat/lon to current position if not provided
311 : // To-Do: use stopping point or position_controller's target instead of current location to avoid jerk?
312 2 : if (circle_center.lat == 0 && circle_center.lng == 0) {
313 0 : circle_center.lat = current_loc.lat;
314 0 : circle_center.lng = current_loc.lng;
315 : }
316 : // calculate radius
317 2 : uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
318 2 : if (cmd.type_specific_bits & (1U << 0)) {
319 0 : circle_radius_m *= 10;
320 : }
321 :
322 :
323 : // true if circle should be ccw
324 2 : const bool circle_direction_ccw = cmd.content.location.loiter_ccw;
325 :
326 : // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
327 2 : mode_auto.auto_circle_movetoedge_start(circle_center, circle_radius_m, circle_direction_ccw);
328 2 : }
329 :
330 : // do_loiter_time - initiate loitering at a point for a given time period
331 : // note: caller should set yaw_mode
332 0 : void Sub::do_loiter_time(const AP_Mission::Mission_Command& cmd)
333 : {
334 : // re-use loiter unlimited
335 0 : do_loiter_unlimited(cmd);
336 :
337 : // setup loiter timer
338 0 : loiter_time = 0;
339 0 : loiter_time_max = cmd.p1; // units are (seconds)
340 0 : }
341 :
342 : #if NAV_GUIDED
343 : // do_nav_guided_enable - initiate accepting commands from external nav computer
344 0 : void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
345 : {
346 0 : if (cmd.p1 > 0) {
347 : // initialise guided limits
348 0 : mode_auto.guided_limit_init_time_and_pos();
349 :
350 : // set navigation target
351 0 : mode_auto.auto_nav_guided_start();
352 : }
353 0 : }
354 : #endif // NAV_GUIDED
355 :
356 : // do_nav_delay - Delay the next navigation command
357 0 : void Sub::do_nav_delay(const AP_Mission::Mission_Command& cmd)
358 : {
359 0 : nav_delay_time_start_ms = AP_HAL::millis();
360 :
361 0 : if (cmd.content.nav_delay.seconds > 0) {
362 : // relative delay
363 0 : nav_delay_time_max_ms = cmd.content.nav_delay.seconds * 1000; // convert seconds to milliseconds
364 : } else {
365 : // absolute delay to utc time
366 : #if AP_RTC_ENABLED
367 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);
368 : #else
369 : nav_delay_time_max_ms = 0;
370 : #endif
371 : }
372 0 : gcs().send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(nav_delay_time_max_ms/1000));
373 0 : }
374 :
375 : #if NAV_GUIDED
376 : // do_guided_limits - pass guided limits to guided controller
377 0 : void Sub::do_guided_limits(const AP_Mission::Mission_Command& cmd)
378 : {
379 0 : mode_guided.guided_limit_set(cmd.p1 * 1000, // convert seconds to ms
380 0 : cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm
381 0 : cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm
382 0 : cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm
383 0 : }
384 : #endif
385 :
386 : /********************************************************************************/
387 : // Verify Nav (Must) commands
388 : /********************************************************************************/
389 :
390 : // verify_nav_wp - check if we have reached the next way point
391 321347 : bool Sub::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
392 : {
393 : // check if we have reached the waypoint
394 321347 : if (!wp_nav.reached_wp_destination()) {
395 321319 : return false;
396 : }
397 :
398 : // play a tone
399 28 : AP_Notify::events.waypoint_complete = 1;
400 :
401 : // start timer if necessary
402 28 : if (loiter_time == 0) {
403 28 : loiter_time = AP_HAL::millis();
404 : }
405 :
406 : // check if timer has run out
407 28 : if (((AP_HAL::millis() - loiter_time) / 1000) >= loiter_time_max) {
408 28 : gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
409 28 : return true;
410 : }
411 :
412 0 : return false;
413 : }
414 :
415 : // verify_surface - returns true if surface procedure has been completed
416 68208 : bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd)
417 : {
418 68208 : bool retval = false;
419 :
420 68208 : switch (auto_surface_state) {
421 68208 : case AUTO_SURFACE_STATE_GO_TO_LOCATION:
422 : // check if we've reached the location
423 68208 : if (wp_nav.reached_wp_destination()) {
424 : // Set target to current xy and zero depth
425 : // TODO get xy target from current wp destination, because current location may be acceptance-radius away from original destination
426 0 : Location target_location(cmd.content.location.lat, cmd.content.location.lng, 0, Location::AltFrame::ABOVE_HOME);
427 :
428 0 : mode_auto.auto_wp_start(target_location);
429 :
430 : // advance to next state
431 0 : auto_surface_state = AUTO_SURFACE_STATE_ASCEND;
432 : }
433 68208 : break;
434 :
435 0 : case AUTO_SURFACE_STATE_ASCEND:
436 0 : if (wp_nav.reached_wp_destination()) {
437 0 : retval = true;
438 : }
439 0 : break;
440 :
441 0 : default:
442 : // this should never happen
443 : // TO-DO: log an error
444 0 : retval = true;
445 0 : break;
446 : }
447 :
448 : // true is returned if we've successfully surfaced
449 68208 : return retval;
450 : }
451 :
452 0 : bool Sub::verify_RTL() {
453 0 : return wp_nav.reached_wp_destination();
454 : }
455 :
456 0 : bool Sub::verify_loiter_unlimited()
457 : {
458 0 : return false;
459 : }
460 :
461 : // verify_loiter_time - check if we have loitered long enough
462 0 : bool Sub::verify_loiter_time()
463 : {
464 : // return immediately if we haven't reached our destination
465 0 : if (!wp_nav.reached_wp_destination()) {
466 0 : return false;
467 : }
468 :
469 : // start our loiter timer
470 0 : if (loiter_time == 0) {
471 0 : loiter_time = AP_HAL::millis();
472 : }
473 :
474 : // check if loiter timer has run out
475 0 : return (((AP_HAL::millis() - loiter_time) / 1000) >= loiter_time_max);
476 : }
477 :
478 : // verify_circle - check if we have circled the point enough
479 136384 : bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
480 : {
481 : // check if we've reached the edge
482 136384 : if (auto_mode == Auto_CircleMoveToEdge) {
483 9280 : if (wp_nav.reached_wp_destination()) {
484 2 : Vector3f circle_center_neu_cm;
485 2 : UNUSED_RESULT(cmd.content.location.get_vector_from_origin_NEU_cm(circle_center_neu_cm));
486 :
487 : // set lat/lon position if not provided
488 2 : if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
489 0 : circle_center_neu_cm.xy() = (sub.pos_control.get_pos_estimate_NED_m().xy() * 100.0f).tofloat();
490 : }
491 :
492 : // start circling
493 2 : mode_auto.auto_circle_start();
494 : }
495 9280 : return false;
496 : }
497 127104 : const float turns = cmd.get_loiter_turns();
498 :
499 : // check if we have completed circling
500 127104 : return fabsf(sub.circle_nav.get_angle_total_rad()/M_2PI) >= turns;
501 : }
502 :
503 : #if NAV_GUIDED
504 : // verify_nav_guided - check if we have breached any limits
505 0 : bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
506 : {
507 : // if disabling guided mode then immediately return true so we move to next command
508 0 : if (cmd.p1 == 0) {
509 0 : return true;
510 : }
511 :
512 : // check time and position limits
513 0 : return mode_auto.guided_limit_check();
514 : }
515 : #endif // NAV_GUIDED
516 :
517 : // verify_nav_delay - check if we have waited long enough
518 0 : bool Sub::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
519 : {
520 0 : if (AP_HAL::millis() - nav_delay_time_start_ms > nav_delay_time_max_ms) {
521 0 : nav_delay_time_max_ms = 0;
522 0 : return true;
523 : }
524 0 : return false;
525 : }
526 :
527 : /********************************************************************************/
528 : // Condition (May) commands
529 : /********************************************************************************/
530 :
531 0 : void Sub::do_wait_delay(const AP_Mission::Mission_Command& cmd)
532 : {
533 0 : condition_start = AP_HAL::millis();
534 0 : condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
535 0 : }
536 :
537 0 : void Sub::do_within_distance(const AP_Mission::Mission_Command& cmd)
538 : {
539 0 : condition_value = cmd.content.distance.meters;
540 0 : }
541 :
542 0 : void Sub::do_yaw(const AP_Mission::Mission_Command& cmd)
543 : {
544 0 : sub.mode_auto.set_auto_yaw_look_at_heading(
545 0 : cmd.content.yaw.angle_deg,
546 0 : cmd.content.yaw.turn_rate_dps,
547 0 : cmd.content.yaw.direction,
548 0 : cmd.content.yaw.relative_angle);
549 0 : }
550 :
551 :
552 : /********************************************************************************/
553 : // Verify Condition (May) commands
554 : /********************************************************************************/
555 :
556 0 : bool Sub::verify_wait_delay()
557 : {
558 0 : if (AP_HAL::millis() - condition_start > (uint32_t)MAX(condition_value, 0)) {
559 0 : condition_value = 0;
560 0 : return true;
561 : }
562 0 : return false;
563 : }
564 :
565 0 : bool Sub::verify_within_distance()
566 : {
567 0 : if (wp_nav.get_wp_distance_to_destination_cm() < (uint32_t)MAX(condition_value,0)) {
568 0 : condition_value = 0;
569 0 : return true;
570 : }
571 0 : return false;
572 : }
573 :
574 : // verify_yaw - return true if we have reached the desired heading
575 0 : bool Sub::verify_yaw()
576 : {
577 : // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
578 0 : if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) {
579 0 : sub.mode_auto.set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
580 : }
581 :
582 : // check if we are within 2 degrees of the target heading
583 0 : return (abs(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200);
584 : }
585 :
586 : /********************************************************************************/
587 : // Do (Now) commands
588 : /********************************************************************************/
589 :
590 : // do_guided - start guided mode
591 0 : bool Sub::do_guided(const AP_Mission::Mission_Command& cmd)
592 : {
593 : // only process guided waypoint if we are in guided mode
594 0 : if (control_mode != Mode::Number::GUIDED && !(control_mode == Mode::Number::AUTO && auto_mode == Auto_NavGuided)) {
595 0 : return false;
596 : }
597 :
598 : // switch to handle different commands
599 0 : switch (cmd.id) {
600 :
601 0 : case MAV_CMD_NAV_WAYPOINT: {
602 : // set wp_nav's destination
603 0 : return sub.mode_guided.guided_set_destination(cmd.content.location);
604 : }
605 :
606 0 : case MAV_CMD_CONDITION_YAW:
607 0 : do_yaw(cmd);
608 0 : return true;
609 :
610 0 : default:
611 : // reject unrecognised command
612 0 : return false;
613 : }
614 :
615 : return true;
616 : }
617 :
618 0 : void Sub::do_change_speed(const AP_Mission::Mission_Command& cmd)
619 : {
620 0 : if (cmd.content.speed.target_ms > 0) {
621 0 : wp_nav.set_speed_NE_cms(cmd.content.speed.target_ms * 100.0f);
622 : }
623 0 : }
624 :
625 0 : void Sub::do_set_home(const AP_Mission::Mission_Command& cmd)
626 : {
627 0 : if (cmd.p1 == 1 || !cmd.content.location.initialised()) {
628 0 : if (!set_home_to_current_location(false)) {
629 : // silently ignore this failure
630 : }
631 : } else {
632 0 : if (!set_home(cmd.content.location, false)) {
633 : // silently ignore this failure
634 : }
635 : }
636 0 : }
637 :
638 : // do_roi - starts actions required by MAV_CMD_NAV_ROI
639 : // this involves either moving the camera to point at the ROI (region of interest)
640 : // and possibly rotating the vehicle to point at the ROI if our mount type does not support a yaw feature
641 : // TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
642 0 : void Sub::do_roi(const AP_Mission::Mission_Command& cmd)
643 : {
644 0 : sub.mode_auto.set_auto_yaw_roi(cmd.content.location);
645 0 : }
646 :
647 : // point the camera to a specified angle
648 0 : void Sub::do_mount_control(const AP_Mission::Mission_Command& cmd)
649 : {
650 : #if HAL_MOUNT_ENABLED
651 0 : camera_mount.set_angle_target(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw, false);
652 : #endif
653 0 : }
|
