Line data Source code
1 : #include "AP_Mount_config.h"
2 :
3 : #if HAL_MOUNT_XACTI_ENABLED
4 :
5 : #include "AP_Mount_Xacti.h"
6 :
7 : #include <AP_HAL/AP_HAL.h>
8 : #include <AP_AHRS/AP_AHRS.h>
9 : #include <GCS_MAVLink/GCS.h>
10 : #include <AP_BoardConfig/AP_BoardConfig.h>
11 : #include <AP_CANManager/AP_CANManager.h>
12 : #include <AP_DroneCAN/AP_DroneCAN.h>
13 : #include <AP_RTC/AP_RTC.h>
14 :
15 : extern const AP_HAL::HAL& hal;
16 :
17 : #define XACTI_MSG_SEND_MIN_MS 20 // messages should not be sent to camera more often than 20ms
18 : #define XACTI_DIGITAL_ZOOM_RATE_UPDATE_INTERVAL_MS 500 // digital zoom rate control updates 11% up or down every 0.5sec
19 : #define XACTI_OPTICAL_ZOOM_RATE_UPDATE_INTERVAL_MS 250 // optical zoom rate control updates 6.6% up or down every 0.25sec
20 : #define XACTI_STATUS_REQ_INTERVAL_MS 3000 // request status every 3 seconds
21 : #define XACTI_SET_PARAM_QUEUE_SIZE 3 // three set-param requests may be queued
22 :
23 : #define AP_MOUNT_XACTI_DEBUG 0
24 : #define debug(fmt, args ...) do { if (AP_MOUNT_XACTI_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Xacti: " fmt, ## args); } } while (0)
25 :
26 : AP_Mount_Xacti::DetectedModules AP_Mount_Xacti::_detected_modules[];
27 : HAL_Semaphore AP_Mount_Xacti::_sem_registry;
28 : const char* AP_Mount_Xacti::send_text_prefix = "Xacti:";
29 : const char* AP_Mount_Xacti::sensor_mode_str[] = { "RGB", "IR", "PIP", "NDVI" };
30 : const char* AP_Mount_Xacti::_param_names[] = {"SingleShot", "Recording", "FocusMode",
31 : "SensorMode", "DigitalZoomMagnification",
32 : "FirmwareVersion", "Status", "DateTime",
33 : "OpticalZoomMagnification"};
34 :
35 : // Constructor
36 0 : AP_Mount_Xacti::AP_Mount_Xacti(class AP_Mount &frontend, class AP_Mount_Params ¶ms, uint8_t instance) :
37 0 : AP_Mount_Backend(frontend, params, instance)
38 : {
39 0 : register_backend();
40 :
41 0 : param_int_cb = FUNCTOR_BIND_MEMBER(&AP_Mount_Xacti::handle_param_get_set_response_int, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &);
42 0 : param_string_cb = FUNCTOR_BIND_MEMBER(&AP_Mount_Xacti::handle_param_get_set_response_string, bool, AP_DroneCAN*, const uint8_t, const char*, AP_DroneCAN::string &);
43 0 : param_save_cb = FUNCTOR_BIND_MEMBER(&AP_Mount_Xacti::handle_param_save_response, void, AP_DroneCAN*, const uint8_t, bool);
44 :
45 : // static assert that Param enum matches parameter names array
46 : static_assert((uint8_t)AP_Mount_Xacti::Param::LAST+1 == ARRAY_SIZE(AP_Mount_Xacti::_param_names), "AP_Mount_Xacti::_param_names array must match Param enum");
47 0 : }
48 :
49 : // init - performs any required initialisation for this instance
50 0 : void AP_Mount_Xacti::init()
51 : {
52 : // instantiate parameter queue, return on failure so init fails
53 0 : _set_param_int32_queue = NEW_NOTHROW ObjectArray<SetParamQueueItem>(XACTI_SET_PARAM_QUEUE_SIZE);
54 0 : if (_set_param_int32_queue == nullptr) {
55 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s init failed", send_text_prefix);
56 0 : return;
57 : }
58 0 : _initialised = true;
59 : }
60 :
61 : // update mount position - should be called periodically
62 0 : void AP_Mount_Xacti::update()
63 : {
64 : // exit immediately if not initialised
65 0 : if (!_initialised) {
66 0 : return;
67 : }
68 :
69 : // return immediately if any message sent is unlikely to be processed
70 0 : const uint32_t now_ms = AP_HAL::millis();
71 0 : if (!is_safe_to_send(now_ms)) {
72 0 : return;
73 : }
74 :
75 : // get firmware version
76 0 : if (request_firmware_version(now_ms)) {
77 0 : return;
78 : }
79 :
80 : // additional initial setup
81 0 : if (_firmware_version.received) {
82 : // set date and time
83 0 : if (set_datetime(now_ms)) {
84 0 : return;
85 : }
86 : // request camera capabilities
87 0 : if (request_capabilities(now_ms)) {
88 0 : return;
89 : }
90 : }
91 :
92 : // request status
93 0 : if (request_status(now_ms)) {
94 0 : return;
95 : }
96 :
97 : // process queue of set parameter items
98 0 : if (process_set_param_int32_queue()) {
99 0 : return;
100 : }
101 :
102 : // periodically send copter attitude and GPS status
103 0 : if (send_copter_att_status(now_ms)) {
104 : // if message sent avoid sending other messages
105 0 : return;
106 : }
107 :
108 : // update zoom rate control
109 0 : if (update_zoom_rate_control(now_ms)) {
110 : // if message sent avoid sending other messages
111 0 : return;
112 : }
113 :
114 : // update based on mount mode
115 0 : update_mnt_target();
116 :
117 : // send target angles or rates depending on the target type
118 0 : send_target_to_gimbal();
119 : }
120 :
121 : // return true if healthy
122 0 : bool AP_Mount_Xacti::healthy() const
123 : {
124 : // unhealthy until gimbal has been found and replied with firmware version and no motor errors
125 0 : if (!_initialised || !_firmware_version.received || _motor_error) {
126 0 : return false;
127 : }
128 :
129 : // unhealthy if attitude information NOT received recently
130 0 : const uint32_t now_ms = AP_HAL::millis();
131 0 : if (now_ms - _last_current_attitude_quat_ms > 1000) {
132 0 : return false;
133 : }
134 :
135 : // if we get this far return healthy
136 0 : return true;
137 : }
138 :
139 : // take a picture. returns true on success
140 0 : bool AP_Mount_Xacti::take_picture()
141 : {
142 : // fail if camera errored
143 0 : if (_camera_error) {
144 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s unable to take pic", send_text_prefix);
145 0 : return false;
146 : }
147 :
148 0 : return set_param_int32(Param::SingleShot, 0);
149 : }
150 :
151 : // start or stop video recording. returns true on success
152 : // set start_recording = true to start record, false to stop recording
153 0 : bool AP_Mount_Xacti::record_video(bool start_recording)
154 : {
155 0 : return set_param_int32(Param::Recording, start_recording ? 1 : 0);
156 : }
157 :
158 : // set zoom specified as a rate or percentage
159 0 : bool AP_Mount_Xacti::set_zoom(ZoomType zoom_type, float zoom_value)
160 : {
161 : // zoom rate
162 0 : if (zoom_type == ZoomType::RATE) {
163 0 : if (is_zero(zoom_value)) {
164 : // stop zooming
165 0 : _zoom_rate_control.enabled = false;
166 : } else {
167 : // zoom in or out
168 0 : _zoom_rate_control.enabled = true;
169 0 : _zoom_rate_control.dir = (zoom_value < 0) ? -1 : 1;
170 : }
171 0 : return true;
172 : }
173 :
174 : // zoom percentage
175 0 : if (zoom_type == ZoomType::PCT) {
176 0 : if (capabilities.optical_zoom == Capability::True) {
177 : // optical zoom capable cameras use combination of optical and digital zoom
178 : // convert zoom percentage (0 ~ 100) to zoom times using linear interpolation
179 : // optical zoom covers 1x to 2.5x, param values are in 100 to 250
180 : // digital zoom covers 2.5x to 25x, param values are 100 to 1000
181 0 : const float zoom_times = linear_interpolate(1, 25, zoom_value, 0, 100);
182 0 : const uint16_t optical_zoom_param = constrain_uint16(uint16_t(zoom_times * 10) * 10, 100, 250);
183 0 : const uint16_t digital_zoom_param = constrain_uint16(uint16_t(zoom_times * 0.4) * 100, 100, 1000);
184 0 : bool ret = true;
185 0 : if (optical_zoom_param != _last_optical_zoom_param_value) {
186 0 : ret = set_param_int32(Param::OpticalZoomMagnification, optical_zoom_param);
187 : }
188 0 : if (digital_zoom_param != _last_digital_zoom_param_value) {
189 0 : ret &= set_param_int32(Param::DigitalZoomMagnification, digital_zoom_param);
190 : }
191 0 : return ret;
192 : }
193 : // digital only zoom
194 : // convert zoom percentage (0 ~ 100) to zoom parameter value (100, 200, 300, ... 1000)
195 : // 0~11pct:100, 12~22pct:200, 23~33pct:300, 34~44pct:400, 45~55pct:500, 56~66pct:600, 67~77pct:700, 78~88pct:800, 89~99pct:900, 100:1000
196 0 : const uint16_t zoom_param_value = uint16_t(linear_interpolate(1, 10, zoom_value, 0, 100)) * 100;
197 0 : return set_param_int32(Param::DigitalZoomMagnification, zoom_param_value);
198 : }
199 :
200 : // unsupported zoom type
201 0 : return false;
202 : }
203 :
204 : // set focus specified as rate, percentage or auto
205 : // focus in = -1, focus hold = 0, focus out = 1
206 0 : SetFocusResult AP_Mount_Xacti::set_focus(FocusType focus_type, float focus_value)
207 : {
208 : // convert focus type and value to parameter value
209 : uint8_t focus_param_value;
210 0 : switch (focus_type) {
211 0 : case FocusType::RATE:
212 : case FocusType::PCT:
213 : // focus rate and percentage control not supported so simply switch to manual focus
214 : // FocusMode of 0:Manual Focus
215 0 : focus_param_value = 0;
216 0 : break;
217 0 : case FocusType::AUTO:
218 : // FocusMode of 1:Single AutoFocus, 2:Continuous AutoFocus
219 0 : focus_param_value = 2;
220 0 : break;
221 0 : default:
222 : // unsupported forucs mode
223 0 : return SetFocusResult::INVALID_PARAMETERS;
224 : }
225 :
226 : // set FocusMode parameter
227 0 : return set_param_int32(Param::FocusMode, focus_param_value) ? SetFocusResult::ACCEPTED : SetFocusResult::FAILED;
228 : }
229 :
230 : // set camera lens as a value from 0 to 5
231 0 : bool AP_Mount_Xacti::set_lens(uint8_t lens)
232 : {
233 : // sanity check
234 0 : if (lens > (uint8_t)SensorsMode::NDVI) {
235 0 : return false;
236 : }
237 :
238 0 : return set_param_int32(Param::SensorMode, lens);
239 : }
240 :
241 : // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
242 : // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
243 0 : bool AP_Mount_Xacti::set_camera_source(uint8_t primary_source, uint8_t secondary_source)
244 : {
245 : // maps primary and secondary source to xacti SensorsMode
246 : SensorsMode new_sensor_mode;
247 0 : switch (primary_source) {
248 0 : case 0: // Default (RGB)
249 : FALLTHROUGH;
250 : case 1: // RGB
251 0 : switch (secondary_source) {
252 0 : case 0: // RGB + Default (None)
253 0 : new_sensor_mode = SensorsMode::RGB;
254 0 : break;
255 0 : case 2: // PIP RGB+IR
256 0 : new_sensor_mode = SensorsMode::PIP;
257 0 : break;
258 0 : default:
259 0 : return false;
260 : }
261 0 : break;
262 0 : case 2: // IR
263 0 : if (secondary_source != 0) {
264 0 : return false;
265 : }
266 0 : new_sensor_mode = SensorsMode::IR;
267 0 : break;
268 0 : case 3: // NDVI
269 0 : if (secondary_source != 0) {
270 0 : return false;
271 : }
272 : // NDVI + Default (None)
273 0 : new_sensor_mode = SensorsMode::NDVI;
274 0 : break;
275 0 : default:
276 0 : return false;
277 : }
278 :
279 : // send desired sensor mode to camera
280 0 : return set_param_int32(Param::SensorMode, (uint8_t)new_sensor_mode);
281 : }
282 :
283 : // send camera settings message to GCS
284 0 : void AP_Mount_Xacti::send_camera_settings(mavlink_channel_t chan) const
285 : {
286 : // send CAMERA_SETTINGS message
287 0 : mavlink_msg_camera_settings_send(
288 : chan,
289 : AP_HAL::millis(), // time_boot_ms
290 0 : _recording_video ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
291 : 0, // zoomLevel float, percentage from 0 to 100, NaN if unknown
292 : NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
293 0 : }
294 :
295 : // get attitude as a quaternion. returns true on success
296 0 : bool AP_Mount_Xacti::get_attitude_quaternion(Quaternion& att_quat)
297 : {
298 0 : att_quat = _current_attitude_quat;
299 0 : return true;
300 : }
301 :
302 : // send target pitch and yaw rates to gimbal
303 0 : void AP_Mount_Xacti::send_target_rates(const MountRateTarget &rate_rads)
304 : {
305 0 : const float pitch_rads = rate_rads.pitch;
306 0 : const float yaw_rads = rate_rads.yaw;
307 :
308 : // send gimbal rate target to gimbal
309 0 : send_gimbal_control(3, degrees(pitch_rads) * 100, degrees(yaw_rads) * 100);
310 0 : }
311 :
312 : // send target pitch and yaw angles to gimbal
313 0 : void AP_Mount_Xacti::send_target_angles(const MountAngleTarget &angle_rad)
314 : {
315 0 : const float pitch_rad = angle_rad.pitch;
316 0 : const float yaw_rad = angle_rad.yaw;
317 0 : const bool yaw_is_ef = angle_rad.yaw_is_ef;
318 :
319 : // convert yaw to body frame
320 0 : const float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().get_yaw_rad()) : yaw_rad;
321 :
322 : // send angle target to gimbal
323 0 : send_gimbal_control(2, degrees(pitch_rad) * 100, degrees(yaw_bf_rad) * 100);
324 0 : }
325 :
326 : // subscribe to Xacti DroneCAN messages
327 5 : bool AP_Mount_Xacti::subscribe_msgs(AP_DroneCAN* ap_dronecan)
328 : {
329 5 : const auto driver_index = ap_dronecan->get_driver_index();
330 :
331 5 : return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_gimbal_attitude_status, driver_index) != nullptr)
332 5 : && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_gnss_status_req, driver_index) != nullptr)
333 : ;
334 : }
335 :
336 : // register backend in detected modules array used to map DroneCAN port and node id to backend
337 0 : void AP_Mount_Xacti::register_backend()
338 : {
339 0 : WITH_SEMAPHORE(_sem_registry);
340 :
341 : // add this backend to _detected_modules array
342 0 : _detected_modules[_instance].driver = this;
343 :
344 : // return if devid is zero meaning this backend has not yet been associated with a mount
345 0 : const uint32_t devid = (uint32_t)_params.dev_id.get();
346 0 : if (devid == 0) {
347 0 : return;
348 : }
349 :
350 : // get DroneCan port from device id
351 0 : const uint8_t can_driver_index = AP_HAL::Device::devid_get_bus(devid);
352 0 : const uint8_t can_num_drivers = AP::can().get_num_drivers();
353 0 : for (uint8_t i = 0; i < can_num_drivers; i++) {
354 0 : AP_DroneCAN *ap_dronecan = AP_DroneCAN::get_dronecan(i);
355 0 : if (ap_dronecan != nullptr && ap_dronecan->get_driver_index() == can_driver_index) {
356 0 : _detected_modules[_instance].ap_dronecan = ap_dronecan;
357 : }
358 : }
359 :
360 : // get node_id from device id
361 0 : _detected_modules[_instance].node_id = AP_HAL::Device::devid_get_address(devid);
362 0 : }
363 :
364 : // find backend associated with the given dronecan port and node_id. also associates backends with zero node ids
365 : // returns pointer to backend on success, nullptr on failure
366 0 : AP_Mount_Xacti* AP_Mount_Xacti::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id)
367 : {
368 0 : WITH_SEMAPHORE(_sem_registry);
369 :
370 : // exit immediately if DroneCAN is unavailable or invalid node id
371 0 : if (ap_dronecan == nullptr || node_id == 0) {
372 0 : return nullptr;
373 : }
374 :
375 : // search for backend with matching dronecan port and node id
376 0 : for (uint8_t i = 0; i < ARRAY_SIZE(_detected_modules); i++) {
377 0 : if (_detected_modules[i].driver != nullptr &&
378 0 : _detected_modules[i].ap_dronecan == ap_dronecan &&
379 0 : _detected_modules[i].node_id == node_id ) {
380 0 : return _detected_modules[i].driver;
381 : }
382 : }
383 :
384 : // if we got this far, this dronecan port and node id are not associated with any backend
385 : // associate with first backend with node id of zero
386 0 : for (uint8_t i = 0; i < ARRAY_SIZE(_detected_modules); i++) {
387 0 : if (_detected_modules[i].driver != nullptr &&
388 0 : _detected_modules[i].node_id == 0) {
389 0 : _detected_modules[i].ap_dronecan = ap_dronecan;
390 0 : _detected_modules[i].node_id = node_id;
391 0 : const auto dev_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN,
392 0 : ap_dronecan->get_driver_index(),
393 : node_id, 0);
394 0 : _detected_modules[i].driver->set_dev_id(dev_id);
395 0 : return _detected_modules[i].driver;
396 : }
397 : }
398 :
399 0 : return nullptr;
400 0 : }
401 :
402 : // handle xacti gimbal attitude status message
403 0 : void AP_Mount_Xacti::handle_gimbal_attitude_status(AP_DroneCAN* ap_dronecan, const CanardRxTransfer& transfer, const com_xacti_GimbalAttitudeStatus &msg)
404 : {
405 : // fetch the matching backend driver, node id and gimbal id backend instance
406 0 : AP_Mount_Xacti* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id);
407 0 : if (driver == nullptr) {
408 0 : return;
409 : }
410 :
411 : // convert body-frame Euler angles to Quaternion. Note yaw direction is reversed from normal
412 0 : driver->_current_attitude_quat.from_euler(radians(msg.gimbal_roll * 0.01), radians(msg.gimbal_pitch * 0.01), radians(-msg.gimbal_yaw * 0.01));
413 0 : driver->_last_current_attitude_quat_ms = AP_HAL::millis();
414 : }
415 :
416 : // handle xacti gnss status request message
417 0 : void AP_Mount_Xacti::handle_gnss_status_req(AP_DroneCAN* ap_dronecan, const CanardRxTransfer& transfer, const com_xacti_GnssStatusReq &msg)
418 : {
419 : // sanity check dronecan port
420 0 : if (ap_dronecan == nullptr) {
421 0 : return;
422 : }
423 :
424 : // get current location
425 0 : uint8_t gps_status = 2;
426 0 : Location loc;
427 0 : if (!AP::ahrs().get_location(loc)) {
428 0 : gps_status = 0;
429 : }
430 :
431 : // get date and time
432 : uint16_t year, ms;
433 : uint8_t month, day, hour, min, sec;
434 : #if AP_RTC_ENABLED
435 0 : if (!AP::rtc().get_date_and_time_utc(year, month, day, hour, min, sec, ms)) {
436 0 : year = month = day = hour = min = sec = 0;
437 : }
438 : #else
439 : year = month = day = hour = min = sec = 0;
440 : (void)ms;
441 : #endif
442 :
443 : // send xacti specific gnss status message
444 0 : com_xacti_GnssStatus xacti_gnss_status_msg {};
445 0 : xacti_gnss_status_msg.gps_status = gps_status;
446 0 : xacti_gnss_status_msg.order = msg.requirement;
447 0 : xacti_gnss_status_msg.remain_buffer = 1;
448 0 : xacti_gnss_status_msg.utc_year = year;
449 0 : xacti_gnss_status_msg.utc_month = month + 1;
450 0 : xacti_gnss_status_msg.utc_day = day;
451 0 : xacti_gnss_status_msg.utc_hour = hour;
452 0 : xacti_gnss_status_msg.utc_minute = min;
453 0 : xacti_gnss_status_msg.utc_seconds = sec;
454 0 : xacti_gnss_status_msg.latitude = loc.lat * 1e-7;
455 0 : xacti_gnss_status_msg.longitude = loc.lng * 1e-7;
456 0 : xacti_gnss_status_msg.altitude = loc.alt * 1e-2;
457 0 : ap_dronecan->xacti_gnss_status.broadcast(xacti_gnss_status_msg);
458 : }
459 :
460 : // handle param get/set response
461 0 : bool AP_Mount_Xacti::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, uint8_t node_id, const char* name, int32_t &value)
462 : {
463 : // error string prefix to save on flash
464 0 : const char* err_prefix_str = "Xacti: failed to";
465 :
466 : // take picture
467 0 : if (strcmp(name, get_param_name_str(Param::SingleShot)) == 0) {
468 0 : if (value < 0) {
469 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s take pic", err_prefix_str);
470 : }
471 0 : return false;
472 : }
473 :
474 : // recording
475 0 : if (strcmp(name, get_param_name_str(Param::Recording)) == 0) {
476 0 : if (value < 0) {
477 0 : _recording_video = false;
478 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s record", err_prefix_str);
479 : } else {
480 0 : _recording_video = (value == 1);
481 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s recording %s", send_text_prefix, _recording_video ? "ON" : "OFF");
482 : }
483 0 : return false;
484 : }
485 :
486 : // focus
487 0 : if (strcmp(name, get_param_name_str(Param::FocusMode)) == 0) {
488 0 : if (value < 0) {
489 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s change focus", err_prefix_str);
490 : } else {
491 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s focus", send_text_prefix, value == 0 ? "manual" : "auto");
492 : }
493 0 : return false;
494 : }
495 :
496 : // camera lens (aka sensor mode)
497 0 : if (strcmp(name, get_param_name_str(Param::SensorMode)) == 0) {
498 0 : if (value < 0) {
499 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s change lens", err_prefix_str);
500 0 : } else if ((uint32_t)value < ARRAY_SIZE(sensor_mode_str)) {
501 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %s", send_text_prefix, sensor_mode_str[(uint8_t)value]);
502 : }
503 0 : return false;
504 : }
505 :
506 : // digital zoom
507 0 : if (strcmp(name, get_param_name_str(Param::DigitalZoomMagnification)) == 0) {
508 0 : if (value < 0) {
509 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s change zoom", err_prefix_str);
510 : // disable zoom rate control (if active) to avoid repeated failures
511 0 : _zoom_rate_control.enabled = false;
512 0 : } else if (value >= 100 && value <= 1000) {
513 0 : _last_digital_zoom_param_value = value;
514 : }
515 0 : return false;
516 : }
517 :
518 : // optical zoom
519 0 : if (strcmp(name, get_param_name_str(Param::OpticalZoomMagnification)) == 0) {
520 0 : if (value < 0) {
521 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s change optical zoom", err_prefix_str);
522 : // disable zoom rate control (if active) to avoid repeated failures
523 0 : _zoom_rate_control.enabled = false;
524 0 : } else if (value >= 100 && value <= 250) {
525 0 : capabilities.optical_zoom = Capability::True;
526 0 : capabilities.received = true;
527 0 : _last_optical_zoom_param_value = value;
528 : }
529 0 : return false;
530 : }
531 :
532 : // unhandled parameter get or set
533 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s get/set %s res:%ld", send_text_prefix, name, (long int)value);
534 0 : return false;
535 : }
536 :
537 : // handle param get/set response
538 0 : bool AP_Mount_Xacti::handle_param_get_set_response_string(AP_DroneCAN* ap_dronecan, uint8_t node_id, const char* name, AP_DroneCAN::string &value)
539 : {
540 0 : if (strcmp(name, get_param_name_str(Param::FirmwareVersion)) == 0) {
541 0 : _firmware_version.received = true;
542 0 : const uint8_t len = MIN(value.len, ARRAY_SIZE(_firmware_version.str)-1);
543 0 : memcpy(_firmware_version.str, (const char*)value.data, len);
544 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: Xacti fw:%s", _firmware_version.str);
545 :
546 : // firmware str from gimbal is of the format YYMMDD[b]xx. Convert to uint32 for reporting to GCS
547 0 : if (len >= 9) {
548 0 : const char major_str[3] = {_firmware_version.str[0], _firmware_version.str[1], 0};
549 0 : const char minor_str[3] = {_firmware_version.str[2], _firmware_version.str[3], 0};
550 0 : const char patch_str[3] = {_firmware_version.str[4], _firmware_version.str[5], 0};
551 0 : const char dev_str[3] = {_firmware_version.str[7], _firmware_version.str[8], 0};
552 0 : const uint8_t major_ver_num = atoi(major_str) & 0xFF;
553 0 : const uint8_t minor_ver_num = atoi(minor_str) & 0xFF;
554 0 : const uint8_t patch_ver_num = atoi(patch_str) & 0xFF;
555 0 : const uint8_t dev_ver_num = atoi(dev_str) & 0xFF;
556 0 : _firmware_version.mav_ver = UINT32_VALUE(dev_ver_num, patch_ver_num, minor_ver_num, major_ver_num);
557 : }
558 0 : return false;
559 0 : } else if (strcmp(name, get_param_name_str(Param::DateTime)) == 0) {
560 : // display when time and date have been set
561 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s datetime set %s", send_text_prefix, (const char*)value.data);
562 0 : return false;
563 0 : } else if (strcmp(name, get_param_name_str(Param::Status)) == 0) {
564 : // check for expected length
565 0 : const char* error_str = "error";
566 0 : if (value.len != sizeof(_status)) {
567 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
568 0 : return false;
569 : }
570 :
571 : // backup error status and copy to structure
572 0 : const uint32_t last_error_status = _status.error_status;
573 0 : memcpy(&_status, value.data, value.len);
574 :
575 : // report change in status
576 0 : uint32_t changed_bits = last_error_status ^ _status.error_status;
577 0 : const char* ok_str = "OK";
578 0 : if (changed_bits & (uint32_t)ErrorStatus::TIME_NOT_SET) {
579 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s time %sset", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::TIME_NOT_SET ? "not " : "");
580 0 : if (_status.error_status & (uint32_t)ErrorStatus::TIME_NOT_SET) {
581 : // try to set time again
582 0 : _datetime.set = false;
583 : }
584 : }
585 0 : if (changed_bits & (uint32_t)ErrorStatus::MEDIA_ERROR) {
586 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s media %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::MEDIA_ERROR ? error_str : ok_str);
587 : }
588 0 : if (changed_bits & (uint32_t)ErrorStatus::LENS_ERROR) {
589 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s lens %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::LENS_ERROR ? error_str : ok_str);
590 : }
591 0 : if (changed_bits & (uint32_t)ErrorStatus::MOTOR_INIT_ERROR) {
592 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s motor %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::MOTOR_INIT_ERROR ? "init error" : ok_str);
593 : }
594 0 : if (changed_bits & (uint32_t)ErrorStatus::MOTOR_OPERATION_ERROR) {
595 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s motor op %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::MOTOR_OPERATION_ERROR ? error_str : ok_str);
596 : }
597 0 : if (changed_bits & (uint32_t)ErrorStatus::GIMBAL_CONTROL_ERROR) {
598 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s control %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::GIMBAL_CONTROL_ERROR ? error_str : ok_str);
599 : }
600 0 : if (changed_bits & (uint32_t)ErrorStatus::TEMP_WARNING) {
601 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s temp %s", send_text_prefix, _status.error_status & (uint32_t)ErrorStatus::TEMP_WARNING ? "warning" : ok_str);
602 : }
603 :
604 : // set motor error for health reporting
605 0 : _motor_error = _status.error_status & ((uint32_t)ErrorStatus::MOTOR_INIT_ERROR | (uint32_t)ErrorStatus::MOTOR_OPERATION_ERROR | (uint32_t)ErrorStatus::GIMBAL_CONTROL_ERROR);
606 0 : _camera_error = _status.error_status & ((uint32_t)ErrorStatus::LENS_ERROR | (uint32_t)ErrorStatus::MEDIA_ERROR);
607 0 : return false;
608 : }
609 :
610 : // unhandled parameter get or set
611 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s get/set string %s res:%s", send_text_prefix, name, (const char*)value.data);
612 0 : return false;
613 : }
614 :
615 0 : void AP_Mount_Xacti::handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success)
616 : {
617 : // display failure to save parameter
618 0 : if (!success) {
619 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s CAM%u failed to set param", send_text_prefix, (int)_instance+1);
620 : }
621 0 : }
622 :
623 : // get parameter name for a particular param enum value
624 : // returns an empty string if not found (which should never happen)
625 0 : const char* AP_Mount_Xacti::get_param_name_str(Param param) const
626 : {
627 : // check to avoid reading beyond end of array. This should never happen
628 0 : if ((uint8_t)param > ARRAY_SIZE(_param_names)) {
629 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
630 0 : return "";
631 : }
632 0 : return _param_names[(uint8_t)param];
633 : }
634 :
635 : // helper function to set integer parameters
636 0 : bool AP_Mount_Xacti::set_param_int32(Param param, int32_t param_value)
637 : {
638 0 : if (_set_param_int32_queue == nullptr) {
639 0 : return false;
640 : }
641 :
642 : // set param request added to queue to be sent. throttling requests improves reliability
643 0 : return _set_param_int32_queue->push(SetParamQueueItem{param, param_value});
644 : }
645 :
646 : // helper function to set string parameters
647 0 : bool AP_Mount_Xacti::set_param_string(Param param, const AP_DroneCAN::string& param_value)
648 : {
649 0 : if (_detected_modules[_instance].ap_dronecan == nullptr) {
650 0 : return false;
651 : }
652 :
653 : // convert param to string
654 0 : const char* param_name_str = get_param_name_str(param);
655 0 : if (param_name_str == nullptr) {
656 0 : return false;
657 : }
658 :
659 0 : if (_detected_modules[_instance].ap_dronecan->set_parameter_on_node(_detected_modules[_instance].node_id, param_name_str, param_value, ¶m_string_cb)) {
660 0 : last_send_getset_param_ms = AP_HAL::millis();
661 0 : return true;
662 : }
663 0 : return false;
664 : }
665 :
666 : // helper function to get string parameters
667 0 : bool AP_Mount_Xacti::get_param_string(Param param)
668 : {
669 0 : if (_detected_modules[_instance].ap_dronecan == nullptr) {
670 0 : return false;
671 : }
672 :
673 : // convert param to string
674 0 : const char* param_name_str = get_param_name_str(param);
675 0 : if (_detected_modules[_instance].ap_dronecan->get_parameter_on_node(_detected_modules[_instance].node_id, param_name_str, ¶m_string_cb)) {
676 0 : last_send_getset_param_ms = AP_HAL::millis();
677 0 : return true;
678 : }
679 0 : return false;
680 : }
681 :
682 : // process queue of set parameter items
683 0 : bool AP_Mount_Xacti::process_set_param_int32_queue()
684 : {
685 0 : if ((_set_param_int32_queue == nullptr) || (_detected_modules[_instance].ap_dronecan == nullptr)) {
686 0 : return false;
687 : }
688 :
689 : SetParamQueueItem param_to_set;
690 0 : if (_set_param_int32_queue->pop(param_to_set)) {
691 : // convert param to string
692 0 : const char* param_name_str = get_param_name_str(param_to_set.param);
693 0 : if (_detected_modules[_instance].ap_dronecan->set_parameter_on_node(_detected_modules[_instance].node_id, param_name_str, param_to_set.value, ¶m_int_cb)) {
694 0 : last_send_getset_param_ms = AP_HAL::millis();
695 0 : return true;
696 : }
697 0 : return false;
698 : }
699 0 : return false;
700 : }
701 :
702 : // send gimbal control message via DroneCAN
703 : // mode is 2:angle control or 3:rate control
704 : // pitch_cd is pitch angle in centi-degrees or pitch rate in cds
705 : // yaw_cd is angle in centi-degrees or yaw rate in cds
706 0 : void AP_Mount_Xacti::send_gimbal_control(uint8_t mode, int16_t pitch_cd, int16_t yaw_cd)
707 : {
708 : // exit immediately if no DroneCAN port
709 0 : if (_detected_modules[_instance].ap_dronecan == nullptr) {
710 0 : return;
711 : }
712 :
713 : // send at no faster than 5hz
714 0 : const uint32_t now_ms = AP_HAL::millis();
715 0 : if (now_ms - last_send_gimbal_control_ms < 200) {
716 0 : return;
717 : }
718 0 : last_send_gimbal_control_ms = now_ms;
719 :
720 : // send xacti specific gimbal control message
721 0 : com_xacti_GimbalControlData gimbal_control_data_msg {};
722 0 : gimbal_control_data_msg.pitch_cmd_type = mode;
723 0 : gimbal_control_data_msg.yaw_cmd_type = mode;
724 0 : gimbal_control_data_msg.pitch_cmd_value = pitch_cd;
725 0 : gimbal_control_data_msg.yaw_cmd_value = -yaw_cd;
726 0 : _detected_modules[_instance].ap_dronecan->xacti_gimbal_control_data.broadcast(gimbal_control_data_msg);
727 : }
728 :
729 : // send copter attitude status message to gimbal. now_ms is current system time
730 : // returns true if sent so that we avoid immediately trying to also send other messages
731 0 : bool AP_Mount_Xacti::send_copter_att_status(uint32_t now_ms)
732 : {
733 : // exit immediately if no DroneCAN port
734 0 : if (_detected_modules[_instance].ap_dronecan == nullptr) {
735 0 : return false;
736 : }
737 :
738 : // send at no faster than 5hz
739 0 : if (now_ms - last_send_copter_att_status_ms < 100) {
740 0 : return false;
741 : }
742 :
743 : // send xacti specific vehicle attitude message
744 0 : Quaternion veh_att;
745 0 : if (!AP::ahrs().get_quaternion(veh_att)) {
746 0 : return false;
747 : }
748 :
749 0 : last_send_copter_att_status_ms = now_ms;
750 0 : com_xacti_CopterAttStatus copter_att_status_msg {};
751 0 : copter_att_status_msg.quaternion_wxyz_e4[0] = veh_att.q1 * 1e4;
752 0 : copter_att_status_msg.quaternion_wxyz_e4[1] = veh_att.q2 * 1e4;
753 0 : copter_att_status_msg.quaternion_wxyz_e4[2] = veh_att.q3 * 1e4;
754 0 : copter_att_status_msg.quaternion_wxyz_e4[3] = veh_att.q4 * 1e4;
755 0 : copter_att_status_msg.reserved.len = 2;
756 0 : copter_att_status_msg.reserved.data[0] = 0;
757 0 : copter_att_status_msg.reserved.data[1] = 0;
758 0 : _detected_modules[_instance].ap_dronecan->xacti_copter_att_status.broadcast(copter_att_status_msg);
759 0 : return true;
760 : }
761 :
762 : // update zoom rate controller. now_ms is current system time
763 : // returns true if sent so that we avoid immediately trying to also send other messages
764 0 : bool AP_Mount_Xacti::update_zoom_rate_control(uint32_t now_ms)
765 : {
766 : // return immediately if zoom rate control is not enabled
767 0 : if (!_zoom_rate_control.enabled) {
768 0 : return false;
769 : }
770 :
771 : // we are controlling optical zoom if the camera has it and we are below the optical zoom upper limit
772 : // or at the optical zoom upper limit, the lower digital zoom limit and are zooming out
773 0 : bool optical_zoom_control = (capabilities.optical_zoom == Capability::True) &&
774 0 : ((_last_optical_zoom_param_value < 250) ||
775 0 : ((_last_optical_zoom_param_value == 250) && (_last_digital_zoom_param_value == 100) && (_zoom_rate_control.dir < 0)));
776 :
777 : // update every 0.25 or 0.5 sec
778 0 : const uint32_t update_interval_ms = optical_zoom_control ? XACTI_OPTICAL_ZOOM_RATE_UPDATE_INTERVAL_MS : XACTI_DIGITAL_ZOOM_RATE_UPDATE_INTERVAL_MS;
779 0 : if (now_ms - _zoom_rate_control.last_update_ms < update_interval_ms) {
780 0 : return false;
781 : }
782 0 : _zoom_rate_control.last_update_ms = now_ms;
783 :
784 : // optical zoom
785 0 : if (optical_zoom_control) {
786 0 : const uint16_t optical_zoom_value = _last_optical_zoom_param_value + _zoom_rate_control.dir * 10;
787 : // if reached lower limit of optical zoom, disable zoom control
788 0 : if (optical_zoom_value < 100) {
789 0 : _zoom_rate_control.enabled = false;
790 0 : return false;
791 : }
792 :
793 : // send desired optical zoom to camera
794 0 : return set_param_int32(Param::OpticalZoomMagnification, MIN(optical_zoom_value, 250));
795 : }
796 :
797 : // digital zoom
798 0 : const uint16_t digital_zoom_value = _last_digital_zoom_param_value + _zoom_rate_control.dir * 100;
799 : // if reached limit then disable zoom
800 0 : if (((capabilities.optical_zoom != Capability::True) && (digital_zoom_value < 100)) || (digital_zoom_value > 1000)) {
801 0 : _zoom_rate_control.enabled = false;
802 0 : return false;
803 : }
804 :
805 : // send desired digital zoom to camera
806 0 : return set_param_int32(Param::DigitalZoomMagnification, digital_zoom_value);
807 : }
808 :
809 : // request firmware version. now_ms should be current system time (reduces calls to AP_HAL::millis)
810 : // returns true if sent so that we avoid immediately trying to also send other messages
811 0 : bool AP_Mount_Xacti::request_firmware_version(uint32_t now_ms)
812 : {
813 : // return immediately if already have version or no dronecan
814 0 : if (_firmware_version.received) {
815 0 : return false;
816 : }
817 :
818 : // send request once per second until received
819 0 : if (now_ms - _firmware_version.last_request_ms < 1000) {
820 0 : return false;
821 : }
822 0 : _firmware_version.last_request_ms = now_ms;
823 0 : return get_param_string(Param::FirmwareVersion);
824 : }
825 :
826 : // request parameters used to determine camera capabilities. now_ms is current system time
827 : // returns true if a param get/set was sent so that we avoid sending other messages
828 0 : bool AP_Mount_Xacti::request_capabilities(uint32_t now_ms)
829 : {
830 : // return immediately if we have already determined this models capabilities
831 0 : if (capabilities.received) {
832 0 : return false;
833 : }
834 :
835 : // send requests once per second until received
836 0 : if (now_ms - capabilities.last_request_ms < 1000) {
837 0 : return false;
838 : }
839 0 : capabilities.last_request_ms = now_ms;
840 :
841 : // record start time
842 0 : if (capabilities.first_request_ms == 0) {
843 0 : capabilities.first_request_ms = now_ms;
844 : }
845 :
846 : // timeout after 10 seconds
847 0 : if (now_ms - capabilities.first_request_ms > 10000) {
848 0 : capabilities.optical_zoom = Capability::False;
849 0 : capabilities.received = true;
850 0 : return false;
851 : }
852 :
853 : // optical zoom: try setting optical zoom to 1x
854 : // return is handled in handle_param_get_set_response_int
855 0 : return set_param_int32(Param::OpticalZoomMagnification, 100);
856 : }
857 :
858 : // set date and time. now_ms is current system time
859 0 : bool AP_Mount_Xacti::set_datetime(uint32_t now_ms)
860 : {
861 : // return immediately if gimbal's date/time has been set
862 0 : if (_datetime.set) {
863 0 : return false;
864 : }
865 :
866 : // attempt to set datetime once per second until received
867 0 : if (now_ms - _datetime.last_request_ms < 1000) {
868 0 : return false;
869 : }
870 0 : _datetime.last_request_ms = now_ms;
871 :
872 : // get date and time
873 : uint16_t year, ms;
874 : uint8_t month, day, hour, min, sec;
875 : #if AP_RTC_ENABLED
876 0 : if (!AP::rtc().get_date_and_time_utc(year, month, day, hour, min, sec, ms)) {
877 0 : return false;
878 : }
879 : #else
880 : (void)ms;
881 : return false;
882 : #endif
883 :
884 : // date time is of the format YYYYMMDDHHMMSS (14 bytes)
885 : // convert month from 0~11 to 1~12 range
886 0 : AP_DroneCAN::string datetime_string {};
887 0 : const int num_bytes = snprintf((char *)datetime_string.data, sizeof(AP_DroneCAN::string::data), "%04u%02u%02u%02u%02u%02u",
888 0 : (unsigned)year, (unsigned)month+1, (unsigned)day, (unsigned)hour, (unsigned)min, (unsigned)sec);
889 : // sanity check bytes to be sent
890 0 : if (num_bytes != 14) {
891 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
892 0 : return false;
893 : }
894 0 : datetime_string.len = num_bytes;
895 0 : _datetime.set = set_param_string(Param::DateTime, datetime_string);
896 0 : if (!_datetime.set) {
897 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s failed to set date/time", send_text_prefix);
898 : }
899 :
900 0 : return _datetime.set;
901 : }
902 :
903 : // request status. now_ms should be current system time (reduces calls to AP_HAL::millis)
904 : // returns true if sent so that we avoid immediately trying to also send other messages
905 0 : bool AP_Mount_Xacti::request_status(uint32_t now_ms)
906 : {
907 : // return immediately if 3 seconds has not passed
908 0 : if (now_ms - _status_report.last_request_ms < XACTI_STATUS_REQ_INTERVAL_MS) {
909 0 : return false;
910 : }
911 :
912 0 : _status_report.last_request_ms = now_ms;
913 0 : return get_param_string(Param::Status);
914 : }
915 :
916 : // check if safe to send message (if messages sent too often camera will not respond)
917 : // now_ms should be current system time (reduces calls to AP_HAL::millis)
918 0 : bool AP_Mount_Xacti::is_safe_to_send(uint32_t now_ms) const
919 : {
920 : // check time since last attitude sent
921 0 : if (now_ms - last_send_copter_att_status_ms < XACTI_MSG_SEND_MIN_MS) {
922 0 : return false;
923 : }
924 :
925 : // check time since last angle target sent
926 0 : if (now_ms - last_send_gimbal_control_ms < XACTI_MSG_SEND_MIN_MS) {
927 0 : return false;
928 : }
929 :
930 : // check time since last set param message sent
931 0 : if (now_ms - last_send_getset_param_ms < XACTI_MSG_SEND_MIN_MS) {
932 0 : return false;
933 : }
934 :
935 0 : return true;
936 : }
937 :
938 : #endif // HAL_MOUNT_XACTI_ENABLED
|
