diff --git a/libraries/AP_Camera/AP_RunCam.cpp b/libraries/AP_Camera/AP_RunCam.cpp new file mode 100644 index 0000000000..64ada36923 --- /dev/null +++ b/libraries/AP_Camera/AP_RunCam.cpp @@ -0,0 +1,928 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + implementation of RunCam camera protocols + + With thanks to betaflight for a great reference + implementation. Several of the functions below are based on + betaflight equivalent functions + + RunCam protocol specification can be found at https://support.runcam.com/hc/en-us/articles/360014537794-RunCam-Device-Protocol + */ +#include "AP_RunCam.h" + +#if HAL_RUNCAM_ENABLED + +#include +#include +#include + +const AP_Param::GroupInfo AP_RunCam::var_info[] = { + // @Param: FEATURES + // @DisplayName: RunCam features available + // @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. + // @User: Advanced + // @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 + AP_GROUPINFO("FEATURES", 1, AP_RunCam, _features, 0), + + // @Param: BT_DELAY + // @DisplayName: RunCam boot delay before allowing updates + // @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. + // @User: Advanced + AP_GROUPINFO("BT_DELAY", 2, AP_RunCam, _boot_delay_ms, 6000), + + // @Param: BTN_DELAY + // @DisplayName: RunCam button delay before allowing further button presses + // @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. + // @User: Advanced + AP_GROUPINFO("BTN_DELAY", 3, AP_RunCam, _button_delay_ms, 300), + + AP_GROUPEND +}; + +#define RUNCAM_DEBUG 0 + +#if RUNCAM_DEBUG +#define debug(fmt, args ...) do { hal.console->printf("RunCam: " fmt, ## args); } while (0) +#else +#define debug(fmt, args ...) +#endif + +extern const AP_HAL::HAL& hal; + +// singleton instance +AP_RunCam *AP_RunCam::_singleton; + +AP_RunCam::Request::Length AP_RunCam::Request::_expected_responses_length[RUNCAM_NUM_EXPECTED_RESPONSES] = { + { Command::RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, 5 }, + { Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, 2 }, + { Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, 2 }, + { Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, 3 }, +}; + +// the protocol for Runcam Device definition +static const uint8_t RUNCAM_HEADER = 0xCC; +static const uint8_t RUNCAM_OSD_MENU_DEPTH = 2; +static const uint32_t RUNCAM_INIT_INTERVAL_MS = 500; +static const uint32_t RUNCAM_OSD_UPDATE_INTERVAL_MS = 100; // 10Hz + +// these are correct for the runcam split micro v2.4.4, others may vary +// Video, Image, TV-OUT, Micro SD Card, General +static const uint8_t RUNCAM_TOP_MENU_LENGTH = 6; +uint8_t AP_RunCam::_sub_menu_lengths[RUNCAM_NUM_SUB_MENUS] = { + 5, 8, 3, 3, 7 +}; + +AP_RunCam::AP_RunCam() +{ + AP_Param::setup_object_defaults(this, var_info); + if (_singleton != nullptr) { + AP_HAL::panic("AP_RunCam must be singleton"); + } + _singleton = this; +} + +// init the runcam device by finding a serial device configured for the RunCam protocol +void AP_RunCam::init() +{ + AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); + if (serial_manager) { + uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, 0); + } + + if (uart == nullptr) { + return; + } + + uart->begin(115200); + + // first transition is from initialized to ready + _transition_start_ms = AP_HAL::millis(); + _transition_timeout_ms = _boot_delay_ms; + + get_device_info(); +} + +// simulate pressing the camera button +bool AP_RunCam::simulate_camera_button(const ControlOperation operation) +{ + if (!uart || _protocol_version != ProtocolVersion::VERSION_1_0) { + return false; + } + + debug("press button %d\n", int(operation)); + send_packet(Command::RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL, uint8_t(operation)); + + return true; +} + +// start the video +void AP_RunCam::start_recording() { + debug("start recording\n"); + _video_recording = true; +} + +// stop the video +void AP_RunCam::stop_recording() { + debug("stop recording\n"); + _video_recording = false; +} + +// input update loop +void AP_RunCam::update() +{ + if (uart == nullptr) { + return; + } + + // process any pending packets + receive(); + + uint32_t now = AP_HAL::millis(); + if ((now - _last_osd_update_ms) > RUNCAM_OSD_UPDATE_INTERVAL_MS) { + update_osd(); + _last_osd_update_ms = now; + } +} + +// pre_arm_check - returns true if all pre-takeoff checks have completed successfully +bool AP_RunCam::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const +{ + // if not enabled return true + if (!uart) { + return true; + } + + // currently in the OSD menu, do not allow arming + if (_in_menu > 0) { + hal.util->snprintf(failure_msg, failure_msg_len, "In OSD menu"); + return false; + } + + if (!camera_ready()) { + hal.util->snprintf(failure_msg, failure_msg_len, "Camera not ready"); + return false; + } + + // if we got this far everything must be ok + return true; + +} + +// OSD update loop +void AP_RunCam::update_osd() +{ + // run a reduced state simulation process when armed + if (AP::arming().is_armed()) { + update_state_machine_armed(); + return; + } + + update_state_machine_disarmed(); +} + +// return radio values as LOW, MIDDLE, HIGH +RC_Channel::aux_switch_pos_t AP_RunCam::get_channel_pos(uint8_t rcmapchan) const +{ + RC_Channel::aux_switch_pos_t position = RC_Channel::LOW; + const RC_Channel* chan = rc().channel(rcmapchan-1); + if (chan == nullptr || !chan->read_3pos_switch(position)) { + return RC_Channel::LOW; + } + + return position; +} + +// update the state machine when armed or flying +void AP_RunCam::update_state_machine_armed() +{ + const uint32_t now = AP_HAL::millis(); + if ((now - _transition_start_ms) < _transition_timeout_ms) { + return; + } + + _transition_start_ms = now; + _transition_timeout_ms = 0; + + switch (_state) { + case State::READY: + handle_ready(_video_recording ? Event::START_RECORDING : Event::NONE); + break; + case State::VIDEO_RECORDING: + handle_recording(!_video_recording ? Event::STOP_RECORDING : Event::NONE); + break; + case State::INITIALIZING: + case State::INITIALIZED: + case State::ENTERING_MENU: + case State::IN_MENU: + case State::EXITING_MENU: + break; + } +} + +// update the state machine when disarmed +void AP_RunCam::update_state_machine_disarmed() +{ + const uint32_t now = AP_HAL::millis(); + if (_waiting_device_response || (now - _transition_start_ms) < _transition_timeout_ms) { + return; + } + + _transition_start_ms = now; + _transition_timeout_ms = 0; + + const Event ev = map_rc_input_to_event(); + if (ev == Event::BUTTON_RELEASE) { + _button_pressed = false; + } + + switch (_state) { + case State::INITIALIZING: + break; + case State::INITIALIZED: + handle_initialized(ev); + break; + case State::READY: + handle_ready(ev); + break; + case State::VIDEO_RECORDING: + handle_recording(ev); + break; + case State::ENTERING_MENU: + handle_in_menu(Event::ENTER_MENU); + break; + case State::IN_MENU: + handle_in_menu(ev); + break; + case State::EXITING_MENU: + handle_in_menu(Event::EXIT_MENU); + break; + } +} + +// handle the initialized state +void AP_RunCam::handle_initialized(Event ev) +{ + // the camera always starts in recording mode by default + if (!_video_recording) { + simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING); + set_mode_change_timeout(); + _state = State::READY; + } else { + _state = State::VIDEO_RECORDING; + } + debug("device fully booted after %ums\n", unsigned(AP_HAL::millis())); +} + +// handle the ready state +void AP_RunCam::handle_ready(Event ev) +{ + switch (ev) { + case Event::ENTER_MENU: + _top_menu_pos = -1; + _sub_menu_pos = 0; + _state = State::ENTERING_MENU; + break; + case Event::START_RECORDING: + simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_START_RECORDING); + set_button_press_timeout(); + _state = State::VIDEO_RECORDING; + break; + case Event::NONE: + case Event::EXIT_MENU: + case Event::IN_MENU_ENTER: + case Event::IN_MENU_RIGHT: + case Event::IN_MENU_UP: + case Event::IN_MENU_DOWN: + case Event::IN_MENU_EXIT: + case Event::BUTTON_RELEASE: + case Event::STOP_RECORDING: + break; + } +} + +// handle the recording state +void AP_RunCam::handle_recording(Event ev) +{ + switch (ev) { + case Event::ENTER_MENU: + simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING); + set_mode_change_timeout(); + _sub_menu_pos = 0; + _top_menu_pos = -1; + _state = State::ENTERING_MENU; + break; + case Event::STOP_RECORDING: + simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING); + set_button_press_timeout(); + _state = State::READY; + break; + case Event::NONE: + case Event::EXIT_MENU: + case Event::IN_MENU_ENTER: + case Event::IN_MENU_RIGHT: + case Event::IN_MENU_UP: + case Event::IN_MENU_DOWN: + case Event::IN_MENU_EXIT: + case Event::BUTTON_RELEASE: + case Event::START_RECORDING: + break; + } +} + +// handle the in_menu state +void AP_RunCam::handle_in_menu(Event ev) +{ + if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE)) { + handle_5_key_simulation_process(ev); + } else if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE) && + has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON) && + has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON)) { + // otherwise the simpler 2 key OSD simulation, requires firmware 2.4.4 on the split micro + handle_2_key_simulation_process(ev); + } +} + +// map rc input to an event +AP_RunCam::Event AP_RunCam::map_rc_input_to_event() const +{ + const RC_Channel::aux_switch_pos_t throttle = get_channel_pos(AP::rcmap()->throttle()); + const RC_Channel::aux_switch_pos_t yaw = get_channel_pos(AP::rcmap()->yaw()); + const RC_Channel::aux_switch_pos_t roll = get_channel_pos(AP::rcmap()->roll()); + const RC_Channel::aux_switch_pos_t pitch = get_channel_pos(AP::rcmap()->pitch()); + + Event result = Event::NONE; + + if (_button_pressed + && yaw == RC_Channel::MIDDLE && pitch == RC_Channel::MIDDLE && roll == RC_Channel::MIDDLE) { + result = Event::BUTTON_RELEASE; + } else if (throttle == RC_Channel::MIDDLE && yaw == RC_Channel::LOW + && pitch == RC_Channel::MIDDLE && roll == RC_Channel::MIDDLE) { + result = Event::EXIT_MENU; + } + if (throttle == RC_Channel::MIDDLE && yaw == RC_Channel::HIGH + && pitch == RC_Channel::MIDDLE && roll == RC_Channel::MIDDLE) { + result = Event::ENTER_MENU; + } else if (roll == RC_Channel::LOW) { + result = Event::IN_MENU_EXIT; + } else if (roll == RC_Channel::HIGH) { + result = Event::IN_MENU_ENTER; + } else if (pitch == RC_Channel::HIGH) { + result = Event::IN_MENU_UP; + } else if (pitch == RC_Channel::LOW) { + result = Event::IN_MENU_DOWN; + } else if (_state == State::READY && _video_recording) { + // generate an event if we are in the wrong recording state + result = Event::START_RECORDING; + } else if (_state == State::VIDEO_RECORDING && !_video_recording) { + result = Event::STOP_RECORDING; + } + return result; +} + +// run the 2-key OSD simulation process, this involves using the power and mode (wifi) buttons +// to cycle through options. unfortunately these are one-way requests so we need to use delays +// to make sure that the camera obeys +void AP_RunCam::handle_2_key_simulation_process(Event ev) +{ +#if RUNCAM_DEBUG + if (_in_menu > 0 && ev != Event::NONE) { + debug("E:%d,M:%d,V:%d\n", int(ev), _in_menu, _video_recording); + } +#endif + switch (ev) { + case Event::ENTER_MENU: + if (_in_menu == 0) { + enter_2_key_osd_menu(); + } + break; + + case Event::EXIT_MENU: + // keep changing mode until we are fully out of the menu + if (_in_menu > 0) { + _in_menu--; + simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_MODE); + set_mode_change_timeout(); + _state = State::EXITING_MENU; + } else { + exit_2_key_osd_menu(); + } + break; + + case Event::IN_MENU_ENTER: + simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN); // change setting + set_button_press_timeout(); + // in a sub-menu and save-and-exit was selected + if (_in_menu > 1 && _sub_menu_pos == (_sub_menu_lengths[_top_menu_pos] - 1)) { + _in_menu--; + // in the top-menu and save-and-exit was selected + } else if (_in_menu == 1 && _top_menu_pos == (RUNCAM_TOP_MENU_LENGTH - 1)) { + _in_menu--; + _state = State::EXITING_MENU; + } else { + _in_menu = MIN(_in_menu + 1, RUNCAM_OSD_MENU_DEPTH); + } + break; + + case Event::IN_MENU_UP: + case Event::IN_MENU_DOWN: + simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN); // move to setting + set_button_press_timeout(); + if (_in_menu > 1) { + // in a sub-menu, keep track of the selected position + _sub_menu_pos = (_sub_menu_pos + 1) % _sub_menu_lengths[_top_menu_pos]; + } else { + // in the top-menu, keep track of the selected position + _top_menu_pos = (_top_menu_pos + 1) % RUNCAM_TOP_MENU_LENGTH; + } + break; + + case Event::IN_MENU_EXIT: + // if we are in a sub-menu this will move us out, if we are in the root menu this will + // exit causing the state machine to get out of sync. the OSD menu hierachy is consistently + // 2 deep so we can count and be reasonably confident of where we are. + // the only exception is if someone hits save and exit on the root menu - then we are lost. + if (_in_menu > 0) { + _in_menu--; + simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_MODE); // move up/out a menu + set_mode_change_timeout(); + } + // no longer in the menu so trigger the OSD re-enablement + if (_in_menu == 0) { + _state = State::EXITING_MENU; + } + break; + + case Event::NONE: + case Event::IN_MENU_RIGHT: + case Event::BUTTON_RELEASE: + case Event::START_RECORDING: + case Event::STOP_RECORDING: + break; + } +} + +// enter the 2 key OSD menu +void AP_RunCam::enter_2_key_osd_menu() +{ + // turn off built-in OSD so that the runcam OSD is visible + disable_osd(); + + simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_MODE); + set_button_press_timeout(); + _in_menu = 1; + _state = State::IN_MENU; +} + +// exit the 2 key OSD menu +void AP_RunCam::exit_2_key_osd_menu() +{ + // turn built-in OSD back on + enable_osd(); + + if (_video_recording) { + simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_CHANGE_START_RECORDING); + set_mode_change_timeout(); + } + _state = State::READY; + _in_menu = 0; +} + +// run the 5-key OSD simulation process +void AP_RunCam::handle_5_key_simulation_process(Event ev) +{ + switch (ev) { + case Event::BUTTON_RELEASE: + send_5_key_OSD_cable_simulation_event(ev); + _waiting_device_response = true; + return; + case Event::ENTER_MENU: + if (_in_menu > 0) { + ev = Event::IN_MENU_RIGHT; + } else { + // turn off built-in OSD so that the runcam OSD is visible + disable_osd(); + } + break; + + case Event::EXIT_MENU: + if (_in_menu > 0) { + // turn built-in OSD back on + enable_osd(); + } + break; + + case Event::NONE: + case Event::IN_MENU_ENTER: + case Event::IN_MENU_RIGHT: + case Event::IN_MENU_UP: + case Event::IN_MENU_DOWN: + case Event::IN_MENU_EXIT: + case Event::START_RECORDING: + case Event::STOP_RECORDING: + break; + } + + if (ev != Event::NONE) { + send_5_key_OSD_cable_simulation_event(ev); + _button_pressed = true; + _waiting_device_response = true; + } +} + +// handle a response +void AP_RunCam::handle_5_key_simulation_response(const Request& request) +{ + debug("response for command %d result: %d\n", int(request._command), int(request._result)); + if (request._result != RequestStatus::SUCCESS) { + simulation_OSD_cable_failed(request); + _waiting_device_response = false; + return; + } + + switch (request._command) { + case Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE: + _button_pressed = false; + break; + case Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION: + { + // the high 4 bits is the operationID that we sent + // the low 4 bits is the result code + _button_pressed = true; + const ConnectionOperation operationID = ConnectionOperation(request._param); + const uint8_t errorCode = (request._recv_buf[1] & 0x0F); + switch (operationID) { + case ConnectionOperation::RCDEVICE_PROTOCOL_5KEY_FUNCTION_OPEN: + if (errorCode > 0) { + _in_menu = 1; + } + break; + case ConnectionOperation::RCDEVICE_PROTOCOL_5KEY_FUNCTION_CLOSE: + if (errorCode > 0) { + _in_menu = 0; + } + break; + } + break; + } + case Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS: + _button_pressed = true; + break; + case Command::RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO: + case Command::RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL: + case Command::COMMAND_NONE: + break; + } + + _waiting_device_response = false; +} + +// process a response from the serial port +void AP_RunCam::receive() +{ + if (!uart) { + return; + } + // process any pending request at least once-per cycle, regardless of available bytes + if (!request_pending(AP_HAL::millis())) { + return; + } + + uint32_t avail = MIN(uart->available(), (uint32_t)RUNCAM_MAX_PACKET_SIZE); + + for (uint32_t i = 0; i < avail; i++) { + + if (!request_pending(AP_HAL::millis())) { + return; + } + + const uint8_t c = uart->read(); + if (_pending_request._recv_response_length == 0) { + // Only start receiving packet when we found a header + if (c != RUNCAM_HEADER) { + continue; + } + } + + _pending_request._recv_buf[_pending_request._recv_response_length] = c; + _pending_request._recv_response_length += 1; + + // if data received done, trigger callback to parse response data, and update RUNCAM state + if (_pending_request._recv_response_length == _pending_request._expected_response_length) { + uint8_t crc = _pending_request.get_crc(); + + _pending_request._result = (crc == 0) ? RequestStatus::SUCCESS : RequestStatus::INCORRECT_CRC; + + debug("received response %d\n", int(_pending_request._command)); + _pending_request.parse_response(); + // we no longer have a pending request + _pending_request._result = RequestStatus::NONE; + } + } +} + +// every time we send a packet to device and want to get a response +// it's better to clear the rx buffer before the sending the packet +// otherwise useless data in rx buffer will cause the response decoding +// to fail +void AP_RunCam::drain() +{ + if (!uart) { + return; + } + + uint32_t avail = uart->available(); + while (avail-- > 0) { + uart->read(); + } +} + +// get the device info (firmware version, protocol version and features) +void AP_RunCam::get_device_info() +{ + send_request_and_waiting_response(Command::RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, 0, RUNCAM_INIT_INTERVAL_MS, + _boot_delay_ms / RUNCAM_INIT_INTERVAL_MS, FUNCTOR_BIND_MEMBER(&AP_RunCam::parse_device_info, void, const Request&)); +} + +// map a Event to a SimulationOperation +AP_RunCam::SimulationOperation AP_RunCam::map_key_to_protocol_operation(const Event key) const +{ + SimulationOperation operation = SimulationOperation::SIMULATION_NONE; + switch (key) { + case Event::IN_MENU_EXIT: + operation = SimulationOperation::RCDEVICE_PROTOCOL_5KEY_SIMULATION_LEFT; + break; + case Event::IN_MENU_UP: + operation = SimulationOperation::RCDEVICE_PROTOCOL_5KEY_SIMULATION_UP; + break; + case Event::IN_MENU_RIGHT: + operation = SimulationOperation::RCDEVICE_PROTOCOL_5KEY_SIMULATION_RIGHT; + break; + case Event::IN_MENU_DOWN: + operation = SimulationOperation::RCDEVICE_PROTOCOL_5KEY_SIMULATION_DOWN; + break; + case Event::IN_MENU_ENTER: + operation = SimulationOperation::RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET; + break; + case Event::BUTTON_RELEASE: + case Event::NONE: + case Event::ENTER_MENU: + case Event::EXIT_MENU: + case Event::STOP_RECORDING: + case Event::START_RECORDING: + break; + } + return operation; +} + +// send an event +void AP_RunCam::send_5_key_OSD_cable_simulation_event(const Event key) +{ + debug("OSD cable simulation event %d\n", int(key)); + switch (key) { + case Event::ENTER_MENU: + open_5_key_OSD_cable_connection(FUNCTOR_BIND_MEMBER(&AP_RunCam::handle_5_key_simulation_response, void, const Request&)); + break; + case Event::EXIT_MENU: + close_5_key_OSD_cable_connection(FUNCTOR_BIND_MEMBER(&AP_RunCam::handle_5_key_simulation_response, void, const Request&)); + break; + case Event::IN_MENU_UP: + case Event::IN_MENU_RIGHT: + case Event::IN_MENU_DOWN: + case Event::IN_MENU_ENTER: + case Event::IN_MENU_EXIT: + 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&)); + break; + case Event::BUTTON_RELEASE: + simulate_5_key_OSD_cable_button_release(FUNCTOR_BIND_MEMBER(&AP_RunCam::handle_5_key_simulation_response, void, const Request&)); + break; + case Event::STOP_RECORDING: + case Event::START_RECORDING: + case Event::NONE: + break; + } +} + +// every time we run the OSD menu simulation it's necessary to open the connection +void AP_RunCam::open_5_key_OSD_cable_connection(parse_func_t parseFunc) +{ + send_request_and_waiting_response(Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, + uint8_t(ConnectionOperation::RCDEVICE_PROTOCOL_5KEY_FUNCTION_OPEN), 400, 2, parseFunc); +} + +// every time we exit the OSD menu simulation it's necessary to close the connection +void AP_RunCam::close_5_key_OSD_cable_connection(parse_func_t parseFunc) +{ + send_request_and_waiting_response(Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, + uint8_t(ConnectionOperation::RCDEVICE_PROTOCOL_5KEY_FUNCTION_CLOSE), 400, 2, parseFunc); +} + +// simulate button press event of 5 key OSD cable with special button +void AP_RunCam::simulate_5_key_OSD_cable_button_press(const SimulationOperation operation, parse_func_t parseFunc) +{ + if (operation == SimulationOperation::SIMULATION_NONE) { + return; + } + + send_request_and_waiting_response(Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, uint8_t(operation), 400, 2, parseFunc); +} + +// simulate button release event of 5 key OSD cable +void AP_RunCam::simulate_5_key_OSD_cable_button_release(parse_func_t parseFunc) +{ + send_request_and_waiting_response(Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, + uint8_t(SimulationOperation::SIMULATION_NONE), 400, 2, parseFunc); +} + +// send a RunCam request and register a response to be processed +void AP_RunCam::send_request_and_waiting_response(Command commandID, uint8_t param, + uint32_t timeout, uint16_t maxRetryTimes, parse_func_t parserFunc) +{ + drain(); + + _pending_request = Request(this, commandID, param, timeout, maxRetryTimes, parserFunc); + debug("sending command: %d, op: %d\n", int(commandID), int(param)); + // send packet + send_packet(commandID, param); +} + +// send a packet to the serial port +void AP_RunCam::send_packet(Command command, uint8_t param) +{ + // is this device open? + if (!uart) { + return; + } + + uint8_t buffer[4]; + + bool have_param = param > 0 || command == Command::RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL; + uint8_t buffer_len = have_param ? 4 : 3; + + buffer[0] = RUNCAM_HEADER; + buffer[1] = uint8_t(command); + if (have_param) { + buffer[2] = param; + } + + uint8_t crc = 0; + for (uint8_t i = 0; i < buffer_len - 1; i++) { + crc = crc8_dvb_s2(crc, buffer[i]); + } + + buffer[buffer_len - 1] = crc; + + // send data if possible + uart->write(buffer, buffer_len); +} + +// crc functions +uint8_t AP_RunCam::crc8_dvb_s2(uint8_t crc, uint8_t a) +{ + crc ^= a; + for (uint8_t i = 0; i < 8; ++i) { + if (crc & 0x80) { + crc = (crc << 1) ^ 0xD5; + } else { + crc = crc << 1; + } + } + return crc; +} + +uint8_t AP_RunCam::crc8_high_first(uint8_t *ptr, uint8_t len) +{ + uint8_t crc = 0x00; + while (len--) { + crc ^= *ptr++; + for (uint8_t i = 8; i > 0; --i) { + if (crc & 0x80) { + crc = (crc << 1) ^ 0x31; + } else { + crc = (crc << 1); + } + } + } + return (crc); +} + +// handle a device info response +void AP_RunCam::parse_device_info(const Request& request) +{ + _protocol_version = ProtocolVersion(request._recv_buf[1]); + + uint8_t featureLowBits = request._recv_buf[2]; + uint8_t featureHighBits = request._recv_buf[3]; + if (_features == 0) { + _features = (featureHighBits << 8) | featureLowBits; + } + _state = State::INITIALIZED; + gcs().send_text(MAV_SEVERITY_INFO, "RunCam device initialized, features 0x%04X\n", _features.get()); +} + +// wait for the RunCam device to be fully ready +bool AP_RunCam::camera_ready() const +{ + if (_state != State::INITIALIZING && _state != State::INITIALIZED) { + return true; + } + return false; +} + +// error handler for OSD simulation +void AP_RunCam::simulation_OSD_cable_failed(const Request& request) +{ + _waiting_device_response = false; + if (request._command == Command::RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION) { + uint8_t operationID = request._param; + if (operationID == uint8_t(ConnectionOperation::RCDEVICE_PROTOCOL_5KEY_FUNCTION_CLOSE)) { + return; + } + } +} + +// process all of the pending responses, retrying as necessary +bool AP_RunCam::request_pending(uint32_t now) +{ + if (_pending_request._result == RequestStatus::NONE) { + return false; + } + + if (_pending_request._request_timestamp_ms != 0 && (now - _pending_request._request_timestamp_ms) < _pending_request._timeout_ms) { + // request still in play + return true; + } + + if (_pending_request._max_retry_times > 0) { + // request timed out, so resend + debug("retrying[%d] command 0x%X, op 0x%X\n", int(_pending_request._max_retry_times), int(_pending_request._command), int(_pending_request._param)); + _pending_request._device->send_packet(_pending_request._command, _pending_request._param); + _pending_request._recv_response_length = 0; + _pending_request._request_timestamp_ms = now; + _pending_request._max_retry_times -= 1; + + return false; + } + debug("timeout command 0x%X, op 0x%X\n", int(_pending_request._command), int(_pending_request._param)); + // too many retries, fail the request + _pending_request._result = RequestStatus::TIMEOUT; + _pending_request.parse_response(); + _pending_request._result = RequestStatus::NONE; + + return false; +} + +// constructor for a response structure +AP_RunCam::Request::Request(AP_RunCam* device, Command commandID, uint8_t param, + uint32_t timeout, uint16_t maxRetryTimes, parse_func_t parserFunc) + : _recv_buf(device->_recv_buf), + _command(commandID), + _max_retry_times(maxRetryTimes), + _timeout_ms(timeout), + _device(device), + _param(param), + _parser_func(parserFunc), + _result(RequestStatus::PENDING) +{ + _request_timestamp_ms = AP_HAL::millis(); + _expected_response_length = get_expected_response_length(commandID); +} + +uint8_t AP_RunCam::Request::get_crc() const +{ + uint8_t crc = 0; + for (int i = 0; i < _recv_response_length; i++) { + crc = AP_RunCam::crc8_dvb_s2(crc, _recv_buf[i]); + } + return crc; +} + +// get the length of a response +uint8_t AP_RunCam::Request::get_expected_response_length(const Command command) const +{ + for (uint16_t i = 0; i < RUNCAM_NUM_EXPECTED_RESPONSES; i++) { + if (_expected_responses_length[i].command == command) { + return _expected_responses_length[i].reponse_length; + } + } + + return 0; +} + +AP_RunCam *AP::runcam() { + return AP_RunCam::get_singleton(); +} + +#endif // HAL_RUNCAM_ENABLED diff --git a/libraries/AP_Camera/AP_RunCam.h b/libraries/AP_Camera/AP_RunCam.h new file mode 100644 index 0000000000..b0cf76a556 --- /dev/null +++ b/libraries/AP_Camera/AP_RunCam.h @@ -0,0 +1,360 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + implementation of RunCam camera protocols + + With thanks to betaflight for a great reference + implementation. Several of the functions below are based on + betaflight equivalent functions + */ +#pragma once + +#include +#include + +#ifndef HAL_RUNCAM_ENABLED +#define HAL_RUNCAM_ENABLED !HAL_MINIMIZE_FEATURES && !APM_BUILD_TYPE(APM_BUILD_Replay) +#endif + +#if HAL_RUNCAM_ENABLED + +#include +#include +#include +#include +#include + +#define RUNCAM_MODE_DELAY_MS 600 +#define RUNCAM_MAX_PACKET_SIZE 64 + + +/// @class AP_RunCam +/// @brief Object managing a RunCam device +class AP_RunCam +{ +public: + AP_RunCam(); + + // do not allow copies + AP_RunCam(const AP_RunCam &other) = delete; + AP_RunCam &operator=(const AP_RunCam &) = delete; + + // get singleton instance + static AP_RunCam *get_singleton() { + return _singleton; + } + + // operation of camera button simulation + enum class ControlOperation { + RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN = 0x00, // WiFi/Mode button + RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN = 0x01, + RCDEVICE_PROTOCOL_CHANGE_MODE = 0x02, + RCDEVICE_PROTOCOL_CHANGE_START_RECORDING = 0x03, + RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING = 0x04, + UNKNOWN_CAMERA_OPERATION = 0xFF + }; + + // initialize the RunCam driver + void init(); + // camera button simulation + bool simulate_camera_button(const ControlOperation operation); + // start the video + void start_recording(); + // stop the video + void stop_recording(); + // update loop + void update(); + // Check whether arming is allowed + bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; + + static const struct AP_Param::GroupInfo var_info[]; + +private: + // definitions prefixed with RCDEVICE taken from https://support.runcam.com/hc/en-us/articles/360014537794-RunCam-Device-Protocol + // possible supported features + enum class Feature { + RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON = (1 << 0), + RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON = (1 << 1), // WiFi/Mode button + RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE = (1 << 2), + RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE = (1 << 3), + RCDEVICE_PROTOCOL_FEATURE_DEVICE_SETTINGS_ACCESS = (1 << 4), + RCDEVICE_PROTOCOL_FEATURE_DISPLAY_PORT = (1 << 5), + RCDEVICE_PROTOCOL_FEATURE_START_RECORDING = (1 << 6), + RCDEVICE_PROTOCOL_FEATURE_STOP_RECORDING = (1 << 7) + }; + + // camera control commands + enum class Command { + RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO = 0x00, + RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL = 0x01, + RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS = 0x02, + RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE = 0x03, + RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION = 0x04, + COMMAND_NONE + }; + + // operation of RC5KEY_CONNECTION + enum class ConnectionOperation { + RCDEVICE_PROTOCOL_5KEY_FUNCTION_OPEN = 0x01, + RCDEVICE_PROTOCOL_5KEY_FUNCTION_CLOSE = 0x02 + }; + + // operation of 5 Key OSD cable simulation + enum class SimulationOperation { + SIMULATION_NONE = 0x00, + RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET = 0x01, + RCDEVICE_PROTOCOL_5KEY_SIMULATION_LEFT = 0x02, + RCDEVICE_PROTOCOL_5KEY_SIMULATION_RIGHT = 0x03, + RCDEVICE_PROTOCOL_5KEY_SIMULATION_UP = 0x04, + RCDEVICE_PROTOCOL_5KEY_SIMULATION_DOWN = 0x05 + }; + + // protocol versions, only version 1.0 is supported + enum class ProtocolVersion { + RCSPLIT_VERSION = 0x00, // unsupported firmware version <= 1.1.0 + VERSION_1_0 = 0x01, + UNKNOWN + }; + + // status of command + enum class RequestStatus { + NONE, + PENDING, + SUCCESS, + INCORRECT_CRC, + TIMEOUT + }; + + enum class State { + INITIALIZING, // uart open + INITIALIZED, // features received + READY, + VIDEO_RECORDING, + ENTERING_MENU, + IN_MENU, + EXITING_MENU + }; + + enum class Event { + NONE, + ENTER_MENU, + EXIT_MENU, + IN_MENU_ENTER, + IN_MENU_RIGHT, // only used by the 5-key process + IN_MENU_UP, + IN_MENU_DOWN, + IN_MENU_EXIT, + BUTTON_RELEASE, + STOP_RECORDING, + START_RECORDING + }; + + static const uint8_t RUNCAM_NUM_SUB_MENUS = 5; + static const uint8_t RUNCAM_NUM_EXPECTED_RESPONSES = 4; + + // supported features, usually probed from the device + AP_Int16 _features; + // number of initialization attempts + AP_Int8 _init_attempts; + // delay between initialization attempts + AP_Int32 _init_attempt_interval_ms; + // delay time to make sure the camera is fully booted + AP_Int32 _boot_delay_ms; + // delay time to make sure a button press has been activated + AP_Int32 _button_delay_ms; + + // video on/off + bool _video_recording = true; + // detected protocol version + ProtocolVersion _protocol_version = ProtocolVersion::UNKNOWN; + // uart for the device + AP_HAL::UARTDriver *uart; + // camera state + State _state = State::INITIALIZING; + // time since last OSD cycle + uint32_t _last_osd_update_ms; + // start time of the current button press or boot sequence + uint32_t _transition_start_ms; + // timeout of the current button press or boot sequence + uint32_t _transition_timeout_ms; + // OSD state machine: button has been pressed + bool _button_pressed; + // OSD state machine: waiting for a response + bool _waiting_device_response; + // OSD state mechine: in the menu, value indicates depth + uint8_t _in_menu; + // OSD state machine: current selection in the top menu + int8_t _top_menu_pos; + // OSD state machine: current selection in the sub menu + uint8_t _sub_menu_pos; + // lengths of the sub-menus + static uint8_t _sub_menu_lengths[RUNCAM_NUM_SUB_MENUS]; + // shared inbound scratch space + uint8_t _recv_buf[RUNCAM_MAX_PACKET_SIZE]; // all the response contexts use same recv buffer + + class Request; + + FUNCTOR_TYPEDEF(parse_func_t, void, const Request&); + + // class to represent a request + class Request + { + friend class AP_RunCam; + + public: + Request(AP_RunCam *device, Command commandID, uint8_t param, + uint32_t timeout, uint16_t maxRetryTimes, parse_func_t parserFunc); + Request() { _command = Command::COMMAND_NONE; } + + uint8_t *_recv_buf; // response data buffer + AP_RunCam *_device; // parent device + Command _command; // command for which a response is expected + uint8_t _param; // parameter data, the protocol can take more but we never use it + + private: + uint8_t _recv_response_length; // length of the data received + uint8_t _expected_response_length; // total length of response data wanted + uint32_t _timeout_ms; // how long to wait before giving up + uint32_t _request_timestamp_ms; // when the request was sent, if it's zero keep waiting for the response + uint16_t _max_retry_times; // number of times to resend the request + parse_func_t _parser_func; // function to parse the response + RequestStatus _result; // whether we were successful or not + + // get the length of the expected response + uint8_t get_expected_response_length(const Command command) const; + // calculate a crc + uint8_t get_crc() const; + // parse the response + void parse_response() { + if (_parser_func != nullptr) { + _parser_func(*this); + } + } + + struct Length { + Command command; + uint8_t reponse_length; + }; + + static Length _expected_responses_length[RUNCAM_NUM_EXPECTED_RESPONSES]; + } _pending_request; + + // start the counter for a button press + void set_button_press_timeout() { + _transition_timeout_ms = _button_delay_ms; + _button_pressed = true; + } + // start the counter for a mode change + void set_mode_change_timeout() { + _transition_timeout_ms = RUNCAM_MODE_DELAY_MS; + _button_pressed = true; + } + + // disable the OSD display + void disable_osd() { +#if OSD_ENABLED + AP_OSD* osd = AP::osd(); + if (osd != nullptr) { + osd->disable(); + } +#endif + } + // enable the OSD display + void enable_osd() { +#if OSD_ENABLED + AP_OSD* osd = AP::osd(); + if (osd != nullptr) { + osd->enable(); + } +#endif + } + + // OSD update loop + void update_osd(); + // return radio values as LOW, MIDDLE, HIGH + RC_Channel::aux_switch_pos_t get_channel_pos(uint8_t rcmapchan) const; + // update the state machine when armed or flying + void update_state_machine_armed(); + // update the state machine when disarmed + void update_state_machine_disarmed(); + // handle the initialized state + void handle_initialized(Event ev); + // handle the ready state + void handle_ready(Event ev); + // handle the recording state + void handle_recording(Event ev); + // run the 2-key OSD simulation process + void handle_in_menu(Event ev); + // map rc input to an event + AP_RunCam::Event map_rc_input_to_event() const; + + // run the 2-key OSD simulation process + void handle_2_key_simulation_process(Event ev); + // enter the 2 key OSD menu + void enter_2_key_osd_menu(); + // eexit the 2 key OSD menu + void exit_2_key_osd_menu(); + + // run the 5-key OSD simulation process + void handle_5_key_simulation_process(Event ev); + // handle a response + void handle_5_key_simulation_response(const Request& request); + // process a response from the serial port + void receive(); + // empty the receive side of the serial port + void drain(); + + // get the RunCam device information + void get_device_info(); + // 5 key osd cable simulation + SimulationOperation map_key_to_protocol_operation(const Event ev) const; + // send an event + void send_5_key_OSD_cable_simulation_event(const Event key); + // enter the menu + void open_5_key_OSD_cable_connection(parse_func_t parseFunc); + // exit the menu + void close_5_key_OSD_cable_connection(parse_func_t parseFunc); + // press a button + void simulate_5_key_OSD_cable_button_press(const SimulationOperation operation, parse_func_t parseFunc); + // release a button + void simulate_5_key_OSD_cable_button_release(parse_func_t parseFunc); + // send a RunCam request and register a response to be processed + void send_request_and_waiting_response(Command commandID, uint8_t param, uint32_t timeout, + uint16_t maxRetryTimes, parse_func_t parseFunc); + // send a packet to the serial port + void send_packet(Command command, uint8_t param); + // crc functions + static uint8_t crc8_high_first(uint8_t *ptr, uint8_t len); + static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a); + // handle a device info response + void parse_device_info(const Request& request); + // wait for the RunCam device to be fully ready + bool camera_ready() const; + // whether or not the requested feature is supported + bool has_feature(const Feature feature) { return _features.get() & uint16_t(feature); } + // error handler for OSD simulation + void simulation_OSD_cable_failed(const Request& request); + // process pending request, retrying as necessary + bool request_pending(uint32_t now); + + static AP_RunCam *_singleton; +}; + +namespace AP +{ +AP_RunCam *runcam(); +}; + +#endif // HAL_RUNCAM_ENABLED