Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : implementation of RunCam camera protocols
17 :
18 : With thanks to betaflight for a great reference
19 : implementation. Several of the functions below are based on
20 : betaflight equivalent functions
21 :
22 : RunCam protocol specification can be found at https://support.runcam.com/hc/en-us/articles/360014537794-RunCam-Device-Protocol
23 : */
24 : #include "AP_RunCam.h"
25 :
26 : #if AP_CAMERA_RUNCAM_ENABLED
27 :
28 : #include <AP_Math/AP_Math.h>
29 : #include <AP_Math/crc.h>
30 : #include <GCS_MAVLink/GCS.h>
31 : #include <AP_Logger/AP_Logger.h>
32 : #include <AP_SerialManager/AP_SerialManager.h>
33 :
34 : const AP_Param::GroupInfo AP_RunCam::var_info[] = {
35 : // @Param: TYPE
36 : // @DisplayName: RunCam device type
37 : // @Description: RunCam device type used to determine OSD menu structure and shutter options.
38 : // @Values: 0:Disabled, 1:RunCam Split Micro/RunCam with UART, 2:RunCam Split, 3:RunCam Split4 4k, 4:RunCam Hybrid/RunCam Thumb Pro, 5:Runcam 2 4k
39 : AP_GROUPINFO_FLAGS("TYPE", 1, AP_RunCam, _cam_type, int(DeviceModel::SplitMicro), AP_PARAM_FLAG_ENABLE),
40 :
41 : // @Param: FEATURES
42 : // @DisplayName: RunCam features available
43 : // @Description: The available features of the attached RunCam device. If 0 then the RunCam device will be queried for the features it supports, otherwise this setting is used.
44 : // @User: Advanced
45 : // @Bitmask: 0:Power Button,1:WiFi Button,2:Change Mode,3:5-Key OSD,4:Settings Access,5:DisplayPort,6:Start Recording,7:Stop Recording
46 : AP_GROUPINFO("FEATURES", 2, AP_RunCam, _features, 0),
47 :
48 : // @Param: BT_DELAY
49 : // @DisplayName: RunCam boot delay before allowing updates
50 : // @Description: Time it takes for the RunCam to become fully ready in ms. If this is too short then commands can get out of sync.
51 : // @User: Advanced
52 : AP_GROUPINFO("BT_DELAY", 3, AP_RunCam, _boot_delay_ms, 7000),
53 :
54 : // @Param: BTN_DELY
55 : // @DisplayName: RunCam button delay before allowing further button presses
56 : // @Description: Time it takes for the a RunCam button press to be actived in ms. If this is too short then commands can get out of sync.
57 : // @User: Advanced
58 : AP_GROUPINFO("BTN_DELY", 4, AP_RunCam, _button_delay_ms, RUNCAM_DEFAULT_BUTTON_PRESS_DELAY),
59 :
60 : // @Param: MDE_DELY
61 : // @DisplayName: RunCam mode delay before allowing further button presses
62 : // @Description: Time it takes for the a RunCam mode button press to be actived in ms. If a mode change first requires a video recording change then double this value is used. If this is too short then commands can get out of sync.
63 : // @User: Advanced
64 : AP_GROUPINFO("MDE_DELY", 5, AP_RunCam, _mode_delay_ms, 800),
65 :
66 : // @Param: CONTROL
67 : // @DisplayName: RunCam control option
68 : // @Description: Specifies the allowed actions required to enter the OSD menu and other option like autorecording
69 : // @Bitmask: 0:Stick yaw right,1:Stick roll right,2:3-position switch,3:2-position switch,4:Autorecording enabled
70 : // @User: Advanced
71 : AP_GROUPINFO("CONTROL", 6, AP_RunCam, _cam_control_option, uint8_t(ControlOption::STICK_ROLL_RIGHT) | uint8_t(ControlOption::TWO_POS_SWITCH)),
72 :
73 : AP_GROUPEND
74 : };
75 :
76 : #define RUNCAM_DEBUG 0
77 :
78 : #if RUNCAM_DEBUG
79 : static const char* event_names[11] = {
80 : "NONE", "ENTER_MENU", "EXIT_MENU",
81 : "IN_MENU_ENTER", "IN_MENU_RIGHT", "IN_MENU_UP", "IN_MENU_DOWN", "IN_MENU_EXIT",
82 : "BUTTON_RELEASE", "STOP_RECORDING", "START_RECORDING"
83 : };
84 : static const char* state_names[7] = {
85 : "INITIALIZING", "INITIALIZED", "READY", "VIDEO_RECORDING", "ENTERING_MENU", "IN_MENU", "EXITING_MENU"
86 : };
87 : #define debug(fmt, args ...) do { hal.console->printf("RunCam[%s]: " fmt, state_names[int(_state)], ## args); } while (0)
88 : #else
89 : #define debug(fmt, args ...)
90 : #endif
91 :
92 : extern const AP_HAL::HAL& hal;
93 :
94 : // singleton instance
95 : AP_RunCam *AP_RunCam::_singleton;
96 :
97 : AP_RunCam::Request::Length AP_RunCam::Request::_expected_responses_length[RUNCAM_NUM_EXPECTED_RESPONSES] = {
98 : { Command::RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, 5 },
99 : { Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, 2 },
100 : { Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, 2 },
101 : { Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, 3 },
102 : };
103 :
104 : // the protocol for Runcam Device definition
105 : static const uint8_t RUNCAM_HEADER = 0xCC;
106 : static const uint8_t RUNCAM_OSD_MENU_DEPTH = 2;
107 : static const uint32_t RUNCAM_INIT_INTERVAL_MS = 1000;
108 : static const uint32_t RUNCAM_OSD_UPDATE_INTERVAL_MS = 100; // 10Hz
109 :
110 : // menu structures of runcam devices
111 : AP_RunCam::Menu AP_RunCam::_menus[RUNCAM_MAX_DEVICE_TYPES] = {
112 : // these are correct for the runcam split micro v2.4.4, others may vary
113 : // Video, Image, TV-OUT, Micro SD Card, General
114 : { 6, { 5, 8, 3, 3, 7 }}, // SplitMicro
115 : { 0, { 0 }}, // Split
116 : { 6, { 4, 10, 3, 3, 7 }}, // Split4 4K
117 : { 1, { 0 }}, // Hybrid, simple mode switch
118 : { 6, { 3, 10, 2, 2, 8 }}, // Runcam 2 4K
119 : };
120 :
121 : const char* AP_RunCam::_models[RUNCAM_MAX_DEVICE_TYPES] = {
122 : "SplitMicro",
123 : "Split",
124 : "Split4k",
125 : "Hybrid",
126 : "Run24k"
127 : };
128 :
129 6 : AP_RunCam::AP_RunCam(AP_Camera &frontend, AP_Camera_Params ¶ms, uint8_t instance, uint8_t runcam_instance)
130 6 : : AP_Camera_Backend(frontend, params, instance), _runcam_instance(runcam_instance)
131 : {
132 6 : AP_Param::setup_object_defaults(this, var_info);
133 6 : if (_singleton != nullptr && _singleton->_instance == instance) {
134 0 : AP_HAL::panic("AP_RunCam instance must be a singleton %u", instance);
135 : }
136 6 : if (_singleton == nullptr) {
137 6 : _singleton = this;
138 : }
139 6 : _cam_type.set(constrain_int16(_cam_type, 0, RUNCAM_MAX_DEVICE_TYPES));
140 6 : _video_recording = VideoOption(_cam_control_option & uint8_t(ControlOption::VIDEO_RECORDING_AT_BOOT));
141 6 : }
142 :
143 : // init the runcam device by finding a serial device configured for the RunCam protocol
144 6 : void AP_RunCam::init()
145 : {
146 6 : AP_SerialManager *serial_manager = AP_SerialManager::get_singleton();
147 6 : if (serial_manager) {
148 6 : uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, _runcam_instance);
149 : }
150 6 : if (uart != nullptr) {
151 : /*
152 : if the user has setup a serial port as a runcam then default
153 : type to the split micro (Andy's development platform!). This makes setup a bit easier for most
154 : users while still enabling parameters to be hidden for users
155 : without a RunCam
156 : */
157 0 : _cam_type.set_default(int8_t(DeviceModel::SplitMicro));
158 0 : AP_Param::invalidate_count();
159 : }
160 6 : if (_cam_type.get() == int8_t(DeviceModel::Disabled)) {
161 0 : uart = nullptr;
162 0 : return;
163 : }
164 :
165 6 : if (uart == nullptr) {
166 6 : return;
167 : }
168 :
169 : // Split and Runcam 2 4k requires two mode presses to get into the menu
170 0 : if (_cam_type.get() == int8_t(DeviceModel::Split) || _cam_type.get() == int8_t(DeviceModel::Run24k)) {
171 0 : _menu_enter_level = -1;
172 0 : _in_menu = -1;
173 : }
174 :
175 0 : start_uart();
176 :
177 : // first transition is from initialized to ready
178 0 : _transition_start_ms = AP_HAL::millis();
179 0 : _transition_timeout_ms = _boot_delay_ms;
180 :
181 0 : get_device_info();
182 : }
183 :
184 : // simulate pressing the camera button
185 0 : bool AP_RunCam::simulate_camera_button(const ControlOperation operation, const uint32_t transition_timeout)
186 : {
187 0 : if (!uart || _protocol_version != ProtocolVersion::VERSION_1_0) {
188 0 : return false;
189 : }
190 :
191 0 : _transition_timeout_ms = transition_timeout;
192 : debug("press button %d, timeout=%dms\n", int(operation), int(transition_timeout));
193 0 : send_packet(Command::RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL, uint8_t(operation));
194 :
195 0 : return true;
196 : }
197 :
198 : // start the video
199 0 : void AP_RunCam::start_recording() {
200 : debug("start recording(%d)\n", int(_state));
201 0 : _video_recording = VideoOption::RECORDING;
202 0 : _osd_option = OSDOption::NO_OPTION;
203 0 : }
204 :
205 : // stop the video
206 0 : void AP_RunCam::stop_recording() {
207 : debug("stop recording(%d)\n", int(_state));
208 0 : _video_recording = VideoOption::NOT_RECORDING;
209 0 : _osd_option = OSDOption::NO_OPTION;
210 0 : }
211 :
212 : // enter the OSD menu
213 0 : void AP_RunCam::enter_osd()
214 : {
215 : debug("enter osd(%d)\n", int(_state));
216 0 : _osd_option = OSDOption::ENTER;
217 0 : }
218 :
219 : // exit the OSD menu
220 0 : void AP_RunCam::exit_osd()
221 : {
222 : debug("exit osd(%d)\n", int(_state));
223 0 : _osd_option = OSDOption::EXIT;
224 0 : }
225 :
226 : // OSD control determined by camera options
227 0 : void AP_RunCam::osd_option() {
228 : debug("osd option\n");
229 0 : _osd_option = OSDOption::OPTION;
230 0 : }
231 :
232 : // input update loop
233 622 : void AP_RunCam::update()
234 : {
235 622 : if (uart == nullptr || _cam_type.get() == int8_t(DeviceModel::Disabled)) {
236 622 : return;
237 : }
238 :
239 : // process any pending packets
240 0 : receive();
241 :
242 0 : uint32_t now = AP_HAL::millis();
243 0 : if ((now - _last_osd_update_ms) > RUNCAM_OSD_UPDATE_INTERVAL_MS) {
244 0 : update_osd();
245 0 : _last_osd_update_ms = now;
246 : }
247 : }
248 :
249 : // pre_arm_check - returns true if all pre-takeoff checks have completed successfully
250 16 : bool AP_RunCam::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const
251 : {
252 : // if not enabled return true
253 16 : if (!uart) {
254 16 : return true;
255 : }
256 :
257 : // currently in the OSD menu, do not allow arming
258 0 : if (is_arming_prevented()) {
259 0 : hal.util->snprintf(failure_msg, failure_msg_len, "In OSD menu");
260 0 : return false;
261 : }
262 :
263 0 : if (!camera_ready()) {
264 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Camera not ready");
265 0 : return false;
266 : }
267 :
268 : // if we got this far everything must be ok
269 0 : return true;
270 :
271 : }
272 :
273 : // OSD update loop
274 0 : void AP_RunCam::update_osd()
275 : {
276 0 : bool use_armed_state_machine = hal.util->get_soft_armed();
277 : #if OSD_ENABLED
278 : // prevent runcam stick gestures interfering with osd stick gestures
279 0 : if (!use_armed_state_machine) {
280 0 : const AP_OSD* osd = AP::osd();
281 0 : if (osd != nullptr) {
282 0 : use_armed_state_machine = !osd->is_readonly_screen();
283 : }
284 : }
285 : #endif
286 : // run a reduced state simulation process when armed
287 0 : if (use_armed_state_machine) {
288 0 : update_state_machine_armed();
289 0 : return;
290 : }
291 :
292 0 : update_state_machine_disarmed();
293 : }
294 :
295 : // update the state machine when armed or flying
296 0 : void AP_RunCam::update_state_machine_armed()
297 : {
298 0 : const uint32_t now = AP_HAL::millis();
299 0 : if ((now - _transition_start_ms) < _transition_timeout_ms) {
300 0 : return;
301 : }
302 :
303 0 : _transition_start_ms = now;
304 0 : _transition_timeout_ms = 0;
305 :
306 0 : switch (_state) {
307 0 : case State::READY:
308 0 : handle_ready(_video_recording == VideoOption::RECORDING && has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_START_RECORDING) ? Event::START_RECORDING : Event::NONE);
309 0 : break;
310 0 : case State::VIDEO_RECORDING:
311 0 : handle_recording(_video_recording == VideoOption::NOT_RECORDING && has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_START_RECORDING) ? Event::STOP_RECORDING : Event::NONE);
312 0 : break;
313 0 : case State::INITIALIZING:
314 : case State::INITIALIZED:
315 : case State::ENTERING_MENU:
316 : case State::IN_MENU:
317 : case State::EXITING_MENU:
318 0 : break;
319 : }
320 : }
321 :
322 : // update the state machine when disarmed
323 0 : void AP_RunCam::update_state_machine_disarmed()
324 : {
325 0 : const uint32_t now = AP_HAL::millis();
326 0 : if (_waiting_device_response || (now - _transition_start_ms) < _transition_timeout_ms) {
327 0 : _last_rc_event = Event::NONE;
328 0 : return;
329 : }
330 :
331 0 : _transition_start_ms = now;
332 0 : _transition_timeout_ms = 0;
333 :
334 0 : const Event ev = map_rc_input_to_event();
335 : // only take action on transitions
336 0 : if (ev == _last_rc_event && _state == _last_state && _osd_option == _last_osd_option
337 0 : && _last_in_menu == _in_menu && _last_video_recording == _video_recording) {
338 0 : return;
339 : }
340 :
341 : debug("update_state_machine_disarmed(%s)\n", event_names[int(ev)]);
342 :
343 0 : _last_rc_event = ev;
344 0 : _last_state = _state;
345 0 : _last_osd_option = _osd_option;
346 0 : _last_in_menu = _in_menu;
347 0 : _last_video_recording = _video_recording;
348 :
349 0 : switch (_state) {
350 0 : case State::INITIALIZING:
351 0 : break;
352 0 : case State::INITIALIZED:
353 0 : handle_initialized(ev);
354 0 : break;
355 0 : case State::READY:
356 0 : handle_ready(ev);
357 0 : break;
358 0 : case State::VIDEO_RECORDING:
359 0 : handle_recording(ev);
360 0 : break;
361 0 : case State::ENTERING_MENU:
362 0 : handle_in_menu(Event::ENTER_MENU);
363 0 : break;
364 0 : case State::IN_MENU:
365 0 : handle_in_menu(ev);
366 0 : break;
367 0 : case State::EXITING_MENU:
368 0 : handle_in_menu(Event::EXIT_MENU);
369 0 : break;
370 : }
371 : }
372 :
373 : // handle the initialized state
374 0 : void AP_RunCam::handle_initialized(Event ev)
375 : {
376 : // the camera should be configured to start with recording mode off by default
377 : // a recording change needs significantly extra time to process
378 0 : if (_video_recording == VideoOption::RECORDING && has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_START_RECORDING)) {
379 0 : if (!(_cam_control_option & uint8_t(ControlOption::VIDEO_RECORDING_AT_BOOT))) {
380 0 : simulate_camera_button(start_recording_command(), _mode_delay_ms * 2);
381 : }
382 0 : _state = State::VIDEO_RECORDING;
383 0 : } else if (_video_recording == VideoOption::NOT_RECORDING && has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_START_RECORDING)) {
384 0 : if (_cam_control_option & uint8_t(ControlOption::VIDEO_RECORDING_AT_BOOT)) {
385 0 : simulate_camera_button(stop_recording_command(), _mode_delay_ms * 2);
386 : }
387 0 : _state = State::READY;
388 : } else {
389 0 : _state = State::READY;
390 : }
391 : debug("device fully booted after %ums\n", unsigned(AP_HAL::millis()));
392 0 : }
393 :
394 : // handle the ready state
395 0 : void AP_RunCam::handle_ready(Event ev)
396 : {
397 0 : switch (ev) {
398 0 : case Event::ENTER_MENU:
399 : case Event::IN_MENU_ENTER:
400 : case Event::IN_MENU_RIGHT:
401 0 : if (ev == Event::ENTER_MENU || _cam_control_option & uint8_t(ControlOption::STICK_ROLL_RIGHT)) {
402 0 : _top_menu_pos = -1;
403 0 : _sub_menu_pos = 0;
404 0 : _state = State::ENTERING_MENU;
405 : }
406 0 : break;
407 0 : case Event::START_RECORDING:
408 0 : simulate_camera_button(start_recording_command(), _mode_delay_ms);
409 0 : _state = State::VIDEO_RECORDING;
410 0 : break;
411 0 : case Event::NONE:
412 : case Event::EXIT_MENU:
413 : case Event::IN_MENU_UP:
414 : case Event::IN_MENU_DOWN:
415 : case Event::IN_MENU_EXIT:
416 : case Event::BUTTON_RELEASE:
417 : case Event::STOP_RECORDING:
418 0 : break;
419 : }
420 0 : }
421 :
422 : // handle the recording state
423 0 : void AP_RunCam::handle_recording(Event ev)
424 : {
425 0 : switch (ev) {
426 0 : case Event::ENTER_MENU:
427 : case Event::IN_MENU_ENTER:
428 : case Event::IN_MENU_RIGHT:
429 0 : if (ev == Event::ENTER_MENU || _cam_control_option & uint8_t(ControlOption::STICK_ROLL_RIGHT)) {
430 0 : simulate_camera_button(stop_recording_command(), _mode_delay_ms);
431 0 : _top_menu_pos = -1;
432 0 : _sub_menu_pos = 0;
433 0 : _state = State::ENTERING_MENU;
434 : }
435 0 : break;
436 0 : case Event::STOP_RECORDING:
437 0 : simulate_camera_button(stop_recording_command(), _mode_delay_ms);
438 0 : _state = State::READY;
439 0 : break;
440 0 : case Event::NONE:
441 : case Event::EXIT_MENU:
442 : case Event::IN_MENU_UP:
443 : case Event::IN_MENU_DOWN:
444 : case Event::IN_MENU_EXIT:
445 : case Event::BUTTON_RELEASE:
446 : case Event::START_RECORDING:
447 0 : break;
448 : }
449 0 : }
450 :
451 : // handle the in_menu state
452 0 : void AP_RunCam::handle_in_menu(Event ev)
453 : {
454 0 : if (has_5_key_OSD()) {
455 0 : handle_5_key_simulation_process(ev);
456 0 : } else if (has_2_key_OSD()) {
457 : // otherwise the simpler 2 key OSD simulation, requires firmware 2.4.4 on the split micro
458 0 : handle_2_key_simulation_process(ev);
459 : }
460 0 : }
461 :
462 : // map rc input to an event
463 0 : AP_RunCam::Event AP_RunCam::map_rc_input_to_event() const
464 : {
465 0 : const RC_Channel::AuxSwitchPos throttle = rc().get_throttle_channel().get_stick_gesture_pos();
466 0 : const RC_Channel::AuxSwitchPos yaw = rc().get_yaw_channel().get_stick_gesture_pos();
467 0 : const RC_Channel::AuxSwitchPos roll = rc().get_roll_channel().get_stick_gesture_pos();
468 0 : const RC_Channel::AuxSwitchPos pitch = rc().get_pitch_channel().get_stick_gesture_pos();
469 :
470 0 : Event result = Event::NONE;
471 :
472 0 : if (_button_pressed != ButtonState::NONE) {
473 0 : if (_button_pressed == ButtonState::PRESSED && yaw == RC_Channel::AuxSwitchPos::MIDDLE && pitch == RC_Channel::AuxSwitchPos::MIDDLE && roll == RC_Channel::AuxSwitchPos::MIDDLE) {
474 0 : result = Event::BUTTON_RELEASE;
475 : } else {
476 0 : result = Event::NONE; // still waiting to be released
477 : }
478 0 : } else if (throttle == RC_Channel::AuxSwitchPos::MIDDLE && yaw == RC_Channel::AuxSwitchPos::LOW
479 0 : && pitch == RC_Channel::AuxSwitchPos::MIDDLE && roll == RC_Channel::AuxSwitchPos::MIDDLE
480 : // don't allow an action close to arming unless the user had configured it or arming is not possible
481 : // but don't prevent the 5-Key control actually working
482 0 : && (_cam_control_option & uint8_t(ControlOption::STICK_YAW_RIGHT) || is_arming_prevented())) {
483 0 : result = Event::EXIT_MENU;
484 0 : } else if (throttle == RC_Channel::AuxSwitchPos::MIDDLE && yaw == RC_Channel::AuxSwitchPos::HIGH
485 0 : && pitch == RC_Channel::AuxSwitchPos::MIDDLE && roll == RC_Channel::AuxSwitchPos::MIDDLE
486 0 : && (_cam_control_option & uint8_t(ControlOption::STICK_YAW_RIGHT) || is_arming_prevented())) {
487 0 : result = Event::ENTER_MENU;
488 0 : } else if (roll == RC_Channel::AuxSwitchPos::LOW) {
489 0 : result = Event::IN_MENU_EXIT;
490 0 : } else if (yaw == RC_Channel::AuxSwitchPos::MIDDLE && pitch == RC_Channel::AuxSwitchPos::MIDDLE && roll == RC_Channel::AuxSwitchPos::HIGH) {
491 0 : if (has_5_key_OSD()) {
492 0 : result = Event::IN_MENU_RIGHT;
493 : } else {
494 0 : result = Event::IN_MENU_ENTER;
495 : }
496 0 : } else if (pitch == RC_Channel::AuxSwitchPos::LOW) {
497 0 : result = Event::IN_MENU_UP;
498 0 : } else if (pitch == RC_Channel::AuxSwitchPos::HIGH) {
499 0 : result = Event::IN_MENU_DOWN;
500 0 : } else if (_video_recording != _last_video_recording) {
501 0 : switch (_video_recording) {
502 0 : case VideoOption::NOT_RECORDING:
503 0 : result = Event::STOP_RECORDING;
504 0 : break;
505 0 : case VideoOption::RECORDING:
506 0 : result = Event::START_RECORDING;
507 0 : break;
508 : }
509 0 : } else if (_osd_option == _last_osd_option) {
510 : // OSD option has not changed so assume stick re-centering
511 0 : result = Event::NONE;
512 0 : } else if (_osd_option == OSDOption::ENTER
513 0 : && _cam_control_option & uint8_t(ControlOption::TWO_POS_SWITCH)) {
514 0 : result = Event::ENTER_MENU;
515 0 : } else if ((_osd_option == OSDOption::OPTION || _osd_option == OSDOption::ENTER)
516 0 : && _cam_control_option & uint8_t(ControlOption::THREE_POS_SWITCH)) {
517 0 : result = Event::ENTER_MENU;
518 0 : } else if (_osd_option == OSDOption::EXIT
519 0 : && _cam_control_option & uint8_t(ControlOption::TWO_POS_SWITCH)) {
520 0 : result = Event::EXIT_MENU;
521 0 : } else if ((_osd_option == OSDOption::NO_OPTION || _osd_option == OSDOption::EXIT)
522 0 : && _cam_control_option & uint8_t(ControlOption::THREE_POS_SWITCH)) {
523 0 : result = Event::EXIT_MENU;
524 : } else {
525 : debug("map_rc_input_to_event(): nothing selected\n");
526 : }
527 0 : return result;
528 : }
529 :
530 : // run the 2-key OSD simulation process, this involves using the power and mode (wifi) buttons
531 : // to cycle through options. unfortunately these are one-way requests so we need to use delays
532 : // to make sure that the camera obeys
533 0 : void AP_RunCam::handle_2_key_simulation_process(Event ev)
534 : {
535 : debug("%s,M:%d,V:%d,O:%d\n", event_names[int(ev)], _in_menu, int(_video_recording), int(_osd_option));
536 :
537 0 : switch (ev) {
538 0 : case Event::ENTER_MENU:
539 0 : if (_in_menu <= 0) {
540 0 : _in_menu++;
541 0 : simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_MODE, _mode_delay_ms);
542 0 : if (_in_menu > 0) {
543 : // turn off built-in OSD so that the runcam OSD is visible
544 0 : disable_osd();
545 0 : _state = State::IN_MENU;
546 : } else {
547 0 : _state = State::ENTERING_MENU;
548 : }
549 : }
550 0 : break;
551 :
552 0 : case Event::EXIT_MENU:
553 : // keep changing mode until we are fully out of the menu
554 0 : if (_in_menu > 0) {
555 0 : _in_menu--;
556 0 : simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_MODE, _mode_delay_ms);
557 0 : _state = State::EXITING_MENU;
558 : } else {
559 0 : exit_2_key_osd_menu();
560 : }
561 0 : break;
562 :
563 0 : case Event::IN_MENU_ENTER:
564 : // in a sub-menu and save-and-exit was selected
565 0 : if (_in_menu > 1 && get_top_menu_length() > 0 && _sub_menu_pos == (get_sub_menu_length(_top_menu_pos) - 1) && DeviceModel(_cam_type.get()) != DeviceModel::Run24k) {
566 0 : simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _button_delay_ms);
567 0 : _sub_menu_pos = 0;
568 0 : _in_menu--;
569 : // in the top-menu and save-and-exit was selected
570 0 : } else if (_in_menu == 1 && get_top_menu_length() > 0 && _top_menu_pos == (get_top_menu_length() - 1) && DeviceModel(_cam_type.get()) != DeviceModel::Run24k) {
571 0 : simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _mode_delay_ms);
572 0 : _in_menu--;
573 0 : _state = State::EXITING_MENU;
574 0 : } else if (_top_menu_pos >= 0 && get_sub_menu_length(_top_menu_pos) > 0) {
575 0 : simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _button_delay_ms);
576 0 : _in_menu = MIN(_in_menu + 1, RUNCAM_OSD_MENU_DEPTH);
577 : }
578 0 : break;
579 :
580 0 : case Event::IN_MENU_UP:
581 : case Event::IN_MENU_DOWN:
582 0 : simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN, _button_delay_ms); // move to setting
583 0 : if (_in_menu > 1) {
584 : // in a sub-menu, keep track of the selected position
585 0 : _sub_menu_pos = (_sub_menu_pos + 1) % get_sub_menu_length(_top_menu_pos);
586 : } else {
587 : // in the top-menu, keep track of the selected position
588 0 : _top_menu_pos = (_top_menu_pos + 1) % get_top_menu_length();
589 : }
590 0 : break;
591 :
592 0 : case Event::IN_MENU_EXIT:
593 : // if we are in a sub-menu this will move us out, if we are in the root menu this will
594 : // exit causing the state machine to get out of sync. the OSD menu hierarchy is consistently
595 : // 2 deep so we can count and be reasonably confident of where we are.
596 : // the only exception is if someone hits save and exit on the root menu - then we are lost.
597 0 : if (_in_menu > 0) {
598 0 : _in_menu--;
599 0 : _sub_menu_pos = 0;
600 0 : simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_MODE, _mode_delay_ms); // move up/out a menu
601 : }
602 : // no longer in the menu so trigger the OSD re-enablement
603 0 : if (_in_menu == 0) {
604 0 : _in_menu = _menu_enter_level;
605 0 : _state = State::EXITING_MENU;
606 : }
607 0 : break;
608 :
609 0 : case Event::NONE:
610 : case Event::IN_MENU_RIGHT:
611 : case Event::BUTTON_RELEASE:
612 : case Event::START_RECORDING:
613 : case Event::STOP_RECORDING:
614 0 : break;
615 : }
616 0 : }
617 :
618 : // exit the 2 key OSD menu
619 0 : void AP_RunCam::exit_2_key_osd_menu()
620 : {
621 0 : _in_menu = _menu_enter_level;
622 :
623 : // turn built-in OSD back on
624 0 : enable_osd();
625 :
626 0 : if (_video_recording == VideoOption::RECORDING && has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_START_RECORDING)) {
627 0 : simulate_camera_button(start_recording_command(), _mode_delay_ms);
628 0 : _state = State::VIDEO_RECORDING;
629 : } else {
630 0 : _state = State::READY;
631 : }
632 0 : }
633 :
634 : // run the 5-key OSD simulation process
635 0 : void AP_RunCam::handle_5_key_simulation_process(Event ev)
636 : {
637 : debug("%s,M:%d,B:%d,O:%d\n", event_names[int(ev)], _in_menu, int(_button_pressed), int(_osd_option));
638 :
639 0 : switch (ev) {
640 0 : case Event::BUTTON_RELEASE:
641 0 : send_5_key_OSD_cable_simulation_event(ev);
642 0 : break;
643 :
644 0 : case Event::ENTER_MENU:
645 0 : if (_in_menu == 0) {
646 : // turn off built-in OSD so that the runcam OSD is visible
647 0 : disable_osd();
648 0 : send_5_key_OSD_cable_simulation_event(ev);
649 0 : _in_menu = 1;
650 : } else {
651 0 : send_5_key_OSD_cable_simulation_event(Event::IN_MENU_ENTER);
652 : }
653 0 : break;
654 :
655 0 : case Event::EXIT_MENU:
656 0 : if (_in_menu > 0) {
657 : // turn built-in OSD back on
658 0 : enable_osd();
659 0 : send_5_key_OSD_cable_simulation_event(Event::EXIT_MENU);
660 0 : _in_menu = 0;
661 : }
662 0 : break;
663 :
664 0 : case Event::NONE:
665 0 : break;
666 :
667 0 : case Event::IN_MENU_EXIT:
668 : case Event::IN_MENU_RIGHT:
669 : case Event::IN_MENU_ENTER:
670 : case Event::IN_MENU_UP:
671 : case Event::IN_MENU_DOWN:
672 : case Event::START_RECORDING:
673 : case Event::STOP_RECORDING:
674 0 : send_5_key_OSD_cable_simulation_event(ev);
675 0 : break;
676 : }
677 0 : }
678 :
679 : // handle a response
680 0 : void AP_RunCam::handle_5_key_simulation_response(const Request& request)
681 : {
682 : debug("response for command %d result: %d\n", int(request._command), int(request._result));
683 0 : if (request._result != RequestStatus::SUCCESS) {
684 0 : simulation_OSD_cable_failed(request);
685 0 : _button_pressed = ButtonState::NONE;
686 0 : _waiting_device_response = false;
687 0 : return;
688 : }
689 :
690 0 : switch (request._command) {
691 0 : case Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE:
692 0 : _button_pressed = ButtonState::NONE;
693 0 : break;
694 0 : case Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION:
695 : {
696 : // the high 4 bits is the operationID that we sent
697 : // the low 4 bits is the result code
698 0 : const ConnectionOperation operationID = ConnectionOperation(request._param);
699 0 : const uint8_t errorCode = (request._recv_buf[1] & 0x0F);
700 : switch (operationID) {
701 0 : case ConnectionOperation::RCDEVICE_PROTOCOL_5KEY_FUNCTION_OPEN:
702 0 : if (errorCode > 0) {
703 0 : _state = State::IN_MENU;
704 : }
705 0 : break;
706 0 : case ConnectionOperation::RCDEVICE_PROTOCOL_5KEY_FUNCTION_CLOSE:
707 0 : if (errorCode > 0) {
708 0 : _state = State::READY;
709 : }
710 0 : break;
711 : }
712 0 : break;
713 : }
714 0 : case Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS:
715 : case Command::RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO:
716 : case Command::RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL:
717 : case Command::COMMAND_NONE:
718 0 : break;
719 : }
720 :
721 0 : _waiting_device_response = false;
722 : }
723 :
724 : // command to start recording
725 0 : AP_RunCam::ControlOperation AP_RunCam::start_recording_command() const {
726 0 : if (DeviceModel(_cam_type.get()) == DeviceModel::Split4k || DeviceModel(_cam_type.get()) == DeviceModel::Hybrid || DeviceModel(_cam_type.get()) == DeviceModel::Run24k) {
727 0 : return ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN;
728 : } else {
729 0 : return ControlOperation::RCDEVICE_PROTOCOL_CHANGE_START_RECORDING;
730 : }
731 : }
732 :
733 : // command to stop recording
734 0 : AP_RunCam::ControlOperation AP_RunCam::stop_recording_command() const {
735 0 : if (DeviceModel(_cam_type.get()) == DeviceModel::Split4k || DeviceModel(_cam_type.get()) == DeviceModel::Hybrid || DeviceModel(_cam_type.get()) == DeviceModel::Run24k) {
736 0 : return ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN;
737 : } else {
738 0 : return ControlOperation::RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING;
739 : }
740 : }
741 :
742 : // process a response from the serial port
743 0 : void AP_RunCam::receive()
744 : {
745 0 : if (!uart) {
746 0 : return;
747 : }
748 : // process any pending request at least once-per cycle, regardless of available bytes
749 0 : if (!request_pending(AP_HAL::millis())) {
750 0 : return;
751 : }
752 :
753 0 : uint32_t avail = MIN(uart->available(), (uint32_t)RUNCAM_MAX_PACKET_SIZE);
754 :
755 0 : for (uint32_t i = 0; i < avail; i++) {
756 :
757 0 : if (!request_pending(AP_HAL::millis())) {
758 0 : return;
759 : }
760 :
761 0 : const uint8_t c = uart->read();
762 0 : if (_pending_request._recv_response_length == 0) {
763 : // Only start receiving packet when we found a header
764 0 : if (c != RUNCAM_HEADER) {
765 0 : continue;
766 : }
767 : }
768 :
769 0 : _pending_request._recv_buf[_pending_request._recv_response_length] = c;
770 0 : _pending_request._recv_response_length += 1;
771 :
772 : // if data received done, trigger callback to parse response data, and update RUNCAM state
773 0 : if (_pending_request._recv_response_length == _pending_request._expected_response_length) {
774 0 : uint8_t crc = _pending_request.get_crc();
775 :
776 0 : _pending_request._result = (crc == 0) ? RequestStatus::SUCCESS : RequestStatus::INCORRECT_CRC;
777 :
778 : debug("received response for command %d\n", int(_pending_request._command));
779 0 : _pending_request.parse_response();
780 : // we no longer have a pending request
781 0 : _pending_request._result = RequestStatus::NONE;
782 : }
783 : }
784 : }
785 :
786 : // every time we send a packet to device and want to get a response
787 : // it's better to clear the rx buffer before the sending the packet
788 : // otherwise useless data in rx buffer will cause the response decoding
789 : // to fail
790 0 : void AP_RunCam::drain()
791 : {
792 0 : if (!uart) {
793 0 : return;
794 : }
795 :
796 0 : uart->discard_input();
797 : }
798 :
799 : // start the uart if we have one
800 0 : void AP_RunCam::start_uart()
801 : {
802 : // 8N1 communication
803 0 : uart->configure_parity(0);
804 0 : uart->set_stop_bits(1);
805 0 : uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
806 0 : uart->set_options(uart->get_options() | AP_HAL::UARTDriver::OPTION_NODMA_TX | AP_HAL::UARTDriver::OPTION_NODMA_RX);
807 0 : uart->begin(115200, 10, 10);
808 0 : uart->discard_input();
809 0 : }
810 :
811 : // get the device info (firmware version, protocol version and features)
812 0 : void AP_RunCam::get_device_info()
813 : {
814 0 : send_request_and_waiting_response(Command::RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, 0, RUNCAM_INIT_INTERVAL_MS * 4,
815 : UINT16_MAX, FUNCTOR_BIND_MEMBER(&AP_RunCam::parse_device_info, void, const Request&));
816 0 : }
817 :
818 : // map a Event to a SimulationOperation
819 0 : AP_RunCam::SimulationOperation AP_RunCam::map_key_to_protocol_operation(const Event key) const
820 : {
821 0 : SimulationOperation operation = SimulationOperation::SIMULATION_NONE;
822 0 : switch (key) {
823 0 : case Event::IN_MENU_EXIT:
824 0 : operation = SimulationOperation::RCDEVICE_PROTOCOL_5KEY_SIMULATION_LEFT;
825 0 : break;
826 0 : case Event::IN_MENU_UP:
827 0 : operation = SimulationOperation::RCDEVICE_PROTOCOL_5KEY_SIMULATION_UP;
828 0 : break;
829 0 : case Event::IN_MENU_RIGHT:
830 0 : operation = SimulationOperation::RCDEVICE_PROTOCOL_5KEY_SIMULATION_RIGHT;
831 0 : break;
832 0 : case Event::IN_MENU_DOWN:
833 0 : operation = SimulationOperation::RCDEVICE_PROTOCOL_5KEY_SIMULATION_DOWN;
834 0 : break;
835 0 : case Event::IN_MENU_ENTER:
836 0 : operation = SimulationOperation::RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET;
837 0 : break;
838 0 : case Event::BUTTON_RELEASE:
839 : case Event::NONE:
840 : case Event::ENTER_MENU:
841 : case Event::EXIT_MENU:
842 : case Event::STOP_RECORDING:
843 : case Event::START_RECORDING:
844 0 : break;
845 : }
846 0 : return operation;
847 : }
848 :
849 : // send an event
850 0 : void AP_RunCam::send_5_key_OSD_cable_simulation_event(const Event key, const uint32_t transition_timeout)
851 : {
852 : debug("OSD cable simulation event %s\n", event_names[int(key)]);
853 0 : _waiting_device_response = true;
854 : // although we can control press/release, this causes the state machine to behave in the same way
855 : // as the 2-key process
856 0 : _transition_timeout_ms = transition_timeout;
857 :
858 0 : switch (key) {
859 0 : case Event::ENTER_MENU:
860 0 : open_5_key_OSD_cable_connection(FUNCTOR_BIND_MEMBER(&AP_RunCam::handle_5_key_simulation_response, void, const Request&));
861 0 : break;
862 0 : case Event::EXIT_MENU:
863 0 : close_5_key_OSD_cable_connection(FUNCTOR_BIND_MEMBER(&AP_RunCam::handle_5_key_simulation_response, void, const Request&));
864 0 : break;
865 0 : case Event::IN_MENU_UP:
866 : case Event::IN_MENU_RIGHT:
867 : case Event::IN_MENU_DOWN:
868 : case Event::IN_MENU_ENTER:
869 : case Event::IN_MENU_EXIT:
870 0 : simulate_5_key_OSD_cable_button_press(map_key_to_protocol_operation(key), FUNCTOR_BIND_MEMBER(&AP_RunCam::handle_5_key_simulation_response, void, const Request&));
871 0 : break;
872 0 : case Event::BUTTON_RELEASE:
873 0 : simulate_5_key_OSD_cable_button_release(FUNCTOR_BIND_MEMBER(&AP_RunCam::handle_5_key_simulation_response, void, const Request&));
874 0 : break;
875 0 : case Event::STOP_RECORDING:
876 : case Event::START_RECORDING:
877 : case Event::NONE:
878 0 : break;
879 : }
880 0 : }
881 :
882 : // every time we run the OSD menu simulation it's necessary to open the connection
883 0 : void AP_RunCam::open_5_key_OSD_cable_connection(parse_func_t parseFunc)
884 : {
885 0 : send_request_and_waiting_response(Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION,
886 : uint8_t(ConnectionOperation::RCDEVICE_PROTOCOL_5KEY_FUNCTION_OPEN), 400, 2, parseFunc);
887 0 : }
888 :
889 : // every time we exit the OSD menu simulation it's necessary to close the connection
890 0 : void AP_RunCam::close_5_key_OSD_cable_connection(parse_func_t parseFunc)
891 : {
892 0 : send_request_and_waiting_response(Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION,
893 : uint8_t(ConnectionOperation::RCDEVICE_PROTOCOL_5KEY_FUNCTION_CLOSE), 400, 2, parseFunc);
894 0 : }
895 :
896 : // simulate button press event of 5 key OSD cable with special button
897 0 : void AP_RunCam::simulate_5_key_OSD_cable_button_press(const SimulationOperation operation, parse_func_t parseFunc)
898 : {
899 0 : if (operation == SimulationOperation::SIMULATION_NONE) {
900 0 : return;
901 : }
902 :
903 0 : _button_pressed = ButtonState::PRESSED;
904 :
905 0 : send_request_and_waiting_response(Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, uint8_t(operation), 400, 2, parseFunc);
906 : }
907 :
908 : // simulate button release event of 5 key OSD cable
909 0 : void AP_RunCam::simulate_5_key_OSD_cable_button_release(parse_func_t parseFunc)
910 : {
911 0 : _button_pressed = ButtonState::RELEASED;
912 :
913 0 : send_request_and_waiting_response(Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE,
914 : uint8_t(SimulationOperation::SIMULATION_NONE), 400, 2, parseFunc);
915 0 : }
916 :
917 : // send a RunCam request and register a response to be processed
918 0 : void AP_RunCam::send_request_and_waiting_response(Command commandID, uint8_t param,
919 : uint32_t timeout, uint16_t maxRetryTimes, parse_func_t parserFunc)
920 : {
921 0 : drain();
922 :
923 0 : _pending_request = Request(this, commandID, param, timeout, maxRetryTimes, parserFunc);
924 : debug("sending command: %d, op: %d\n", int(commandID), int(param));
925 : // send packet
926 0 : send_packet(commandID, param);
927 0 : }
928 :
929 : // send a packet to the serial port
930 0 : void AP_RunCam::send_packet(Command command, uint8_t param)
931 : {
932 : // is this device open?
933 0 : if (!uart) {
934 0 : return;
935 : }
936 :
937 : uint8_t buffer[4];
938 :
939 0 : bool have_param = param > 0 || command == Command::RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL;
940 0 : uint8_t buffer_len = have_param ? 4 : 3;
941 :
942 0 : buffer[0] = RUNCAM_HEADER;
943 0 : buffer[1] = uint8_t(command);
944 0 : if (have_param) {
945 0 : buffer[2] = param;
946 : }
947 :
948 0 : uint8_t crc = 0;
949 0 : for (uint8_t i = 0; i < buffer_len - 1; i++) {
950 0 : crc = crc8_dvb_s2(crc, buffer[i]);
951 : }
952 :
953 0 : buffer[buffer_len - 1] = crc;
954 :
955 : // send data if possible
956 0 : uart->write(buffer, buffer_len);
957 0 : uart->flush();
958 : }
959 :
960 : // handle a device info response
961 0 : void AP_RunCam::parse_device_info(const Request& request)
962 : {
963 0 : _protocol_version = ProtocolVersion(request._recv_buf[1]);
964 :
965 0 : uint8_t featureLowBits = request._recv_buf[2];
966 0 : uint8_t featureHighBits = request._recv_buf[3];
967 0 : if (!has_feature(Feature::FEATURES_OVERRIDE)) {
968 0 : _features.set((featureHighBits << 8) | featureLowBits);
969 : }
970 0 : if (_features > 0) {
971 0 : _state = State::INITIALIZED;
972 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "RunCam initialized, features 0x%04X, %d-key OSD", _features.get(),
973 : has_5_key_OSD() ? 5 : has_2_key_OSD() ? 2 : 0);
974 : } else {
975 : // nothing as as nothing does
976 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RunCam device not found");
977 : }
978 : debug("RunCam: initialized state: video: %d, osd: %d, cam: %d\n", int(_video_recording), int(_osd_option), int(_cam_control_option));
979 0 : }
980 :
981 : // wait for the RunCam device to be fully ready
982 0 : bool AP_RunCam::camera_ready() const
983 : {
984 0 : if (_state != State::INITIALIZING && _state != State::INITIALIZED) {
985 0 : return true;
986 : }
987 0 : return false;
988 : }
989 :
990 : // error handler for OSD simulation
991 0 : void AP_RunCam::simulation_OSD_cable_failed(const Request& request)
992 : {
993 0 : _waiting_device_response = false;
994 0 : if (request._command == Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION) {
995 0 : uint8_t operationID = request._param;
996 0 : if (operationID == uint8_t(ConnectionOperation::RCDEVICE_PROTOCOL_5KEY_FUNCTION_CLOSE)) {
997 0 : return;
998 : }
999 : }
1000 : }
1001 :
1002 : // process all of the pending responses, retrying as necessary
1003 0 : bool AP_RunCam::request_pending(uint32_t now)
1004 : {
1005 0 : if (_pending_request._result == RequestStatus::NONE) {
1006 0 : return false;
1007 : }
1008 :
1009 0 : if (_pending_request._request_timestamp_ms != 0 && (now - _pending_request._request_timestamp_ms) < _pending_request._timeout_ms) {
1010 : // request still in play
1011 0 : return true;
1012 : }
1013 :
1014 0 : if (_pending_request._max_retry_times > 0) {
1015 : // request timed out, so resend
1016 : debug("retrying[%d] command 0x%X, op 0x%X\n", int(_pending_request._max_retry_times), int(_pending_request._command), int(_pending_request._param));
1017 0 : start_uart();
1018 0 : _pending_request._device->send_packet(_pending_request._command, _pending_request._param);
1019 0 : _pending_request._recv_response_length = 0;
1020 0 : _pending_request._request_timestamp_ms = now;
1021 0 : _pending_request._max_retry_times -= 1;
1022 :
1023 0 : return false;
1024 : }
1025 : debug("timeout command 0x%X, op 0x%X\n", int(_pending_request._command), int(_pending_request._param));
1026 : // too many retries, fail the request
1027 0 : _pending_request._result = RequestStatus::TIMEOUT;
1028 0 : _pending_request.parse_response();
1029 0 : _pending_request._result = RequestStatus::NONE;
1030 :
1031 0 : return false;
1032 : }
1033 :
1034 : // constructor for a response structure
1035 0 : AP_RunCam::Request::Request(AP_RunCam* device, Command commandID, uint8_t param,
1036 0 : uint32_t timeout, uint16_t maxRetryTimes, parse_func_t parserFunc)
1037 0 : : _recv_buf(device->_recv_buf),
1038 0 : _device(device),
1039 0 : _command(commandID),
1040 0 : _param(param),
1041 0 : _recv_response_length(0),
1042 0 : _timeout_ms(timeout),
1043 0 : _max_retry_times(maxRetryTimes),
1044 0 : _parser_func(parserFunc),
1045 0 : _result(RequestStatus::PENDING)
1046 : {
1047 0 : _request_timestamp_ms = AP_HAL::millis();
1048 0 : _expected_response_length = get_expected_response_length(commandID);
1049 0 : }
1050 :
1051 0 : uint8_t AP_RunCam::Request::get_crc() const
1052 : {
1053 0 : uint8_t crc = 0;
1054 0 : for (int i = 0; i < _recv_response_length; i++) {
1055 0 : crc = crc8_dvb_s2(crc, _recv_buf[i]);
1056 : }
1057 0 : return crc;
1058 : }
1059 :
1060 : // get the length of a response
1061 0 : uint8_t AP_RunCam::Request::get_expected_response_length(const Command command) const
1062 : {
1063 0 : for (uint16_t i = 0; i < RUNCAM_NUM_EXPECTED_RESPONSES; i++) {
1064 0 : if (_expected_responses_length[i].command == command) {
1065 0 : return _expected_responses_length[i].reponse_length;
1066 : }
1067 : }
1068 :
1069 0 : return 0;
1070 : }
1071 :
1072 : // AP_Camera API
1073 :
1074 : // return true if healthy
1075 0 : bool AP_RunCam::healthy() const
1076 : {
1077 0 : return camera_ready();
1078 : }
1079 :
1080 : // momentary switch to change camera between picture and video modes
1081 0 : void AP_RunCam::cam_mode_toggle()
1082 : {
1083 :
1084 0 : }
1085 :
1086 : // entry point to actually take a picture. returns true on success
1087 0 : bool AP_RunCam::trigger_pic()
1088 : {
1089 0 : return false;
1090 : }
1091 :
1092 : // send camera information message to GCS
1093 0 : void AP_RunCam::send_camera_information(mavlink_channel_t chan) const
1094 : {
1095 : // exit immediately if not initialised
1096 0 : if (!camera_ready() || _cam_type.get() <= 0 || _cam_type.get() > int8_t(ARRAY_SIZE(_models))) {
1097 0 : return;
1098 : }
1099 :
1100 : static const uint8_t vendor_name[32] = "RunCam";
1101 0 : uint8_t model_name[32] {};
1102 0 : strncpy((char *)model_name, _models[_cam_type.get()-1], MIN(sizeof(model_name), sizeof(_models[_cam_type.get()-1])));
1103 0 : const char cam_definition_uri[140] {};
1104 :
1105 : // capability flags
1106 0 : uint32_t flags = 0;
1107 :
1108 0 : if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_START_RECORDING)) {
1109 0 : flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO;
1110 : }
1111 :
1112 0 : if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE)) {
1113 0 : flags |= CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
1114 : }
1115 :
1116 : // send CAMERA_INFORMATION message
1117 0 : mavlink_msg_camera_information_send(
1118 : chan,
1119 : AP_HAL::millis(), // time_boot_ms
1120 : vendor_name, // vendor_name uint8_t[32]
1121 : model_name, // model_name uint8_t[32]
1122 : 0, // firmware version uint32_t
1123 : NaNf, // focal_length float (mm)
1124 : NaNf, // sensor_size_h float (mm)
1125 : NaNf, // sensor_size_v float (mm)
1126 : 0, // resolution_h uint16_t (pix)
1127 : 0, // resolution_v uint16_t (pix)
1128 : 0, // lens_id uint8_t
1129 : flags, // flags uint32_t (CAMERA_CAP_FLAGS)
1130 : 0, // cam_definition_version uint16_t
1131 : cam_definition_uri, // cam_definition_uri char[140]
1132 0 : _instance + 1); // gimbal_device_id uint8_t
1133 : }
1134 :
1135 : // send camera settings message to GCS
1136 0 : void AP_RunCam::send_camera_settings(mavlink_channel_t chan) const
1137 : {
1138 : // exit immediately if not initialised
1139 0 : if (!camera_ready()) {
1140 0 : return;
1141 : }
1142 :
1143 : // send CAMERA_SETTINGS message
1144 0 : mavlink_msg_camera_settings_send(
1145 : chan,
1146 : AP_HAL::millis(), // time_boot_ms
1147 0 : _video_recording == VideoOption::RECORDING ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
1148 : NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown
1149 : NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
1150 : }
1151 :
1152 133080 : AP_RunCam *AP::runcam() {
1153 133080 : return AP_RunCam::get_singleton();
1154 : }
1155 :
1156 : #endif // AP_CAMERA_RUNCAM_ENABLED
|
