Merge pull request #4251 from allenfor2017/add-runcam-device-protocol
Add RunCam Device Protocol Support(DisplayPort, DeviceSetting Access,…
This commit is contained in:
commit
8d1a90b999
|
@ -58,7 +58,6 @@ COMMON_SRC = \
|
|||
io/serial.c \
|
||||
io/statusindicator.c \
|
||||
io/transponder_ir.c \
|
||||
io/rcsplit.c \
|
||||
msp/msp_serial.c \
|
||||
scheduler/scheduler.c \
|
||||
sensors/battery.c \
|
||||
|
@ -146,6 +145,10 @@ FC_SRC = \
|
|||
io/displayport_max7456.c \
|
||||
io/displayport_msp.c \
|
||||
io/displayport_oled.c \
|
||||
io/displayport_rcdevice.c \
|
||||
io/rcdevice_cam.c \
|
||||
io/rcdevice.c \
|
||||
io/rcdevice_osd.c \
|
||||
io/gps.c \
|
||||
io/ledstrip.c \
|
||||
io/osd.c \
|
||||
|
|
|
@ -61,6 +61,7 @@
|
|||
|
||||
// For VISIBLE*
|
||||
#include "io/osd.h"
|
||||
#include "io/rcdevice_cam.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
|
@ -887,6 +888,12 @@ uint16_t cmsHandleKeyWithRepeat(displayPort_t *pDisplay, uint8_t key, int repeat
|
|||
|
||||
void cmsUpdate(uint32_t currentTimeUs)
|
||||
{
|
||||
#ifdef USE_RCDEVICE
|
||||
if(rcdeviceInMenu) {
|
||||
return ;
|
||||
}
|
||||
#endif
|
||||
|
||||
static int16_t rcDelayMs = BUTTON_TIME;
|
||||
static int holdCount = 1;
|
||||
static int repeatCount = 1;
|
||||
|
@ -1000,6 +1007,8 @@ void cmsHandler(timeUs_t currentTimeUs)
|
|||
if (cmsDeviceCount < 0)
|
||||
return;
|
||||
|
||||
|
||||
|
||||
static timeUs_t lastCalledUs = 0;
|
||||
|
||||
if (currentTimeUs >= lastCalledUs + CMS_UPDATE_INTERVAL_US) {
|
||||
|
|
|
@ -86,6 +86,7 @@
|
|||
|
||||
#include "io/beeper.h"
|
||||
#include "io/displayport_max7456.h"
|
||||
#include "io/displayport_rcdevice.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/flashfs.h"
|
||||
#include "io/gps.h"
|
||||
|
@ -126,7 +127,7 @@
|
|||
#include "flight/pid.h"
|
||||
#include "flight/servos.h"
|
||||
|
||||
#include "io/rcsplit.h"
|
||||
#include "io/rcdevice_cam.h"
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
|
@ -717,9 +718,9 @@ void init(void)
|
|||
LED2_ON;
|
||||
#endif
|
||||
|
||||
#ifdef USE_RCSPLIT
|
||||
rcSplitInit();
|
||||
#endif // USE_RCSPLIT
|
||||
#ifdef USE_RCDEVICE
|
||||
rcdeviceInit();
|
||||
#endif // USE_RCDEVICE
|
||||
|
||||
// Latch active features AGAIN since some may be modified by init().
|
||||
latchActiveFeatures();
|
||||
|
@ -730,4 +731,4 @@ void init(void)
|
|||
fcTasksInit();
|
||||
|
||||
systemState |= SYSTEM_STATE_READY;
|
||||
}
|
||||
}
|
|
@ -249,7 +249,7 @@ void initActiveBoxIds(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_RCSPLIT
|
||||
#ifdef USE_RCDEVICE
|
||||
BME(BOXCAMERA1);
|
||||
BME(BOXCAMERA2);
|
||||
BME(BOXCAMERA3);
|
||||
|
|
|
@ -59,10 +59,10 @@
|
|||
#include "io/ledstrip.h"
|
||||
#include "io/osd.h"
|
||||
#include "io/osd_slave.h"
|
||||
#include "io/rcsplit.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/transponder_ir.h"
|
||||
#include "io/vtx_tramp.h" // Will be gone
|
||||
#include "io/rcdevice_cam.h"
|
||||
|
||||
#include "msp/msp_serial.h"
|
||||
|
||||
|
@ -81,7 +81,6 @@
|
|||
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
|
||||
#ifdef USE_BST
|
||||
void taskBstMasterProcess(timeUs_t currentTimeUs);
|
||||
#endif
|
||||
|
@ -342,8 +341,8 @@ void fcTasksInit(void)
|
|||
#ifdef USE_CAMERA_CONTROL
|
||||
setTaskEnabled(TASK_CAMCTRL, true);
|
||||
#endif
|
||||
#ifdef USE_RCSPLIT
|
||||
setTaskEnabled(TASK_RCSPLIT, rcSplitIsEnabled());
|
||||
#ifdef USE_RCDEVICE
|
||||
setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
@ -581,10 +580,10 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
},
|
||||
#endif
|
||||
|
||||
#ifdef USE_RCSPLIT
|
||||
[TASK_RCSPLIT] = {
|
||||
.taskName = "RCSPLIT",
|
||||
.taskFunc = rcSplitUpdate,
|
||||
#ifdef USE_RCDEVICE
|
||||
[TASK_RCDEVICE] = {
|
||||
.taskName = "RCDEVICE",
|
||||
.taskFunc = rcdeviceUpdate,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz, 100ms
|
||||
.staticPriority = TASK_PRIORITY_MEDIUM,
|
||||
},
|
||||
|
|
|
@ -155,6 +155,16 @@ static const uint8_t beep_gyroCalibrated[] = {
|
|||
20, 10, 20, 10, 20, 10, BEEPER_COMMAND_STOP
|
||||
};
|
||||
|
||||
// Cam connection opened
|
||||
static const uint8_t beep_camOpenBeep[] = {
|
||||
5, 15, 10, 15, 20, BEEPER_COMMAND_STOP
|
||||
};
|
||||
|
||||
// Cam connection close
|
||||
static const uint8_t beep_camCloseBeep[] = {
|
||||
10, 8, 5, BEEPER_COMMAND_STOP
|
||||
};
|
||||
|
||||
// array used for variable # of beeps (reporting GPS sat count, etc)
|
||||
static uint8_t beep_multiBeeps[MAX_MULTI_BEEPS + 1];
|
||||
|
||||
|
@ -213,8 +223,10 @@ typedef struct beeperTableEntry_s {
|
|||
{ BEEPER_ENTRY(BEEPER_USB, 17, NULL, "ON_USB") },
|
||||
{ BEEPER_ENTRY(BEEPER_BLACKBOX_ERASE, 18, beep_2shortBeeps, "BLACKBOX_ERASE") },
|
||||
{ BEEPER_ENTRY(BEEPER_CRASH_FLIP_MODE, 19, beep_2longerBeeps, "CRASH FLIP") },
|
||||
{ BEEPER_ENTRY(BEEPER_ALL, 20, NULL, "ALL") },
|
||||
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 21, NULL, "PREFERRED") },
|
||||
{ BEEPER_ENTRY(BEEPER_CAM_CONNECTION_OPEN, 20, beep_camOpenBeep, "CAM_CONNECTION_OPEN") },
|
||||
{ BEEPER_ENTRY(BEEPER_CAM_CONNECTION_CLOSE, 21, beep_camCloseBeep, "CAM_CONNECTION_CLOSED") },
|
||||
{ BEEPER_ENTRY(BEEPER_ALL, 22, NULL, "ALL") },
|
||||
{ BEEPER_ENTRY(BEEPER_PREFERENCE, 23, NULL, "PREFERRED") },
|
||||
};
|
||||
|
||||
static const beeperTableEntry_t *currentBeeperEntry = NULL;
|
||||
|
|
|
@ -44,8 +44,10 @@ typedef enum {
|
|||
BEEPER_USB, // Some boards have beeper powered USB connected
|
||||
BEEPER_BLACKBOX_ERASE, // Beep when blackbox erase completes
|
||||
BEEPER_CRASH_FLIP_MODE, // Crash flip mode is active
|
||||
BEEPER_CAM_CONNECTION_OPEN, // When the 5 key simulation stated
|
||||
BEEPER_CAM_CONNECTION_CLOSE, // When the 5 key simulation stop
|
||||
BEEPER_ALL, // Turn ON or OFF all beeper conditions
|
||||
BEEPER_PREFERENCE // Save preferred beeper configuration
|
||||
BEEPER_PREFERENCE, // Save preferred beeper configuration
|
||||
// BEEPER_ALL and BEEPER_PREFERENCE must remain at the bottom of this enum
|
||||
} beeperMode_e;
|
||||
|
||||
|
|
|
@ -0,0 +1,58 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "drivers/display.h"
|
||||
#include "drivers/vcd.h"
|
||||
|
||||
#include "io/rcdevice.h"
|
||||
#include "io/rcdevice_osd.h"
|
||||
|
||||
#include "io/displayport_rcdevice.h"
|
||||
|
||||
#ifdef USE_RCDEVICE
|
||||
|
||||
displayPort_t rcdeviceOSDDisplayPort;
|
||||
|
||||
static const displayPortVTable_t rcdeviceOSDVTable = {
|
||||
.grab = rcdeviceOSDGrab,
|
||||
.release = rcdeviceOSDRelease,
|
||||
.clearScreen = rcdeviceOSDClearScreen,
|
||||
.drawScreen = rcdeviceOSDDrawScreen,
|
||||
.writeString = rcdeviceOSDWriteString,
|
||||
.writeChar = rcdeviceOSDWriteChar,
|
||||
.isTransferInProgress = rcdeviceOSDIsTransferInProgress,
|
||||
.heartbeat = rcdeviceOSDHeartbeat,
|
||||
.resync = rcdeviceOSDResync,
|
||||
.txBytesFree = rcdeviceOSDTxBytesFree,
|
||||
.screenSize = rcdeviceScreenSize,
|
||||
};
|
||||
|
||||
displayPort_t *rcdeviceDisplayPortInit(const vcdProfile_t *vcdProfile)
|
||||
{
|
||||
if (rcdeviceOSDInit(vcdProfile)) {
|
||||
displayInit(&rcdeviceOSDDisplayPort, &rcdeviceOSDVTable);
|
||||
rcdeviceOSDResync(&rcdeviceOSDDisplayPort);
|
||||
return &rcdeviceOSDDisplayPort;
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // defined(USE_RCDEVICE)
|
|
@ -0,0 +1,21 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
struct vcdProfile_s;
|
||||
displayPort_t *rcdeviceDisplayPortInit(const struct vcdProfile_s *vcdProfile);
|
|
@ -0,0 +1,674 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "common/crc.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/streambuf.h"
|
||||
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
|
||||
#include "rcdevice.h"
|
||||
|
||||
#ifdef USE_RCDEVICE
|
||||
|
||||
typedef enum {
|
||||
RCDP_SETTING_PARSE_WAITING_ID,
|
||||
RCDP_SETTING_PARSE_WAITING_NAME,
|
||||
RCDP_SETTING_PARSE_WAITING_VALUE,
|
||||
} runcamDeviceSettingParseStep_e;
|
||||
|
||||
typedef struct runcamDeviceExpectedResponseLength_s {
|
||||
uint8_t command;
|
||||
uint8_t reponseLength;
|
||||
} runcamDeviceExpectedResponseLength_t;
|
||||
|
||||
static runcamDeviceExpectedResponseLength_t expectedResponsesLength[] = {
|
||||
{ RCDEVICE_PROTOCOL_COMMAND_GET_SETTINGS, 0xFF},
|
||||
{ RCDEVICE_PROTOCOL_COMMAND_READ_SETTING_DETAIL, 0xFF},
|
||||
{ RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, 5},
|
||||
{ RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, 2},
|
||||
{ RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, 2},
|
||||
{ RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, 3},
|
||||
{ RCDEVICE_PROTOCOL_COMMAND_WRITE_SETTING, 4},
|
||||
};
|
||||
|
||||
static uint8_t runcamDeviceGetResponseLength(uint8_t command)
|
||||
{
|
||||
for (unsigned int i = 0; i < ARRAYLEN(expectedResponsesLength); i++) {
|
||||
if (expectedResponsesLength[i].command == command) {
|
||||
return expectedResponsesLength[i].reponseLength;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Verify the response data has received done, return true if the data is still receiving or it was received done.
|
||||
// return false if the packet has incorrect
|
||||
static uint8_t runcamDeviceIsResponseReceiveDone(uint8_t command, uint8_t *data, uint8_t dataLen, bool *isDone)
|
||||
{
|
||||
uint8_t expectedResponseDataLength = runcamDeviceGetResponseLength(command);
|
||||
if (expectedResponseDataLength == 0xFF) {
|
||||
uint8_t settingDataLength = 0x00;
|
||||
// get setting datalen first
|
||||
if (dataLen >= 3) {
|
||||
settingDataLength = data[2];
|
||||
if (dataLen >= (settingDataLength + 4)) {
|
||||
*isDone = true;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
if (settingDataLength > 60) {
|
||||
return false;
|
||||
}
|
||||
} else if (dataLen >= expectedResponseDataLength) {
|
||||
*isDone = true;
|
||||
return true;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// a common way to receive packet and verify it
|
||||
static uint8_t runcamDeviceReceivePacket(runcamDevice_t *device, uint8_t command, uint8_t *data, int timeoutms)
|
||||
{
|
||||
uint8_t dataPos = 0;
|
||||
uint8_t crc = 0;
|
||||
uint8_t responseDataLen = 0;
|
||||
|
||||
// wait for reply until timeout(specialy by timeoutms)
|
||||
timeMs_t timeout = millis() + timeoutms;
|
||||
bool isWaitingHeader = true;
|
||||
while (millis() < timeout) {
|
||||
if (serialRxBytesWaiting(device->serialPort) > 0) {
|
||||
uint8_t c = serialRead(device->serialPort);
|
||||
crc = crc8_dvb_s2(crc, c);
|
||||
|
||||
if (data) {
|
||||
data[dataPos] = c;
|
||||
}
|
||||
dataPos++;
|
||||
|
||||
if (isWaitingHeader) {
|
||||
if (c == RCDEVICE_PROTOCOL_HEADER) {
|
||||
isWaitingHeader = false;
|
||||
}
|
||||
} else {
|
||||
bool isDone = false;
|
||||
if (!runcamDeviceIsResponseReceiveDone(command, data, dataPos, &isDone)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (isDone) {
|
||||
responseDataLen = dataPos;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// check crc
|
||||
if (crc != 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return responseDataLen;
|
||||
}
|
||||
|
||||
// every time send packet to device, and want to get something from device,
|
||||
// it'd better call the method to clear the rx buffer before the packet send,
|
||||
// else may be the useless data in rx buffer will cause the response decoding
|
||||
// failed.
|
||||
static void runcamDeviceFlushRxBuffer(runcamDevice_t *device)
|
||||
{
|
||||
while (serialRxBytesWaiting(device->serialPort) > 0) {
|
||||
serialRead(device->serialPort);
|
||||
}
|
||||
}
|
||||
|
||||
// a common way to send packet to device
|
||||
static void runcamDeviceSendPacket(runcamDevice_t *device, uint8_t command, uint8_t *paramData, int paramDataLen)
|
||||
{
|
||||
// is this device open?
|
||||
if (!device->serialPort) {
|
||||
return;
|
||||
}
|
||||
|
||||
sbuf_t buf;
|
||||
// prepare pointer
|
||||
buf.ptr = device->buffer;
|
||||
buf.end = ARRAYEND(device->buffer);
|
||||
|
||||
sbufWriteU8(&buf, RCDEVICE_PROTOCOL_HEADER);
|
||||
sbufWriteU8(&buf, command);
|
||||
|
||||
if (paramData) {
|
||||
sbufWriteData(&buf, paramData, paramDataLen);
|
||||
}
|
||||
|
||||
// add crc over (all) data
|
||||
crc8_dvb_s2_sbuf_append(&buf, device->buffer);
|
||||
|
||||
// switch to reader
|
||||
sbufSwitchToReader(&buf, device->buffer);
|
||||
|
||||
// send data if possible
|
||||
serialWriteBuf(device->serialPort, sbufPtr(&buf), sbufBytesRemaining(&buf));
|
||||
}
|
||||
|
||||
// a common way to send a packet to device, and get response from the device.
|
||||
static bool runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_t commandID, uint8_t *paramData, uint8_t paramDataLen, uint8_t *outputBuffer, uint8_t *outputBufferLen)
|
||||
{
|
||||
int max_retries = 1;
|
||||
// here using 1000ms as timeout, because the response from 5 key simulation command need a long time about >= 600ms,
|
||||
// so set a max value to ensure we can receive the response
|
||||
int timeoutMs = 1000;
|
||||
|
||||
// only the command sending on initializing step need retry logic,
|
||||
// otherwise, the timeout of 1000 ms is enough for the response from device
|
||||
if (commandID == RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO) {
|
||||
max_retries = 3;
|
||||
timeoutMs = 100; // we have test some device, 100ms as timeout, and retry times be 3, it's stable for most case
|
||||
}
|
||||
|
||||
while (max_retries--) {
|
||||
// flush rx buffer
|
||||
runcamDeviceFlushRxBuffer(device);
|
||||
|
||||
// send packet
|
||||
runcamDeviceSendPacket(device, commandID, paramData, paramDataLen);
|
||||
|
||||
// waiting response
|
||||
uint8_t responseLength = runcamDeviceReceivePacket(device, commandID, outputBuffer, timeoutMs);
|
||||
if (responseLength) {
|
||||
if (outputBufferLen) {
|
||||
*outputBufferLen = responseLength;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// get the device info(firmware version, protocol version and features, see the
|
||||
// definition of runcamDeviceInfo_t to know more)
|
||||
static bool runcamDeviceGetDeviceInfo(runcamDevice_t *device, uint8_t *outputBuffer)
|
||||
{
|
||||
return runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, NULL, 0, outputBuffer, NULL);
|
||||
}
|
||||
|
||||
static bool runcamDeviceSend5KeyOSDCableConnectionEvent(runcamDevice_t *device, uint8_t operation, uint8_t *outActionID, uint8_t *outErrorCode)
|
||||
{
|
||||
uint8_t outputDataLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE;
|
||||
uint8_t respBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE];
|
||||
if (!runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), respBuf, &outputDataLen)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// the high 4 bits is the operationID that we sent
|
||||
// the low 4 bits is the result code
|
||||
uint8_t operationID = (respBuf[1] & 0xF0) >> 4;
|
||||
bool errorCode = (respBuf[1] & 0x0F);
|
||||
if (outActionID) {
|
||||
*outActionID = operationID;
|
||||
}
|
||||
|
||||
if (outErrorCode) {
|
||||
*outErrorCode = errorCode;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// init the runcam device, it'll search the UART port with FUNCTION_RCDEVICE id
|
||||
// this function will delay 400ms in the first loop to wait the device prepared,
|
||||
// as we know, there are has some camera need about 200~400ms to initialization,
|
||||
// and then we can send/receive from it.
|
||||
bool runcamDeviceInit(runcamDevice_t *device)
|
||||
{
|
||||
serialPortFunction_e portID = FUNCTION_RCDEVICE;
|
||||
serialPortConfig_t *portConfig = findSerialPortConfig(portID);
|
||||
if (portConfig != NULL) {
|
||||
device->serialPort = openSerialPort(portConfig->identifier, portID, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
|
||||
|
||||
if (device->serialPort != NULL) {
|
||||
// send RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO to device to retrive
|
||||
// device info, e.g protocol version, supported features
|
||||
uint8_t respBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE];
|
||||
if (runcamDeviceGetDeviceInfo(device, respBuf)) {
|
||||
device->info.protocolVersion = respBuf[1];
|
||||
|
||||
uint8_t featureLowBits = respBuf[2];
|
||||
uint8_t featureHighBits = respBuf[3];
|
||||
device->info.features = (featureHighBits << 8) | featureLowBits;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
closeSerialPort(device->serialPort);
|
||||
}
|
||||
}
|
||||
|
||||
device->serialPort = NULL;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation)
|
||||
{
|
||||
runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL, &operation, sizeof(operation));
|
||||
return true;
|
||||
}
|
||||
|
||||
// every time start to control the OSD menu of camera, must call this method to
|
||||
// camera
|
||||
bool runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device)
|
||||
{
|
||||
uint8_t actionID = 0xFF;
|
||||
uint8_t code = 0xFF;
|
||||
bool r = runcamDeviceSend5KeyOSDCableConnectionEvent(device, RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN, &actionID, &code);
|
||||
return r && (code == 1) && (actionID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN);
|
||||
}
|
||||
|
||||
// when the control was stop, must call this method to the camera to disconnect
|
||||
// with camera.
|
||||
bool runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, uint8_t *resultCode)
|
||||
{
|
||||
uint8_t actionID = 0xFF;
|
||||
uint8_t code = 0xFF;
|
||||
bool r = runcamDeviceSend5KeyOSDCableConnectionEvent(device, RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE, &actionID, &code);
|
||||
if (resultCode) {
|
||||
*resultCode = code;
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
// simulate button press event of 5 key osd cable with special button
|
||||
bool runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation)
|
||||
{
|
||||
if (operation == RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), NULL, NULL)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// simulate button release event of 5 key osd cable
|
||||
bool runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device)
|
||||
{
|
||||
return runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, NULL, NULL);
|
||||
}
|
||||
|
||||
// fill a region with same char on screen, this is used to DisplayPort feature
|
||||
// support
|
||||
void runcamDeviceDispFillRegion(runcamDevice_t *device, uint8_t x, uint8_t y, uint8_t width, uint8_t height, uint8_t c)
|
||||
{
|
||||
uint8_t paramsBuf[5];
|
||||
|
||||
// fill parameters buf
|
||||
paramsBuf[0] = x;
|
||||
paramsBuf[1] = y;
|
||||
paramsBuf[2] = width;
|
||||
paramsBuf[3] = height;
|
||||
paramsBuf[4] = c;
|
||||
|
||||
runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_DISP_FILL_REGION, paramsBuf, sizeof(paramsBuf));
|
||||
}
|
||||
|
||||
// draw a single char on special position on screen, this is used to DisplayPort
|
||||
// feature support
|
||||
void runcamDeviceDispWriteChar(runcamDevice_t *device, uint8_t x, uint8_t y, uint8_t c)
|
||||
{
|
||||
uint8_t paramsBuf[3];
|
||||
|
||||
// fill parameters buf
|
||||
paramsBuf[0] = x;
|
||||
paramsBuf[1] = y;
|
||||
paramsBuf[2] = c;
|
||||
|
||||
runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_CHAR, paramsBuf, sizeof(paramsBuf));
|
||||
}
|
||||
|
||||
static void runcamDeviceDispWriteString(runcamDevice_t *device, uint8_t x, uint8_t y, const char *text, bool isHorizontal)
|
||||
{
|
||||
uint8_t textLen = strlen(text);
|
||||
if (textLen > 60) { // if text len more then 60 chars, cut it to 60
|
||||
textLen = 60;
|
||||
}
|
||||
|
||||
uint8_t paramsBufLen = 3 + textLen;
|
||||
uint8_t paramsBuf[RCDEVICE_PROTOCOL_MAX_DATA_SIZE];
|
||||
|
||||
paramsBuf[0] = paramsBufLen - 1;
|
||||
paramsBuf[1] = x;
|
||||
paramsBuf[2] = y;
|
||||
memcpy(paramsBuf + 3, text, textLen);
|
||||
|
||||
uint8_t command = isHorizontal ? RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_HORIZONTAL_STRING : RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_VERTICAL_STRING;
|
||||
runcamDeviceSendPacket(device, command, paramsBuf, paramsBufLen);
|
||||
}
|
||||
|
||||
// draw a string on special position on screen, this is used to DisplayPort
|
||||
// feature support
|
||||
void runcamDeviceDispWriteHorizontalString(runcamDevice_t *device, uint8_t x, uint8_t y, const char *text)
|
||||
{
|
||||
runcamDeviceDispWriteString(device, x, y, text, true);
|
||||
}
|
||||
|
||||
void runcamDeviceDispWriteVerticalString(runcamDevice_t *device, uint8_t x, uint8_t y, const char *text)
|
||||
{
|
||||
runcamDeviceDispWriteString(device, x, y, text, false);
|
||||
}
|
||||
|
||||
void runcamDeviceDispWriteChars(runcamDevice_t *device, uint8_t *data, uint8_t datalen)
|
||||
{
|
||||
uint8_t adjustedDataLen = datalen;
|
||||
if (adjustedDataLen > 60) { // if data len more then 60 chars, cut it to 60
|
||||
adjustedDataLen = 60;
|
||||
}
|
||||
|
||||
uint8_t paramsBufLen = adjustedDataLen + 1;
|
||||
uint8_t paramsBuf[RCDEVICE_PROTOCOL_MAX_DATA_SIZE];
|
||||
|
||||
paramsBuf[0] = adjustedDataLen;
|
||||
memcpy(paramsBuf + 1, data, adjustedDataLen);
|
||||
|
||||
runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_CHARS, paramsBuf, paramsBufLen);
|
||||
}
|
||||
|
||||
static bool runcamDeviceDecodeSettings(sbuf_t *buf, runcamDeviceSetting_t *outSettingList, int maxSettingItemCount)
|
||||
{
|
||||
if (outSettingList == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (maxSettingItemCount > RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE)
|
||||
maxSettingItemCount = RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE;
|
||||
|
||||
runcamDeviceSettingParseStep_e parseStep = RCDP_SETTING_PARSE_WAITING_ID;
|
||||
memset(outSettingList, 0, maxSettingItemCount * sizeof(runcamDeviceSetting_t));
|
||||
runcamDeviceSetting_t *settingIterator = outSettingList;
|
||||
while (sbufBytesRemaining(buf) > 0) {
|
||||
if (settingIterator >= (outSettingList + maxSettingItemCount)) {
|
||||
break;
|
||||
}
|
||||
|
||||
switch (parseStep) {
|
||||
case RCDP_SETTING_PARSE_WAITING_ID: {
|
||||
settingIterator->id = sbufReadU8(buf);
|
||||
parseStep = RCDP_SETTING_PARSE_WAITING_NAME;
|
||||
}
|
||||
break;
|
||||
case RCDP_SETTING_PARSE_WAITING_NAME: {
|
||||
const char *str = (const char *)sbufConstPtr(buf);
|
||||
uint8_t nameLen = strlen(str) + 1;
|
||||
memset(settingIterator->name, 0, RCDEVICE_PROTOCOL_MAX_SETTING_NAME_LENGTH);
|
||||
strncpy(settingIterator->name, str, RCDEVICE_PROTOCOL_MAX_SETTING_NAME_LENGTH);
|
||||
sbufAdvance(buf, nameLen);
|
||||
|
||||
parseStep = RCDP_SETTING_PARSE_WAITING_VALUE;
|
||||
}
|
||||
break;
|
||||
case RCDP_SETTING_PARSE_WAITING_VALUE: {
|
||||
const char *str = (const char *)sbufConstPtr(buf);
|
||||
uint8_t valueLen = strlen(str) + 1;
|
||||
memset(settingIterator->value, 0, RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH);
|
||||
strcpy(settingIterator->value, str);
|
||||
sbufAdvance(buf, valueLen);
|
||||
parseStep = RCDP_SETTING_PARSE_WAITING_ID;
|
||||
|
||||
settingIterator++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (RCDP_SETTING_PARSE_WAITING_ID != parseStep) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool runcamDeviceGetResponseWithMultipleChunk(runcamDevice_t *device, uint8_t command, uint8_t settingID, uint8_t *responseData, uint16_t *responseDatalen)
|
||||
{
|
||||
if (responseData == NULL || responseDatalen == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// fill parameters buf
|
||||
uint8_t paramsBuf[2];
|
||||
uint8_t chunkIndex = 0;
|
||||
paramsBuf[0] = settingID; // parent setting id
|
||||
paramsBuf[1] = chunkIndex; // chunk index
|
||||
|
||||
uint8_t outputBufLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE;
|
||||
uint8_t outputBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE];
|
||||
bool result = runcamDeviceSendRequestAndWaitingResp(device, command, paramsBuf, sizeof(paramsBuf), outputBuf, &outputBufLen);
|
||||
if (!result) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t remainingChunk = outputBuf[1];
|
||||
// Every response chunk count must less than or equal to RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE
|
||||
if (remainingChunk >= RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// save setting data to sbuf_t object
|
||||
const uint16_t maxDataLen = RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE * RCDEVICE_PROTOCOL_MAX_DATA_SIZE;
|
||||
// uint8_t data[maxDataLen];
|
||||
sbuf_t dataBuf;
|
||||
dataBuf.ptr = responseData;
|
||||
dataBuf.end = responseData + maxDataLen;
|
||||
sbufWriteData(&dataBuf, outputBuf + 3, outputBufLen - 4);
|
||||
|
||||
// get the remaining chunks
|
||||
while (remainingChunk > 0) {
|
||||
paramsBuf[1] = ++chunkIndex; // chunk index
|
||||
|
||||
outputBufLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE;
|
||||
result = runcamDeviceSendRequestAndWaitingResp(device, command, paramsBuf, sizeof(paramsBuf), outputBuf, &outputBufLen);
|
||||
|
||||
if (!result) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// append the trailing chunk to the sbuf_t object,
|
||||
// but only append the actually setting data
|
||||
sbufWriteData(&dataBuf, outputBuf + 3, outputBufLen - 4);
|
||||
|
||||
remainingChunk--;
|
||||
}
|
||||
|
||||
sbufSwitchToReader(&dataBuf, responseData);
|
||||
*responseDatalen = sbufBytesRemaining(&dataBuf);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// get settings with parent setting id, the type of parent setting must be a
|
||||
// FOLDER after this function called, the settings will fill into outSettingList
|
||||
// argument
|
||||
bool runcamDeviceGetSettings(runcamDevice_t *device, uint8_t parentSettingID, runcamDeviceSetting_t *outSettingList, int maxSettingItemCount)
|
||||
{
|
||||
if (outSettingList == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t responseDataLength = 0;
|
||||
uint8_t data[RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE * RCDEVICE_PROTOCOL_MAX_DATA_SIZE];
|
||||
if (!runcamDeviceGetResponseWithMultipleChunk(device, RCDEVICE_PROTOCOL_COMMAND_GET_SETTINGS, parentSettingID, data, &responseDataLength)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
sbuf_t dataBuf;
|
||||
dataBuf.ptr = data;
|
||||
dataBuf.end = data + responseDataLength;
|
||||
|
||||
// parse the settings data and convert them into a runcamDeviceSetting_t list
|
||||
if (!runcamDeviceDecodeSettings(&dataBuf, outSettingList, maxSettingItemCount)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool runcamDeviceDecodeSettingDetail(sbuf_t *buf, runcamDeviceSettingDetail_t *outSettingDetail)
|
||||
{
|
||||
if (outSettingDetail == NULL || sbufBytesRemaining(buf) == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
rcdeviceSettingType_e settingType = sbufReadU8(buf);
|
||||
outSettingDetail->type = settingType;
|
||||
switch (settingType) {
|
||||
case RCDEVICE_PROTOCOL_SETTINGTYPE_UINT8:
|
||||
case RCDEVICE_PROTOCOL_SETTINGTYPE_INT8:
|
||||
outSettingDetail->value = sbufReadU8(buf);
|
||||
outSettingDetail->minValue = sbufReadU8(buf);
|
||||
outSettingDetail->maxValue = sbufReadU8(buf);
|
||||
outSettingDetail->stepSize = sbufReadU8(buf);
|
||||
break;
|
||||
case RCDEVICE_PROTOCOL_SETTINGTYPE_UINT16:
|
||||
case RCDEVICE_PROTOCOL_SETTINGTYPE_INT16:
|
||||
outSettingDetail->value = sbufReadU16(buf);
|
||||
outSettingDetail->minValue = sbufReadU16(buf);
|
||||
outSettingDetail->maxValue = sbufReadU16(buf);
|
||||
outSettingDetail->stepSize = sbufReadU8(buf);
|
||||
break;
|
||||
case RCDEVICE_PROTOCOL_SETTINGTYPE_FLOAT:
|
||||
outSettingDetail->value = sbufReadU32(buf);
|
||||
outSettingDetail->minValue = sbufReadU32(buf);
|
||||
outSettingDetail->maxValue = sbufReadU32(buf);
|
||||
outSettingDetail->decimalPoint = sbufReadU8(buf);
|
||||
outSettingDetail->stepSize = sbufReadU32(buf);
|
||||
break;
|
||||
case RCDEVICE_PROTOCOL_SETTINGTYPE_TEXT_SELECTION: {
|
||||
outSettingDetail->value = sbufReadU8(buf);
|
||||
|
||||
const char *tmp = (const char *)sbufConstPtr(buf);
|
||||
const uint16_t maxLen = RCDEVICE_PROTOCOL_MAX_DATA_SIZE * RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS;
|
||||
char textSels[maxLen];
|
||||
memset(textSels, 0, maxLen);
|
||||
strncpy(textSels, tmp, maxLen);
|
||||
char delims[] = ";";
|
||||
char *result = strtok(textSels, delims);
|
||||
int i = 0;
|
||||
runcamDeviceSettingTextSelection_t *iterator = outSettingDetail->textSelections;
|
||||
while (result != NULL) {
|
||||
if (i >= RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS) {
|
||||
break;
|
||||
}
|
||||
|
||||
memset(iterator->text, 0, RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH);
|
||||
strncpy(iterator->text, result, RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH);
|
||||
iterator++;
|
||||
result = strtok(NULL, delims);
|
||||
i++;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case RCDEVICE_PROTOCOL_SETTINGTYPE_STRING: {
|
||||
const char *tmp = (const char *)sbufConstPtr(buf);
|
||||
strncpy(outSettingDetail->stringValue, tmp, RCDEVICE_PROTOCOL_MAX_STRING_LENGTH);
|
||||
sbufAdvance(buf, strlen(tmp) + 1);
|
||||
|
||||
outSettingDetail->maxStringSize = sbufReadU8(buf);
|
||||
}
|
||||
break;
|
||||
case RCDEVICE_PROTOCOL_SETTINGTYPE_FOLDER:
|
||||
break;
|
||||
case RCDEVICE_PROTOCOL_SETTINGTYPE_INFO: {
|
||||
const char *tmp = (const char *)sbufConstPtr(buf);
|
||||
strncpy(outSettingDetail->stringValue, tmp, RCDEVICE_PROTOCOL_MAX_STRING_LENGTH);
|
||||
sbufAdvance(buf, strlen(outSettingDetail->stringValue) + 1);
|
||||
}
|
||||
break;
|
||||
case RCDEVICE_PROTOCOL_SETTINGTYPE_UNKNOWN:
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// get the setting details with setting id
|
||||
// after this function called, the setting detail will fill into
|
||||
// outSettingDetail argument
|
||||
bool runcamDeviceGetSettingDetail(runcamDevice_t *device, uint8_t settingID, runcamDeviceSettingDetail_t *outSettingDetail)
|
||||
{
|
||||
if (outSettingDetail == NULL)
|
||||
return false;
|
||||
|
||||
uint16_t responseDataLength = 0;
|
||||
uint8_t data[RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE * RCDEVICE_PROTOCOL_MAX_DATA_SIZE];
|
||||
if (!runcamDeviceGetResponseWithMultipleChunk(device, RCDEVICE_PROTOCOL_COMMAND_READ_SETTING_DETAIL, settingID, data, &responseDataLength)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
sbuf_t dataBuf;
|
||||
dataBuf.ptr = data;
|
||||
dataBuf.end = data + responseDataLength;
|
||||
|
||||
// parse the settings data and convert them into a runcamDeviceSettingDetail_t
|
||||
if (!runcamDeviceDecodeSettingDetail(&dataBuf, outSettingDetail)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// write new value with to the setting
|
||||
bool runcamDeviceWriteSetting(runcamDevice_t *device, uint8_t settingID, uint8_t *paramData, uint8_t paramDataLen, runcamDeviceWriteSettingResponse_t *response)
|
||||
{
|
||||
if (response == NULL || paramDataLen > (RCDEVICE_PROTOCOL_MAX_DATA_SIZE - 1)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
memset(response, 0, sizeof(runcamDeviceWriteSettingResponse_t));
|
||||
response->resultCode = 1; // initialize the result code to failed
|
||||
|
||||
uint8_t paramsBufLen = sizeof(uint8_t) + paramDataLen;
|
||||
uint8_t paramsBuf[RCDEVICE_PROTOCOL_MAX_DATA_SIZE];
|
||||
paramsBuf[0] = settingID;
|
||||
memcpy(paramsBuf + 1, paramData, paramDataLen);
|
||||
|
||||
uint8_t outputBufLen = RCDEVICE_PROTOCOL_MAX_PACKET_SIZE;
|
||||
uint8_t outputBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE];
|
||||
bool result = runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_WRITE_SETTING, paramsBuf, paramsBufLen, outputBuf, &outputBufLen);
|
||||
if (!result) {
|
||||
return false;
|
||||
}
|
||||
|
||||
response->resultCode = outputBuf[1];
|
||||
response->needUpdateMenuItems = outputBuf[2];
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
|
@ -0,0 +1,239 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "drivers/serial.h"
|
||||
|
||||
//
|
||||
// The protocol for Runcam Device definition
|
||||
//
|
||||
#define RCDEVICE_PROTOCOL_HEADER 0xCC
|
||||
|
||||
#define RCDEVICE_PROTOCOL_MAX_PACKET_SIZE 64
|
||||
#define RCDEVICE_PROTOCOL_MAX_DATA_SIZE 62
|
||||
#define RCDEVICE_PROTOCOL_MAX_DATA_SIZE_WITH_CRC_FIELD 63
|
||||
#define RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE 32
|
||||
#define RCDEVICE_PROTOCOL_MAX_SETTING_NAME_LENGTH 20
|
||||
#define RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH 20
|
||||
#define RCDEVICE_PROTOCOL_MAX_CHUNK_PER_RESPONSE 12
|
||||
#define RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS 30
|
||||
#define RCDEVICE_PROTOCOL_MAX_STRING_LENGTH 58
|
||||
|
||||
// Commands
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO 0x00
|
||||
// camera control
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL 0x01
|
||||
// 5 key osd cable simulation
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS 0x02
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE 0x03
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION 0x04
|
||||
// device setting access
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_GET_SETTINGS 0x10
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_READ_SETTING_DETAIL 0x11
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_READ_SETTING 0x12
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_WRITE_SETTING 0x13
|
||||
// display port support
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_DISP_FILL_REGION 0x20
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_CHAR 0x21
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_HORIZONTAL_STRING 0x22
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_VERTICAL_STRING 0x23
|
||||
#define RCDEVICE_PROTOCOL_COMMAND_DISP_WRITE_CHARS 0x24
|
||||
|
||||
// Feature Flag sets, it's a uint16_t flag
|
||||
typedef enum {
|
||||
RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON = (1 << 0),
|
||||
RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON = (1 << 1),
|
||||
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_DISPLAYP_PORT = (1 << 5),
|
||||
RCDEVICE_PROTOCOL_FEATURE_START_RECORDING = (1 << 6),
|
||||
RCDEVICE_PROTOCOL_FEATURE_STOP_RECORDING = (1 << 7),
|
||||
RCDEVICE_PROTOCOL_FEATURE_CMS_MENU = (1 << 8),
|
||||
} rcdevice_features_e;
|
||||
|
||||
// Operation of Camera Button Simulation
|
||||
typedef enum {
|
||||
RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN = 0x00,
|
||||
RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN = 0x01,
|
||||
RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE = 0x02,
|
||||
RCDEVICE_PROTOCOL_CAM_CTRL_START_RECORDING = 0x03,
|
||||
RCDEVICE_PROTOCOL_CAM_CTRL_STOP_RECORDING = 0x04,
|
||||
RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION = 0xFF
|
||||
} rcdevice_camera_control_opeation_e;
|
||||
|
||||
// Operation Of 5 Key OSD Cable Simulation
|
||||
typedef enum {
|
||||
RCDEVICE_PROTOCOL_5KEY_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
|
||||
} rcdevice_5key_simulation_operation_e;
|
||||
|
||||
// Operation of RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION
|
||||
typedef enum {
|
||||
RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN = 0x01,
|
||||
RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE = 0x02
|
||||
} RCDEVICE_5key_connection_event_e;
|
||||
|
||||
typedef enum {
|
||||
RCDEVICE_CAM_KEY_NONE,
|
||||
RCDEVICE_CAM_KEY_ENTER,
|
||||
RCDEVICE_CAM_KEY_LEFT,
|
||||
RCDEVICE_CAM_KEY_UP,
|
||||
RCDEVICE_CAM_KEY_RIGHT,
|
||||
RCDEVICE_CAM_KEY_DOWN,
|
||||
RCDEVICE_CAM_KEY_CONNECTION_CLOSE,
|
||||
RCDEVICE_CAM_KEY_CONNECTION_OPEN,
|
||||
RCDEVICE_CAM_KEY_RELEASE,
|
||||
} rcdeviceCamSimulationKeyEvent_e;
|
||||
|
||||
typedef enum {
|
||||
RCDEVICE_PROTOCOL_RCSPLIT_VERSION = 0x00, // this is used to indicate the
|
||||
// device that using rcsplit
|
||||
// firmware version that <= 1.1.0
|
||||
RCDEVICE_PROTOCOL_VERSION_1_0 = 0x01,
|
||||
RCDEVICE_PROTOCOL_UNKNOWN
|
||||
} rcdevice_protocol_version_e;
|
||||
|
||||
// Reserved setting ids
|
||||
typedef enum {
|
||||
RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET = 0, // type: text_selection, read&write, 0: use charset with betaflight logo, 1 use
|
||||
// charset with cleanflight logo, other id are not used
|
||||
RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS = 1, // type: uint8_t, read only, the column count of the OSD layer
|
||||
RCDEVICE_PROTOCOL_SETTINGID_DISP_TV_MODE = 2, // type: text_selection, read&write, 0:NTSC, 1:PAL
|
||||
RCDEVICE_PROTOCOL_SETTINGID_SDCARD_CAPACITY = 3, // type: info, read only, return sd card capacity
|
||||
RCDEVICE_PROTOCOL_SETTINGID_REMAINING_RECORDING_TIME = 4, // type: info, read only, return remaining recording time
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESOLUTION = 5, // type: text selection, read&write, return the current resolution and all available resolutions
|
||||
RCDEVICE_PROTOCOL_SETTINGID_CAMERA_TIME = 6, // type: string, read&write, update the camera time, the time attribute of medias file in camera will use this time.
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED7 = 7,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED8 = 8,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED9 = 9,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED10 = 10,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED11 = 11,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED12 = 12,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED13 = 13,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED14 = 14,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED15 = 15,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED16 = 16,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED17 = 17,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED18 = 18,
|
||||
RCDEVICE_PROTOCOL_SETTINGID_RESERVED19 = 19,
|
||||
} rcdeviceReservedSettingID_e;
|
||||
|
||||
typedef enum {
|
||||
RCDEVICE_PROTOCOL_SETTINGTYPE_UINT8 = 0,
|
||||
RCDEVICE_PROTOCOL_SETTINGTYPE_INT8 = 1,
|
||||
RCDEVICE_PROTOCOL_SETTINGTYPE_UINT16 = 2,
|
||||
RCDEVICE_PROTOCOL_SETTINGTYPE_INT16 = 3,
|
||||
RCDEVICE_PROTOCOL_SETTINGTYPE_FLOAT = 8,
|
||||
RCDEVICE_PROTOCOL_SETTINGTYPE_TEXT_SELECTION = 9,
|
||||
RCDEVICE_PROTOCOL_SETTINGTYPE_STRING = 10,
|
||||
RCDEVICE_PROTOCOL_SETTINGTYPE_FOLDER = 11,
|
||||
RCDEVICE_PROTOCOL_SETTINGTYPE_INFO = 12,
|
||||
RCDEVICE_PROTOCOL_SETTINGTYPE_UNKNOWN
|
||||
} rcdeviceSettingType_e;
|
||||
|
||||
typedef enum {
|
||||
RCDEVICE_SUCCEED = 0,
|
||||
RCDEVICE_INVALID = 1,
|
||||
RCDEVICE_NODEV = 2,
|
||||
RCDEVICE_DEVBUSY = 3,
|
||||
} rcdeviceErrorCode_e;
|
||||
|
||||
// end of Runcam Device definition
|
||||
|
||||
// Old version defination(RCSplit firmware v1.0.0 and v1.1.0)
|
||||
// packet header and tail
|
||||
#define RCSPLIT_PACKET_HEADER 0x55
|
||||
#define RCSPLIT_PACKET_CMD_CTRL 0x01
|
||||
#define RCSPLIT_PACKET_TAIL 0xaa
|
||||
|
||||
|
||||
|
||||
typedef enum {
|
||||
RCSPLIT_CTRL_ARGU_INVALID = 0x0,
|
||||
RCSPLIT_CTRL_ARGU_WIFI_BTN = 0x1,
|
||||
RCSPLIT_CTRL_ARGU_POWER_BTN = 0x2,
|
||||
RCSPLIT_CTRL_ARGU_CHANGE_MODE = 0x3,
|
||||
RCSPLIT_CTRL_ARGU_WHO_ARE_YOU = 0xFF,
|
||||
} rcsplit_ctrl_argument_e;
|
||||
// end of old version protocol definition
|
||||
|
||||
typedef struct runcamDeviceInfo_s {
|
||||
rcdevice_protocol_version_e protocolVersion;
|
||||
uint16_t features;
|
||||
} runcamDeviceInfo_t;
|
||||
|
||||
typedef struct runcamDeviceSetting_s {
|
||||
uint8_t id;
|
||||
char name[RCDEVICE_PROTOCOL_MAX_SETTING_NAME_LENGTH];
|
||||
char value[RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH];
|
||||
} runcamDeviceSetting_t;
|
||||
|
||||
typedef struct runcamDeviceSettingTextSelection_s {
|
||||
char text[RCDEVICE_PROTOCOL_MAX_SETTING_VALUE_LENGTH];
|
||||
} runcamDeviceSettingTextSelection_t;
|
||||
|
||||
typedef struct runcamDeviceSettingDetail_s {
|
||||
uint8_t type;
|
||||
uint32_t value;
|
||||
uint32_t minValue;
|
||||
uint32_t maxValue;
|
||||
uint8_t decimalPoint;
|
||||
uint32_t stepSize;
|
||||
uint8_t maxStringSize;
|
||||
char stringValue[RCDEVICE_PROTOCOL_MAX_STRING_LENGTH]; // when settingType is RCDEVICE_PROTOCOL_SETTINGTYPE_INFO or RCDEVICE_PROTOCOL_SETTINGTYPE_STRING, this field store the string/info value;
|
||||
runcamDeviceSettingTextSelection_t textSelections[RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS];
|
||||
} runcamDeviceSettingDetail_t;
|
||||
|
||||
typedef struct runcamDeviceWriteSettingResponse_s {
|
||||
uint8_t resultCode;
|
||||
uint8_t needUpdateMenuItems;
|
||||
} runcamDeviceWriteSettingResponse_t;
|
||||
|
||||
typedef struct runcamDevice_s {
|
||||
serialPort_t *serialPort;
|
||||
uint8_t buffer[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE];
|
||||
runcamDeviceInfo_t info;
|
||||
} runcamDevice_t;
|
||||
|
||||
bool runcamDeviceInit(runcamDevice_t *device);
|
||||
|
||||
// camera button simulation
|
||||
bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation);
|
||||
|
||||
// 5 key osd cable simulation
|
||||
bool runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device);
|
||||
bool runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, uint8_t *resultCode);
|
||||
bool runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation);
|
||||
bool runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device);
|
||||
|
||||
// DisplayPort feature support
|
||||
void runcamDeviceDispFillRegion(runcamDevice_t *device, uint8_t x, uint8_t y, uint8_t width, uint8_t height, uint8_t c);
|
||||
void runcamDeviceDispWriteChar(runcamDevice_t *device, uint8_t x, uint8_t y, uint8_t c);
|
||||
void runcamDeviceDispWriteHorizontalString(runcamDevice_t *device, uint8_t x, uint8_t y, const char *text);
|
||||
void runcamDeviceDispWriteVerticalString(runcamDevice_t *device, uint8_t x, uint8_t y, const char *text);
|
||||
void runcamDeviceDispWriteChars(runcamDevice_t *device, uint8_t *data, uint8_t datalen);
|
||||
|
||||
// Device Setting Access
|
||||
bool runcamDeviceGetSettings(runcamDevice_t *device, uint8_t parentSettingID, runcamDeviceSetting_t *outSettingList, int maxSettingItemCount);
|
||||
bool runcamDeviceGetSettingDetail(runcamDevice_t *device, uint8_t settingID, runcamDeviceSettingDetail_t *outSettingDetail);
|
||||
bool runcamDeviceWriteSetting(runcamDevice_t *device, uint8_t settingID, uint8_t *data, uint8_t dataLen, runcamDeviceWriteSettingResponse_t *response);
|
|
@ -0,0 +1,283 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "cms/cms.h"
|
||||
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
#include "io/rcdevice_cam.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#ifdef USE_RCDEVICE
|
||||
|
||||
#define IS_HI(X) (rcData[X] > FIVE_KEY_CABLE_JOYSTICK_MAX)
|
||||
#define IS_LO(X) (rcData[X] < FIVE_KEY_CABLE_JOYSTICK_MIN)
|
||||
#define IS_MID(X) (rcData[X] > FIVE_KEY_CABLE_JOYSTICK_MID_START && rcData[X] < FIVE_KEY_CABLE_JOYSTICK_MID_END)
|
||||
|
||||
static runcamDevice_t runcamDevice;
|
||||
runcamDevice_t *camDevice = &runcamDevice;
|
||||
rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
|
||||
bool rcdeviceInMenu;
|
||||
bool needRelease = false;
|
||||
|
||||
static bool isFeatureSupported(uint8_t feature)
|
||||
{
|
||||
if (camDevice->info.features & feature) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool rcdeviceIsCameraControlEnabled(void)
|
||||
{
|
||||
bool isPowerSimulationSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON);
|
||||
bool isWiFiSimulationSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON);
|
||||
bool isChangeModeSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE);
|
||||
bool isStartRecordingSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_START_RECORDING);
|
||||
bool isStopRecordingSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_STOP_RECORDING);
|
||||
|
||||
if (camDevice->serialPort != NULL && (isPowerSimulationSupported || isWiFiSimulationSupported || isChangeModeSupported || isStartRecordingSupported || isStopRecordingSupported)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool rcdeviceIsEnabled(void)
|
||||
{
|
||||
bool is5KeySimulationSupported = isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE);
|
||||
|
||||
if (camDevice->serialPort != NULL && (rcdeviceIsCameraControlEnabled() || is5KeySimulationSupported)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool rcdeviceIs5KeyEnabled(void)
|
||||
{
|
||||
if (camDevice->serialPort != NULL && isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static void rcdeviceCameraControlProcess(void)
|
||||
{
|
||||
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
|
||||
uint8_t switchIndex = i - BOXCAMERA1;
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(i)) {
|
||||
// check last state of this mode, if it's true, then ignore it.
|
||||
// Here is a logic to make a toggle control for this mode
|
||||
if (switchStates[switchIndex].isActivated) {
|
||||
continue;
|
||||
}
|
||||
|
||||
uint8_t behavior = RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION;
|
||||
switch (i) {
|
||||
case BOXCAMERA1:
|
||||
if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON)) {
|
||||
behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN;
|
||||
}
|
||||
break;
|
||||
case BOXCAMERA2:
|
||||
if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON)) {
|
||||
behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN;
|
||||
}
|
||||
break;
|
||||
case BOXCAMERA3:
|
||||
if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE)) {
|
||||
behavior = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if (behavior != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) {
|
||||
runcamDeviceSimulateCameraButton(camDevice, behavior);
|
||||
switchStates[switchIndex].isActivated = true;
|
||||
}
|
||||
} else {
|
||||
switchStates[switchIndex].isActivated = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool rcdeviceCamSimulate5KeyCablePress(rcdeviceCamSimulationKeyEvent_e key)
|
||||
{
|
||||
uint8_t operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE;
|
||||
switch (key) {
|
||||
case RCDEVICE_CAM_KEY_LEFT:
|
||||
operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_LEFT;
|
||||
break;
|
||||
case RCDEVICE_CAM_KEY_UP:
|
||||
operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_UP;
|
||||
break;
|
||||
case RCDEVICE_CAM_KEY_RIGHT:
|
||||
operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_RIGHT;
|
||||
break;
|
||||
case RCDEVICE_CAM_KEY_DOWN:
|
||||
operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_DOWN;
|
||||
break;
|
||||
case RCDEVICE_CAM_KEY_ENTER:
|
||||
operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET;
|
||||
break;
|
||||
default:
|
||||
operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE;
|
||||
break;
|
||||
}
|
||||
|
||||
return runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, operation);
|
||||
}
|
||||
|
||||
static bool rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key)
|
||||
{
|
||||
bool reqResult = false;
|
||||
switch (key) {
|
||||
case RCDEVICE_CAM_KEY_CONNECTION_OPEN:
|
||||
reqResult = runcamDeviceOpen5KeyOSDCableConnection(camDevice);
|
||||
if (reqResult) {
|
||||
rcdeviceInMenu = true;
|
||||
beeper(BEEPER_CAM_CONNECTION_OPEN);
|
||||
}
|
||||
break;
|
||||
case RCDEVICE_CAM_KEY_CONNECTION_CLOSE: {
|
||||
uint8_t resultCode = 0;
|
||||
reqResult = runcamDeviceClose5KeyOSDCableConnection(camDevice, &resultCode);
|
||||
if (resultCode == 1) {
|
||||
rcdeviceInMenu = false;
|
||||
beeper(BEEPER_CAM_CONNECTION_CLOSE);
|
||||
}
|
||||
}
|
||||
break;
|
||||
case RCDEVICE_CAM_KEY_ENTER:
|
||||
case RCDEVICE_CAM_KEY_LEFT:
|
||||
case RCDEVICE_CAM_KEY_UP:
|
||||
case RCDEVICE_CAM_KEY_RIGHT:
|
||||
case RCDEVICE_CAM_KEY_DOWN:
|
||||
reqResult = rcdeviceCamSimulate5KeyCablePress(key);
|
||||
break;
|
||||
case RCDEVICE_CAM_KEY_RELEASE:
|
||||
reqResult = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice);
|
||||
break;
|
||||
default:
|
||||
reqResult = false;
|
||||
break;
|
||||
}
|
||||
|
||||
return reqResult;
|
||||
}
|
||||
|
||||
static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
|
||||
#ifdef CMS
|
||||
if (cmsInMenu) {
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (camDevice->serialPort == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
rcdeviceCamSimulationKeyEvent_e key = RCDEVICE_CAM_KEY_NONE;
|
||||
|
||||
if (needRelease) {
|
||||
if (IS_MID(YAW) && IS_MID(PITCH) && IS_MID(ROLL)) {
|
||||
key = RCDEVICE_CAM_KEY_RELEASE;
|
||||
if (rcdeviceSend5KeyOSDCableSimualtionEvent(key)) {
|
||||
needRelease = false;
|
||||
} else {
|
||||
rcdeviceInMenu = false;
|
||||
}
|
||||
return;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_LO(YAW)) { // Disconnect HI YAW
|
||||
if (rcdeviceInMenu) {
|
||||
key = RCDEVICE_CAM_KEY_CONNECTION_CLOSE;
|
||||
}
|
||||
} else {
|
||||
if (rcdeviceInMenu) {
|
||||
if (IS_LO(ROLL)) { // Left LO ROLL
|
||||
key = RCDEVICE_CAM_KEY_LEFT;
|
||||
} else if (IS_HI(PITCH)) { // Up HI PITCH
|
||||
key = RCDEVICE_CAM_KEY_UP;
|
||||
} else if (IS_HI(ROLL)) { // Right HI ROLL
|
||||
key = RCDEVICE_CAM_KEY_RIGHT;
|
||||
} else if (IS_LO(PITCH)) { // Down LO PITCH
|
||||
key = RCDEVICE_CAM_KEY_DOWN;
|
||||
} else if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_HI(YAW)) { // Enter HI YAW
|
||||
key = RCDEVICE_CAM_KEY_ENTER;
|
||||
}
|
||||
} else {
|
||||
if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_HI(YAW) && !ARMING_FLAG(ARMED)) { // Enter HI YAW
|
||||
key = RCDEVICE_CAM_KEY_CONNECTION_OPEN;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (key != RCDEVICE_CAM_KEY_NONE) {
|
||||
if (rcdeviceSend5KeyOSDCableSimualtionEvent(key)) {
|
||||
needRelease = true;
|
||||
} else {
|
||||
rcdeviceInMenu = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void rcdeviceUpdate(timeUs_t currentTimeUs)
|
||||
{
|
||||
if (rcdeviceIsCameraControlEnabled()) {
|
||||
rcdeviceCameraControlProcess();
|
||||
}
|
||||
|
||||
if (rcdeviceIs5KeyEnabled()) {
|
||||
rcdevice5KeySimulationProcess(currentTimeUs);
|
||||
}
|
||||
}
|
||||
|
||||
bool rcdeviceInit(void)
|
||||
{
|
||||
// open serial port
|
||||
if (!runcamDeviceInit(camDevice)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
|
||||
uint8_t switchIndex = i - BOXCAMERA1;
|
||||
switchStates[switchIndex].isActivated = true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
|
@ -18,33 +18,28 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "common/time.h"
|
||||
#include "io/rcdevice.h"
|
||||
#include "fc/rc_modes.h"
|
||||
|
||||
typedef struct rcsplitSwitchState_s {
|
||||
#define FIVE_KEY_CABLE_JOYSTICK_MIN 1080
|
||||
#define FIVE_KEY_CABLE_JOYSTICK_MAX 1920
|
||||
#define FIVE_KEY_CABLE_JOYSTICK_MID_START 1350
|
||||
#define FIVE_KEY_CABLE_JOYSTICK_MID_END 1650
|
||||
|
||||
typedef struct rcdeviceSwitchState_s {
|
||||
bool isActivated;
|
||||
} rcsplitSwitchState_t;
|
||||
} rcdeviceSwitchState_t;
|
||||
|
||||
typedef enum {
|
||||
RCSPLIT_STATE_UNKNOWN = 0,
|
||||
RCSPLIT_STATE_INITIALIZING,
|
||||
RCSPLIT_STATE_IS_READY,
|
||||
} rcsplitState_e;
|
||||
extern runcamDevice_t *camDevice;
|
||||
extern bool rcdeviceInMenu;
|
||||
|
||||
// packet header and tail
|
||||
#define RCSPLIT_PACKET_HEADER 0x55
|
||||
#define RCSPLIT_PACKET_CMD_CTRL 0x01
|
||||
#define RCSPLIT_PACKET_TAIL 0xaa
|
||||
bool rcdeviceInit(void);
|
||||
void rcdeviceUpdate(timeUs_t currentTimeUs);
|
||||
|
||||
bool rcdeviceIsEnabled(void);
|
||||
|
||||
// the commands of RunCam Split serial protocol
|
||||
typedef enum {
|
||||
RCSPLIT_CTRL_ARGU_INVALID = 0x0,
|
||||
RCSPLIT_CTRL_ARGU_WIFI_BTN = 0x1,
|
||||
RCSPLIT_CTRL_ARGU_POWER_BTN = 0x2,
|
||||
RCSPLIT_CTRL_ARGU_CHANGE_MODE = 0x3,
|
||||
RCSPLIT_CTRL_ARGU_WHO_ARE_YOU = 0xFF,
|
||||
} rcsplit_ctrl_argument_e;
|
||||
|
||||
bool rcSplitInit(void);
|
||||
bool rcSplitIsEnabled(void);
|
||||
void rcSplitUpdate(timeUs_t currentTimeUs);
|
||||
// used for unit test
|
||||
rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
|
|
@ -0,0 +1,235 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "drivers/vcd.h"
|
||||
|
||||
#include "rcdevice.h"
|
||||
#include "rcdevice_osd.h"
|
||||
|
||||
#ifdef USE_RCDEVICE
|
||||
|
||||
#define VIDEO_BUFFER_CHARS_PAL 480
|
||||
|
||||
static uint8_t columnCount = 30;
|
||||
|
||||
static runcamDevice_t runcamOSDDevice;
|
||||
runcamDevice_t *osdDevice = &runcamOSDDevice;
|
||||
|
||||
static uint8_t video_system;
|
||||
static uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
|
||||
|
||||
#ifdef USE_PARTICLE_DRAW
|
||||
#define MAX_CHARS2UPDATE 20
|
||||
static uint8_t screenBuffer[VIDEO_BUFFER_CHARS_PAL + 40]; // For faster writes
|
||||
// we use memcpy so we
|
||||
// need some space to
|
||||
// don't overwrite
|
||||
// buffer
|
||||
static uint8_t shadowBuffer[VIDEO_BUFFER_CHARS_PAL];
|
||||
static bool rcdeviceOSDLock = false;
|
||||
#endif
|
||||
|
||||
bool rcdeviceOSDInit(const vcdProfile_t *vcdProfile)
|
||||
{
|
||||
if (!runcamDeviceInit(osdDevice)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if ((osdDevice->info.features & RCDEVICE_PROTOCOL_FEATURE_DISPLAYP_PORT) == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// get screen column count
|
||||
runcamDeviceSettingDetail_t settingDetail;
|
||||
if (!runcamDeviceGetSettingDetail(osdDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &settingDetail)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
columnCount = settingDetail.value;
|
||||
|
||||
video_system = vcdProfile->video_system;
|
||||
if (video_system == VIDEO_SYSTEM_AUTO) {
|
||||
// fetch current video mode from device
|
||||
runcamDeviceSettingDetail_t settingDetail;
|
||||
if (!runcamDeviceGetSettingDetail(osdDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_TV_MODE, &settingDetail)) {
|
||||
return false;
|
||||
}
|
||||
video_system = settingDetail.value;
|
||||
} else {
|
||||
// set video system
|
||||
runcamDeviceWriteSettingResponse_t response;
|
||||
uint8_t tvMode = 0;
|
||||
if (video_system == VIDEO_SYSTEM_NTSC) {
|
||||
tvMode = 0;
|
||||
} else if (video_system == VIDEO_SYSTEM_PAL) {
|
||||
tvMode = 1;
|
||||
}
|
||||
|
||||
if (!runcamDeviceWriteSetting(osdDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_TV_MODE, &tvMode, sizeof(uint8_t), &response)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (response.resultCode) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// user bf charset
|
||||
uint8_t charsetID = 0;
|
||||
runcamDeviceWriteSettingResponse_t updateCharsetResp;
|
||||
if (!runcamDeviceWriteSetting(osdDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetID, sizeof(uint8_t), &updateCharsetResp) || updateCharsetResp.resultCode != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef USE_PARTICLE_DRAW
|
||||
memset(shadowBuffer, 2, VIDEO_BUFFER_CHARS_PAL);
|
||||
#endif
|
||||
|
||||
// fill screen with ' '
|
||||
rcdeviceOSDClearScreen(NULL);
|
||||
return true;
|
||||
}
|
||||
|
||||
int rcdeviceOSDGrab(displayPort_t *displayPort)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
// osdResetAlarms();
|
||||
// resumeRefreshAt = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int rcdeviceOSDRelease(displayPort_t *displayPort)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int rcdeviceOSDDrawScreen(displayPort_t *displayPort)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
|
||||
#ifdef USE_PARTICLE_DRAW
|
||||
static uint16_t pos = 0;
|
||||
int k = 0;
|
||||
|
||||
if (!rcdeviceOSDLock) {
|
||||
rcdeviceOSDLock = true;
|
||||
|
||||
uint8_t data[60];
|
||||
uint8_t dataLen = 0;
|
||||
for (k = 0; k < MAX_CHARS2UPDATE; k++) {
|
||||
if (screenBuffer[pos] != shadowBuffer[pos]) {
|
||||
shadowBuffer[pos] = screenBuffer[pos];
|
||||
uint8_t x = pos % columnCount;
|
||||
uint8_t y = pos / columnCount;
|
||||
data[dataLen++] = x;
|
||||
data[dataLen++] = y;
|
||||
data[dataLen++] = screenBuffer[pos];
|
||||
}
|
||||
|
||||
if (++pos >= maxScreenSize) {
|
||||
pos = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
runcamDeviceDispWriteChars(osdDevice, data, dataLen);
|
||||
|
||||
rcdeviceOSDLock = false;
|
||||
}
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
|
||||
int rcdeviceOSDWriteString(displayPort_t *displayPort, uint8_t x, uint8_t y, const char *buff)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
#if !defined(USE_PARTICLE_DRAW)
|
||||
runcamDeviceDispWriteHorizontalString(osdDevice, x, y, buff);
|
||||
#else
|
||||
for (int xpos = x; *buff && xpos < columnCount; xpos++) {
|
||||
screenBuffer[y * columnCount + xpos] = *buff++;
|
||||
}
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
|
||||
int rcdeviceOSDWriteChar(displayPort_t *displayPort, uint8_t x, uint8_t y, uint8_t c)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
#if !defined(USE_PARTICLE_DRAW)
|
||||
runcamDeviceDispWriteChar(osdDevice, x, y, c);
|
||||
#else
|
||||
screenBuffer[y * columnCount + x] = c;
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int rcdeviceOSDClearScreen(displayPort_t *displayPort)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
|
||||
#if defined(USE_PARTICLE_DRAW)
|
||||
memset(screenBuffer, 0x20, sizeof(screenBuffer));
|
||||
#else
|
||||
runcamDeviceDispFillRegion(osdDevice, 0, 0, 255, 255, ' ');
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool rcdeviceOSDIsTransferInProgress(const displayPort_t *displayPort)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
return false;
|
||||
}
|
||||
|
||||
int rcdeviceOSDHeartbeat(displayPort_t *displayPort)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void rcdeviceOSDResync(displayPort_t *displayPort)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
|
||||
if (video_system == VIDEO_SYSTEM_PAL) {
|
||||
displayPort->rows = RCDEVICE_PROTOCOL_OSD_VIDEO_LINES_PAL;
|
||||
} else {
|
||||
displayPort->rows = RCDEVICE_PROTOCOL_OSD_VIDEO_LINES_NTSC;
|
||||
}
|
||||
|
||||
displayPort->cols = columnCount;
|
||||
maxScreenSize = displayPort->rows * displayPort->cols;
|
||||
}
|
||||
|
||||
uint32_t rcdeviceOSDTxBytesFree(const displayPort_t *displayPort)
|
||||
{
|
||||
UNUSED(displayPort);
|
||||
return INT32_MAX;
|
||||
}
|
||||
|
||||
int rcdeviceScreenSize(const displayPort_t *displayPort)
|
||||
{
|
||||
return displayPort->rows * displayPort->cols;
|
||||
}
|
||||
|
||||
#endif
|
|
@ -0,0 +1,39 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "drivers/display.h"
|
||||
|
||||
#define RCDEVICE_PROTOCOL_OSD_VIDEO_LINES_NTSC 13
|
||||
#define RCDEVICE_PROTOCOL_OSD_VIDEO_LINES_PAL 16
|
||||
|
||||
struct vcdProfile_s;
|
||||
|
||||
bool rcdeviceOSDInit(const struct vcdProfile_s *vcdProfile);
|
||||
int rcdeviceOSDGrab(displayPort_t *displayPort);
|
||||
int rcdeviceOSDRelease(displayPort_t *displayPort);
|
||||
|
||||
int rcdeviceOSDDrawScreen(displayPort_t *);
|
||||
|
||||
int rcdeviceOSDWriteString(displayPort_t *, uint8_t x, uint8_t y, const char *buff);
|
||||
int rcdeviceOSDWriteChar(displayPort_t *, uint8_t x, uint8_t y, uint8_t c);
|
||||
int rcdeviceOSDReloadProfile(displayPort_t *);
|
||||
int rcdeviceOSDClearScreen(displayPort_t *);
|
||||
bool rcdeviceOSDIsTransferInProgress(const displayPort_t *);
|
||||
int rcdeviceOSDHeartbeat(displayPort_t *displayPort);
|
||||
void rcdeviceOSDResync(displayPort_t *displayPort);
|
||||
uint32_t rcdeviceOSDTxBytesFree(const displayPort_t *displayPort);
|
||||
int rcdeviceScreenSize(const displayPort_t *displayPort);
|
|
@ -1,146 +0,0 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "io/rcsplit.h"
|
||||
#include "io/serial.h"
|
||||
|
||||
|
||||
// communicate with camera device variables
|
||||
STATIC_UNIT_TESTED serialPort_t *rcSplitSerialPort = NULL;
|
||||
// only for unit test
|
||||
STATIC_UNIT_TESTED rcsplitSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
|
||||
|
||||
static uint8_t crc_high_first(uint8_t *ptr, uint8_t len)
|
||||
{
|
||||
uint8_t crc = 0x00;
|
||||
while (len--) {
|
||||
crc ^= *ptr++;
|
||||
for (int i=8; i>0; --i) {
|
||||
if (crc & 0x80) {
|
||||
crc = (crc << 1) ^ 0x31;
|
||||
} else {
|
||||
crc = (crc << 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
static void sendCtrlCommand(rcsplit_ctrl_argument_e argument)
|
||||
{
|
||||
uint8_t uart_buffer[5] = {0};
|
||||
|
||||
uart_buffer[0] = RCSPLIT_PACKET_HEADER;
|
||||
uart_buffer[1] = RCSPLIT_PACKET_CMD_CTRL;
|
||||
uart_buffer[2] = argument;
|
||||
uart_buffer[3] = RCSPLIT_PACKET_TAIL;
|
||||
uint8_t crc = crc_high_first(uart_buffer, 4);
|
||||
|
||||
// build up a full request [header]+[command]+[argument]+[crc]+[tail]
|
||||
uart_buffer[3] = crc;
|
||||
uart_buffer[4] = RCSPLIT_PACKET_TAIL;
|
||||
|
||||
// write to device
|
||||
serialWriteBuf(rcSplitSerialPort, uart_buffer, 5);
|
||||
}
|
||||
|
||||
static void rcSplitProcessMode(void)
|
||||
{
|
||||
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
|
||||
const uint8_t switchIndex = i - BOXCAMERA1;
|
||||
if (IS_RC_MODE_ACTIVE(i)) {
|
||||
// check last state of this mode, if it's true, then ignore it.
|
||||
// Here is a logic to make a toggle control for this mode
|
||||
if (switchStates[switchIndex].isActivated) {
|
||||
continue;
|
||||
}
|
||||
|
||||
uint8_t argument = RCSPLIT_CTRL_ARGU_INVALID;
|
||||
switch (i) {
|
||||
case BOXCAMERA1:
|
||||
// check whether arm unlock, we found a bug in rcsplit firmware:
|
||||
// if rcsplit running without Wi-Fi module, and user try to turn on wifi, it'll cause rcsplit to turn off itself, this is danger
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
argument = RCSPLIT_CTRL_ARGU_WIFI_BTN;
|
||||
}
|
||||
break;
|
||||
case BOXCAMERA2:
|
||||
argument = RCSPLIT_CTRL_ARGU_POWER_BTN;
|
||||
break;
|
||||
case BOXCAMERA3:
|
||||
argument = RCSPLIT_CTRL_ARGU_CHANGE_MODE;
|
||||
break;
|
||||
default:
|
||||
argument = RCSPLIT_CTRL_ARGU_INVALID;
|
||||
break;
|
||||
}
|
||||
|
||||
if (argument != RCSPLIT_CTRL_ARGU_INVALID) {
|
||||
sendCtrlCommand(argument);
|
||||
switchStates[switchIndex].isActivated = true;
|
||||
}
|
||||
} else {
|
||||
switchStates[switchIndex].isActivated = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool rcSplitIsEnabled(void)
|
||||
{
|
||||
return rcSplitSerialPort ? true : false;
|
||||
}
|
||||
|
||||
bool rcSplitInit(void)
|
||||
{
|
||||
// found the port config with FUNCTION_RUNCAM_SPLIT_CONTROL
|
||||
// User must set some UART inteface with RunCam Split at peripherals column in Ports tab
|
||||
serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RCSPLIT);
|
||||
if (portConfig) {
|
||||
rcSplitSerialPort = openSerialPort(portConfig->identifier, FUNCTION_RCSPLIT, NULL, 115200, MODE_RXTX, 0);
|
||||
}
|
||||
|
||||
if (!rcSplitSerialPort) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// set init value to true, to avoid the action auto run when the flight board start and the switch is on.
|
||||
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
|
||||
uint8_t switchIndex = i - BOXCAMERA1;
|
||||
switchStates[switchIndex].isActivated = true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void rcSplitUpdate(timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
|
||||
// process rcsplit custom mode if has any changed
|
||||
rcSplitProcessMode();
|
||||
}
|
|
@ -44,7 +44,7 @@ typedef enum {
|
|||
FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048
|
||||
FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096
|
||||
FUNCTION_VTX_TRAMP = (1 << 13), // 8192
|
||||
FUNCTION_RCSPLIT = (1 << 14), // 16384
|
||||
FUNCTION_RCDEVICE = (1 << 14), // 16384
|
||||
} serialPortFunction_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
|
@ -114,8 +114,8 @@ typedef enum {
|
|||
TASK_CAMCTRL,
|
||||
#endif
|
||||
|
||||
#ifdef USE_RCSPLIT
|
||||
TASK_RCSPLIT,
|
||||
#ifdef USE_RCDEVICE
|
||||
TASK_RCDEVICE,
|
||||
#endif
|
||||
|
||||
/* Count of real tasks */
|
||||
|
|
|
@ -21,6 +21,7 @@
|
|||
#undef TELEMETRY_HOTT //no space left
|
||||
#undef TELEMETRY_JETIEXBUS // no space left
|
||||
#undef TELEMETRY_MAVLINK // no space left
|
||||
#undef USE_RCDEVICE // no space left
|
||||
|
||||
#define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus
|
||||
|
||||
|
|
|
@ -114,7 +114,7 @@
|
|||
#define TELEMETRY_SRXL
|
||||
#define USE_DASHBOARD
|
||||
#define USE_MSP_DISPLAYPORT
|
||||
#define USE_RCSPLIT
|
||||
#define USE_RCDEVICE
|
||||
#define USE_RX_MSP
|
||||
#define USE_SERIALRX_JETIEXBUS
|
||||
#define USE_SENSOR_NAMES
|
||||
|
|
|
@ -268,15 +268,6 @@ type_conversion_unittest_SRC := \
|
|||
ws2811_unittest_SRC := \
|
||||
$(USER_DIR)/drivers/light_ws2811strip.c
|
||||
|
||||
rcsplit_unittest_SRC := \
|
||||
$(USER_DIR)/common/bitarray.c \
|
||||
$(USER_DIR)/fc/rc_modes.c \
|
||||
$(USER_DIR)/io/rcsplit.c
|
||||
|
||||
rcsplit_unitest_DEFINES := \
|
||||
USE_UART3 \
|
||||
USE_RCSPLIT \
|
||||
|
||||
huffman_unittest_SRC := \
|
||||
$(USER_DIR)/common/huffman.c \
|
||||
$(USER_DIR)/common/huffman_table.c
|
||||
|
@ -284,6 +275,17 @@ huffman_unittest_SRC := \
|
|||
huffman_unittest_DEFINES := \
|
||||
USE_HUFFMAN
|
||||
|
||||
rcdevice_unittest_SRC := \
|
||||
$(USER_DIR)/common/crc.c \
|
||||
$(USER_DIR)/common/bitarray.c \
|
||||
$(USER_DIR)/fc/rc_modes.c \
|
||||
$(USER_DIR)/io/rcdevice.c \
|
||||
$(USER_DIR)/io/rcdevice_osd.c \
|
||||
$(USER_DIR)/io/rcdevice_cam.c \
|
||||
|
||||
rcdevice_unittest_DEFINES := \
|
||||
USE_RCDEVICE
|
||||
|
||||
# Please tweak the following variable definitions as needed by your
|
||||
# project, except GTEST_HEADERS, which you can use in your own targets
|
||||
# but shouldn't modify.
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -1,427 +0,0 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight 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.
|
||||
*
|
||||
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
extern "C" {
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/bitarray.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/streambuf.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_modes.h"
|
||||
|
||||
|
||||
#include "io/beeper.h"
|
||||
#include "io/rcsplit.h"
|
||||
#include "io/serial.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
extern serialPort_t *rcSplitSerialPort;
|
||||
extern rcsplitSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
|
||||
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||
|
||||
bool unitTestIsSwitchActivited(boxId_e boxId)
|
||||
{
|
||||
uint8_t adjustBoxID = boxId - BOXCAMERA1;
|
||||
rcsplitSwitchState_t switchState = switchStates[adjustBoxID];
|
||||
return switchState.isActivated;
|
||||
}
|
||||
|
||||
void unitTestResetRCSplit()
|
||||
{
|
||||
rcSplitSerialPort = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
typedef struct testData_s {
|
||||
bool isRunCamSplitPortConfigurated;
|
||||
bool isRunCamSplitOpenPortSupported;
|
||||
int8_t maxTimesOfRespDataAvailable;
|
||||
bool isAllowBufferReadWrite;
|
||||
} testData_t;
|
||||
|
||||
static testData_t testData;
|
||||
|
||||
TEST(RCSplitTest, TestRCSplitInitWithoutPortConfigurated)
|
||||
{
|
||||
memset(&testData, 0, sizeof(testData));
|
||||
unitTestResetRCSplit();
|
||||
bool result = rcSplitInit();
|
||||
EXPECT_EQ(false, result);
|
||||
EXPECT_EQ(false, rcSplitIsEnabled());
|
||||
}
|
||||
|
||||
TEST(RCSplitTest, TestRCSplitInitWithoutOpenPortConfigurated)
|
||||
{
|
||||
memset(&testData, 0, sizeof(testData));
|
||||
unitTestResetRCSplit();
|
||||
testData.isRunCamSplitOpenPortSupported = false;
|
||||
testData.isRunCamSplitPortConfigurated = true;
|
||||
|
||||
bool result = rcSplitInit();
|
||||
EXPECT_EQ(false, result);
|
||||
EXPECT_EQ(false, rcSplitIsEnabled());
|
||||
}
|
||||
|
||||
TEST(RCSplitTest, TestRCSplitInit)
|
||||
{
|
||||
memset(&testData, 0, sizeof(testData));
|
||||
unitTestResetRCSplit();
|
||||
testData.isRunCamSplitOpenPortSupported = true;
|
||||
testData.isRunCamSplitPortConfigurated = true;
|
||||
|
||||
bool result = rcSplitInit();
|
||||
EXPECT_EQ(true, result);
|
||||
EXPECT_EQ(true, rcSplitIsEnabled());
|
||||
}
|
||||
|
||||
TEST(RCSplitTest, TestRecvWhoAreYouResponse)
|
||||
{
|
||||
memset(&testData, 0, sizeof(testData));
|
||||
unitTestResetRCSplit();
|
||||
testData.isRunCamSplitOpenPortSupported = true;
|
||||
testData.isRunCamSplitPortConfigurated = true;
|
||||
|
||||
bool result = rcSplitInit();
|
||||
EXPECT_EQ(true, result);
|
||||
|
||||
// here will generate a number in [6-255], it's make the serialRxBytesWaiting() and serialRead() run at least 5 times,
|
||||
// so the "who are you response" will full received, and cause the state change to RCSPLIT_STATE_IS_READY;
|
||||
int8_t randNum = rand() % 127 + 6;
|
||||
testData.maxTimesOfRespDataAvailable = randNum;
|
||||
rcSplitUpdate((timeUs_t)0);
|
||||
|
||||
EXPECT_EQ(true, rcSplitIsEnabled());
|
||||
}
|
||||
|
||||
TEST(RCSplitTest, TestWifiModeChangeWithDeviceUnready)
|
||||
{
|
||||
memset(&testData, 0, sizeof(testData));
|
||||
unitTestResetRCSplit();
|
||||
testData.isRunCamSplitOpenPortSupported = true;
|
||||
testData.isRunCamSplitPortConfigurated = true;
|
||||
testData.maxTimesOfRespDataAvailable = 0;
|
||||
|
||||
bool result = rcSplitInit();
|
||||
EXPECT_EQ(true, result);
|
||||
|
||||
// bind aux1, aux2, aux3 channel to wifi button, power button and change mode
|
||||
for (uint8_t i = 0; i <= (BOXCAMERA3 - BOXCAMERA1); i++) {
|
||||
memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t));
|
||||
}
|
||||
|
||||
// bind aux1 to wifi button with range [900,1600]
|
||||
modeActivationConditionsMutable(0)->auxChannelIndex = 0;
|
||||
modeActivationConditionsMutable(0)->modeId = BOXCAMERA1;
|
||||
modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
|
||||
modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
|
||||
|
||||
// bind aux2 to power button with range [1900, 2100]
|
||||
modeActivationConditionsMutable(1)->auxChannelIndex = 1;
|
||||
modeActivationConditionsMutable(1)->modeId = BOXCAMERA2;
|
||||
modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
|
||||
modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
|
||||
|
||||
// bind aux3 to change mode with range [1300, 1600]
|
||||
modeActivationConditionsMutable(2)->auxChannelIndex = 2;
|
||||
modeActivationConditionsMutable(2)->modeId = BOXCAMERA3;
|
||||
modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1300);
|
||||
modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
|
||||
|
||||
// make the binded mode inactive
|
||||
rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1800;
|
||||
rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 900;
|
||||
rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 900;
|
||||
|
||||
updateActivatedModes();
|
||||
|
||||
// run update
|
||||
rcSplitUpdate(0);
|
||||
|
||||
EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1));
|
||||
EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA2));
|
||||
EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
|
||||
}
|
||||
|
||||
TEST(RCSplitTest, TestWifiModeChangeWithDeviceReady)
|
||||
{
|
||||
memset(&testData, 0, sizeof(testData));
|
||||
unitTestResetRCSplit();
|
||||
testData.isRunCamSplitOpenPortSupported = true;
|
||||
testData.isRunCamSplitPortConfigurated = true;
|
||||
testData.maxTimesOfRespDataAvailable = 0;
|
||||
|
||||
bool result = rcSplitInit();
|
||||
EXPECT_EQ(true, result);
|
||||
|
||||
// bind aux1, aux2, aux3 channel to wifi button, power button and change mode
|
||||
for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) {
|
||||
memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t));
|
||||
}
|
||||
|
||||
|
||||
// bind aux1 to wifi button with range [900,1600]
|
||||
modeActivationConditionsMutable(0)->auxChannelIndex = 0;
|
||||
modeActivationConditionsMutable(0)->modeId = BOXCAMERA1;
|
||||
modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
|
||||
modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
|
||||
|
||||
// bind aux2 to power button with range [1900, 2100]
|
||||
modeActivationConditionsMutable(1)->auxChannelIndex = 1;
|
||||
modeActivationConditionsMutable(1)->modeId = BOXCAMERA2;
|
||||
modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
|
||||
modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
|
||||
|
||||
// bind aux3 to change mode with range [1300, 1600]
|
||||
modeActivationConditionsMutable(2)->auxChannelIndex = 2;
|
||||
modeActivationConditionsMutable(2)->modeId = BOXCAMERA3;
|
||||
modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
|
||||
modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
|
||||
|
||||
rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
|
||||
rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000;
|
||||
rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
|
||||
|
||||
updateActivatedModes();
|
||||
|
||||
// run update
|
||||
int8_t randNum = rand() % 127 + 6;
|
||||
testData.maxTimesOfRespDataAvailable = randNum;
|
||||
rcSplitUpdate((timeUs_t)0);
|
||||
|
||||
EXPECT_EQ(true, rcSplitIsEnabled());
|
||||
|
||||
EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1));
|
||||
EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2));
|
||||
EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
|
||||
}
|
||||
|
||||
TEST(RCSplitTest, TestWifiModeChangeCombine)
|
||||
{
|
||||
memset(&testData, 0, sizeof(testData));
|
||||
unitTestResetRCSplit();
|
||||
testData.isRunCamSplitOpenPortSupported = true;
|
||||
testData.isRunCamSplitPortConfigurated = true;
|
||||
testData.maxTimesOfRespDataAvailable = 0;
|
||||
|
||||
bool result = rcSplitInit();
|
||||
EXPECT_EQ(true, result);
|
||||
|
||||
// bind aux1, aux2, aux3 channel to wifi button, power button and change mode
|
||||
for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) {
|
||||
memset(modeActivationConditionsMutable(i), 0, sizeof(modeActivationCondition_t));
|
||||
}
|
||||
|
||||
|
||||
// bind aux1 to wifi button with range [900,1600]
|
||||
modeActivationConditionsMutable(0)->auxChannelIndex = 0;
|
||||
modeActivationConditionsMutable(0)->modeId = BOXCAMERA1;
|
||||
modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
|
||||
modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(1600);
|
||||
|
||||
// bind aux2 to power button with range [1900, 2100]
|
||||
modeActivationConditionsMutable(1)->auxChannelIndex = 1;
|
||||
modeActivationConditionsMutable(1)->modeId = BOXCAMERA2;
|
||||
modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
|
||||
modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
|
||||
|
||||
// bind aux3 to change mode with range [1300, 1600]
|
||||
modeActivationConditionsMutable(2)->auxChannelIndex = 2;
|
||||
modeActivationConditionsMutable(2)->modeId = BOXCAMERA3;
|
||||
modeActivationConditionsMutable(2)->range.startStep = CHANNEL_VALUE_TO_STEP(1900);
|
||||
modeActivationConditionsMutable(2)->range.endStep = CHANNEL_VALUE_TO_STEP(2100);
|
||||
|
||||
// // make the binded mode inactive
|
||||
rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
|
||||
rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2000;
|
||||
rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1700;
|
||||
updateActivatedModes();
|
||||
|
||||
// run update
|
||||
int8_t randNum = rand() % 127 + 6;
|
||||
testData.maxTimesOfRespDataAvailable = randNum;
|
||||
rcSplitUpdate((timeUs_t)0);
|
||||
|
||||
EXPECT_EQ(true, rcSplitIsEnabled());
|
||||
|
||||
EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1));
|
||||
EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2));
|
||||
EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
|
||||
|
||||
|
||||
// // make the binded mode inactive
|
||||
rcData[modeActivationConditions(0)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1500;
|
||||
rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1300;
|
||||
rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1900;
|
||||
updateActivatedModes();
|
||||
rcSplitUpdate((timeUs_t)0);
|
||||
EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA1));
|
||||
EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA2));
|
||||
EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA3));
|
||||
|
||||
|
||||
rcData[modeActivationConditions(2)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 1899;
|
||||
updateActivatedModes();
|
||||
rcSplitUpdate((timeUs_t)0);
|
||||
EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
|
||||
|
||||
rcData[modeActivationConditions(1)->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = 2001;
|
||||
updateActivatedModes();
|
||||
rcSplitUpdate((timeUs_t)0);
|
||||
EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA1));
|
||||
EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2));
|
||||
EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3));
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_e mode, portOptions_e options)
|
||||
{
|
||||
UNUSED(identifier);
|
||||
UNUSED(functionMask);
|
||||
UNUSED(baudRate);
|
||||
UNUSED(callback);
|
||||
UNUSED(mode);
|
||||
UNUSED(options);
|
||||
|
||||
if (testData.isRunCamSplitOpenPortSupported) {
|
||||
static serialPort_t s;
|
||||
s.vTable = NULL;
|
||||
|
||||
// common serial initialisation code should move to serialPort::init()
|
||||
s.rxBufferHead = s.rxBufferTail = 0;
|
||||
s.txBufferHead = s.txBufferTail = 0;
|
||||
s.rxBufferSize = 0;
|
||||
s.txBufferSize = 0;
|
||||
s.rxBuffer = s.rxBuffer;
|
||||
s.txBuffer = s.txBuffer;
|
||||
|
||||
// callback works for IRQ-based RX ONLY
|
||||
s.rxCallback = NULL;
|
||||
s.baudRate = 0;
|
||||
|
||||
return (serialPort_t *)&s;
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function)
|
||||
{
|
||||
UNUSED(function);
|
||||
if (testData.isRunCamSplitPortConfigurated) {
|
||||
static serialPortConfig_t portConfig;
|
||||
|
||||
portConfig.identifier = SERIAL_PORT_USART3;
|
||||
portConfig.msp_baudrateIndex = BAUD_115200;
|
||||
portConfig.gps_baudrateIndex = BAUD_57600;
|
||||
portConfig.telemetry_baudrateIndex = BAUD_AUTO;
|
||||
portConfig.blackbox_baudrateIndex = BAUD_115200;
|
||||
portConfig.functionMask = FUNCTION_MSP;
|
||||
|
||||
return &portConfig;
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
uint32_t serialRxBytesWaiting(const serialPort_t *instance)
|
||||
{
|
||||
UNUSED(instance);
|
||||
|
||||
testData.maxTimesOfRespDataAvailable--;
|
||||
if (testData.maxTimesOfRespDataAvailable > 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint8_t serialRead(serialPort_t *instance)
|
||||
{
|
||||
UNUSED(instance);
|
||||
|
||||
if (testData.maxTimesOfRespDataAvailable > 0) {
|
||||
static uint8_t i = 0;
|
||||
static uint8_t buffer[] = { 0x55, 0x01, 0xFF, 0xad, 0xaa };
|
||||
|
||||
if (i >= 5) {
|
||||
i = 0;
|
||||
}
|
||||
|
||||
return buffer[i++];
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void sbufWriteString(sbuf_t *dst, const char *string)
|
||||
{
|
||||
UNUSED(dst); UNUSED(string);
|
||||
|
||||
if (testData.isAllowBufferReadWrite) {
|
||||
sbufWriteData(dst, string, strlen(string));
|
||||
}
|
||||
}
|
||||
void sbufWriteU8(sbuf_t *dst, uint8_t val)
|
||||
{
|
||||
UNUSED(dst); UNUSED(val);
|
||||
|
||||
if (testData.isAllowBufferReadWrite) {
|
||||
*dst->ptr++ = val;
|
||||
}
|
||||
}
|
||||
|
||||
void sbufWriteData(sbuf_t *dst, const void *data, int len)
|
||||
{
|
||||
UNUSED(dst); UNUSED(data); UNUSED(len);
|
||||
|
||||
if (testData.isAllowBufferReadWrite) {
|
||||
memcpy(dst->ptr, data, len);
|
||||
dst->ptr += len;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// modifies streambuf so that written data are prepared for reading
|
||||
void sbufSwitchToReader(sbuf_t *buf, uint8_t *base)
|
||||
{
|
||||
UNUSED(buf); UNUSED(base);
|
||||
|
||||
if (testData.isAllowBufferReadWrite) {
|
||||
buf->end = buf->ptr;
|
||||
buf->ptr = base;
|
||||
}
|
||||
}
|
||||
|
||||
bool feature(uint32_t) { return false;}
|
||||
void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { UNUSED(instance); UNUSED(data); UNUSED(count); }
|
||||
uint8_t armingFlags = 0;
|
||||
}
|
Loading…
Reference in New Issue