Line data Source code
1 : #include "AP_Camera_MAVLinkCamV2.h"
2 :
3 : #if AP_CAMERA_MAVLINKCAMV2_ENABLED
4 : #include <GCS_MAVLink/GCS.h>
5 :
6 : extern const AP_HAL::HAL& hal;
7 :
8 : #define AP_CAMERA_MAVLINKCAMV2_SEARCH_MS 60000 // search for camera for 60 seconds after startup
9 :
10 : // update - should be called at 50hz
11 13118 : void AP_Camera_MAVLinkCamV2::update()
12 : {
13 : // exit immediately if not initialised
14 13118 : if (!_initialised) {
15 1516 : find_camera();
16 : }
17 :
18 : // call parent update
19 13118 : AP_Camera_Backend::update();
20 13118 : }
21 :
22 : // entry point to actually take a picture. returns true on success
23 17 : bool AP_Camera_MAVLinkCamV2::trigger_pic()
24 : {
25 : // exit immediately if have not found camera or does not support taking pictures
26 17 : if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE)) {
27 0 : return false;
28 : }
29 :
30 : // prepare and send message
31 17 : mavlink_command_long_t pkt {};
32 17 : pkt.target_system = _sysid;
33 17 : pkt.target_component = _compid;
34 17 : pkt.command = MAV_CMD_IMAGE_START_CAPTURE;
35 17 : pkt.param3 = 1; // number of images to take
36 17 : pkt.param4 = image_index+1; // starting sequence number
37 :
38 17 : _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
39 :
40 17 : return true;
41 : }
42 :
43 : // start or stop video recording. returns true on success
44 : // set start_recording = true to start record, false to stop recording
45 0 : bool AP_Camera_MAVLinkCamV2::record_video(bool start_recording)
46 : {
47 : // exit immediately if have not found camera or does not support recording video
48 0 : if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO)) {
49 0 : return false;
50 : }
51 :
52 : // prepare and send message
53 0 : mavlink_command_long_t pkt {};
54 0 : pkt.target_system = _sysid;
55 0 : pkt.target_component = _compid;
56 :
57 0 : if (start_recording) {
58 0 : pkt.command = MAV_CMD_VIDEO_START_CAPTURE;
59 : // param1 = 0, video stream id. 0 for all streams
60 : // param2 = 0, status frequency. frequency that CAMERA_CAPTURE_STATUS messages should be sent while recording. 0 for no messages
61 : } else {
62 0 : pkt.command = MAV_CMD_VIDEO_STOP_CAPTURE;
63 : // param1 = 0, video stream id. 0 for all streams
64 : }
65 :
66 0 : _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
67 :
68 0 : return true;
69 : }
70 :
71 : // set zoom specified as a rate or percentage
72 6 : bool AP_Camera_MAVLinkCamV2::set_zoom(ZoomType zoom_type, float zoom_value)
73 : {
74 : // exit immediately if have not found camera or does not support zoom
75 6 : if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM)) {
76 0 : return false;
77 : }
78 :
79 : // prepare and send message
80 6 : mavlink_command_long_t pkt {};
81 6 : pkt.target_system = _sysid;
82 6 : pkt.target_component = _compid;
83 6 : pkt.command = MAV_CMD_SET_CAMERA_ZOOM;
84 6 : switch (zoom_type) {
85 0 : case ZoomType::RATE:
86 0 : pkt.param1 = ZOOM_TYPE_CONTINUOUS;
87 0 : break;
88 6 : case ZoomType::PCT:
89 6 : pkt.param1 = ZOOM_TYPE_RANGE;
90 6 : break;
91 : }
92 6 : pkt.param2 = zoom_value; // Zoom Value
93 :
94 6 : _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
95 :
96 6 : return true;
97 : }
98 :
99 : // set focus specified as rate, percentage or auto
100 : // focus in = -1, focus hold = 0, focus out = 1
101 6 : SetFocusResult AP_Camera_MAVLinkCamV2::set_focus(FocusType focus_type, float focus_value)
102 : {
103 : // exit immediately if have not found camera or does not support focus
104 6 : if (_link == nullptr || !(_cam_info.flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) {
105 0 : return SetFocusResult::FAILED;
106 : }
107 :
108 : // prepare and send message
109 6 : mavlink_command_long_t pkt {};
110 6 : pkt.target_system = _sysid;
111 6 : pkt.target_component = _compid;
112 6 : pkt.command = MAV_CMD_SET_CAMERA_FOCUS;
113 6 : switch (focus_type) {
114 0 : case FocusType::RATE:
115 : // focus in, out or hold (focus in = -1, hold = 0, focus out = 1). Same as FOCUS_TYPE_CONTINUOUS
116 0 : pkt.param1 = FOCUS_TYPE_CONTINUOUS;
117 0 : break;
118 6 : case FocusType::PCT:
119 : // focus to a percentage (from 0 to 100) of the full range. Same as FOCUS_TYPE_RANGE
120 6 : pkt.param1 = FOCUS_TYPE_RANGE;
121 6 : break;
122 0 : case FocusType::AUTO:
123 : // focus automatically. Same as FOCUS_TYPE_AUTO
124 0 : pkt.param1 = FOCUS_TYPE_AUTO;
125 0 : break;
126 : }
127 6 : pkt.param2 = focus_value;
128 :
129 6 : _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
130 :
131 6 : return SetFocusResult::ACCEPTED;
132 : }
133 :
134 : // handle incoming mavlink message including CAMERA_INFORMATION
135 8 : void AP_Camera_MAVLinkCamV2::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
136 : {
137 : // exit immediately if this is not our message
138 8 : if (msg.sysid != _sysid || msg.compid != _compid) {
139 4 : return;
140 : }
141 :
142 : // handle CAMERA_INFORMATION
143 4 : if (msg.msgid == MAVLINK_MSG_ID_CAMERA_INFORMATION) {
144 4 : mavlink_msg_camera_information_decode(&msg, &_cam_info);
145 :
146 4 : const uint8_t fw_ver_major = _cam_info.firmware_version & 0x000000FF;
147 4 : const uint8_t fw_ver_minor = (_cam_info.firmware_version & 0x0000FF00) >> 8;
148 4 : const uint8_t fw_ver_revision = (_cam_info.firmware_version & 0x00FF0000) >> 16;
149 4 : const uint8_t fw_ver_build = (_cam_info.firmware_version & 0xFF000000) >> 24;
150 :
151 : // display camera info to user
152 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Camera: %.32s %.32s fw:%u.%u.%u.%u",
153 : _cam_info.vendor_name,
154 : _cam_info.model_name,
155 : (unsigned)fw_ver_major,
156 : (unsigned)fw_ver_minor,
157 : (unsigned)fw_ver_revision,
158 : (unsigned)fw_ver_build);
159 :
160 4 : _got_camera_info = true;
161 : }
162 : }
163 :
164 : // send camera information message to GCS
165 8 : void AP_Camera_MAVLinkCamV2::send_camera_information(mavlink_channel_t chan) const
166 : {
167 : // exit immediately if we have not yet received cam info
168 8 : if (!_got_camera_info) {
169 4 : return;
170 : }
171 :
172 : // send CAMERA_INFORMATION message
173 4 : mavlink_msg_camera_information_send(
174 : chan,
175 : AP_HAL::millis(), // time_boot_ms
176 4 : _cam_info.vendor_name, // vendor_name uint8_t[32]
177 4 : _cam_info.model_name, // model_name uint8_t[32]
178 4 : _cam_info.firmware_version, // firmware version uint32_t
179 4 : _cam_info.focal_length, // focal_length float (mm)
180 4 : _cam_info.sensor_size_h, // sensor_size_h float (mm)
181 4 : _cam_info.sensor_size_v, // sensor_size_v float (mm)
182 4 : _cam_info.resolution_h, // resolution_h uint16_t (pix)
183 4 : _cam_info.resolution_v, // resolution_v uint16_t (pix)
184 4 : _cam_info.lens_id, // lens_id, uint8_t
185 4 : _cam_info.flags, // flags uint32_t (CAMERA_CAP_FLAGS)
186 4 : _cam_info.cam_definition_version, // cam_definition_version uint16_t
187 4 : _cam_info.cam_definition_uri, // cam_definition_uri char[140]
188 4 : get_gimbal_device_id()); // gimbal_device_id uint8_t
189 : }
190 :
191 : // search for camera in GCS_MAVLink routing table
192 1516 : void AP_Camera_MAVLinkCamV2::find_camera()
193 : {
194 : // do not look for camera for first 10 seconds so user may see banner
195 1516 : uint32_t now_ms = AP_HAL::millis();
196 1516 : if (now_ms < 10000) {
197 1508 : return;
198 : }
199 :
200 : // search for camera for 60 seconds or until armed
201 8 : if ((now_ms > AP_CAMERA_MAVLINKCAMV2_SEARCH_MS) && hal.util->get_soft_armed()) {
202 0 : return;
203 : }
204 :
205 : // search for a mavlink enabled camera
206 8 : if (_link == nullptr) {
207 : // we expect that instance 0 has compid = MAV_COMP_ID_CAMERA, instance 1 has compid = MAV_COMP_ID_CAMERA2, etc
208 4 : uint8_t compid = MIN(MAV_COMP_ID_CAMERA + _instance, MAV_COMP_ID_CAMERA6);
209 4 : _link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_CAMERA, compid, _sysid);
210 4 : if (_link == nullptr) {
211 : // have not yet found a camera so return
212 0 : return;
213 : }
214 4 : _compid = compid;
215 : }
216 :
217 : // request CAMERA_INFORMATION
218 8 : if (!_got_camera_info) {
219 4 : if (now_ms - _last_caminfo_req_ms > 1000) {
220 4 : _last_caminfo_req_ms = now_ms;
221 4 : request_camera_information();
222 : }
223 4 : return;
224 : }
225 :
226 4 : _initialised = true;
227 : }
228 :
229 : // request CAMERA_INFORMATION (holds vendor and model name)
230 4 : void AP_Camera_MAVLinkCamV2::request_camera_information() const
231 : {
232 4 : if (_link == nullptr) {
233 0 : return;
234 : }
235 :
236 4 : const mavlink_command_long_t pkt {
237 : MAVLINK_MSG_ID_CAMERA_INFORMATION, // param1
238 : 0, // param2
239 : 0, // param3
240 : 0, // param4
241 : 0, // param5
242 : 0, // param6
243 : 0, // param7
244 : MAV_CMD_REQUEST_MESSAGE,
245 4 : _sysid,
246 4 : _compid,
247 : 0 // confirmation
248 4 : };
249 :
250 4 : _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
251 : }
252 :
253 : #endif // AP_CAMERA_MAVLINKCAMV2_ENABLED
|
