Line data Source code
1 : #include "AP_Mission_config.h"
2 :
3 : #if AP_MISSION_ENABLED
4 :
5 : #include "AP_Mission.h"
6 :
7 : #include <GCS_MAVLink/GCS.h>
8 : #include <AP_Camera/AP_Camera.h>
9 : #include <AP_Camera/AP_Camera_Backend.h>
10 : #include <AP_Gripper/AP_Gripper.h>
11 : #include <AP_Parachute/AP_Parachute.h>
12 : #include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
13 : #include <AC_Sprayer/AC_Sprayer.h>
14 : #include <AP_Scripting/AP_Scripting.h>
15 : #include <RC_Channel/RC_Channel.h>
16 : #include <AP_Mount/AP_Mount.h>
17 : #include <AC_Fence/AC_Fence.h>
18 :
19 : #if AP_RC_CHANNEL_ENABLED
20 1 : bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd)
21 : {
22 1 : const RC_Channel::AUX_FUNC function = (RC_Channel::AUX_FUNC)cmd.content.auxfunction.function;
23 1 : const RC_Channel::AuxSwitchPos pos = (RC_Channel::AuxSwitchPos)cmd.content.auxfunction.switchpos;
24 :
25 : // sanity check the switch position. Could map from the mavlink
26 : // enumeration if we were really keen
27 1 : switch (pos) {
28 1 : case RC_Channel::AuxSwitchPos::HIGH:
29 : case RC_Channel::AuxSwitchPos::MIDDLE:
30 : case RC_Channel::AuxSwitchPos::LOW:
31 1 : break;
32 0 : default:
33 0 : return false;
34 : }
35 1 : rc().run_aux_function(function, pos, RC_Channel::AuxFuncTrigger::Source::MISSION, cmd.index);
36 1 : return true;
37 : }
38 : #endif // AP_RC_CHANNEL_ENABLED
39 :
40 : #if AP_GRIPPER_ENABLED
41 12 : bool AP_Mission::start_command_do_gripper(const AP_Mission::Mission_Command& cmd)
42 : {
43 12 : AP_Gripper &gripper = AP::gripper();
44 :
45 : // Note: we ignore the gripper num parameter because we only
46 : // support one gripper
47 12 : switch (cmd.content.gripper.action) {
48 6 : case GRIPPER_ACTION_RELEASE:
49 6 : gripper.release();
50 : // Log_Write_Event(DATA_GRIPPER_RELEASE);
51 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper Released");
52 6 : return true;
53 6 : case GRIPPER_ACTION_GRAB:
54 6 : gripper.grab();
55 : // Log_Write_Event(DATA_GRIPPER_GRAB);
56 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper Grabbed");
57 6 : return true;
58 0 : default:
59 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
60 0 : AP_HAL::panic("Unhandled gripper case");
61 : #endif
62 0 : return false;
63 : }
64 : }
65 : #endif // AP_GRIPPER_ENABLED
66 :
67 : #if AP_SERVORELAYEVENTS_ENABLED
68 0 : bool AP_Mission::start_command_do_servorelayevents(const AP_Mission::Mission_Command& cmd)
69 : {
70 0 : AP_ServoRelayEvents *sre = AP::servorelayevents();
71 0 : if (sre == nullptr) {
72 0 : return false;
73 : }
74 :
75 0 : switch (cmd.id) {
76 0 : case MAV_CMD_DO_SET_SERVO:
77 0 : return sre->do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
78 :
79 : #if AP_RELAY_ENABLED
80 0 : case MAV_CMD_DO_SET_RELAY:
81 0 : return sre->do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
82 : #endif
83 :
84 0 : case MAV_CMD_DO_REPEAT_SERVO:
85 0 : return sre->do_repeat_servo(cmd.content.repeat_servo.channel,
86 0 : cmd.content.repeat_servo.pwm,
87 0 : cmd.content.repeat_servo.repeat_count,
88 0 : cmd.content.repeat_servo.cycle_time * 1000.0f);
89 :
90 : #if AP_RELAY_ENABLED
91 0 : case MAV_CMD_DO_REPEAT_RELAY:
92 0 : return sre->do_repeat_relay(cmd.content.repeat_relay.num,
93 0 : cmd.content.repeat_relay.repeat_count,
94 0 : cmd.content.repeat_relay.cycle_time * 1000.0f);
95 : #endif
96 :
97 0 : default:
98 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
99 0 : AP_HAL::panic("Unhandled servo/relay case");
100 : #endif
101 0 : return false;
102 : }
103 : }
104 : #endif // AP_SERVORELAYEVENTS_ENABLED
105 :
106 : #if AP_CAMERA_ENABLED
107 21 : bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
108 : {
109 21 : AP_Camera *camera = AP::camera();
110 21 : if (camera == nullptr) {
111 0 : return false;
112 : }
113 :
114 21 : switch (cmd.id) {
115 :
116 0 : case MAV_CMD_DO_DIGICAM_CONFIGURE: // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
117 0 : camera->configure(
118 0 : cmd.content.digicam_configure.shooting_mode,
119 0 : cmd.content.digicam_configure.shutter_speed,
120 0 : cmd.content.digicam_configure.aperture,
121 0 : cmd.content.digicam_configure.ISO,
122 0 : cmd.content.digicam_configure.exposure_type,
123 0 : cmd.content.digicam_configure.cmd_id,
124 0 : cmd.content.digicam_configure.engine_cutoff_time);
125 0 : return true;
126 :
127 1 : case MAV_CMD_DO_DIGICAM_CONTROL: // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
128 1 : camera->control(
129 1 : cmd.content.digicam_control.session,
130 1 : cmd.content.digicam_control.zoom_pos,
131 1 : cmd.content.digicam_control.zoom_step,
132 1 : cmd.content.digicam_control.focus_lock,
133 1 : cmd.content.digicam_control.shooting_cmd,
134 1 : cmd.content.digicam_control.cmd_id);
135 1 : return true;
136 :
137 12 : case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
138 12 : return camera->handle_mav_DO_SET_CAM_TRIGG_DISTANCE(
139 12 : cmd.content.cam_trigg_dist.camera_id,
140 12 : cmd.content.cam_trigg_dist.trigger,
141 12 : cmd.content.cam_trigg_dist.meters
142 12 : ) == MAV_RESULT_ACCEPTED;
143 :
144 2 : case MAV_CMD_SET_CAMERA_ZOOM:
145 2 : return camera->handle_mav_SET_CAMERA_ZOOM(
146 2 : cmd.content.set_camera_zoom.camera_id,
147 2 : (CAMERA_ZOOM_TYPE)cmd.content.set_camera_zoom.zoom_type,
148 2 : cmd.content.set_camera_zoom.zoom_value
149 2 : ) == MAV_RESULT_ACCEPTED;
150 :
151 2 : case MAV_CMD_SET_CAMERA_FOCUS:
152 2 : return camera->handle_mav_SET_CAMERA_FOCUS(
153 2 : cmd.content.set_camera_focus.camera_id,
154 2 : (SET_FOCUS_TYPE)cmd.content.set_camera_focus.focus_type,
155 2 : cmd.content.set_camera_focus.focus_value
156 2 : ) == MAV_RESULT_ACCEPTED;
157 :
158 : #if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
159 0 : case MAV_CMD_SET_CAMERA_SOURCE:
160 0 : if (cmd.content.set_camera_source.instance == 0) {
161 : // set lens for every backend
162 0 : bool ret = false;
163 0 : for (uint8_t i=0; i<AP_CAMERA_MAX_INSTANCES; i++) {
164 0 : ret |= camera->set_camera_source(i, (AP_Camera::CameraSource)cmd.content.set_camera_source.primary_source, (AP_Camera::CameraSource)cmd.content.set_camera_source.secondary_source);
165 : }
166 0 : return ret;
167 : }
168 0 : return camera->set_camera_source(cmd.content.set_camera_source.instance-1, (AP_Camera::CameraSource)cmd.content.set_camera_source.primary_source, (AP_Camera::CameraSource)cmd.content.set_camera_source.secondary_source);
169 : #endif
170 :
171 3 : case MAV_CMD_IMAGE_START_CAPTURE:
172 : // check if this is a single picture request (e.g. total images is 1 or interval and total images are zero)
173 5 : if ((cmd.content.image_start_capture.total_num_images == 1) ||
174 2 : (cmd.content.image_start_capture.total_num_images == 0 && is_zero(cmd.content.image_start_capture.interval_s))) {
175 1 : if (cmd.content.image_start_capture.instance == 0) {
176 : // take pictures for every backend
177 0 : return camera->take_picture();
178 : }
179 1 : return camera->take_picture(cmd.content.image_start_capture.instance-1);
180 2 : } else if (cmd.content.image_start_capture.total_num_images == 0) {
181 : // multiple picture request, take pictures forever
182 1 : if (cmd.content.image_start_capture.instance == 0) {
183 : // take pictures for every backend
184 0 : return camera->take_multiple_pictures(cmd.content.image_start_capture.interval_s*1000, -1);
185 : }
186 1 : return camera->take_multiple_pictures(cmd.content.image_start_capture.instance-1, cmd.content.image_start_capture.interval_s*1000, -1);
187 : } else {
188 1 : if (cmd.content.image_start_capture.instance == 0) {
189 : // take pictures for every backend
190 0 : return camera->take_multiple_pictures(cmd.content.image_start_capture.interval_s*1000, cmd.content.image_start_capture.total_num_images);
191 : }
192 1 : return camera->take_multiple_pictures(cmd.content.image_start_capture.instance-1, cmd.content.image_start_capture.interval_s*1000, cmd.content.image_start_capture.total_num_images);
193 : }
194 1 : case MAV_CMD_IMAGE_STOP_CAPTURE:
195 1 : if (cmd.p1 == 0) {
196 : // stop capture for each backend
197 0 : camera->stop_capture();
198 0 : return true;
199 : }
200 1 : return camera->stop_capture(cmd.p1 - 1);
201 :
202 0 : case MAV_CMD_VIDEO_START_CAPTURE:
203 : case MAV_CMD_VIDEO_STOP_CAPTURE:
204 : {
205 0 : const bool start_recording = (cmd.id == MAV_CMD_VIDEO_START_CAPTURE);
206 0 : if (cmd.content.video_start_capture.video_stream_id == 0) {
207 : // stream id of zero interpreted as primary camera
208 0 : return camera->record_video(start_recording);
209 : } else {
210 : // non-zero stream id is converted to camera instance
211 0 : return camera->record_video(cmd.content.video_start_capture.video_stream_id - 1, start_recording);
212 : }
213 : }
214 :
215 0 : default:
216 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
217 0 : AP_HAL::panic("Unhandled camera case");
218 : #endif
219 0 : return false;
220 : }
221 : }
222 : #endif
223 :
224 3 : bool AP_Mission::start_command_parachute(const AP_Mission::Mission_Command& cmd)
225 : {
226 : #if HAL_PARACHUTE_ENABLED
227 3 : AP_Parachute *parachute = AP::parachute();
228 3 : if (parachute == nullptr) {
229 0 : return false;
230 : }
231 :
232 3 : switch (cmd.p1) {
233 0 : case PARACHUTE_DISABLE:
234 0 : parachute->enabled(false);
235 0 : break;
236 0 : case PARACHUTE_ENABLE:
237 0 : parachute->enabled(true);
238 0 : break;
239 3 : case PARACHUTE_RELEASE:
240 3 : parachute->release();
241 3 : break;
242 0 : default:
243 : // do nothing
244 0 : return false;
245 : }
246 :
247 3 : return true;
248 : #else
249 : return false;
250 : #endif // HAL_PARACHUTE_ENABLED
251 : }
252 :
253 0 : bool AP_Mission::command_do_set_repeat_dist(const AP_Mission::Mission_Command& cmd)
254 : {
255 0 : _repeat_dist = cmd.p1;
256 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Resume repeat dist set to %u m",_repeat_dist);
257 0 : return true;
258 : }
259 :
260 1 : bool AP_Mission::start_command_do_sprayer(const AP_Mission::Mission_Command& cmd)
261 : {
262 : #if HAL_SPRAYER_ENABLED
263 1 : AC_Sprayer *sprayer = AP::sprayer();
264 1 : if (sprayer == nullptr) {
265 0 : return false;
266 : }
267 :
268 1 : if (cmd.p1 == 1) {
269 1 : sprayer->run(true);
270 : } else {
271 0 : sprayer->run(false);
272 : }
273 :
274 1 : return true;
275 : #else
276 : return false;
277 : #endif // HAL_SPRAYER_ENABLED
278 : }
279 :
280 0 : bool AP_Mission::start_command_do_scripting(const AP_Mission::Mission_Command& cmd)
281 : {
282 : #if AP_SCRIPTING_ENABLED
283 0 : AP_Scripting *scripting = AP_Scripting::get_singleton();
284 0 : if (scripting == nullptr) {
285 0 : return false;
286 : }
287 :
288 0 : scripting->handle_mission_command(cmd);
289 :
290 0 : return true;
291 : #else
292 : return false;
293 : #endif // AP_SCRIPTING_ENABLED
294 : }
295 :
296 0 : bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd)
297 : {
298 : #if HAL_MOUNT_ENABLED
299 0 : AP_Mount *mount = AP::mount();
300 0 : if (mount == nullptr) {
301 0 : return false;
302 : }
303 :
304 : // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc
305 0 : uint8_t gimbal_instance = mount->get_primary_instance();
306 0 : if (cmd.content.gimbal_manager_pitchyaw.gimbal_id > 0) {
307 0 : gimbal_instance = cmd.content.gimbal_manager_pitchyaw.gimbal_id - 1;
308 : }
309 :
310 : // check flags for change to RETRACT
311 0 : if ((cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
312 0 : mount->set_mode(gimbal_instance, MAV_MOUNT_MODE_RETRACT);
313 0 : return true;
314 : }
315 : // check flags for change to NEUTRAL
316 0 : if ((cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
317 0 : mount->set_mode(gimbal_instance, MAV_MOUNT_MODE_NEUTRAL);
318 0 : return true;
319 : }
320 :
321 : // handle angle target
322 0 : const bool pitch_angle_valid = abs(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) <= 90;
323 0 : const bool yaw_angle_valid = abs(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg) <= 360;
324 0 : if (pitch_angle_valid && yaw_angle_valid) {
325 0 : mount->set_angle_target(gimbal_instance, 0, cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg, cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg, cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
326 0 : return true;
327 : }
328 :
329 : // handle rate target
330 0 : if (!isnan(cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs) && !isnan(cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs)) {
331 0 : mount->set_rate_target(gimbal_instance, 0, cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs, cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs, cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
332 0 : return true;
333 : }
334 :
335 : #endif // HAL_MOUNT_ENABLED
336 : // if we got this far then message is not handled
337 0 : return false;
338 : }
339 :
340 0 : bool AP_Mission::start_command_fence(const AP_Mission::Mission_Command& cmd)
341 : {
342 : #if AP_FENCE_ENABLED
343 0 : AC_Fence* fence = AP::fence();
344 :
345 0 : if (fence == nullptr) {
346 0 : return false;
347 : }
348 :
349 0 : if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::DISABLE_FENCE)) { // disable fence
350 0 : uint8_t fences = fence->enable_configured(false);
351 0 : fence->print_fence_message("disabled", fences);
352 0 : return true;
353 0 : } else if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::ENABLE_FENCE)) { // enable fence
354 0 : uint8_t fences = fence->enable_configured(true);
355 0 : fence->print_fence_message("enabled", fences);
356 0 : return true;
357 0 : } else if (cmd.p1 == uint8_t(AC_Fence::MavlinkFenceActions::DISABLE_ALT_MIN_FENCE)) { // disable fence floor only
358 0 : fence->disable_floor();
359 0 : fence->print_fence_message("disabled", AC_FENCE_TYPE_ALT_MIN);
360 0 : return true;
361 : }
362 : #endif // AP_FENCE_ENABLED
363 0 : return false;
364 : }
365 :
366 : #endif // AP_MISSION_ENABLED
367 :
368 : #if AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
369 3 : bool AP_Mission::start_command_do_set_roi_wpnext_offset(const AP_Mission::Mission_Command& cmd)
370 : {
371 : #if HAL_MOUNT_ENABLED
372 3 : AP_Mount *mount = AP::mount();
373 3 : if (mount == nullptr) {
374 0 : return false;
375 : }
376 :
377 : // we do not use the gimbal ID here; we want to use the proper
378 : // mavlink semantics and lack the infrastructure.
379 3 : mount->set_roi_target_wpnext_offset(Vector3f{
380 3 : cmd.content.wpnext_offset.roll_offset_cd * 0.01f,
381 3 : cmd.content.wpnext_offset.pitch_offset_cd * 0.01f,
382 3 : cmd.content.wpnext_offset.yaw_offset_cd * 0.01f
383 : });
384 :
385 3 : return true;
386 : #else
387 : return false;
388 : #endif // HAL_MOUNT_ENABLED
389 : }
390 : #endif // AP_MISSION_MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET_ENABLED
|
