Line data Source code
1 : #include "AP_Camera_config.h"
2 :
3 : #if AP_CAMERA_ENABLED
4 :
5 : #include "AP_Camera_Backend.h"
6 :
7 : #include <GCS_MAVLink/GCS.h>
8 : #include <AP_GPS/AP_GPS.h>
9 : #include <AP_Mount/AP_Mount.h>
10 : #include <AP_AHRS/AP_AHRS.h>
11 :
12 : extern const AP_HAL::HAL& hal;
13 :
14 : // Constructor
15 21 : AP_Camera_Backend::AP_Camera_Backend(AP_Camera &frontend, AP_Camera_Params ¶ms, uint8_t instance) :
16 21 : _frontend(frontend),
17 21 : _params(params),
18 21 : _instance(instance)
19 21 : {}
20 :
21 15 : void AP_Camera_Backend::init()
22 : {
23 : #if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
24 15 : _camera_info.focal_length = NaNf;
25 15 : _camera_info.sensor_size_h = NaNf;
26 15 : _camera_info.sensor_size_v = NaNf;
27 :
28 15 : _camera_info.flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
29 : #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
30 15 : }
31 :
32 : // update - should be called at 50hz
33 69734 : void AP_Camera_Backend::update()
34 : {
35 : // Check camera options and start/stop recording based on arm/disarm
36 69734 : if (option_is_enabled(Option::RecordWhileArmed)) {
37 0 : if (hal.util->get_soft_armed() != last_is_armed) {
38 0 : last_is_armed = hal.util->get_soft_armed();
39 0 : if (!record_video(last_is_armed)) {
40 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Camera: failed to %s recording", last_is_armed ? "start" : "stop");
41 : }
42 : }
43 : }
44 :
45 : // try to take picture if pending
46 69734 : if (trigger_pending) {
47 0 : take_picture();
48 : }
49 :
50 : // check feedback pin
51 69734 : check_feedback();
52 :
53 : // time based triggering
54 : // if time and distance triggering both are enabled then we only do time based triggering
55 69734 : if (time_interval_settings.num_remaining != 0) {
56 587 : uint32_t delta_ms = AP_HAL::millis() - last_picture_time_ms;
57 587 : if (delta_ms > time_interval_settings.time_interval_ms) {
58 12 : if (take_picture()) {
59 : // decrease num_remaining except when its -1 i.e. capture forever
60 12 : if (time_interval_settings.num_remaining > 0) {
61 2 : time_interval_settings.num_remaining--;
62 : }
63 : }
64 : }
65 69691 : return;
66 : }
67 :
68 : // implement trigger distance
69 69147 : if (!is_positive(_params.trigg_dist)) {
70 67087 : last_location.lat = 0;
71 67087 : last_location.lng = 0;
72 67087 : return;
73 : }
74 :
75 2060 : const AP_AHRS &ahrs = AP::ahrs();
76 :
77 2060 : Location current_loc;
78 2060 : if (!ahrs.get_location(current_loc)) {
79 0 : return;
80 : }
81 :
82 : // check vehicle flight mode supports trigg dist
83 2060 : if (!_frontend.vehicle_mode_ok_for_trigg_dist()) {
84 0 : return;
85 : }
86 :
87 : // check vehicle roll angle is less than configured maximum
88 2060 : if ((_frontend.get_roll_max() > 0) && (fabsf(ahrs.get_roll_deg()) > _frontend.get_roll_max())) {
89 0 : return;
90 : }
91 :
92 : // initialise last location to current location
93 2060 : if (last_location.lat == 0 && last_location.lng == 0) {
94 1 : last_location = current_loc;
95 1 : return;
96 : }
97 2059 : if (last_location.lat == current_loc.lat && last_location.lng == current_loc.lng) {
98 : // we haven't moved - this can happen as update() may
99 : // be called without a new GPS fix
100 508 : return;
101 : }
102 :
103 : // check vehicle has moved at least trigg_dist meters
104 1551 : if (current_loc.get_distance(last_location) < _params.trigg_dist) {
105 1508 : return;
106 : }
107 :
108 43 : take_picture();
109 : }
110 :
111 : // get corresponding mount instance for the camera
112 75 : uint8_t AP_Camera_Backend::get_mount_instance() const
113 : {
114 : // instance 0 means default
115 75 : if (_params.mount_instance.get() == 0) {
116 75 : return _instance;
117 : }
118 0 : return _params.mount_instance.get() - 1;
119 : }
120 :
121 : // get mavlink gimbal device id which is normally mount_instance+1
122 4 : uint8_t AP_Camera_Backend::get_gimbal_device_id() const
123 : {
124 : #if HAL_MOUNT_ENABLED
125 4 : const uint8_t mount_instance = get_mount_instance();
126 4 : AP_Mount* mount = AP::mount();
127 4 : if (mount != nullptr) {
128 4 : if (mount->get_mount_type(mount_instance) != AP_Mount::Type::None) {
129 4 : return (mount_instance + 1);
130 : }
131 : }
132 : #endif
133 0 : return 0;
134 : }
135 :
136 :
137 : // take a picture. returns true on success
138 66 : bool AP_Camera_Backend::take_picture()
139 : {
140 : // setup feedback pin interrupt or timer
141 66 : setup_feedback_callback();
142 :
143 : // check minimum time interval since last picture taken
144 66 : uint32_t now_ms = AP_HAL::millis();
145 66 : if (now_ms - last_picture_time_ms < (uint32_t)(_params.interval_min * 1000)) {
146 0 : trigger_pending = true;
147 0 : return false;
148 : }
149 :
150 66 : trigger_pending = false;
151 :
152 : // trigger actually taking picture and update image count
153 66 : if (trigger_pic()) {
154 66 : image_index++;
155 66 : last_picture_time_ms = now_ms;
156 66 : IGNORE_RETURN(AP::ahrs().get_location(last_location));
157 : #if HAL_LOGGING_ENABLED
158 66 : log_picture();
159 : #endif
160 66 : return true;
161 : }
162 :
163 0 : return false;
164 : }
165 :
166 : // take multiple pictures, time_interval between two consecutive pictures is in miliseconds
167 : // total_num is number of pictures to be taken, -1 means capture forever
168 2 : void AP_Camera_Backend::take_multiple_pictures(uint32_t time_interval_ms, int16_t total_num)
169 : {
170 2 : time_interval_settings = {time_interval_ms, total_num};
171 2 : }
172 :
173 : // stop capturing multiple image sequence
174 1 : void AP_Camera_Backend::stop_capture()
175 : {
176 1 : time_interval_settings = {0, 0};
177 1 : }
178 :
179 : // handle camera control
180 0 : void AP_Camera_Backend::control(float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_t cmd_id)
181 : {
182 : // take picture
183 0 : if (shooting_cmd == 1) {
184 0 : take_picture();
185 : }
186 0 : }
187 :
188 : // send camera feedback message to GCS
189 2917 : void AP_Camera_Backend::send_camera_feedback(mavlink_channel_t chan)
190 : {
191 2917 : int32_t altitude = 0;
192 2917 : if (camera_feedback.location.initialised() && !camera_feedback.location.get_alt_cm(Location::AltFrame::ABSOLUTE, altitude)) {
193 : // completely ignore this failure! this is a shouldn't-happen
194 : // as current_loc should never be in an altitude we can't
195 : // convert.
196 : }
197 2917 : int32_t altitude_rel = 0;
198 2917 : if (camera_feedback.location.initialised() && !camera_feedback.location.get_alt_cm(Location::AltFrame::ABOVE_HOME, altitude_rel)) {
199 : // completely ignore this failure! this is a shouldn't-happen
200 : // as current_loc should never be in an altitude we can't
201 : // convert.
202 : }
203 :
204 : // send camera feedback message
205 2917 : mavlink_msg_camera_feedback_send(
206 : chan,
207 : camera_feedback.timestamp_us, // image timestamp
208 : 0, // target system id
209 2917 : _instance, // camera id
210 2917 : image_index, // image index
211 : camera_feedback.location.lat, // latitude
212 : camera_feedback.location.lng, // longitude
213 : altitude*1e-2f, // alt MSL
214 : altitude_rel*1e-2f, // alt relative to home
215 : camera_feedback.roll_deg, // roll angle (deg)
216 : camera_feedback.pitch_deg, // pitch angle (deg)
217 : camera_feedback.yaw_deg, // yaw angle (deg)
218 : 0.0f, // focal length
219 : CAMERA_FEEDBACK_PHOTO, // flags
220 2917 : camera_feedback.feedback_trigger_logged_count); // completed image captures
221 2917 : }
222 :
223 : // send camera information message to GCS
224 0 : void AP_Camera_Backend::send_camera_information(mavlink_channel_t chan) const
225 : {
226 : mavlink_camera_information_t camera_info;
227 : #if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
228 :
229 0 : camera_info = _camera_info;
230 :
231 : #else
232 :
233 : memset(&camera_info, 0, sizeof(camera_info));
234 :
235 : camera_info.focal_length = NaNf;
236 : camera_info.sensor_size_h = NaNf;
237 : camera_info.sensor_size_v = NaNf;
238 :
239 : camera_info.flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
240 :
241 : #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
242 :
243 : // Set fixed fields
244 : // lens_id is populated with the instance number, to disambiguate multiple cameras
245 0 : camera_info.lens_id = _instance;
246 0 : camera_info.gimbal_device_id = get_gimbal_device_id();
247 0 : camera_info.time_boot_ms = AP_HAL::millis();
248 :
249 : // Send CAMERA_INFORMATION message
250 0 : mavlink_msg_camera_information_send_struct(chan, &camera_info);
251 0 : }
252 :
253 : #if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
254 0 : void AP_Camera_Backend::set_camera_information(mavlink_camera_information_t camera_info)
255 : {
256 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Camera %u CAMERA_INFORMATION (%s %s) set from script", _instance, camera_info.vendor_name, camera_info.model_name);
257 0 : _camera_info = camera_info;
258 0 : };
259 : #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
260 :
261 : #if AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED
262 : // send video stream information message to GCS
263 0 : void AP_Camera_Backend::send_video_stream_information(mavlink_channel_t chan) const
264 : {
265 : #if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
266 :
267 : // Send VIDEO_STREAM_INFORMATION message
268 0 : mavlink_msg_video_stream_information_send_struct(chan, &_stream_info);
269 :
270 : #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
271 0 : }
272 : #endif // AP_MAVLINK_MSG_VIDEO_STREAM_INFORMATION_ENABLED
273 :
274 : #if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
275 0 : void AP_Camera_Backend::set_stream_information(mavlink_video_stream_information_t stream_info)
276 : {
277 0 : _stream_info = stream_info;
278 0 : };
279 : #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED
280 :
281 : // send camera settings message to GCS
282 0 : void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const
283 : {
284 : // send CAMERA_SETTINGS message
285 0 : mavlink_msg_camera_settings_send(
286 : chan,
287 : AP_HAL::millis(), // time_boot_ms
288 : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
289 : NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown
290 : NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
291 0 : }
292 :
293 : #if AP_CAMERA_SEND_FOV_STATUS_ENABLED
294 : // send camera field of view status
295 0 : void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const
296 : {
297 : // getting corresponding mount instance for camera
298 0 : AP_Mount* mount = AP::mount();
299 0 : if (mount == nullptr) {
300 0 : return;
301 : }
302 :
303 : // get latest POI from mount
304 0 : Quaternion quat;
305 0 : Location camera_loc;
306 0 : Location poi_loc;
307 0 : const bool have_poi_loc = mount->get_poi(get_mount_instance(), quat, camera_loc, poi_loc);
308 :
309 : // if failed to get POI, get camera location directly from AHRS
310 : // and attitude directly from mount
311 0 : bool have_camera_loc = have_poi_loc;
312 0 : if (!have_camera_loc) {
313 0 : have_camera_loc = AP::ahrs().get_location(camera_loc);
314 0 : mount->get_attitude_quaternion(get_mount_instance(), quat);
315 : }
316 :
317 : // calculate attitude quaternion in earth frame using AHRS yaw
318 0 : Quaternion quat_ef;
319 0 : quat_ef.from_euler(0, 0, AP::ahrs().get_yaw_rad());
320 0 : quat_ef *= quat;
321 :
322 : // send camera fov status message only if the last calculated values aren't stale
323 : const float quat_array[4] = {
324 0 : quat_ef.q1,
325 0 : quat_ef.q2,
326 0 : quat_ef.q3,
327 0 : quat_ef.q4
328 0 : };
329 0 : mavlink_msg_camera_fov_status_send(
330 : chan,
331 : AP_HAL::millis(),
332 : have_camera_loc ? camera_loc.lat : INT32_MAX,
333 : have_camera_loc ? camera_loc.lng : INT32_MAX,
334 0 : have_camera_loc ? camera_loc.alt * 10 : INT32_MAX,
335 : have_poi_loc ? poi_loc.lat : INT32_MAX,
336 : have_poi_loc ? poi_loc.lng : INT32_MAX,
337 0 : have_poi_loc ? poi_loc.alt * 10 : INT32_MAX,
338 : quat_array,
339 0 : horizontal_fov() > 0 ? horizontal_fov() : NaNf,
340 0 : vertical_fov() > 0 ? vertical_fov() : NaNf
341 : );
342 : }
343 : #endif
344 :
345 : // send camera capture status message to GCS
346 4 : void AP_Camera_Backend::send_camera_capture_status(mavlink_channel_t chan) const
347 : {
348 : // Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress)
349 : // num_remaining is -1 when capturing until stopped, so test the same way update() does
350 4 : const uint8_t image_status = (time_interval_settings.num_remaining != 0) ? 2 : 0;
351 :
352 : // send CAMERA_CAPTURE_STATUS message
353 4 : mavlink_msg_camera_capture_status_send(
354 : chan,
355 : AP_HAL::millis(),
356 : image_status,
357 : 0, // current status of video capturing (0: idle, 1: capture in progress)
358 4 : static_cast<float>(time_interval_settings.time_interval_ms) / 1000.0, // image capture interval (s)
359 : 0, // elapsed time since recording started (ms)
360 : NaNf, // available storage capacity (ms)
361 4 : image_index); // total number of images captured
362 4 : }
363 :
364 : // setup a callback for a feedback pin. When on PX4 with the right FMU
365 : // mode we can use the microsecond timer.
366 66 : void AP_Camera_Backend::setup_feedback_callback()
367 : {
368 66 : if (_params.feedback_pin <= 0 || timer_installed || isr_installed) {
369 : // invalid or already installed
370 66 : return;
371 : }
372 :
373 : // ensure we are in input mode
374 0 : hal.gpio->pinMode(_params.feedback_pin, HAL_GPIO_INPUT);
375 :
376 : // enable pullup/pulldown
377 0 : uint8_t trigger_polarity = _params.feedback_polarity == 0 ? 0 : 1;
378 0 : hal.gpio->write(_params.feedback_pin, !trigger_polarity);
379 :
380 0 : if (hal.gpio->attach_interrupt(_params.feedback_pin, FUNCTOR_BIND_MEMBER(&AP_Camera_Backend::feedback_pin_isr, void, uint8_t, bool, uint32_t),
381 : trigger_polarity?AP_HAL::GPIO::INTERRUPT_RISING:AP_HAL::GPIO::INTERRUPT_FALLING)) {
382 0 : isr_installed = true;
383 : } else {
384 : // install a 1kHz timer to check feedback pin
385 0 : hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Camera_Backend::feedback_pin_timer, void));
386 :
387 0 : timer_installed = true;
388 : }
389 : }
390 :
391 : // interrupt handler for interrupt based feedback trigger
392 0 : void AP_Camera_Backend::feedback_pin_isr(uint8_t pin, bool high, uint32_t timestamp_us)
393 : {
394 0 : feedback_trigger_timestamp_us = timestamp_us;
395 0 : feedback_trigger_count++;
396 0 : }
397 :
398 : // check if feedback pin is high for timer based feedback trigger, when
399 : // attach_interrupt fails
400 0 : void AP_Camera_Backend::feedback_pin_timer()
401 : {
402 0 : uint8_t pin_state = hal.gpio->read(_params.feedback_pin);
403 0 : uint8_t trigger_polarity = _params.feedback_polarity == 0 ? 0 : 1;
404 0 : if (pin_state == trigger_polarity &&
405 0 : last_pin_state != trigger_polarity) {
406 0 : feedback_trigger_timestamp_us = AP_HAL::micros();
407 0 : feedback_trigger_count++;
408 : }
409 0 : last_pin_state = pin_state;
410 0 : }
411 :
412 : // check for feedback pin update and log if necessary
413 69734 : void AP_Camera_Backend::check_feedback()
414 : {
415 69734 : if (feedback_trigger_logged_count != feedback_trigger_count) {
416 : #if HAL_LOGGING_ENABLED
417 0 : const uint32_t timestamp32 = feedback_trigger_timestamp_us;
418 : #endif
419 0 : feedback_trigger_logged_count = feedback_trigger_count;
420 :
421 : // we should consider doing this inside the ISR and pin_timer
422 0 : prep_mavlink_msg_camera_feedback(feedback_trigger_timestamp_us);
423 :
424 : #if HAL_LOGGING_ENABLED
425 : // log camera message
426 0 : uint32_t tdiff = AP_HAL::micros() - timestamp32;
427 0 : uint64_t timestamp = AP_HAL::micros64();
428 0 : Write_Camera(timestamp - tdiff);
429 : #endif
430 : }
431 69734 : }
432 :
433 66 : void AP_Camera_Backend::prep_mavlink_msg_camera_feedback(uint64_t timestamp_us)
434 : {
435 66 : const AP_AHRS &ahrs = AP::ahrs();
436 66 : if (!ahrs.get_location(camera_feedback.location)) {
437 : // completely ignore this failure! AHRS will provide its best guess.
438 : }
439 66 : camera_feedback.timestamp_us = timestamp_us;
440 66 : camera_feedback.roll_deg = ahrs.get_roll_deg();
441 66 : camera_feedback.pitch_deg = ahrs.get_pitch_deg();
442 66 : camera_feedback.yaw_deg = ahrs.get_yaw_deg();
443 66 : camera_feedback.feedback_trigger_logged_count = feedback_trigger_logged_count;
444 :
445 66 : GCS_SEND_MESSAGE(MSG_CAMERA_FEEDBACK);
446 66 : }
447 :
448 : #if HAL_LOGGING_ENABLED
449 : // log picture
450 66 : void AP_Camera_Backend::log_picture()
451 : {
452 66 : const bool using_feedback_pin = _params.feedback_pin > 0;
453 :
454 66 : if (!using_feedback_pin) {
455 : // if we're using a feedback pin then when the event occurs we
456 : // stash the feedback data. Since we're not using a feedback
457 : // pin, we just use "now".
458 66 : prep_mavlink_msg_camera_feedback(AP::gps().time_epoch_usec());
459 : }
460 :
461 66 : if (!using_feedback_pin) {
462 66 : Write_Camera();
463 : } else {
464 0 : Write_Trigger();
465 : }
466 66 : }
467 : #endif
468 :
469 : #endif // AP_CAMERA_ENABLED
|
