Line data Source code
1 : #include "AP_Mount_config.h"
2 :
3 : #if HAL_MOUNT_ENABLED
4 :
5 : #include <AP_Common/AP_Common.h>
6 : #include <AP_Param/AP_Param.h>
7 : #include "AP_Mount.h"
8 :
9 : #include "AP_Mount_Backend.h"
10 : #include "AP_Mount_Servo.h"
11 : #include "AP_Mount_SoloGimbal.h"
12 : #include "AP_Mount_Alexmos.h"
13 : #include "AP_Mount_SToRM32.h"
14 : #include "AP_Mount_SToRM32_serial.h"
15 : #include "AP_Mount_MAVLink.h"
16 : #include "AP_Mount_Siyi.h"
17 : #include "AP_Mount_Scripting.h"
18 : #include "AP_Mount_Xacti.h"
19 : #include "AP_Mount_Viewpro.h"
20 : #include "AP_Mount_Topotek.h"
21 : #include "AP_Mount_CADDX.h"
22 : #include "AP_Mount_XFRobot.h"
23 : #include <stdio.h>
24 : #include <AP_Math/location.h>
25 : #include <SRV_Channel/SRV_Channel.h>
26 : #include <AP_Logger/AP_Logger.h>
27 :
28 : const AP_Param::GroupInfo AP_Mount::var_info[] = {
29 :
30 : // @Group: 1
31 : // @Path: AP_Mount_Params.cpp
32 : AP_SUBGROUPINFO(_params[0], "1", 43, AP_Mount, AP_Mount_Params),
33 :
34 : #if AP_MOUNT_MAX_INSTANCES > 1
35 : // @Group: 2
36 : // @Path: AP_Mount_Params.cpp
37 : AP_SUBGROUPINFO(_params[1], "2", 44, AP_Mount, AP_Mount_Params),
38 : #endif
39 :
40 : AP_GROUPEND
41 : };
42 :
43 6183 : AP_Mount::AP_Mount()
44 : {
45 2061 : if (_singleton != nullptr) {
46 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
47 0 : AP_HAL::panic("Mount must be singleton");
48 : #endif
49 0 : return;
50 : }
51 2061 : _singleton = this;
52 :
53 2061 : AP_Param::setup_object_defaults(this, var_info);
54 : }
55 :
56 : // init - detect and initialise all mounts
57 2059 : void AP_Mount::init()
58 : {
59 : // check init has not been called before
60 2059 : if (_num_instances != 0) {
61 0 : return;
62 : }
63 :
64 : // primary is reset to the first instantiated mount
65 2059 : bool primary_set = false;
66 :
67 : // keep track of number of serial instances for initialisation
68 2059 : uint8_t serial_instance = 0;
69 :
70 : // create each instance
71 6177 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
72 4118 : switch (get_mount_type(instance)) {
73 4006 : case Type::None:
74 4006 : break;
75 : #if HAL_MOUNT_SERVO_ENABLED
76 16 : case Type::Servo:
77 16 : _backends[instance] = NEW_NOTHROW AP_Mount_Servo(*this, _params[instance], true, instance);
78 16 : _num_instances++;
79 16 : break;
80 : #endif
81 : #if HAL_SOLO_GIMBAL_ENABLED
82 1 : case Type::SoloGimbal:
83 1 : _backends[instance] = NEW_NOTHROW AP_Mount_SoloGimbal(*this, _params[instance], instance);
84 1 : _num_instances++;
85 1 : break;
86 : #endif // HAL_SOLO_GIMBAL_ENABLED
87 :
88 : #if HAL_MOUNT_ALEXMOS_ENABLED
89 0 : case Type::Alexmos:
90 0 : _backends[instance] = NEW_NOTHROW AP_Mount_Alexmos(*this, _params[instance], instance);
91 0 : _num_instances++;
92 0 : break;
93 : #endif
94 :
95 : #if HAL_MOUNT_STORM32MAVLINK_ENABLED
96 : // check for SToRM32 mounts using MAVLink protocol
97 0 : case Type::SToRM32:
98 0 : _backends[instance] = NEW_NOTHROW AP_Mount_SToRM32(*this, _params[instance], instance);
99 0 : _num_instances++;
100 0 : break;
101 : #endif
102 :
103 : #if HAL_MOUNT_STORM32SERIAL_ENABLED
104 : // check for SToRM32 mounts using serial protocol
105 0 : case Type::SToRM32_serial:
106 0 : _backends[instance] = NEW_NOTHROW AP_Mount_SToRM32_serial(*this, _params[instance], instance, serial_instance);
107 0 : _num_instances++;
108 0 : serial_instance++;
109 0 : break;
110 : #endif
111 :
112 : #if HAL_MOUNT_MAVLINK_ENABLED
113 : // check for MAVLink mounts
114 5 : case Type::MAVLink:
115 5 : _backends[instance] = NEW_NOTHROW AP_Mount_MAVLink(*this, _params[instance], instance);
116 5 : _num_instances++;
117 5 : break;
118 : #endif // HAL_MOUNT_MAVLINK_ENABLED
119 :
120 : #if HAL_MOUNT_SERVO_ENABLED
121 : // check for BrushlessPWM mounts (uses Servo backend)
122 82 : case Type::BrushlessPWM:
123 82 : _backends[instance] = NEW_NOTHROW AP_Mount_Servo(*this, _params[instance], false, instance);
124 82 : _num_instances++;
125 82 : break;
126 : #endif
127 :
128 : #if HAL_MOUNT_SIYI_ENABLED
129 : // check for Siyi gimbal
130 1 : case Type::Siyi:
131 1 : _backends[instance] = NEW_NOTHROW AP_Mount_Siyi(*this, _params[instance], instance, serial_instance);
132 1 : _num_instances++;
133 1 : serial_instance++;
134 1 : break;
135 : #endif // HAL_MOUNT_SIYI_ENABLED
136 :
137 : #if HAL_MOUNT_SCRIPTING_ENABLED
138 : // check for Scripting gimbal
139 4 : case Type::Scripting:
140 4 : _backends[instance] = NEW_NOTHROW AP_Mount_Scripting(*this, _params[instance], instance);
141 4 : _num_instances++;
142 4 : break;
143 : #endif // HAL_MOUNT_SCRIPTING_ENABLED
144 :
145 : #if HAL_MOUNT_XACTI_ENABLED
146 : // check for Xacti gimbal
147 0 : case Type::Xacti:
148 0 : _backends[instance] = NEW_NOTHROW AP_Mount_Xacti(*this, _params[instance], instance);
149 0 : _num_instances++;
150 0 : break;
151 : #endif // HAL_MOUNT_XACTI_ENABLED
152 :
153 : #if HAL_MOUNT_VIEWPRO_ENABLED
154 : // check for Xacti gimbal
155 1 : case Type::Viewpro:
156 1 : _backends[instance] = NEW_NOTHROW AP_Mount_Viewpro(*this, _params[instance], instance, serial_instance);
157 1 : _num_instances++;
158 1 : serial_instance++;
159 1 : break;
160 : #endif // HAL_MOUNT_VIEWPRO_ENABLED
161 :
162 : #if HAL_MOUNT_TOPOTEK_ENABLED
163 : // check for Topotek gimbal
164 2 : case Type::Topotek:
165 2 : _backends[instance] = NEW_NOTHROW AP_Mount_Topotek(*this, _params[instance], instance, serial_instance);
166 2 : _num_instances++;
167 2 : serial_instance++;
168 2 : break;
169 : #endif // HAL_MOUNT_TOPOTEK_ENABLED
170 :
171 : #if HAL_MOUNT_CADDX_ENABLED
172 : // check for CADDX gimbal
173 0 : case Type::CADDX:
174 0 : _backends[instance] = NEW_NOTHROW AP_Mount_CADDX(*this, _params[instance], instance, serial_instance);
175 0 : _num_instances++;
176 0 : serial_instance++;
177 0 : break;
178 : #endif // HAL_MOUNT_CADDX_ENABLED
179 :
180 : #if HAL_MOUNT_XFROBOT_ENABLED
181 : // check for XFRobot gimbal
182 0 : case Type::XFRobot:
183 0 : _backends[instance] = NEW_NOTHROW AP_Mount_XFRobot(*this, _params[instance], instance, serial_instance);
184 0 : _num_instances++;
185 0 : serial_instance++;
186 0 : break;
187 : #endif // HAL_MOUNT_XFROBOT_ENABLED
188 : }
189 :
190 : // init new instance
191 4118 : if (_backends[instance] != nullptr) {
192 112 : if (!primary_set) {
193 110 : _primary = instance;
194 110 : primary_set = true;
195 : }
196 : }
197 : }
198 :
199 : // init each instance, do it after all instances were created, so that they all know things
200 6177 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
201 4118 : if (_backends[instance] != nullptr) {
202 112 : _backends[instance]->init();
203 112 : set_mode_to_default(instance);
204 : }
205 : }
206 :
207 : (void)serial_instance;
208 : }
209 :
210 : // update - give mount opportunity to update servos. should be called at 10hz or higher
211 7438628 : void AP_Mount::update()
212 : {
213 : // update each instance
214 22315884 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
215 14877256 : if (_backends[instance] != nullptr) {
216 399951 : _backends[instance]->update();
217 : }
218 : }
219 7438628 : }
220 :
221 : // used for gimbals that need to read INS data at full rate
222 32593131 : void AP_Mount::update_fast()
223 : {
224 : // update each instance
225 97779393 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
226 65186262 : if (_backends[instance] != nullptr) {
227 3181628 : _backends[instance]->update_fast();
228 : }
229 : }
230 32593131 : }
231 :
232 : // get_mount_type - returns the type of mount
233 266868 : AP_Mount::Type AP_Mount::get_mount_type(uint8_t instance) const
234 : {
235 266868 : if (instance >= AP_MOUNT_MAX_INSTANCES) {
236 0 : return Type::None;
237 : }
238 :
239 266868 : return (Type)_params[instance].type.get();
240 : }
241 :
242 : // has_pan_control - returns true if the mount has yaw control (required for copters)
243 42 : bool AP_Mount::has_pan_control(uint8_t instance) const
244 : {
245 42 : const auto *backend = get_instance(instance);
246 42 : if (backend == nullptr) {
247 6 : return false;
248 : }
249 :
250 : // ask backend if it support pan
251 36 : return backend->has_pan_control();
252 : }
253 :
254 : // get_mode - returns current mode of mount (i.e. Retracted, Neutral, RC_Targeting, GPS Point)
255 67 : MAV_MOUNT_MODE AP_Mount::get_mode(uint8_t instance) const
256 : {
257 67 : const auto *backend = get_instance(instance);
258 67 : if (backend == nullptr) {
259 0 : return MAV_MOUNT_MODE_RETRACT;
260 : }
261 :
262 : // ask backend its mode
263 67 : return backend->get_mode();
264 : }
265 :
266 : // set_mode_to_default - restores the mode to it's default mode held in the MNTx__DEFLT_MODE parameter
267 : // this operation requires 60us on a Pixhawk/PX4
268 358 : void AP_Mount::set_mode_to_default(uint8_t instance)
269 : {
270 358 : auto *backend = get_instance(instance);
271 358 : if (backend == nullptr) {
272 163 : return;
273 : }
274 195 : backend->set_mode((enum MAV_MOUNT_MODE)_params[instance].default_mode.get());
275 : }
276 :
277 : // set_mode - sets mount's mode
278 83 : void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode)
279 : {
280 83 : auto *backend = get_instance(instance);
281 83 : if (backend == nullptr) {
282 0 : return;
283 : }
284 :
285 : // call backend's set_mode
286 83 : backend->set_mode(mode);
287 : }
288 :
289 : // set yaw_lock used in RC_TARGETING mode. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
290 : // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle
291 0 : void AP_Mount::set_yaw_lock(uint8_t instance, bool yaw_lock)
292 : {
293 0 : auto *backend = get_instance(instance);
294 0 : if (backend == nullptr) {
295 0 : return;
296 : }
297 :
298 : // call backend's set_yaw_lock
299 0 : backend->set_yaw_lock(yaw_lock);
300 : }
301 :
302 : // set roll_lock used in RC_TARGETING mode. If true, the gimbal's roll target is maintained in earth-frame
303 : // If false (aka "follow") the gimbal's roll is maintained in body-frame meaning it will rotate with the vehicle
304 0 : void AP_Mount::set_roll_lock(uint8_t instance, bool roll_lock)
305 : {
306 0 : auto *backend = get_instance(instance);
307 0 : if (backend == nullptr) {
308 0 : return;
309 : }
310 :
311 : // call backend's set_roll_lock
312 0 : backend->set_roll_lock(roll_lock);
313 : }
314 :
315 : // set pitch_lock used in RC_TARGETING mode. If true, the gimbal's tilt target is maintained in earth-frame
316 : // If false (aka "follow") the gimbal's tilt is maintained in body-frame meaning it will tilt with the vehicle
317 0 : void AP_Mount::set_pitch_lock(uint8_t instance, bool pitch_lock)
318 : {
319 0 : auto *backend = get_instance(instance);
320 0 : if (backend == nullptr) {
321 0 : return;
322 : }
323 :
324 : // call backend's set_pitch_lock
325 0 : backend->set_pitch_lock(pitch_lock);
326 : }
327 :
328 :
329 : // set angle target in degrees
330 : // roll and pitch are in earth-frame
331 : // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
332 83 : void AP_Mount::set_angle_target(uint8_t instance, float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame)
333 : {
334 83 : auto *backend = get_instance(instance);
335 83 : if (backend == nullptr) {
336 0 : return;
337 : }
338 :
339 : // send command to backend
340 83 : backend->set_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame);
341 : }
342 :
343 : // sets rate target in deg/s
344 : // yaw_lock should be true if the yaw rate is earth-frame, false if body-frame (e.g. rotates with body of vehicle)
345 0 : void AP_Mount::set_rate_target(uint8_t instance, float roll_degs, float pitch_degs, float yaw_degs, bool yaw_lock)
346 : {
347 0 : auto *backend = get_instance(instance);
348 0 : if (backend == nullptr) {
349 0 : return;
350 : }
351 :
352 : // send command to backend
353 0 : backend->set_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_lock);
354 : }
355 :
356 374 : MAV_RESULT AP_Mount::handle_command_do_mount_configure(const mavlink_command_int_t &packet)
357 : {
358 374 : auto *backend = get_primary();
359 374 : if (backend == nullptr) {
360 0 : return MAV_RESULT_FAILED;
361 : }
362 :
363 374 : backend->set_mode((MAV_MOUNT_MODE)packet.param1);
364 :
365 374 : return MAV_RESULT_ACCEPTED;
366 : }
367 :
368 :
369 36 : MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_int_t &packet)
370 : {
371 36 : auto *backend = get_primary();
372 36 : if (backend == nullptr) {
373 0 : return MAV_RESULT_FAILED;
374 : }
375 :
376 36 : return backend->handle_command_do_mount_control(packet);
377 : }
378 :
379 10 : MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet)
380 : {
381 10 : auto *backend = mount_device_from_mavlink_gimbal_id(packet.z);
382 :
383 10 : if (backend == nullptr) {
384 0 : return MAV_RESULT_FAILED;
385 : }
386 :
387 : // check flags for change to RETRACT
388 10 : const uint32_t flags = packet.x;
389 10 : if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
390 0 : backend->set_mode(MAV_MOUNT_MODE_RETRACT);
391 0 : return MAV_RESULT_ACCEPTED;
392 : }
393 : // check flags for change to NEUTRAL
394 10 : if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
395 0 : backend->set_mode(MAV_MOUNT_MODE_NEUTRAL);
396 0 : return MAV_RESULT_ACCEPTED;
397 : }
398 :
399 : // param1 : pitch_angle (in degrees)
400 : // param2 : yaw angle (in degrees)
401 10 : const float pitch_angle_deg = packet.param1;
402 10 : const float yaw_angle_deg = packet.param2;
403 10 : if (!isnan(pitch_angle_deg) && !isnan(yaw_angle_deg)) {
404 9 : backend->set_angle_target(0, pitch_angle_deg, yaw_angle_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
405 9 : return MAV_RESULT_ACCEPTED;
406 : }
407 :
408 : // param3 : pitch_rate (in deg/s)
409 : // param4 : yaw rate (in deg/s)
410 1 : const float pitch_rate_degs = packet.param3;
411 1 : const float yaw_rate_degs = packet.param4;
412 1 : if (!isnan(pitch_rate_degs) && !isnan(yaw_rate_degs)) {
413 1 : backend->set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
414 1 : return MAV_RESULT_ACCEPTED;
415 : }
416 :
417 : // if neither angles nor rates were provided set the RC_TARGETING yaw lock state
418 0 : if (isnan(pitch_angle_deg) && isnan(yaw_angle_deg) && isnan(pitch_rate_degs) && isnan(yaw_rate_degs)) {
419 0 : backend->set_yaw_lock(flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
420 0 : return MAV_RESULT_ACCEPTED;
421 : }
422 :
423 0 : return MAV_RESULT_FAILED;
424 : }
425 :
426 : // handle mav_cmd_do_gimbal_manager_configure for deconflicting different mavlink message senders
427 8 : MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
428 : {
429 8 : auto *backend = mount_device_from_mavlink_gimbal_id(packet.z);
430 :
431 8 : if (backend == nullptr) {
432 0 : return MAV_RESULT_FAILED;
433 : }
434 :
435 8 : return backend->handle_command_do_gimbal_manager_configure(packet, msg);
436 : }
437 :
438 0 : void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg)
439 : {
440 : mavlink_gimbal_manager_set_attitude_t packet;
441 0 : mavlink_msg_gimbal_manager_set_attitude_decode(&msg,&packet);
442 :
443 0 : auto *backend = mount_device_from_mavlink_gimbal_id(packet.gimbal_device_id);
444 :
445 0 : if (backend == nullptr) {
446 0 : return;
447 : }
448 :
449 : // check flags for change to RETRACT
450 0 : const uint32_t flags = packet.flags;
451 0 : if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
452 0 : backend->set_mode(MAV_MOUNT_MODE_RETRACT);
453 0 : return;
454 : }
455 :
456 : // check flags for change to NEUTRAL
457 0 : if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
458 0 : backend->set_mode(MAV_MOUNT_MODE_NEUTRAL);
459 0 : return;
460 : }
461 :
462 0 : const Quaternion att_quat{packet.q};
463 : const Vector3f att_rate_degs {
464 : packet.angular_velocity_x,
465 : packet.angular_velocity_y,
466 : packet.angular_velocity_z
467 0 : };
468 :
469 : // ensure that we are only demanded to a specific attitude or to
470 : // achieve a specific rate. Do not allow both to be specified at
471 : // the same time:
472 0 : if (!att_quat.is_nan() && !att_rate_degs.is_nan()) {
473 0 : return;
474 : }
475 :
476 0 : if (!att_quat.is_nan()) {
477 : // convert quaternion to euler angles
478 0 : Vector3f attitude;
479 0 : att_quat.to_euler(attitude); // attitude is in radians here
480 0 : attitude *= RAD_TO_DEG; // convert to degrees
481 :
482 0 : backend->set_angle_target(attitude.x, attitude.y, attitude.z, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
483 0 : return;
484 : }
485 :
486 : {
487 0 : const float roll_rate_degs = degrees(packet.angular_velocity_x);
488 0 : const float pitch_rate_degs = degrees(packet.angular_velocity_y);
489 0 : const float yaw_rate_degs = degrees(packet.angular_velocity_z);
490 0 : backend->set_rate_target(roll_rate_degs, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
491 0 : return;
492 : }
493 : }
494 :
495 0 : void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg)
496 : {
497 : mavlink_gimbal_manager_set_pitchyaw_t packet;
498 0 : mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg,&packet);
499 :
500 0 : auto *backend = mount_device_from_mavlink_gimbal_id(packet.gimbal_device_id);
501 :
502 0 : if (backend == nullptr) {
503 0 : return;
504 : }
505 :
506 : // check flags for change to RETRACT
507 0 : uint32_t flags = (uint32_t)packet.flags;
508 0 : if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
509 0 : backend->set_mode(MAV_MOUNT_MODE_RETRACT);
510 0 : return;
511 : }
512 : // check flags for change to NEUTRAL
513 0 : if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
514 0 : backend->set_mode(MAV_MOUNT_MODE_NEUTRAL);
515 0 : return;
516 : }
517 :
518 : // Do not allow both angle and rate to be specified at the same time
519 0 : if (!isnan(packet.pitch) && !isnan(packet.yaw) && !isnan(packet.pitch_rate) && !isnan(packet.yaw_rate)) {
520 0 : return;
521 : }
522 :
523 : // pitch and yaw from packet are in radians
524 0 : if (!isnan(packet.pitch) && !isnan(packet.yaw)) {
525 0 : const float pitch_angle_deg = degrees(packet.pitch);
526 0 : const float yaw_angle_deg = degrees(packet.yaw);
527 0 : backend->set_angle_target(0, pitch_angle_deg, yaw_angle_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
528 0 : return;
529 : }
530 :
531 : // pitch_rate and yaw_rate from packet are in rad/s
532 0 : if (!isnan(packet.pitch_rate) && !isnan(packet.yaw_rate)) {
533 0 : const float pitch_rate_degs = degrees(packet.pitch_rate);
534 0 : const float yaw_rate_degs = degrees(packet.yaw_rate);
535 0 : backend->set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
536 0 : return;
537 : }
538 :
539 : // if neither angles nor rates were provided set the RC_TARGETING yaw lock state
540 0 : if (isnan(packet.pitch) && isnan(packet.yaw) && isnan(packet.pitch_rate) && isnan(packet.yaw_rate)) {
541 0 : backend->set_yaw_lock(flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
542 0 : return;
543 : }
544 : }
545 :
546 9 : MAV_RESULT AP_Mount::handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet)
547 : {
548 9 : set_target_sysid((uint8_t)packet.param1);
549 9 : return MAV_RESULT_ACCEPTED;
550 : }
551 :
552 437 : MAV_RESULT AP_Mount::handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
553 : {
554 437 : switch (packet.command) {
555 374 : case MAV_CMD_DO_MOUNT_CONFIGURE:
556 374 : return handle_command_do_mount_configure(packet);
557 36 : case MAV_CMD_DO_MOUNT_CONTROL:
558 36 : return handle_command_do_mount_control(packet);
559 10 : case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
560 10 : return handle_command_do_gimbal_manager_pitchyaw(packet);
561 8 : case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
562 8 : return handle_command_do_gimbal_manager_configure(packet, msg);
563 9 : case MAV_CMD_DO_SET_ROI_SYSID:
564 9 : return handle_command_do_set_roi_sysid(packet);
565 0 : default:
566 0 : return MAV_RESULT_UNSUPPORTED;
567 : }
568 : }
569 :
570 : /// Change the configuration of the mount
571 2801 : void AP_Mount::handle_global_position_int(const mavlink_message_t &msg)
572 : {
573 : mavlink_global_position_int_t packet;
574 2801 : mavlink_msg_global_position_int_decode(&msg, &packet);
575 :
576 2801 : if (!check_latlng(packet.lat, packet.lon)) {
577 0 : return;
578 : }
579 :
580 8403 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
581 5602 : if (_backends[instance] != nullptr) {
582 12 : _backends[instance]->handle_global_position_int(msg.sysid, packet);
583 : }
584 : }
585 : }
586 :
587 : #if HAL_GCS_ENABLED
588 : // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
589 1115995 : void AP_Mount::send_gimbal_device_attitude_status(mavlink_channel_t chan)
590 : {
591 : // call send_gimbal_device_attitude_status for each instance
592 3347985 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
593 2231990 : if (_backends[instance] != nullptr) {
594 65396 : _backends[instance]->send_gimbal_device_attitude_status(chan);
595 : }
596 : }
597 1115995 : }
598 :
599 : // send a GIMBAL_MANAGER_INFORMATION message to GCS
600 0 : void AP_Mount::send_gimbal_manager_information(mavlink_channel_t chan)
601 : {
602 : // call send_gimbal_device_attitude_status for each instance
603 0 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
604 0 : if (_backends[instance] != nullptr) {
605 0 : _backends[instance]->send_gimbal_manager_information(chan);
606 : }
607 : }
608 0 : }
609 :
610 : // send a GIMBAL_MANAGER_STATUS message to GCS
611 25 : void AP_Mount::send_gimbal_manager_status(mavlink_channel_t chan)
612 : {
613 : // call send_gimbal_device_attitude_status for each instance
614 75 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
615 50 : if (_backends[instance] != nullptr) {
616 25 : _backends[instance]->send_gimbal_manager_status(chan);
617 : }
618 : }
619 25 : }
620 : #endif // HAL_GCS_ENABLED
621 :
622 : #if AP_MOUNT_POI_TO_LATLONALT_ENABLED
623 : // get poi information. Returns true on success and fills in gimbal attitude, location and poi location
624 0 : bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const
625 : {
626 0 : auto *backend = get_instance(instance);
627 0 : if (backend == nullptr) {
628 0 : return false;
629 : }
630 0 : return backend->get_poi(instance, quat, loc, poi_loc);
631 : }
632 : #endif
633 :
634 : #if AP_MOUNT_POI_LOCK_ENABLED
635 : // lock currently viewed GPS point and switch to GPS Targeting mode
636 4 : void AP_Mount::set_poi_lock(uint8_t instance)
637 : {
638 4 : auto *backend = get_instance(instance);
639 4 : if (backend == nullptr) {
640 0 : return;
641 : }
642 :
643 : // call backend's set_poi_lock
644 4 : backend->set_poi_lock();
645 : }
646 :
647 2 : void AP_Mount::clear_poi_lock(uint8_t instance)
648 : {
649 2 : auto *backend = get_instance(instance);
650 2 : if (backend == nullptr) {
651 2 : return;
652 : }
653 :
654 : // call backend's clear_poi_lock
655 0 : backend->clear_poi_lock();
656 : }
657 :
658 7 : void AP_Mount::suspend_poi_lock(uint8_t instance)
659 : {
660 7 : auto *backend = get_instance(instance);
661 7 : if (backend == nullptr) {
662 1 : return;
663 : }
664 :
665 : // call backend's suspend_poi_lock
666 6 : backend->suspend_poi_lock();
667 : }
668 : #endif // AP_MOUNT_POI_LOCK_ENABLED
669 :
670 : // get attitude as a quaternion. returns true on success.
671 : // att_quat will be an earth-frame quaternion rotated such that
672 : // yaw is in body-frame.
673 77801 : bool AP_Mount::get_attitude_quaternion(uint8_t instance, Quaternion& att_quat)
674 : {
675 77801 : auto *backend = get_instance(instance);
676 77801 : if (backend == nullptr) {
677 0 : return false;
678 : }
679 77801 : return backend->get_attitude_quaternion(att_quat);
680 : }
681 :
682 : // get mount's current attitude in euler angles in degrees. yaw angle is in body-frame
683 : // returns true on success
684 77801 : bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg)
685 : {
686 : // re-use get_attitude_quaternion and convert to Euler angles
687 77801 : Quaternion att_quat;
688 77801 : if (!get_attitude_quaternion(instance, att_quat)) {
689 227 : return false;
690 : }
691 :
692 : float roll_rad, pitch_rad, yaw_rad;
693 77574 : att_quat.to_euler(roll_rad, pitch_rad, yaw_rad);
694 77574 : roll_deg = degrees(roll_rad);
695 77574 : pitch_deg = degrees(pitch_rad);
696 77574 : yaw_bf_deg = degrees(yaw_rad);
697 77574 : return true;
698 : }
699 :
700 : // run pre-arm check. returns false on failure and fills in failure_msg
701 : // any failure_msg returned will not include a prefix
702 131378 : bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
703 : {
704 : // check type parameters
705 394098 : for (uint8_t i=0; i<AP_MOUNT_MAX_INSTANCES; i++) {
706 262738 : if ((get_mount_type(i) != Type::None) && (_backends[i] == nullptr)) {
707 18 : strncpy(failure_msg, "check TYPE", failure_msg_len);
708 18 : return false;
709 : }
710 : }
711 :
712 : // return true if no mount configured
713 131360 : if (_num_instances == 0) {
714 130534 : return true;
715 : }
716 :
717 : // check healthy
718 2436 : for (uint8_t i=0; i<AP_MOUNT_MAX_INSTANCES; i++) {
719 1631 : if ((_backends[i] != nullptr) && !_backends[i]->healthy()) {
720 21 : strncpy(failure_msg, "not healthy", failure_msg_len);
721 21 : return false;
722 : }
723 : }
724 :
725 805 : return true;
726 : }
727 :
728 : #if AP_SCRIPTING_ENABLED
729 : // get target rate in deg/sec. returns true on success
730 0 : bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
731 : {
732 0 : auto *backend = get_instance(instance);
733 0 : if (backend == nullptr) {
734 0 : return false;
735 : }
736 0 : return backend->get_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_is_earth_frame);
737 : }
738 :
739 : // get target angle in deg. returns true on success
740 1195 : bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame)
741 : {
742 1195 : auto *backend = get_instance(instance);
743 1195 : if (backend == nullptr) {
744 0 : return false;
745 : }
746 1195 : return backend->get_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame);
747 : }
748 :
749 : // get mount target location. returns true on success
750 67 : bool AP_Mount::get_location_target(uint8_t instance, Location& target_loc)
751 : {
752 67 : auto *backend = get_instance(instance);
753 67 : if (backend == nullptr) {
754 0 : return false;
755 : }
756 67 : return backend->get_location_target(target_loc);
757 : }
758 : #endif
759 :
760 : // accessory for scripting backends and logging
761 2548 : void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_deg, float yaw_bf_deg)
762 : {
763 2548 : auto *backend = get_instance(instance);
764 2548 : if (backend == nullptr) {
765 0 : return;
766 : }
767 2548 : backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg);
768 : }
769 :
770 : #if AP_SCRIPTING_ENABLED
771 1 : void AP_Mount::set_natively_supported_mount_target_types(uint8_t instance, uint8_t types_mask)
772 : {
773 1 : auto *backend = get_instance(instance);
774 1 : if (backend == nullptr) {
775 0 : return;
776 : }
777 1 : backend->set_natively_supported_mount_target_types(types_mask);
778 : }
779 : #endif // AP_SCRIPTING_ENABLED
780 :
781 : #if HAL_LOGGING_ENABLED
782 : // write mount log packet for all backends
783 1392105 : void AP_Mount::write_log()
784 : {
785 : // each instance writes log
786 4176315 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
787 2784210 : if (_backends[instance] != nullptr) {
788 77783 : _backends[instance]->write_log(0);
789 : }
790 : }
791 1392105 : }
792 :
793 66 : void AP_Mount::write_log(uint8_t instance, uint64_t timestamp_us)
794 : {
795 66 : auto *backend = get_instance(instance);
796 66 : if (backend == nullptr) {
797 49 : return;
798 : }
799 17 : backend->write_log(timestamp_us);
800 : }
801 : #endif
802 :
803 : // point at system ID sysid
804 9 : void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid)
805 : {
806 9 : auto *backend = get_instance(instance);
807 9 : if (backend == nullptr) {
808 0 : return;
809 : }
810 : // call instance's set target SYSID cmd
811 9 : backend->set_target_sysid(sysid);
812 : }
813 :
814 : // set_roi_target - sets target location that mount should attempt to point towards
815 34 : void AP_Mount::set_roi_target(uint8_t instance, const Location &target_loc)
816 : {
817 34 : auto *backend = get_instance(instance);
818 34 : if (backend == nullptr) {
819 6 : return;
820 : }
821 28 : backend->set_roi_target(target_loc);
822 : }
823 :
824 : // clear_roi_target - clears target location that mount should attempt to point towards
825 18 : void AP_Mount::clear_roi_target(uint8_t instance)
826 : {
827 18 : auto *backend = get_instance(instance);
828 18 : if (backend == nullptr) {
829 6 : return;
830 : }
831 12 : backend->clear_roi_target();
832 : }
833 :
834 : #if AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
835 3 : void AP_Mount::set_roi_target_wpnext_offset(uint8_t instance, const Vector3f &rpy)
836 : {
837 3 : auto *backend = get_instance(instance);
838 3 : if (backend == nullptr) {
839 0 : return;
840 : }
841 3 : backend->set_roi_target_wpnext_offset(rpy);
842 : }
843 : #endif // AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
844 :
845 : //
846 : // camera controls for gimbals that include a camera
847 : //
848 :
849 : // take a picture. returns true on success
850 0 : bool AP_Mount::take_picture(uint8_t instance)
851 : {
852 0 : auto *backend = get_instance(instance);
853 0 : if (backend == nullptr) {
854 0 : return false;
855 : }
856 0 : return backend->take_picture();
857 : }
858 :
859 : // start or stop video recording. returns true on success
860 : // set start_recording = true to start record, false to stop recording
861 0 : bool AP_Mount::record_video(uint8_t instance, bool start_recording)
862 : {
863 0 : auto *backend = get_instance(instance);
864 0 : if (backend == nullptr) {
865 0 : return false;
866 : }
867 0 : return backend->record_video(start_recording);
868 : }
869 :
870 : // set zoom specified as a rate or percentage
871 0 : bool AP_Mount::set_zoom(uint8_t instance, ZoomType zoom_type, float zoom_value)
872 : {
873 0 : auto *backend = get_instance(instance);
874 0 : if (backend == nullptr) {
875 0 : return false;
876 : }
877 0 : return backend->set_zoom(zoom_type, zoom_value);
878 : }
879 :
880 : // set focus specified as rate, percentage or auto
881 : // focus in = -1, focus hold = 0, focus out = 1
882 0 : SetFocusResult AP_Mount::set_focus(uint8_t instance, FocusType focus_type, float focus_value)
883 : {
884 0 : auto *backend = get_instance(instance);
885 0 : if (backend == nullptr) {
886 0 : return SetFocusResult::FAILED;
887 : }
888 0 : return backend->set_focus(focus_type, focus_value);
889 : }
890 :
891 : // set tracking to none, point or rectangle (see TrackingType enum)
892 : // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
893 : // p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
894 0 : bool AP_Mount::set_tracking(uint8_t instance, TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2)
895 : {
896 0 : auto *backend = get_instance(instance);
897 0 : if (backend == nullptr) {
898 0 : return false;
899 : }
900 0 : return backend->set_tracking(tracking_type, p1, p2);
901 : }
902 :
903 : // set camera lens as a value from 0 to 5
904 0 : bool AP_Mount::set_lens(uint8_t instance, uint8_t lens)
905 : {
906 0 : auto *backend = get_instance(instance);
907 0 : if (backend == nullptr) {
908 0 : return false;
909 : }
910 0 : return backend->set_lens(lens);
911 : }
912 :
913 : #if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
914 : // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
915 : // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
916 0 : bool AP_Mount::set_camera_source(uint8_t instance, uint8_t primary_source, uint8_t secondary_source)
917 : {
918 0 : auto *backend = get_instance(instance);
919 0 : if (backend == nullptr) {
920 0 : return false;
921 : }
922 0 : return backend->set_camera_source(primary_source, secondary_source);
923 : }
924 : #endif
925 :
926 : // send camera information message to GCS
927 5 : void AP_Mount::send_camera_information(uint8_t instance, mavlink_channel_t chan) const
928 : {
929 5 : auto *backend = get_instance(instance);
930 5 : if (backend == nullptr) {
931 0 : return;
932 : }
933 5 : backend->send_camera_information(chan);
934 : }
935 :
936 : // send camera settings message to GCS
937 0 : void AP_Mount::send_camera_settings(uint8_t instance, mavlink_channel_t chan) const
938 : {
939 0 : auto *backend = get_instance(instance);
940 0 : if (backend == nullptr) {
941 0 : return;
942 : }
943 0 : backend->send_camera_settings(chan);
944 : }
945 :
946 : // send camera capture status message to GCS
947 0 : void AP_Mount::send_camera_capture_status(uint8_t instance, mavlink_channel_t chan) const
948 : {
949 0 : auto *backend = get_instance(instance);
950 0 : if (backend == nullptr) {
951 0 : return;
952 : }
953 0 : backend->send_camera_capture_status(chan);
954 : }
955 :
956 : #if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
957 : // send camera thermal range message to GCS
958 0 : void AP_Mount::send_camera_thermal_range(uint8_t instance, mavlink_channel_t chan) const
959 : {
960 0 : auto *backend = get_instance(instance);
961 0 : if (backend == nullptr) {
962 0 : return;
963 : }
964 0 : backend->send_camera_thermal_range(chan);
965 : }
966 : #endif
967 :
968 : // change camera settings not normally used by autopilot
969 : // setting values from AP_Camera::Setting enum
970 0 : bool AP_Mount::change_setting(uint8_t instance, CameraSetting setting, float value)
971 : {
972 0 : auto *backend = get_instance(instance);
973 0 : if (backend == nullptr) {
974 0 : return false;
975 : }
976 0 : return backend->change_setting(setting, value);
977 : }
978 :
979 : // get rangefinder distance. Returns true on success
980 0 : bool AP_Mount::get_rangefinder_distance(uint8_t instance, float& distance_m) const
981 : {
982 0 : auto *backend = get_instance(instance);
983 0 : if (backend == nullptr) {
984 0 : return false;
985 : }
986 0 : return backend->get_rangefinder_distance(distance_m);
987 : }
988 :
989 : // enable/disable rangefinder. Returns true on success
990 0 : bool AP_Mount::set_rangefinder_enable(uint8_t instance, bool enable)
991 : {
992 0 : auto *backend = get_instance(instance);
993 0 : if (backend == nullptr) {
994 0 : return false;
995 : }
996 0 : return backend->set_rangefinder_enable(enable);
997 : }
998 :
999 428 : AP_Mount_Backend *AP_Mount::get_primary() const
1000 : {
1001 428 : return get_instance(_primary);
1002 : }
1003 :
1004 82821 : AP_Mount_Backend *AP_Mount::get_instance(uint8_t instance) const
1005 : {
1006 82821 : if (instance >= ARRAY_SIZE(_backends)) {
1007 0 : return nullptr;
1008 : }
1009 82821 : return _backends[instance];
1010 : }
1011 :
1012 : // This is the mapping between gimbal_device_id (defined by MAVLink) and actual devices (aka 'instances', 'backends')
1013 18 : AP_Mount_Backend *AP_Mount::mount_device_from_mavlink_gimbal_id(uint8_t gimbal_device_id) const
1014 : {
1015 : // FIXME: This function's behavior when gimbal_device_id == 0 is a bug. (That should indicate 'all mounts', not 'primary'.)
1016 : // Affects: Users working with multiple mounts.
1017 : // Workaround: Leave this as-is until it can be fixed in synchrony with upstream to prevent unexpected behavior-change.
1018 : // See: https://github.com/ArduPilot/ardupilot/issues/31940
1019 18 : if (gimbal_device_id == 0) {
1020 18 : return get_primary();
1021 : }
1022 0 : return get_instance(gimbal_device_id - 1);
1023 : }
1024 :
1025 : // pass a GIMBAL_REPORT message to the backend
1026 18195 : void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg)
1027 : {
1028 54585 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
1029 36390 : if (_backends[instance] != nullptr) {
1030 18195 : _backends[instance]->handle_gimbal_report(chan, msg);
1031 : }
1032 : }
1033 18195 : }
1034 :
1035 23720 : void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
1036 : {
1037 23720 : switch (msg.msgid) {
1038 18195 : case MAVLINK_MSG_ID_GIMBAL_REPORT:
1039 18195 : handle_gimbal_report(chan, msg);
1040 18195 : break;
1041 2801 : case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
1042 2801 : handle_global_position_int(msg);
1043 2801 : break;
1044 0 : case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE:
1045 0 : handle_gimbal_manager_set_attitude(msg);
1046 0 : break;
1047 0 : case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW:
1048 0 : handle_gimbal_manager_set_pitchyaw(msg);
1049 0 : break;
1050 4 : case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
1051 4 : handle_gimbal_device_information(msg);
1052 4 : break;
1053 2720 : case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS:
1054 2720 : handle_gimbal_device_attitude_status(msg);
1055 2720 : break;
1056 0 : default:
1057 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1058 0 : AP_HAL::panic("Unhandled mount case");
1059 : #endif
1060 0 : break;
1061 : }
1062 23720 : }
1063 :
1064 : // handle PARAM_VALUE
1065 26 : void AP_Mount::handle_param_value(const mavlink_message_t &msg)
1066 : {
1067 78 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
1068 52 : if (_backends[instance] != nullptr) {
1069 26 : _backends[instance]->handle_param_value(msg);
1070 : }
1071 : }
1072 26 : }
1073 :
1074 :
1075 : // handle GIMBAL_DEVICE_INFORMATION message
1076 4 : void AP_Mount::handle_gimbal_device_information(const mavlink_message_t &msg)
1077 : {
1078 12 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
1079 8 : if (_backends[instance] != nullptr) {
1080 8 : _backends[instance]->handle_gimbal_device_information(msg);
1081 : }
1082 : }
1083 4 : }
1084 :
1085 : // handle GIMBAL_DEVICE_ATTITUDE_STATUS message
1086 2720 : void AP_Mount::handle_gimbal_device_attitude_status(const mavlink_message_t &msg)
1087 : {
1088 8160 : for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
1089 5440 : if (_backends[instance] != nullptr) {
1090 5400 : _backends[instance]->handle_gimbal_device_attitude_status(msg);
1091 : }
1092 : }
1093 2720 : }
1094 :
1095 : // singleton instance
1096 : AP_Mount *AP_Mount::_singleton;
1097 :
1098 : namespace AP {
1099 :
1100 1275068 : AP_Mount *mount()
1101 : {
1102 1275068 : return AP_Mount::get_singleton();
1103 : }
1104 :
1105 : };
1106 :
1107 : #endif /* HAL_MOUNT_ENABLED */
|
