2019-11-09 09:08:41 -04:00
/*
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 [ ] = {
2019-12-29 20:14:02 -04:00
// @Param: TYPE
// @DisplayName: RunCam device type
2020-03-09 18:08:42 -03:00
// @Description: RunCam deviee type used to determine OSD menu structure and shutter options.
// @Values: 0:Disabled, 1:RunCam Split Micro/RunCam with UART, 2:RunCam Split
2019-12-29 20:47:53 -04:00
AP_GROUPINFO_FLAGS ( " TYPE " , 1 , AP_RunCam , _cam_type , int ( DeviceType : : Disabled ) , AP_PARAM_FLAG_ENABLE ) ,
2019-12-29 20:14:02 -04:00
2019-11-09 09:08:41 -04:00
// @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
2019-12-29 20:14:02 -04:00
AP_GROUPINFO ( " FEATURES " , 2 , AP_RunCam , _features , 0 ) ,
2019-11-09 09:08:41 -04:00
// @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
2019-12-29 20:14:02 -04:00
AP_GROUPINFO ( " BT_DELAY " , 3 , AP_RunCam , _boot_delay_ms , 7000 ) ,
2019-11-09 09:08:41 -04:00
// @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
2020-02-02 13:15:57 -04:00
AP_GROUPINFO ( " BTN_DELAY " , 4 , AP_RunCam , _button_delay_ms , RUNCAM_DEFAULT_BUTTON_PRESS_DELAY ) ,
2019-11-09 09:08:41 -04:00
2019-12-22 10:49:44 -04:00
// @Param: MDE_DELAY
// @DisplayName: RunCam mode delay before allowing further button presses
2020-02-02 13:15:57 -04:00
// @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.
2019-12-22 10:49:44 -04:00
// @User: Advanced
2019-12-29 20:14:02 -04:00
AP_GROUPINFO ( " MDE_DELAY " , 5 , AP_RunCam , _mode_delay_ms , 800 ) ,
2019-12-21 16:06:53 -04:00
// @Param: CONTROL
// @DisplayName: RunCam control option
2019-12-23 11:24:41 -04:00
// @Description: Specifies the allowed actions required to enter the OSD menu
2020-02-02 13:15:57 -04:00
// @Bitmask: 0:Stick yaw right,1:Stick roll right,2:3-position switch,3:2-position switch,4:Autorecording enabled
2019-12-21 16:06:53 -04:00
// @User: Advanced
2019-12-23 11:24:41 -04:00
AP_GROUPINFO ( " CONTROL " , 6 , AP_RunCam , _cam_control_option , uint8_t ( ControlOption : : STICK_ROLL_RIGHT ) | uint8_t ( ControlOption : : TWO_POS_SWITCH ) ) ,
2019-12-21 16:06:53 -04:00
2019-11-09 09:08:41 -04:00
AP_GROUPEND
} ;
# define RUNCAM_DEBUG 0
# if RUNCAM_DEBUG
2020-02-02 13:15:57 -04:00
static const char * event_names [ 11 ] = {
" NONE " , " ENTER_MENU " , " EXIT_MENU " ,
" IN_MENU_ENTER " , " IN_MENU_RIGHT " , " IN_MENU_UP " , " IN_MENU_DOWN " , " IN_MENU_EXIT " ,
" BUTTON_RELEASE " , " STOP_RECORDING " , " START_RECORDING "
} ;
static const char * state_names [ 7 ] = {
" INITIALIZING " , " INITIALIZED " , " READY " , " VIDEO_RECORDING " , " ENTERING_MENU " , " IN_MENU " , " EXITING_MENU "
} ;
# define debug(fmt, args ...) do { hal.console->printf("RunCam[%s]: " fmt, state_names[int(_state)], ## args); } while (0)
2019-11-09 09:08:41 -04:00
# 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
2019-12-21 16:06:53 -04:00
// menu structures of runcam devices
AP_RunCam : : Menu AP_RunCam : : _menus [ RUNCAM_MAX_DEVICE_TYPES ] = {
// these are correct for the runcam split micro v2.4.4, others may vary
// Video, Image, TV-OUT, Micro SD Card, General
2020-02-02 13:15:57 -04:00
{ 6 , { 5 , 8 , 3 , 3 , 7 } } , // SplitMicro
{ 0 , { 0 } } , // Split
2019-11-09 09:08:41 -04:00
} ;
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 ;
2020-02-02 13:15:57 -04:00
_cam_type = constrain_int16 ( _cam_type , 0 , RUNCAM_MAX_DEVICE_TYPES ) ;
_video_recording = VideoOption ( _cam_control_option & uint8_t ( ControlOption : : VIDEO_RECORDING_AT_BOOT ) ) ;
2019-11-09 09:08:41 -04:00
}
// 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 ) ;
}
2019-12-29 20:14:02 -04:00
if ( uart ! = nullptr ) {
/*
if the user has setup a serial port as a runcam then default
2020-02-02 13:15:57 -04:00
type to the split micro ( Andy ' s development platform ! ) . This makes setup a bit easier for most
2019-12-29 20:14:02 -04:00
users while still enabling parameters to be hidden for users
without a runcam
*/
2020-02-02 13:15:57 -04:00
_cam_type . set_default ( int8_t ( DeviceType : : SplitMicro ) ) ;
2019-12-29 20:14:02 -04:00
}
2019-12-29 20:47:53 -04:00
if ( _cam_type . get ( ) = = int8_t ( DeviceType : : Disabled ) ) {
2019-12-29 20:14:02 -04:00
uart = nullptr ;
return ;
}
2019-11-09 09:08:41 -04:00
if ( uart = = nullptr ) {
return ;
}
2020-02-02 13:15:57 -04:00
// Split requires two mode presses to get into the menu
if ( _cam_type . get ( ) = = int8_t ( DeviceType : : Split ) ) {
_menu_enter_level = - 1 ;
_in_menu = - 1 ;
}
2019-11-09 09:08:41 -04:00
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
2020-02-02 13:15:57 -04:00
bool AP_RunCam : : simulate_camera_button ( const ControlOperation operation , const uint32_t transition_timeout )
2019-11-09 09:08:41 -04:00
{
if ( ! uart | | _protocol_version ! = ProtocolVersion : : VERSION_1_0 ) {
return false ;
}
2020-02-02 13:15:57 -04:00
_transition_timeout_ms = transition_timeout ;
debug ( " press button %d, timeout=%dms \n " , int ( operation ) , int ( transition_timeout ) ) ;
2019-11-09 09:08:41 -04:00
send_packet ( Command : : RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL , uint8_t ( operation ) ) ;
return true ;
}
// start the video
void AP_RunCam : : start_recording ( ) {
2020-02-02 13:15:57 -04:00
debug ( " start recording(%d) \n " , int ( _state ) ) ;
_video_recording = VideoOption : : RECORDING ;
2019-12-21 18:06:03 -04:00
_osd_option = OSDOption : : NO_OPTION ;
2019-11-09 09:08:41 -04:00
}
// stop the video
void AP_RunCam : : stop_recording ( ) {
2020-02-02 13:15:57 -04:00
debug ( " stop recording(%d) \n " , int ( _state ) ) ;
_video_recording = VideoOption : : NOT_RECORDING ;
2019-12-21 18:06:03 -04:00
_osd_option = OSDOption : : NO_OPTION ;
2019-12-21 16:06:53 -04:00
}
// enter the OSD menu
void AP_RunCam : : enter_osd ( )
{
2020-02-02 13:15:57 -04:00
debug ( " enter osd(%d) \n " , int ( _state ) ) ;
2019-12-21 18:06:03 -04:00
_osd_option = OSDOption : : ENTER ;
2019-12-21 16:06:53 -04:00
}
// exit the OSD menu
void AP_RunCam : : exit_osd ( )
{
2020-02-02 13:15:57 -04:00
debug ( " exit osd(%d) \n " , int ( _state ) ) ;
2019-12-21 18:06:03 -04:00
_osd_option = OSDOption : : EXIT ;
2019-12-21 16:06:53 -04:00
}
// OSD control determined by camera options
void AP_RunCam : : osd_option ( ) {
2019-12-22 10:49:44 -04:00
debug ( " osd option \n " ) ;
2019-12-21 18:06:03 -04:00
_osd_option = OSDOption : : OPTION ;
2019-11-09 09:08:41 -04:00
}
// input update loop
void AP_RunCam : : update ( )
{
2019-12-29 20:47:53 -04:00
if ( uart = = nullptr | | _cam_type . get ( ) = = int8_t ( DeviceType : : Disabled ) ) {
2019-11-09 09:08:41 -04:00
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
2020-02-02 13:15:57 -04:00
if ( is_arming_prevented ( ) ) {
2019-11-09 09:08:41 -04:00
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 ( ) ;
}
// 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 :
2020-02-02 13:15:57 -04:00
handle_ready ( _video_recording = = VideoOption : : RECORDING & & has_feature ( Feature : : RCDEVICE_PROTOCOL_FEATURE_START_RECORDING ) ? Event : : START_RECORDING : Event : : NONE ) ;
2019-11-09 09:08:41 -04:00
break ;
case State : : VIDEO_RECORDING :
2020-02-02 13:15:57 -04:00
handle_recording ( _video_recording = = VideoOption : : NOT_RECORDING & & has_feature ( Feature : : RCDEVICE_PROTOCOL_FEATURE_START_RECORDING ) ? Event : : STOP_RECORDING : Event : : NONE ) ;
2019-11-09 09:08:41 -04:00
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 ) {
2020-02-02 13:15:57 -04:00
_last_rc_event = Event : : NONE ;
2019-11-09 09:08:41 -04:00
return ;
}
_transition_start_ms = now ;
_transition_timeout_ms = 0 ;
const Event ev = map_rc_input_to_event ( ) ;
2020-02-02 13:15:57 -04:00
// only take action on transitions
if ( ev = = _last_rc_event & & _state = = _last_state & & _osd_option = = _last_osd_option
& & _last_in_menu = = _in_menu & & _last_video_recording = = _video_recording ) {
return ;
2019-11-09 09:08:41 -04:00
}
2020-02-02 13:15:57 -04:00
debug ( " update_state_machine_disarmed(%s) \n " , event_names [ int ( ev ) ] ) ;
_last_rc_event = ev ;
_last_state = _state ;
_last_osd_option = _osd_option ;
_last_in_menu = _in_menu ;
_last_video_recording = _video_recording ;
2019-11-09 09:08:41 -04:00
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 )
{
2020-02-02 13:15:57 -04:00
// the camera should be configured to start with recording mode off by default
// a recording change needs significantly extra time to process
if ( _video_recording = = VideoOption : : RECORDING & & has_feature ( Feature : : RCDEVICE_PROTOCOL_FEATURE_START_RECORDING ) ) {
if ( ! ( _cam_control_option & uint8_t ( ControlOption : : VIDEO_RECORDING_AT_BOOT ) ) ) {
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_CHANGE_START_RECORDING , _mode_delay_ms * 2 ) ;
}
_state = State : : VIDEO_RECORDING ;
} else if ( _video_recording = = VideoOption : : NOT_RECORDING & & has_feature ( Feature : : RCDEVICE_PROTOCOL_FEATURE_START_RECORDING ) ) {
if ( _cam_control_option & uint8_t ( ControlOption : : VIDEO_RECORDING_AT_BOOT ) ) {
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING , _mode_delay_ms * 2 ) ;
}
2019-11-09 09:08:41 -04:00
_state = State : : READY ;
} else {
2020-02-02 13:15:57 -04:00
_state = State : : READY ;
2019-11-09 09:08:41 -04:00
}
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 :
2019-12-23 11:24:41 -04:00
case Event : : IN_MENU_ENTER :
2020-02-02 13:15:57 -04:00
case Event : : IN_MENU_RIGHT :
2019-12-23 11:24:41 -04:00
if ( ev = = Event : : ENTER_MENU | | _cam_control_option & uint8_t ( ControlOption : : STICK_ROLL_RIGHT ) ) {
_top_menu_pos = - 1 ;
_sub_menu_pos = 0 ;
_state = State : : ENTERING_MENU ;
}
2019-11-09 09:08:41 -04:00
break ;
case Event : : START_RECORDING :
2020-02-02 13:15:57 -04:00
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_CHANGE_START_RECORDING , _mode_delay_ms ) ;
2019-11-09 09:08:41 -04:00
_state = State : : VIDEO_RECORDING ;
break ;
case Event : : NONE :
case Event : : EXIT_MENU :
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 :
2019-12-23 11:24:41 -04:00
case Event : : IN_MENU_ENTER :
2020-02-02 13:15:57 -04:00
case Event : : IN_MENU_RIGHT :
2019-12-23 11:24:41 -04:00
if ( ev = = Event : : ENTER_MENU | | _cam_control_option & uint8_t ( ControlOption : : STICK_ROLL_RIGHT ) ) {
2020-02-02 13:15:57 -04:00
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING , _mode_delay_ms ) ;
2019-12-23 11:24:41 -04:00
_top_menu_pos = - 1 ;
_sub_menu_pos = 0 ;
_state = State : : ENTERING_MENU ;
}
2019-11-09 09:08:41 -04:00
break ;
case Event : : STOP_RECORDING :
2020-02-02 13:15:57 -04:00
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING , _mode_delay_ms ) ;
2019-11-09 09:08:41 -04:00
_state = State : : READY ;
break ;
case Event : : NONE :
case Event : : EXIT_MENU :
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
{
2020-05-02 11:12:48 -03:00
const RC_Channel : : aux_switch_pos_t throttle = rc ( ) . get_channel_pos ( AP : : rcmap ( ) - > throttle ( ) ) ;
const RC_Channel : : aux_switch_pos_t yaw = rc ( ) . get_channel_pos ( AP : : rcmap ( ) - > yaw ( ) ) ;
const RC_Channel : : aux_switch_pos_t roll = rc ( ) . get_channel_pos ( AP : : rcmap ( ) - > roll ( ) ) ;
const RC_Channel : : aux_switch_pos_t pitch = rc ( ) . get_channel_pos ( AP : : rcmap ( ) - > pitch ( ) ) ;
2019-11-09 09:08:41 -04:00
Event result = Event : : NONE ;
2020-02-02 13:15:57 -04:00
if ( _button_pressed ! = ButtonState : : NONE ) {
if ( _button_pressed = = ButtonState : : PRESSED & & yaw = = RC_Channel : : MIDDLE & & pitch = = RC_Channel : : MIDDLE & & roll = = RC_Channel : : MIDDLE ) {
result = Event : : BUTTON_RELEASE ;
} else {
result = Event : : NONE ; // still waiting to be released
}
2019-11-09 09:08:41 -04:00
} else if ( throttle = = RC_Channel : : MIDDLE & & yaw = = RC_Channel : : LOW
2019-12-21 18:06:03 -04:00
& & pitch = = RC_Channel : : MIDDLE & & roll = = RC_Channel : : MIDDLE
2020-02-02 13:15:57 -04:00
// don't allow an action close to arming unless the user had configured it or arming is not possible
// but don't prevent the 5-Key control actually working
& & ( _cam_control_option & uint8_t ( ControlOption : : STICK_YAW_RIGHT ) | | is_arming_prevented ( ) ) ) {
2019-11-09 09:08:41 -04:00
result = Event : : EXIT_MENU ;
2020-02-02 13:15:57 -04:00
} else if ( throttle = = RC_Channel : : MIDDLE & & yaw = = RC_Channel : : HIGH
2019-12-21 16:06:53 -04:00
& & pitch = = RC_Channel : : MIDDLE & & roll = = RC_Channel : : MIDDLE
2020-02-02 13:15:57 -04:00
& & ( _cam_control_option & uint8_t ( ControlOption : : STICK_YAW_RIGHT ) | | is_arming_prevented ( ) ) ) {
2019-11-09 09:08:41 -04:00
result = Event : : ENTER_MENU ;
} else if ( roll = = RC_Channel : : LOW ) {
result = Event : : IN_MENU_EXIT ;
2019-12-23 11:24:41 -04:00
} else if ( yaw = = RC_Channel : : MIDDLE & & pitch = = RC_Channel : : MIDDLE & & roll = = RC_Channel : : HIGH ) {
2020-02-02 13:15:57 -04:00
if ( has_feature ( Feature : : RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE ) ) {
result = Event : : IN_MENU_RIGHT ;
} else {
result = Event : : IN_MENU_ENTER ;
}
2019-11-09 09:08:41 -04:00
} else if ( pitch = = RC_Channel : : LOW ) {
2020-02-02 13:15:57 -04:00
result = Event : : IN_MENU_UP ;
} else if ( pitch = = RC_Channel : : HIGH ) {
2019-11-09 09:08:41 -04:00
result = Event : : IN_MENU_DOWN ;
2020-02-02 13:15:57 -04:00
} else if ( _video_recording ! = _last_video_recording ) {
switch ( _video_recording ) {
case VideoOption : : NOT_RECORDING :
result = Event : : STOP_RECORDING ;
break ;
case VideoOption : : RECORDING :
result = Event : : START_RECORDING ;
break ;
}
} else if ( _osd_option = = _last_osd_option ) {
// OSD option has not changed so assume stick re-centering
result = Event : : NONE ;
2019-12-21 18:06:03 -04:00
} else if ( _osd_option = = OSDOption : : ENTER
& & _cam_control_option & uint8_t ( ControlOption : : TWO_POS_SWITCH ) ) {
result = Event : : ENTER_MENU ;
} else if ( _osd_option = = OSDOption : : OPTION
& & _cam_control_option & uint8_t ( ControlOption : : THREE_POS_SWITCH ) ) {
2019-12-21 16:06:53 -04:00
result = Event : : ENTER_MENU ;
2019-12-21 18:06:03 -04:00
} else if ( _osd_option = = OSDOption : : EXIT
& & _cam_control_option & uint8_t ( ControlOption : : TWO_POS_SWITCH ) ) {
result = Event : : EXIT_MENU ;
} else if ( _osd_option = = OSDOption : : NO_OPTION
& & _cam_control_option & uint8_t ( ControlOption : : THREE_POS_SWITCH ) ) {
result = Event : : EXIT_MENU ;
2019-11-09 09:08:41 -04:00
}
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 )
{
2020-02-02 13:15:57 -04:00
debug ( " %s,M:%d,V:%d,O:%d \n " , event_names [ int ( ev ) ] , _in_menu , int ( _video_recording ) , int ( _osd_option ) ) ;
2019-11-09 09:08:41 -04:00
switch ( ev ) {
case Event : : ENTER_MENU :
2020-02-02 13:15:57 -04:00
if ( _in_menu < = 0 ) {
_in_menu + + ;
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_CHANGE_MODE , _mode_delay_ms ) ;
if ( _in_menu > 0 ) {
// turn off built-in OSD so that the runcam OSD is visible
disable_osd ( ) ;
_state = State : : IN_MENU ;
} else {
_state = State : : ENTERING_MENU ;
}
2019-11-09 09:08:41 -04:00
}
break ;
case Event : : EXIT_MENU :
// keep changing mode until we are fully out of the menu
if ( _in_menu > 0 ) {
_in_menu - - ;
2020-02-02 13:15:57 -04:00
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_CHANGE_MODE , _mode_delay_ms ) ;
2019-11-09 09:08:41 -04:00
_state = State : : EXITING_MENU ;
} else {
exit_2_key_osd_menu ( ) ;
}
break ;
case Event : : IN_MENU_ENTER :
// in a sub-menu and save-and-exit was selected
2020-02-02 13:15:57 -04:00
if ( _in_menu > 1 & & get_top_menu_length ( ) > 0 & & _sub_menu_pos = = ( get_sub_menu_length ( _top_menu_pos ) - 1 ) ) {
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN , _button_delay_ms ) ;
2020-03-29 14:09:22 -03:00
_sub_menu_pos = 0 ;
2019-11-09 09:08:41 -04:00
_in_menu - - ;
// in the top-menu and save-and-exit was selected
2020-02-02 13:15:57 -04:00
} else if ( _in_menu = = 1 & & get_top_menu_length ( ) > 0 & & _top_menu_pos = = ( get_top_menu_length ( ) - 1 ) ) {
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN , _mode_delay_ms ) ;
2019-11-09 09:08:41 -04:00
_in_menu - - ;
_state = State : : EXITING_MENU ;
} else {
2020-02-02 13:15:57 -04:00
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN , _button_delay_ms ) ;
2019-11-09 09:08:41 -04:00
_in_menu = MIN ( _in_menu + 1 , RUNCAM_OSD_MENU_DEPTH ) ;
}
break ;
case Event : : IN_MENU_UP :
case Event : : IN_MENU_DOWN :
2020-02-02 13:15:57 -04:00
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN , _button_delay_ms ) ; // move to setting
2019-11-09 09:08:41 -04:00
if ( _in_menu > 1 ) {
// in a sub-menu, keep track of the selected position
2019-12-21 16:06:53 -04:00
_sub_menu_pos = ( _sub_menu_pos + 1 ) % get_sub_menu_length ( _top_menu_pos ) ;
2019-11-09 09:08:41 -04:00
} else {
// in the top-menu, keep track of the selected position
2019-12-21 16:06:53 -04:00
_top_menu_pos = ( _top_menu_pos + 1 ) % get_top_menu_length ( ) ;
2019-11-09 09:08:41 -04:00
}
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 - - ;
2020-03-29 14:09:22 -03:00
_sub_menu_pos = 0 ;
2020-02-02 13:15:57 -04:00
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_CHANGE_MODE , _mode_delay_ms ) ; // move up/out a menu
2019-11-09 09:08:41 -04:00
}
// no longer in the menu so trigger the OSD re-enablement
if ( _in_menu = = 0 ) {
2020-02-02 13:15:57 -04:00
_in_menu = _menu_enter_level ;
2019-11-09 09:08:41 -04:00
_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 ;
}
}
// exit the 2 key OSD menu
void AP_RunCam : : exit_2_key_osd_menu ( )
{
2020-02-02 13:15:57 -04:00
_in_menu = _menu_enter_level ;
2019-11-09 09:08:41 -04:00
// turn built-in OSD back on
enable_osd ( ) ;
2020-02-02 13:15:57 -04:00
if ( _video_recording = = VideoOption : : RECORDING & & has_feature ( Feature : : RCDEVICE_PROTOCOL_FEATURE_START_RECORDING ) ) {
simulate_camera_button ( ControlOperation : : RCDEVICE_PROTOCOL_CHANGE_START_RECORDING , _mode_delay_ms ) ;
2019-12-22 10:49:44 -04:00
_state = State : : VIDEO_RECORDING ;
2020-02-02 13:15:57 -04:00
} else {
2019-12-22 10:49:44 -04:00
_state = State : : READY ;
2019-11-09 09:08:41 -04:00
}
}
// run the 5-key OSD simulation process
void AP_RunCam : : handle_5_key_simulation_process ( Event ev )
{
2020-02-02 13:15:57 -04:00
debug ( " %s,M:%d,B:%d,O:%d \n " , event_names [ int ( ev ) ] , _in_menu , int ( _button_pressed ) , int ( _osd_option ) ) ;
2019-11-09 09:08:41 -04:00
switch ( ev ) {
case Event : : BUTTON_RELEASE :
send_5_key_OSD_cable_simulation_event ( ev ) ;
2020-02-02 13:15:57 -04:00
break ;
2019-11-09 09:08:41 -04:00
case Event : : ENTER_MENU :
2020-02-02 13:15:57 -04:00
if ( _in_menu = = 0 ) {
2019-11-09 09:08:41 -04:00
// turn off built-in OSD so that the runcam OSD is visible
disable_osd ( ) ;
2020-02-02 13:15:57 -04:00
send_5_key_OSD_cable_simulation_event ( ev ) ;
_in_menu = 1 ;
} else {
send_5_key_OSD_cable_simulation_event ( Event : : IN_MENU_ENTER ) ;
2019-11-09 09:08:41 -04:00
}
break ;
case Event : : EXIT_MENU :
if ( _in_menu > 0 ) {
// turn built-in OSD back on
enable_osd ( ) ;
2020-02-02 13:15:57 -04:00
send_5_key_OSD_cable_simulation_event ( Event : : EXIT_MENU ) ;
_in_menu = 0 ;
2019-11-09 09:08:41 -04:00
}
break ;
case Event : : NONE :
2020-02-02 13:15:57 -04:00
break ;
case Event : : IN_MENU_EXIT :
2019-11-09 09:08:41 -04:00
case Event : : IN_MENU_RIGHT :
2020-02-02 13:15:57 -04:00
case Event : : IN_MENU_ENTER :
2019-11-09 09:08:41 -04:00
case Event : : IN_MENU_UP :
case Event : : IN_MENU_DOWN :
case Event : : START_RECORDING :
case Event : : STOP_RECORDING :
send_5_key_OSD_cable_simulation_event ( ev ) ;
2020-02-02 13:15:57 -04:00
break ;
2019-11-09 09:08:41 -04:00
}
}
// 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 ) ;
2020-02-02 13:15:57 -04:00
_button_pressed = ButtonState : : NONE ;
2019-11-09 09:08:41 -04:00
_waiting_device_response = false ;
return ;
}
switch ( request . _command ) {
case Command : : RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE :
2020-02-02 13:15:57 -04:00
_button_pressed = ButtonState : : NONE ;
2019-11-09 09:08:41 -04:00
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
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 ) {
2020-02-02 13:15:57 -04:00
_state = State : : IN_MENU ;
2019-11-09 09:08:41 -04:00
}
break ;
case ConnectionOperation : : RCDEVICE_PROTOCOL_5KEY_FUNCTION_CLOSE :
if ( errorCode > 0 ) {
2020-02-02 13:15:57 -04:00
_state = State : : READY ;
2019-11-09 09:08:41 -04:00
}
break ;
}
break ;
}
case Command : : RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS :
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 ;
2020-02-02 13:15:57 -04:00
debug ( " received response for command %d \n " , int ( _pending_request . _command ) ) ;
2019-11-09 09:08:41 -04:00
_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 ;
}
2020-05-22 21:24:32 -03:00
uart - > discard_input ( ) ;
2019-11-09 09:08:41 -04:00
}
// 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
2020-02-02 13:15:57 -04:00
void AP_RunCam : : send_5_key_OSD_cable_simulation_event ( const Event key , const uint32_t transition_timeout )
2019-11-09 09:08:41 -04:00
{
2020-02-02 13:15:57 -04:00
debug ( " OSD cable simulation event %s \n " , event_names [ int ( key ) ] ) ;
_waiting_device_response = true ;
// although we can control press/release, this causes the state machine to behave in the same way
// as the 2-key process
_transition_timeout_ms = transition_timeout ;
2019-11-09 09:08:41 -04:00
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 ;
}
2020-02-02 13:15:57 -04:00
_button_pressed = ButtonState : : PRESSED ;
2019-11-09 09:08:41 -04:00
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 )
{
2020-02-02 13:15:57 -04:00
_button_pressed = ButtonState : : RELEASED ;
2019-11-09 09:08:41 -04:00
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 ) ;
2020-02-02 13:15:57 -04:00
uart - > flush ( ) ;
2019-11-09 09:08:41 -04:00
}
// 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 ] ;
2019-12-28 15:15:46 -04:00
if ( ! has_feature ( Feature : : FEATURES_OVERRIDE ) ) {
_features = ( featureHighBits < < 8 ) | featureLowBits ;
}
2020-02-02 13:15:57 -04:00
if ( _features > 0 ) {
_state = State : : INITIALIZED ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " RunCam initialized, features 0x%04X, %d-key OSD \n " , _features . get ( ) ,
has_feature ( Feature : : RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE ) ? 5 :
( 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 ) ) ? 2 : 0 ) ;
} else {
// nothing as as nothing does
gcs ( ) . send_text ( MAV_SEVERITY_WARNING , " RunCam device not found \n " ) ;
}
debug ( " RunCam: initialized state: video: %d, osd: %d, cam: %d \n " , int ( _video_recording ) , int ( _osd_option ) , int ( _cam_control_option ) ) ;
2019-11-09 09:08:41 -04:00
}
// 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 ) ,
2020-02-02 13:15:57 -04:00
_recv_response_length ( 0 ) ,
2019-11-09 09:08:41 -04:00
_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