@@ -47,11 +47,16 @@ class AP_Mount_MAVLink : public AP_Mount_Backend
4747 (1U <<unsigned (MountTargetType::RETRACTED))
4848 );
4949
50- // temporary hack until we get GIMBAL_DEVICE_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL
50+ // remove in ArduPilot-4.9; was a temporary hack until we got
51+ // GIMBAL_DEVICE_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL
5152 if ((strncmp (vendor_name, " AVTA" , 4 ) == 0 ) && (strncmp (model_name, " CM41" , 4 ) != 0 )){
5253 supported_target |= (1U <<unsigned (MountTargetType::LOCATION));
5354 }
54-
55+
56+ if (has_capability (GIMBAL_DEVICE_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL)) {
57+ supported_target |= (1U <<unsigned (MountTargetType::LOCATION));
58+ }
59+
5560 return supported_target;
5661 };
5762
@@ -60,6 +65,12 @@ class AP_Mount_MAVLink : public AP_Mount_Backend
6065
6166private:
6267
68+ // returns true if the camera is reporting that it has the
69+ // capability indicated by flag
70+ bool has_capability (GIMBAL_DEVICE_CAP_FLAGS flag) const {
71+ return device_capapability_flags & flag;
72+ }
73+
6374 // search for gimbal in GCS_MAVLink routing table
6475 void find_gimbal ();
6576
@@ -93,5 +104,6 @@ class AP_Mount_MAVLink : public AP_Mount_Backend
93104 uint32_t _last_attitude_status_ms; // system time last attitude status was received (used for health reporting)
94105 char vendor_name[MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_VENDOR_NAME_LEN]; // vendor name
95106 char model_name[MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_MODEL_NAME_LEN]; // model name
107+ uint32_t device_capapability_flags; // from GIMBAL_DEVICE_INFORMATION
96108};
97109#endif // HAL_MOUNT_MAVLINK_ENABLED
0 commit comments