Browse Source
Allows control of 2-key and 5-key OSD menus and recording start/stop OSD is disabled while in the RunCam menu but reinstated on exit add arming checks to prevent arming while the OSD menu is enabledc415-sdk
Andy Piper
5 years ago
committed by
Andrew Tridgell
2 changed files with 1288 additions and 0 deletions
@ -0,0 +1,928 @@
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
/*
|
||||
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 <AP_Math/AP_Math.h> |
||||
#include <GCS_MAVLink/GCS.h> |
||||
#include <AP_Logger/AP_Logger.h> |
||||
|
||||
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
|
@ -0,0 +1,360 @@
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
/*
|
||||
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 <AP_HAL/AP_HAL.h> |
||||
#include <AP_Vehicle/AP_Vehicle_Type.h> |
||||
|
||||
#ifndef HAL_RUNCAM_ENABLED |
||||
#define HAL_RUNCAM_ENABLED !HAL_MINIMIZE_FEATURES && !APM_BUILD_TYPE(APM_BUILD_Replay) |
||||
#endif |
||||
|
||||
#if HAL_RUNCAM_ENABLED |
||||
|
||||
#include <AP_Param/AP_Param.h> |
||||
#include <RC_Channel/RC_Channel.h> |
||||
#include <AP_RCMapper/AP_RCMapper.h> |
||||
#include <AP_Arming/AP_Arming.h> |
||||
#include <AP_OSD/AP_OSD.h> |
||||
|
||||
#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
|
Loading…
Reference in new issue