diff --git a/make/source.mk b/make/source.mk
index 9c074650ed..142ab48576 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -141,12 +141,10 @@ FC_SRC = \
io/displayport_max7456.c \
io/displayport_msp.c \
io/displayport_oled.c \
- io/displayport_rcdevice.c \
io/displayport_srxl.c \
io/displayport_crsf.c \
io/rcdevice_cam.c \
io/rcdevice.c \
- io/rcdevice_osd.c \
io/gps.c \
io/ledstrip.c \
io/osd.c \
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index c4c9e38d48..ee8da47916 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -111,7 +111,6 @@
#include "io/beeper.h"
#include "io/displayport_max7456.h"
-#include "io/displayport_rcdevice.h"
#include "io/displayport_srxl.h"
#include "io/displayport_crsf.h"
#include "io/serial.h"
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index cd73fc6f58..ebbc38d9a1 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -591,7 +591,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_RCDEVICE] = {
.taskName = "RCDEVICE",
.taskFunc = rcdeviceUpdate,
- .desiredPeriod = TASK_PERIOD_HZ(10), // 10 Hz, 100ms
+ .desiredPeriod = TASK_PERIOD_HZ(20), // 10 Hz, 100ms
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
diff --git a/src/main/io/displayport_rcdevice.c b/src/main/io/displayport_rcdevice.c
deleted file mode 100644
index 8b3185f7d0..0000000000
--- a/src/main/io/displayport_rcdevice.c
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * This file is part of Cleanflight and Betaflight.
- *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
- * 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 software.
- *
- * If not, see .
- */
-
-#include
-#include
-
-#include "platform.h"
-
-#ifdef USE_RCDEVICE
-
-#include "drivers/display.h"
-
-#include "io/rcdevice.h"
-#include "io/rcdevice_osd.h"
-
-#include "pg/vcd.h"
-
-#include "displayport_rcdevice.h"
-
-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,
- .isSynced = rcdeviceOSDIsSynced,
- .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)
diff --git a/src/main/io/displayport_rcdevice.h b/src/main/io/displayport_rcdevice.h
deleted file mode 100644
index 78bfa6272a..0000000000
--- a/src/main/io/displayport_rcdevice.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*
- * This file is part of Cleanflight and Betaflight.
- *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
- * 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 software.
- *
- * If not, see .
- */
-
-#pragma once
-
-struct vcdProfile_s;
-displayPort_t *rcdeviceDisplayPortInit(const struct vcdProfile_s *vcdProfile);
\ No newline at end of file
diff --git a/src/main/io/rcdevice.c b/src/main/io/rcdevice.c
index 4196c66b5f..ca35f262e0 100644
--- a/src/main/io/rcdevice.c
+++ b/src/main/io/rcdevice.c
@@ -32,13 +32,10 @@
#include "rcdevice.h"
-#ifdef USE_RCDEVICE
+#include "fc/config.h"
+#include "config/feature.h"
-typedef enum {
- RCDP_SETTING_PARSE_WAITING_ID,
- RCDP_SETTING_PARSE_WAITING_NAME,
- RCDP_SETTING_PARSE_WAITING_VALUE,
-} runcamDeviceSettingParseStep_e;
+#ifdef USE_RCDEVICE
typedef struct runcamDeviceExpectedResponseLength_s {
uint8_t command;
@@ -46,16 +43,15 @@ typedef struct runcamDeviceExpectedResponseLength_s {
} 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)
+rcdeviceWaitingResponseQueue watingResponseQueue;
+
+static uint8_t runcamDeviceGetRespLen(uint8_t command)
{
for (unsigned int i = 0; i < ARRAYLEN(expectedResponsesLength); i++) {
if (expectedResponsesLength[i].command == command) {
@@ -66,76 +62,49 @@ static uint8_t runcamDeviceGetResponseLength(uint8_t command)
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)
+static bool rcdeviceRespCtxQueuePushRespCtx(rcdeviceWaitingResponseQueue *queue, rcdeviceResponseParseContext_t *respCtx)
{
- 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;
+ if (queue == NULL || (queue->itemCount + 1) > MAX_WAITING_RESPONSES || queue->tailPos >= MAX_WAITING_RESPONSES) {
+ return false;
}
+
+ queue->buffer[queue->tailPos] = *respCtx;
+
+ int newTailPos = queue->tailPos + 1;
+ if (newTailPos >= MAX_WAITING_RESPONSES) {
+ newTailPos = 0;
+ }
+ queue->itemCount += 1;
+ queue->tailPos = newTailPos;
+
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)
+static rcdeviceResponseParseContext_t* rcdeviceRespCtxQueuePeekFront(rcdeviceWaitingResponseQueue *queue)
{
- 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;
- }
- }
- }
+ if (queue == NULL || queue->itemCount == 0 || queue->headPos >= MAX_WAITING_RESPONSES) {
+ return NULL;
}
- // check crc
- if (crc != 0) {
- return 0;
- }
+ rcdeviceResponseParseContext_t *ctx = &queue->buffer[queue->headPos];
+ return ctx;
+}
- return responseDataLen;
+static rcdeviceResponseParseContext_t* rcdeviceRespCtxQueueShift(rcdeviceWaitingResponseQueue *queue)
+{
+ if (queue == NULL || queue->itemCount == 0 || queue->headPos >= MAX_WAITING_RESPONSES) {
+ return NULL;
+ }
+
+ rcdeviceResponseParseContext_t *ctx = &queue->buffer[queue->headPos];
+ int newHeadPos = queue->headPos + 1;
+ if (newHeadPos >= MAX_WAITING_RESPONSES) {
+ newHeadPos = 0;
+ }
+ queue->itemCount -= 1;
+ queue->headPos = newHeadPos;
+
+ return ctx;
}
// every time send packet to device, and want to get something from device,
@@ -180,39 +149,27 @@ static void runcamDeviceSendPacket(runcamDevice_t *device, uint8_t command, uint
}
// 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)
+static void runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_t commandID, uint8_t *paramData, uint8_t paramDataLen, timeUs_t tiemout, int maxRetryTimes, void *userInfo, rcdeviceRespParseFunc parseFunc)
{
- 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;
+ runcamDeviceFlushRxBuffer(device);
- // 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 = 5;
- timeoutMs = 60; // we have test some device, 60ms as timeout, and retry times be 5, it's stable for most case
- }
+ rcdeviceResponseParseContext_t responseCtx;
+ memset(&responseCtx, 0, sizeof(rcdeviceResponseParseContext_t));
+ responseCtx.command = commandID;
+ responseCtx.maxRetryTimes = maxRetryTimes;
+ responseCtx.expectedRespLen = runcamDeviceGetRespLen(commandID);
+ responseCtx.timeout = tiemout;
+ responseCtx.timeoutTimestamp = millis() + tiemout;
+ responseCtx.parserFunc = parseFunc;
+ responseCtx.device = device;
+ responseCtx.protocolVer = RCDEVICE_PROTOCOL_VERSION_1_0;
+ memcpy(responseCtx.paramData, paramData, paramDataLen);
+ responseCtx.paramDataLen = paramDataLen;
+ responseCtx.userInfo = userInfo;
+ rcdeviceRespCtxQueuePushRespCtx(&watingResponseQueue, &responseCtx);
- for (int i = 0; i < max_retries; i++) {
- // 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;
+ // send packet
+ runcamDeviceSendPacket(device, commandID, paramData, paramDataLen);
}
static uint8_t calcCRCFromData(uint8_t *ptr, uint8_t len)
@@ -232,110 +189,40 @@ static uint8_t calcCRCFromData(uint8_t *ptr, uint8_t len)
return crc;
}
-static void sendCtrlCommand(runcamDevice_t *device, rcsplit_ctrl_argument_e argument)
+static void runcamDeviceParseV2DeviceInfo(rcdeviceResponseParseContext_t *ctx)
{
- if (!device->serialPort) {
- return ;
+ if (ctx->result != RCDEVICE_RESP_SUCCESS) {
+ ctx->device->isReady = false;
+ return;
}
+ runcamDevice_t *device = ctx->device;
+ device->info.protocolVersion = ctx->recvBuf[1];
- uint8_t uart_buffer[5] = {0};
- uint8_t crc = 0;
-
- uart_buffer[0] = RCSPLIT_PACKET_HEADER;
- uart_buffer[1] = RCSPLIT_PACKET_CMD_CTRL;
- uart_buffer[2] = argument;
- uart_buffer[3] = RCSPLIT_PACKET_TAIL;
- crc = calcCRCFromData(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(device->serialPort, uart_buffer, 5);
+ uint8_t featureLowBits = ctx->recvBuf[2];
+ uint8_t featureHighBits = ctx->recvBuf[3];
+ device->info.features = (featureHighBits << 8) | featureLowBits;
+ device->isReady = true;
}
// 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)
+static void runcamDeviceGetDeviceInfo(runcamDevice_t *device)
{
- // Send "who are you" command to device to detect the device whether is running RCSplit FW1.0 or RCSplit FW1.1
- int max_retries = 2;
- for (int i = 0; i < max_retries; i++) {
- runcamDeviceFlushRxBuffer(device);
- sendCtrlCommand(device, RCSPLIT_CTRL_ARGU_WHO_ARE_YOU);
-
- timeMs_t timeout = millis() + 500;
- uint8_t response[5] = { 0 };
- while (millis() < timeout) {
- if (serialRxBytesWaiting(device->serialPort) >= 5) {
- response[0] = serialRead(device->serialPort);
- response[1] = serialRead(device->serialPort);
- response[2] = serialRead(device->serialPort);
- response[3] = serialRead(device->serialPort);
- response[4] = serialRead(device->serialPort);
- if (response[0] != RCSPLIT_PACKET_HEADER || response[1] != RCSPLIT_PACKET_CMD_CTRL || response[2] != RCSPLIT_CTRL_ARGU_WHO_ARE_YOU || response[4] != RCSPLIT_PACKET_TAIL) {
- break;
- }
-
- uint8_t crcFromPacket = response[3];
- response[3] = response[4]; // move packet tail field to crc field, and calc crc with first 4 bytes
- uint8_t crc = calcCRCFromData(response, 4);
- if (crc != crcFromPacket) {
- break;
- }
-
- // generate response for RCSplit FW 1.0 and FW 1.1
- outputBuffer[0] = RCDEVICE_PROTOCOL_HEADER;
- // protocol version
- outputBuffer[1] = RCDEVICE_PROTOCOL_RCSPLIT_VERSION;
- // features
- outputBuffer[2] = RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON | RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON | RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE;
- outputBuffer[3] = 0;
-
- crc = 0;
- const uint8_t * const end = outputBuffer + 4;
- for (const uint8_t *ptr = outputBuffer; ptr < end; ++ptr) {
- crc = crc8_dvb_s2(crc, *ptr);
- }
- outputBuffer[4] = crc;
- return true;
- }
- }
- }
-
- return runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, NULL, 0, outputBuffer, NULL);
+ runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, NULL, 0, 5000, 0, NULL, runcamDeviceParseV2DeviceInfo);
}
-static bool runcamDeviceSend5KeyOSDCableConnectionEvent(runcamDevice_t *device, uint8_t operation, uint8_t *outActionID, uint8_t *outErrorCode)
+static void runcamDeviceSend5KeyOSDCableConnectionEvent(runcamDevice_t *device, uint8_t operation, rcdeviceRespParseFunc parseFunc)
{
- 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;
+ runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 200, 1, NULL, parseFunc);
}
// 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)
+void runcamDeviceInit(runcamDevice_t *device)
{
+ device->isReady = false;
serialPortFunction_e portID = FUNCTION_RCDEVICE;
serialPortConfig_t *portConfig = findSerialPortConfig(portID);
if (portConfig != NULL) {
@@ -344,30 +231,14 @@ bool runcamDeviceInit(runcamDevice_t *device)
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);
+ runcamDeviceGetDeviceInfo(device);
}
}
-
- device->serialPort = NULL;
- return false;
}
bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation)
{
- if (device->info.protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) {
- sendCtrlCommand(device, operation + 1);
- } else if (device->info.protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) {
+ if (device->info.protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) {
runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL, &operation, sizeof(operation));
} else {
return false;
@@ -378,392 +249,95 @@ bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation)
// every time start to control the OSD menu of camera, must call this method to
// camera
-bool runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device)
+void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
{
- 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);
+ runcamDeviceSend5KeyOSDCableConnectionEvent(device, RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN, parseFunc);
}
// when the control was stop, must call this method to the camera to disconnect
// with camera.
-bool runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, uint8_t *resultCode)
+void runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
{
- 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;
+ runcamDeviceSend5KeyOSDCableConnectionEvent(device, RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE, parseFunc);
}
// simulate button press event of 5 key osd cable with special button
-bool runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation)
+void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation, rcdeviceRespParseFunc parseFunc)
{
if (operation == RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE) {
- return false;
+ return;
}
- if (runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), NULL, NULL)) {
- return true;
- }
-
- return false;
+ runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), 200, 1, NULL, parseFunc);
}
// simulate button release event of 5 key osd cable
-bool runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device)
+void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
{
- return runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, NULL, NULL);
+ runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, 200, 1, NULL, parseFunc);
}
-// 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)
+static rcdeviceResponseParseContext_t* getWaitingResponse(timeUs_t currentTimeUs)
{
- 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) {
+ rcdeviceResponseParseContext_t *respCtx = rcdeviceRespCtxQueuePeekFront(&watingResponseQueue);
+ while (respCtx != NULL && respCtx->timeoutTimestamp != 0 && currentTimeUs > respCtx->timeoutTimestamp) {
+ if (respCtx->timeoutTimestamp != 0 && currentTimeUs > respCtx->timeoutTimestamp) {
+ if (respCtx->maxRetryTimes > 0) {
+ runcamDeviceSendPacket(respCtx->device, respCtx->command, respCtx->paramData, respCtx->paramDataLen);
+ respCtx->timeoutTimestamp = currentTimeUs + respCtx->timeout;
+ respCtx->maxRetryTimes -= 1;
+ respCtx = NULL;
break;
+ } else {
+ respCtx->result = RCDEVICE_RESP_TIMEOUT;
+ if (respCtx->parserFunc != NULL) {
+ respCtx->parserFunc(respCtx);
+ }
+
+ // dequeue and get next waiting response context
+ rcdeviceRespCtxQueueShift(&watingResponseQueue);
+ respCtx = rcdeviceRespCtxQueuePeekFront(&watingResponseQueue);
+ }
+ }
+ }
+
+ return respCtx;
+}
+
+void rcdeviceReceive(timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+ rcdeviceResponseParseContext_t *respCtx = NULL;
+ while ((respCtx = getWaitingResponse(millis())) != NULL && serialRxBytesWaiting(respCtx->device->serialPort)) {
+ const uint8_t c = serialRead(respCtx->device->serialPort);
+ respCtx->recvBuf[respCtx->recvRespLen] = c;
+ respCtx->recvRespLen += 1;
+
+ // if data received done, trigger callback to parse response data, and update rcdevice state
+ if (respCtx->recvRespLen == respCtx->expectedRespLen) {
+ // verify the crc value
+ if (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) {
+ uint8_t crcFromPacket = respCtx->recvBuf[3];
+ respCtx->recvBuf[3] = respCtx->recvBuf[4]; // move packet tail field to crc field, and calc crc with first 4 bytes
+ uint8_t crc = calcCRCFromData(respCtx->recvBuf, 4);
+ respCtx->result = (crc == crcFromPacket) ? RCDEVICE_RESP_SUCCESS : RCDEVICE_RESP_INCORRECT_CRC;
+
+ } else if (respCtx->protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0) {
+ uint8_t crc = 0;
+ for (int i = 0; i < respCtx->recvRespLen; i++) {
+ crc = crc8_dvb_s2(crc, respCtx->recvBuf[i]);
+ }
+ respCtx->result = (crc == 0) ? RCDEVICE_RESP_SUCCESS : RCDEVICE_RESP_INCORRECT_CRC;
}
- 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++;
+ if (respCtx->parserFunc != NULL) {
+ respCtx->parserFunc(respCtx);
+ }
+
+ // dequeue current response context
+ rcdeviceRespCtxQueueShift(&watingResponseQueue);
}
}
- 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
diff --git a/src/main/io/rcdevice.h b/src/main/io/rcdevice.h
index 52a9030249..8119b95f5a 100644
--- a/src/main/io/rcdevice.h
+++ b/src/main/io/rcdevice.h
@@ -29,13 +29,6 @@
#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
@@ -45,17 +38,6 @@
#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 {
@@ -63,8 +45,6 @@ typedef enum {
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),
@@ -116,127 +96,65 @@ typedef enum {
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;
+ bool isReady;
} runcamDevice_t;
-bool runcamDeviceInit(runcamDevice_t *device);
+#define MAX_WAITING_RESPONSES 20
+
+typedef enum {
+ RCDEVICE_RESP_SUCCESS = 0,
+ RCDEVICE_RESP_INCORRECT_CRC = 1,
+ RCDEVICE_RESP_TIMEOUT = 2
+} rcdeviceResponseStatus_e;
+
+typedef struct rcdeviceResponseParseContext_s rcdeviceResponseParseContext_t;
+typedef void(*rcdeviceRespParseFunc)(rcdeviceResponseParseContext_t*);
+struct rcdeviceResponseParseContext_s {
+ uint8_t command;
+ uint8_t expectedRespLen; // total length of response data
+ uint8_t recvRespLen; // length of the data received
+ uint8_t recvBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; // response data buffer
+ timeUs_t timeout;
+ timeUs_t timeoutTimestamp; // if zero, it's means keep waiting for the response
+ rcdeviceRespParseFunc parserFunc;
+ runcamDevice_t *device;
+ uint8_t paramData[RCDEVICE_PROTOCOL_MAX_DATA_SIZE];
+ uint8_t paramDataLen;
+ uint8_t protocolVer;
+ int maxRetryTimes;
+ void *userInfo;
+ rcdeviceResponseStatus_e result;
+};
+
+typedef struct {
+ uint8_t headPos; // current head position of the queue
+ uint8_t tailPos;
+ uint8_t itemCount; // the item count in the queue
+ rcdeviceResponseParseContext_t buffer[MAX_WAITING_RESPONSES];
+ rcdeviceRespParseFunc parseFunc;
+} rcdeviceWaitingResponseQueue;
+
+void runcamDeviceInit(runcamDevice_t *device);
+void rcdeviceReceive(timeUs_t currentTimeUs);
// 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);
+void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc);
+void runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc);
+void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation, rcdeviceRespParseFunc parseFunc);
+void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc);
-// 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);
+void rcdeviceReceive(timeUs_t currentTimeUs);
\ No newline at end of file
diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c
index faf1978733..e88c79001e 100644
--- a/src/main/io/rcdevice_cam.c
+++ b/src/main/io/rcdevice_cam.c
@@ -34,21 +34,28 @@
#include "io/beeper.h"
#include "io/serial.h"
+#include "io/osd.h"
#include "io/rcdevice_cam.h"
#include "rx/rx.h"
+#include "fc/config.h"
+#include "config/feature.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;
+bool rcdeviceInMenu = false;
+bool isButtonPressed = false;
+bool waitingDeviceResponse = false;
+
static bool isFeatureSupported(uint8_t feature)
{
@@ -59,21 +66,6 @@ static bool isFeatureSupported(uint8_t feature)
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)
{
return camDevice->serialPort != NULL;
@@ -88,39 +80,13 @@ static bool rcdeviceIs5KeyEnabled(void)
return false;
}
-static bool reInitializeDevice() {
-#define MAX_RETRY_COUNT 4
-#define RETRY_INTERVAL_MS 500
- static timeMs_t lastInitializeTime = 0;
- static int tryInitCount = 0;
- bool result = false;
-
- if (ARMING_FLAG(ARMED)) {
- return false;
- }
-
- if (tryInitCount < MAX_RETRY_COUNT) {
- timeMs_t nextRetryTime = lastInitializeTime + RETRY_INTERVAL_MS;
- if (millis() >= nextRetryTime) {
- result = rcdeviceInit();
- tryInitCount++;
- lastInitializeTime = millis();
- }
- }
-
- return result;
-}
-
static void rcdeviceCameraControlProcess(void)
{
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
uint8_t switchIndex = i - BOXCAMERA1;
if (IS_RC_MODE_ACTIVE(i)) {
- if (!rcdeviceIsCameraControlEnabled()) {
- reInitializeDevice();
- }
-
+
// 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) {
@@ -136,7 +102,7 @@ static void rcdeviceCameraControlProcess(void)
break;
case BOXCAMERA2:
if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON)) {
- behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN;
+ behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN;
}
break;
case BOXCAMERA3:
@@ -157,7 +123,69 @@ static void rcdeviceCameraControlProcess(void)
}
}
-static bool rcdeviceCamSimulate5KeyCablePress(rcdeviceCamSimulationKeyEvent_e key)
+static void rcdeviceSimulationOSDCableFailed(rcdeviceResponseParseContext_t *ctx)
+{
+ if (ctx->command == RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION) {
+ uint8_t operationID = ctx->paramData[0];
+ if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE) {
+ waitingDeviceResponse = false;
+ return;
+ }
+ } else {
+ rcdeviceInMenu = false;
+ waitingDeviceResponse = false;
+ }
+}
+
+static void rcdeviceSimulationRespHandle(rcdeviceResponseParseContext_t *ctx)
+{
+ if (ctx->result != RCDEVICE_RESP_SUCCESS) {
+ rcdeviceSimulationOSDCableFailed(ctx);
+ return ;
+ }
+
+ switch (ctx->command) {
+ case RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE:
+ {
+ UNUSED(ctx);
+ isButtonPressed = false;
+ }
+ break;
+ case RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION:
+ {
+ // the high 4 bits is the operationID that we sent
+ // the low 4 bits is the result code
+ uint8_t operationID = ctx->paramData[0];
+ bool errorCode = (ctx->recvBuf[1] & 0x0F);
+ if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN) {
+ if (errorCode == 1) {
+ rcdeviceInMenu = true;
+ beeper(BEEPER_CAM_CONNECTION_OPEN);
+ } else {
+ rcdeviceInMenu = false;
+ beeper(BEEPER_CAM_CONNECTION_CLOSE);
+ }
+ } else if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE) {
+ if (errorCode == 1) {
+ rcdeviceInMenu = false;
+ beeper(BEEPER_CAM_CONNECTION_CLOSE);
+ }
+ }
+ }
+ break;
+ case RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS:
+ {
+ isButtonPressed = true;
+ }
+ break;
+ }
+
+ waitingDeviceResponse = false;
+
+ return ;
+}
+
+static void rcdeviceCamSimulate5KeyCablePress(rcdeviceCamSimulationKeyEvent_e key)
{
uint8_t operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE;
switch (key) {
@@ -176,32 +204,23 @@ static bool rcdeviceCamSimulate5KeyCablePress(rcdeviceCamSimulationKeyEvent_e ke
case RCDEVICE_CAM_KEY_ENTER:
operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET;
break;
+ case RCDEVICE_CAM_KEY_NONE:
default:
operation = RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE;
break;
}
- return runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, operation);
+ runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, operation, rcdeviceSimulationRespHandle);
}
-static bool rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key)
+void 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);
- }
+ runcamDeviceOpen5KeyOSDCableConnection(camDevice, rcdeviceSimulationRespHandle);
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);
- }
+ runcamDeviceClose5KeyOSDCableConnection(camDevice, rcdeviceSimulationRespHandle);
}
break;
case RCDEVICE_CAM_KEY_ENTER:
@@ -209,17 +228,17 @@ static bool rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEven
case RCDEVICE_CAM_KEY_UP:
case RCDEVICE_CAM_KEY_RIGHT:
case RCDEVICE_CAM_KEY_DOWN:
- reqResult = rcdeviceCamSimulate5KeyCablePress(key);
+ rcdeviceCamSimulate5KeyCablePress(key);
break;
case RCDEVICE_CAM_KEY_RELEASE:
- reqResult = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice);
+ runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice, rcdeviceSimulationRespHandle);
break;
+ case RCDEVICE_CAM_KEY_NONE:
default:
- reqResult = false;
break;
}
- return reqResult;
+ return;
}
static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs)
@@ -236,21 +255,20 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs)
return;
}
- rcdeviceCamSimulationKeyEvent_e key = RCDEVICE_CAM_KEY_NONE;
+ if (waitingDeviceResponse) {
+ return;
+ }
- if (needRelease) {
+ if (isButtonPressed) {
if (IS_MID(YAW) && IS_MID(PITCH) && IS_MID(ROLL)) {
- if ((camDevice->serialPort != NULL || reInitializeDevice()) && rcdeviceIs5KeyEnabled()) {
- key = RCDEVICE_CAM_KEY_RELEASE;
- if (rcdeviceSend5KeyOSDCableSimualtionEvent(key)) {
- needRelease = false;
- } else {
- rcdeviceInMenu = false;
- }
+ if (rcdeviceIs5KeyEnabled()) {
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
+ waitingDeviceResponse = true;
}
}
- return;
} else {
+ rcdeviceCamSimulationKeyEvent_e key = RCDEVICE_CAM_KEY_NONE;
+
if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_LO(YAW)) { // Disconnect HI YAW
if (rcdeviceInMenu) {
key = RCDEVICE_CAM_KEY_CONNECTION_CLOSE;
@@ -274,14 +292,11 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs)
}
}
}
- }
- if (key != RCDEVICE_CAM_KEY_NONE) {
- if ((camDevice->serialPort != NULL || reInitializeDevice()) && rcdeviceIs5KeyEnabled()) {
- if (rcdeviceSend5KeyOSDCableSimualtionEvent(key)) {
- needRelease = true;
- } else {
- rcdeviceInMenu = false;
+ if (key != RCDEVICE_CAM_KEY_NONE) {
+ if (rcdeviceIs5KeyEnabled()) {
+ rcdeviceSend5KeyOSDCableSimualtionEvent(key);
+ waitingDeviceResponse = true;
}
}
}
@@ -289,23 +304,23 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs)
void rcdeviceUpdate(timeUs_t currentTimeUs)
{
+ rcdeviceReceive(currentTimeUs);
+
rcdeviceCameraControlProcess();
rcdevice5KeySimulationProcess(currentTimeUs);
}
-bool rcdeviceInit(void)
+void rcdeviceInit(void)
{
// open serial port
- if (!runcamDeviceInit(camDevice)) {
- return false;
- }
+ runcamDeviceInit(camDevice);
for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) {
uint8_t switchIndex = i - BOXCAMERA1;
switchStates[switchIndex].isActivated = true;
}
- return true;
+ return;
}
#endif
diff --git a/src/main/io/rcdevice_cam.h b/src/main/io/rcdevice_cam.h
index 6d36c77642..d03a114a7e 100644
--- a/src/main/io/rcdevice_cam.h
+++ b/src/main/io/rcdevice_cam.h
@@ -39,10 +39,12 @@ typedef struct rcdeviceSwitchState_s {
extern runcamDevice_t *camDevice;
extern bool rcdeviceInMenu;
-bool rcdeviceInit(void);
+void rcdeviceInit(void);
void rcdeviceUpdate(timeUs_t currentTimeUs);
bool rcdeviceIsEnabled(void);
// used for unit test
rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
+
+void rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key);
diff --git a/src/main/io/rcdevice_osd.c b/src/main/io/rcdevice_osd.c
deleted file mode 100644
index 252b551ef2..0000000000
--- a/src/main/io/rcdevice_osd.c
+++ /dev/null
@@ -1,247 +0,0 @@
-/*
- * This file is part of Cleanflight and Betaflight.
- *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
- * 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 software.
- *
- * If not, see .
- */
-
-#include
-#include
-
-#include "platform.h"
-
-#ifdef USE_RCDEVICE
-
-#include "io/rcdevice.h"
-
-#include "pg/vcd.h"
-
-#include "rcdevice_osd.h"
-
-#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;
-}
-
-bool rcdeviceOSDIsSynced(const displayPort_t *displayPort)
-{
- UNUSED(displayPort);
- return true;
-}
-
-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
diff --git a/src/main/io/rcdevice_osd.h b/src/main/io/rcdevice_osd.h
deleted file mode 100644
index 3199581c0d..0000000000
--- a/src/main/io/rcdevice_osd.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * This file is part of Cleanflight and Betaflight.
- *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
- * 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 software.
- *
- * If not, see .
- */
-
-#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);
-bool rcdeviceOSDIsSynced(const displayPort_t *displayPort);
-uint32_t rcdeviceOSDTxBytesFree(const displayPort_t *displayPort);
-int rcdeviceScreenSize(const displayPort_t *displayPort);
diff --git a/src/test/Makefile b/src/test/Makefile
index f7f89443b0..02d0cd7bb7 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -289,7 +289,6 @@ rcdevice_unittest_SRC := \
$(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 \
pid_unittest_SRC := \
diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc
index fb67d99ff5..d1e5c98996 100644
--- a/src/test/unit/rcdevice_unittest.cc
+++ b/src/test/unit/rcdevice_unittest.cc
@@ -42,7 +42,6 @@ extern "C" {
#include "io/rcdevice_cam.h"
#include "io/osd.h"
#include "io/rcdevice.h"
- #include "io/rcdevice_osd.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
@@ -55,13 +54,18 @@ extern "C" {
extern rcdeviceSwitchState_t switchStates[BOXCAMERA3 - BOXCAMERA1 + 1];
extern runcamDevice_t *camDevice;
- extern bool needRelease;
+ extern bool isButtonPressed;
+ extern bool rcdeviceInMenu;
+ extern rcdeviceWaitingResponseQueue watingResponseQueue;
bool unitTestIsSwitchActivited(boxId_e boxId)
{
uint8_t adjustBoxID = boxId - BOXCAMERA1;
rcdeviceSwitchState_s switchState = switchStates[adjustBoxID];
return switchState.isActivated;
}
+
+ uint32_t millis(void);
+ int minTimeout = 400;
}
#define MAX_RESPONSES_COUNT 10
@@ -94,13 +98,7 @@ static void clearResponseBuff()
static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlushSerial)
{
- if (withDataForFlushSerial) {
- memcpy(testData.responesBufs[testData.responseBufCount], "0", 1);
- testData.responseBufsLen[testData.responseBufCount] = 1;
- testData.responseBufCount++;
- }
-
-
+ UNUSED(withDataForFlushSerial);
memcpy(testData.responesBufs[testData.responseBufCount], data, dataLen);
testData.responseBufsLen[testData.responseBufCount] = dataLen;
testData.responseBufCount++;
@@ -109,22 +107,27 @@ static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlus
TEST(RCDeviceTest, TestRCSplitInitWithoutPortConfigurated)
{
runcamDevice_t device;
-
+ watingResponseQueue.headPos = 0;
+ watingResponseQueue.tailPos = 0;
+ watingResponseQueue.itemCount = 0;
memset(&testData, 0, sizeof(testData));
- bool result = runcamDeviceInit(&device);
- EXPECT_EQ(false, result);
+ runcamDeviceInit(&device);
+ EXPECT_EQ(false, device.isReady);
}
TEST(RCDeviceTest, TestRCSplitInitWithoutOpenPortConfigurated)
{
runcamDevice_t device;
+ watingResponseQueue.headPos = 0;
+ watingResponseQueue.tailPos = 0;
+ watingResponseQueue.itemCount = 0;
memset(&testData, 0, sizeof(testData));
testData.isRunCamSplitOpenPortSupported = false;
testData.isRunCamSplitPortConfigurated = true;
- bool result = runcamDeviceInit(&device);
- EXPECT_EQ(false, result);
+ runcamDeviceInit(&device);
+ EXPECT_EQ(false, device.isReady);
}
TEST(RCDeviceTest, TestInitDevice)
@@ -132,6 +135,9 @@ TEST(RCDeviceTest, TestInitDevice)
runcamDevice_t device;
// test correct response
+ watingResponseQueue.headPos = 0;
+ watingResponseQueue.tailPos = 0;
+ watingResponseQueue.itemCount = 0;
memset(&testData, 0, sizeof(testData));
testData.isRunCamSplitOpenPortSupported = true;
testData.isRunCamSplitPortConfigurated = true;
@@ -139,8 +145,14 @@ TEST(RCDeviceTest, TestInitDevice)
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
addResponseData(responseData, sizeof(responseData), true);
- bool result = runcamDeviceInit(&device);
- EXPECT_EQ(result, true);
+ runcamDeviceInit(&device);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ testData.responseDataReadPos = 0;
+ testData.indexOfCurrentRespBuf = 0;
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(device.isReady, true);
}
TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse)
@@ -148,6 +160,9 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse)
runcamDevice_t device;
// test correct response data with incorrect len
+ watingResponseQueue.headPos = 0;
+ watingResponseQueue.tailPos = 0;
+ watingResponseQueue.itemCount = 0;
memset(&testData, 0, sizeof(testData));
testData.isRunCamSplitOpenPortSupported = true;
testData.isRunCamSplitPortConfigurated = true;
@@ -155,46 +170,84 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse)
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD, 0x33 };
addResponseData(responseData, sizeof(responseData), true);
- bool result = runcamDeviceInit(&device);
- EXPECT_EQ(result, true);
+ runcamDeviceInit(&device);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ testData.responseDataReadPos = 0;
+ testData.indexOfCurrentRespBuf = 0;
+ printf("call receiver func again\n");
+ rcdeviceReceive(millis());
+ EXPECT_EQ(device.isReady, true);
clearResponseBuff();
+ testData.millis += minTimeout;
// invalid crc
uint8_t responseDataWithInvalidCRC[] = { 0xCC, 0x01, 0x37, 0x00, 0xBE };
addResponseData(responseDataWithInvalidCRC, sizeof(responseDataWithInvalidCRC), true);
- result = runcamDeviceInit(&device);
- EXPECT_EQ(result, false);
+ runcamDeviceInit(&device);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ testData.responseDataReadPos = 0;
+ testData.indexOfCurrentRespBuf = 0;
+ printf("call receiver func again11\n");
+ rcdeviceReceive(millis());
+ EXPECT_EQ(device.isReady, false);
clearResponseBuff();
+ testData.millis += minTimeout;
// incomplete response data
uint8_t incompleteResponseData[] = { 0xCC, 0x01, 0x37 };
addResponseData(incompleteResponseData, sizeof(incompleteResponseData), true);
- result = runcamDeviceInit(&device);
- EXPECT_EQ(result, false);
+ runcamDeviceInit(&device);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ testData.responseDataReadPos = 0;
+ testData.indexOfCurrentRespBuf = 0;
+ printf("call receiver func again2222\n");
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(device.isReady, false);
clearResponseBuff();
+ testData.millis += minTimeout;
// test timeout
memset(&testData, 0, sizeof(testData));
testData.isRunCamSplitOpenPortSupported = true;
testData.isRunCamSplitPortConfigurated = true;
testData.isAllowBufferReadWrite = true;
- result = runcamDeviceInit(&device);
- EXPECT_EQ(result, false);
+ runcamDeviceInit(&device);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ testData.responseDataReadPos = 0;
+ testData.indexOfCurrentRespBuf = 0;
+ printf("call receiver func again3333\n");
+ rcdeviceReceive(millis());
+ EXPECT_EQ(device.isReady, false);
clearResponseBuff();
+ testData.millis += minTimeout;
}
TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready)
{
// test correct response
+ watingResponseQueue.headPos = 0;
+ watingResponseQueue.tailPos = 0;
+ watingResponseQueue.itemCount = 0;
memset(&testData, 0, sizeof(testData));
testData.isRunCamSplitOpenPortSupported = true;
testData.isRunCamSplitPortConfigurated = true;
testData.isAllowBufferReadWrite = true;
testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBC };
+ uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBC }; // wrong response
addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, false);
+ rcdeviceInit();
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ testData.responseDataReadPos = 0;
+ testData.indexOfCurrentRespBuf = 0;
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(camDevice->isReady, false);
// bind aux1, aux2, aux3 channel to wifi button, power button and change mode
for (uint8_t i = 0; i <= (BOXCAMERA3 - BOXCAMERA1); i++) {
@@ -244,8 +297,16 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady)
testData.maxTimesOfRespDataAvailable = 0;
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, true);
+ camDevice->info.features = 15;
+ rcdeviceInit();
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ testData.responseDataReadPos = 0;
+ testData.indexOfCurrentRespBuf = 0;
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ printf("pr222222otocol ver:%d\n", camDevice->info.features);
+ EXPECT_EQ(camDevice->isReady, true);
// bind aux1, aux2, aux3 channel to wifi button, power button and change mode
for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) {
@@ -296,8 +357,14 @@ TEST(RCDeviceTest, TestWifiModeChangeCombine)
testData.maxTimesOfRespDataAvailable = 0;
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(true, result);
+ rcdeviceInit();
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ testData.responseDataReadPos = 0;
+ testData.indexOfCurrentRespBuf = 0;
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, camDevice->isReady);
// bind aux1, aux2, aux3 channel to wifi button, power button and change mode
for (uint8_t i = 0; i <= BOXCAMERA3 - BOXCAMERA1; i++) {
@@ -372,111 +439,192 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
testData.maxTimesOfRespDataAvailable = 0;
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(true, result);
+ rcdeviceInit();
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ testData.responseDataReadPos = 0;
+ testData.indexOfCurrentRespBuf = 0;
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, camDevice->isReady);
+ clearResponseBuff();
+ printf("pass init device\n");
// test timeout of open connection
- result = runcamDeviceOpen5KeyOSDCableConnection(camDevice);
- EXPECT_EQ(false, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN);
+ printf("waiting open connection \n");
+ rcdeviceReceive(millis());
+ testData.millis += 3000;
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(false, rcdeviceInMenu);
clearResponseBuff();
// open connection with correct response
uint8_t responseDataOfOpenConnection[] = { 0xCC, 0x11, 0xe7 };
addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true);
- result = runcamDeviceOpen5KeyOSDCableConnection(camDevice);
- EXPECT_EQ(true, result);
+ printf("waiting open connection 222222\n");
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN);
+ printf("start to recei vdedata\n");
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, rcdeviceInMenu);
clearResponseBuff();
// open connection with correct response but wrong data length
uint8_t incorrectResponseDataOfOpenConnection1[] = { 0xCC, 0x11, 0xe7, 0x55 };
addResponseData(incorrectResponseDataOfOpenConnection1, sizeof(incorrectResponseDataOfOpenConnection1), true);
- result = runcamDeviceOpen5KeyOSDCableConnection(camDevice);
- EXPECT_EQ(true, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, rcdeviceInMenu);
clearResponseBuff();
// open connection with invalid crc
uint8_t incorrectResponseDataOfOpenConnection2[] = { 0xCC, 0x10, 0x42 };
addResponseData(incorrectResponseDataOfOpenConnection2, sizeof(incorrectResponseDataOfOpenConnection2), true);
- result = runcamDeviceOpen5KeyOSDCableConnection(camDevice);
- EXPECT_EQ(false, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, rcdeviceInMenu); // when crc wrong won't change the menu state
clearResponseBuff();
// test timeout of close connection
- runcamDeviceClose5KeyOSDCableConnection(camDevice, NULL);
- EXPECT_EQ(false, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE);
+ rcdeviceReceive(millis());
+ testData.millis += 3000;
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, rcdeviceInMenu); // close menu timeout won't change the menu state
clearResponseBuff();
// close connection with correct response
uint8_t responseDataOfCloseConnection[] = { 0xCC, 0x21, 0x11 };
addResponseData(responseDataOfCloseConnection, sizeof(responseDataOfCloseConnection), true);
- result = runcamDeviceClose5KeyOSDCableConnection(camDevice, NULL);
- EXPECT_EQ(true, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(false, rcdeviceInMenu);
clearResponseBuff();
// close connection with correct response but wrong data length
+ addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); // open menu again
+ printf("start to recei vdedata\n");
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, rcdeviceInMenu);
+ clearResponseBuff();
+
uint8_t responseDataOfCloseConnection1[] = { 0xCC, 0x21, 0x11, 0xC1 };
addResponseData(responseDataOfCloseConnection1, sizeof(responseDataOfCloseConnection1), true);
- result = runcamDeviceClose5KeyOSDCableConnection(camDevice, NULL);
- EXPECT_EQ(true, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(false, rcdeviceInMenu);
clearResponseBuff();
// close connection with response that invalid crc
+ addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN); // open menu again
+ printf("start to recei vdedata\n");
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, rcdeviceInMenu);
+ clearResponseBuff();
+
uint8_t responseDataOfCloseConnection2[] = { 0xCC, 0x21, 0xA1 };
addResponseData(responseDataOfCloseConnection2, sizeof(responseDataOfCloseConnection2), true);
- result = runcamDeviceClose5KeyOSDCableConnection(camDevice, NULL);
- EXPECT_EQ(false, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, rcdeviceInMenu);
clearResponseBuff();
// simulate press button with no response
- result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET);
- EXPECT_EQ(false, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER);
+ rcdeviceReceive(millis());
+ testData.millis += 2000;
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(false, isButtonPressed);
clearResponseBuff();
// simulate press button with correct response
uint8_t responseDataOfSimulation1[] = { 0xCC, 0xA5 };
addResponseData(responseDataOfSimulation1, sizeof(responseDataOfSimulation1), true);
- result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET);
- EXPECT_EQ(true, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, isButtonPressed);
clearResponseBuff();
// simulate press button with correct response but wrong data length
+ uint8_t responseDataOfSimulation4[] = { 0xCC, 0xA5 };
+ addResponseData(responseDataOfSimulation4, sizeof(responseDataOfSimulation4), true); // release first
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(false, isButtonPressed);
+ clearResponseBuff();
+
uint8_t responseDataOfSimulation2[] = { 0xCC, 0xA5, 0x22 };
addResponseData(responseDataOfSimulation2, sizeof(responseDataOfSimulation2), true);
- result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET);
- EXPECT_EQ(true, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, isButtonPressed);
clearResponseBuff();
// simulate press button event with incorrect response
uint8_t responseDataOfSimulation3[] = { 0xCC, 0xB5, 0x22 };
addResponseData(responseDataOfSimulation3, sizeof(responseDataOfSimulation3), true);
- result = runcamDeviceSimulate5KeyOSDCableButtonPress(camDevice, RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET);
- EXPECT_EQ(false, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, isButtonPressed);
clearResponseBuff();
// simulate release button event
- result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice);
- EXPECT_EQ(false, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
+ rcdeviceReceive(millis());
+ testData.millis += 1200;
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, isButtonPressed);
clearResponseBuff();
// simulate release button with correct response
- uint8_t responseDataOfSimulation4[] = { 0xCC, 0xA5 };
addResponseData(responseDataOfSimulation4, sizeof(responseDataOfSimulation4), true);
- result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice);
- EXPECT_EQ(true, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(false, isButtonPressed);
clearResponseBuff();
// simulate release button with correct response but wrong data length
+ addResponseData(responseDataOfSimulation1, sizeof(responseDataOfSimulation1), true); // press first
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(true, isButtonPressed);
+ clearResponseBuff();
+
uint8_t responseDataOfSimulation5[] = { 0xCC, 0xA5, 0xFF };
addResponseData(responseDataOfSimulation5, sizeof(responseDataOfSimulation5), true);
- result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice);
- EXPECT_EQ(true, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(false, isButtonPressed);
clearResponseBuff();
// simulate release button with incorrect response
uint8_t responseDataOfSimulation6[] = { 0xCC, 0x31, 0xFF };
addResponseData(responseDataOfSimulation6, sizeof(responseDataOfSimulation6), true);
- result = runcamDeviceSimulate5KeyOSDCableButtonRelease(camDevice);
- EXPECT_EQ(false, result);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
+ rcdeviceReceive(millis());
+ testData.millis += minTimeout;
+ EXPECT_EQ(false, isButtonPressed);
clearResponseBuff();
}
@@ -498,8 +646,14 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport)
testData.maxTimesOfRespDataAvailable = 0;
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, true);
+ rcdeviceInit();
+ rcdeviceReceive(millis());
+ testData.millis += 200;
+ testData.responseDataReadPos = 0;
+ testData.indexOfCurrentRespBuf = 0;
+ rcdeviceReceive(millis());
+ testData.millis += 200;
+ EXPECT_EQ(camDevice->isReady, true);
clearResponseBuff();
// open connection, rcdeviceInMenu will be false if the codes is right
@@ -510,877 +664,6 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport)
clearResponseBuff();
}
-TEST(RCDeviceTest, Test5KeyOSDCableSimulationWith5KeyFeatureSupport)
-{
- // test simulation without device init
- rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MAX; // Yaw High
- rcdeviceUpdate(0);
- EXPECT_EQ(false, rcdeviceInMenu);
-
- // init device that have not 5 key OSD cable simulation feature
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 };
- addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // open connection
- uint8_t responseDataOfOpenConnection[] = { 0xCC, 0x11, 0xe7 };
- addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(true, rcdeviceInMenu);
- EXPECT_EQ(true, needRelease);
- clearResponseBuff();
- // relase button
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid
- uint8_t responseDataOfReleaseButton[] = { 0xCC, 0xA5 };
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, needRelease);
- clearResponseBuff();
-
- // close connection
- rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MIN; // Yaw Low
- uint8_t responseDataOfCloseConnection[] = { 0xCC, 0x21, 0x11 };
- addResponseData(responseDataOfCloseConnection, sizeof(responseDataOfCloseConnection), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, rcdeviceInMenu);
- EXPECT_EQ(true, needRelease);
- clearResponseBuff();
- // relase button
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, needRelease);
- clearResponseBuff();
-
- // open osd menu again
- rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MAX; // Yaw High
- addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(true, rcdeviceInMenu);
- EXPECT_EQ(true, needRelease);
- clearResponseBuff();
- // relase button
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, needRelease);
- clearResponseBuff();
-
- // send down button event
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MIN;
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(true, needRelease);
- clearResponseBuff();
- // relase button
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, needRelease);
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // rest down button
- clearResponseBuff();
-
- // simulate right button long press
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MAX;
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(true, needRelease);
- rcdeviceUpdate(0);
- EXPECT_EQ(true, needRelease);
- rcdeviceUpdate(0);
- EXPECT_EQ(true, needRelease);
- clearResponseBuff();
- // send relase button event
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, needRelease);
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // reset right button
- clearResponseBuff();
-
- // simulate right button and get failed response, then FC should release the controller of joysticks
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MAX;
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(true, needRelease);
- clearResponseBuff();
- // send relase button with empty response, so the release will failed
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid
- rcdeviceUpdate(0);
- EXPECT_EQ(true, needRelease);
- EXPECT_EQ(false, rcdeviceInMenu); // if failed on release button event, then FC side need release the controller of joysticks
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // rest right button
- // send again release button with correct response
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid
- clearResponseBuff();
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, needRelease);
- EXPECT_EQ(false, rcdeviceInMenu);
- clearResponseBuff();
-
- // open OSD menu again
- rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MAX; // Yaw High
- clearResponseBuff();
- addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(true, rcdeviceInMenu);
- EXPECT_EQ(true, needRelease);
- clearResponseBuff();
-
- // relase button
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, needRelease);
- clearResponseBuff();
-
- // send left event
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MIN;
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(true, needRelease);
- clearResponseBuff();
- // send relase button
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, needRelease);
- EXPECT_EQ(true, rcdeviceInMenu);
- clearResponseBuff();
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // rest right button
-
- // close connection
- rcData[THROTTLE] = FIVE_KEY_JOYSTICK_MID; // THROTTLE Mid
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MIN; // Yaw High
- addResponseData(responseDataOfCloseConnection, sizeof(responseDataOfCloseConnection), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, rcdeviceInMenu);
- EXPECT_EQ(true, needRelease);
- clearResponseBuff();
- // relase button
- rcData[ROLL] = FIVE_KEY_JOYSTICK_MID; // ROLL Mid
- rcData[PITCH] = FIVE_KEY_JOYSTICK_MID; // PITCH Mid
- rcData[YAW] = FIVE_KEY_JOYSTICK_MID; // Yaw Mid
- addResponseData(responseDataOfReleaseButton, sizeof(responseDataOfReleaseButton), true);
- rcdeviceUpdate(0);
- EXPECT_EQ(false, needRelease);
- clearResponseBuff();
-}
-
-TEST(RCDeviceTest, TestOSDDeviceSupportWithAutoDetectVideoSystem)
-{
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
- uint8_t respDataOfGettingColumnsDetail[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0 };
- uint8_t respDataOfGettingTVModeDetail[] = { 0xCC, 0x00, 0x0B, 0x09, 0x00, 0x4E, 0x54, 0x53, 0x43, 0x3B, 0x50, 0x41, 0x4C, 0x00, 0x08 };
- uint8_t respDataOfWriteBFCharset[] = { 0xCC, 0x00, 0x00, 0x39 };
- addResponseData(responseData, sizeof(responseData), true); // device init response
- addResponseData(respDataOfGettingColumnsDetail, sizeof(respDataOfGettingColumnsDetail), false); // get column counts response
- addResponseData(respDataOfGettingTVModeDetail, sizeof(respDataOfGettingTVModeDetail), false); // get tv mode response
- addResponseData(respDataOfWriteBFCharset, sizeof(respDataOfWriteBFCharset), false); // update charset with BF
- vcdProfile_t profile;
- profile.video_system = VIDEO_SYSTEM_AUTO; // use auto detect
- memset(&profile, 0, sizeof(vcdProfile_t));
- bool r = rcdeviceOSDInit(&profile);
- EXPECT_EQ(true, r);
-}
-
-TEST(RCDeviceTest, TestOSDDeviceSupportWithGetColumnsFailed)
-{
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
- uint8_t respDataOfGettingColumnsDetail[] = { 0xCC, 0x00, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xFf };
- uint8_t respDataOfWriteDataFailed[] = { 0xCC, 0x01, 0x00, 0x32 };
- uint8_t respDataOfWriteDataSuccess[] = { 0xCC, 0x00, 0x00, 0x39 };
- addResponseData(responseData, sizeof(responseData), true); // device init response
- addResponseData(respDataOfGettingColumnsDetail, sizeof(respDataOfGettingColumnsDetail), false); // get column counts response
- addResponseData(respDataOfWriteDataSuccess, sizeof(respDataOfWriteDataSuccess), false); // update TV mode with Pal
- addResponseData(respDataOfWriteDataFailed, sizeof(respDataOfWriteDataFailed), false); // update charset with BF
- vcdProfile_t profile;
- memset(&profile, 0, sizeof(vcdProfile_t));
- profile.video_system = VIDEO_SYSTEM_PAL; // use auto detect
- bool r = rcdeviceOSDInit(&profile);
- EXPECT_EQ(false, r);
-}
-
-TEST(RCDeviceTest, TestOSDDeviceSupportWithPal)
-{
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
- uint8_t respDataOfGettingColumnsDetail[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0 };
- uint8_t respDataOfWriteDataSuccess[] = { 0xCC, 0x00, 0x00, 0x39 };
- addResponseData(responseData, sizeof(responseData), true); // device init response
- addResponseData(respDataOfGettingColumnsDetail, sizeof(respDataOfGettingColumnsDetail), false); // get column counts response
- addResponseData(respDataOfWriteDataSuccess, sizeof(respDataOfWriteDataSuccess), false); // update TV mode with Pal
- addResponseData(respDataOfWriteDataSuccess, sizeof(respDataOfWriteDataSuccess), false); // update charset with BF
- vcdProfile_t profile;
- memset(&profile, 0, sizeof(vcdProfile_t));
- profile.video_system = VIDEO_SYSTEM_PAL; // use auto detect
- bool r = rcdeviceOSDInit(&profile);
- EXPECT_EQ(true, r);
-}
-
-TEST(RCDeviceTest, TestOSDDeviceSupportWithWriteTVModeFailed)
-{
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
- uint8_t respDataOfGettingColumnsDetail[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0 };
- uint8_t respDataOfWriteDataFailed[] = { 0xCC, 0x01, 0x00, 0x32 };
- uint8_t respDataOfWriteDataSuccess[] = { 0xCC, 0x00, 0x00, 0x39 };
- addResponseData(responseData, sizeof(responseData), true); // device init response
- addResponseData(respDataOfGettingColumnsDetail, sizeof(respDataOfGettingColumnsDetail), false); // get column counts response
- addResponseData(respDataOfWriteDataFailed, sizeof(respDataOfWriteDataFailed), false); // update TV mode with Pal
- addResponseData(respDataOfWriteDataSuccess, sizeof(respDataOfWriteDataSuccess), false); // update charset with BF
- vcdProfile_t profile;
- memset(&profile, 0, sizeof(vcdProfile_t));
- profile.video_system = VIDEO_SYSTEM_PAL; // use auto detect
- bool r = rcdeviceOSDInit(&profile);
- EXPECT_EQ(false, r);
-}
-
-TEST(RCDeviceTest, TestOSDDeviceSupportWithWriteCharsetFailed)
-{
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
- uint8_t respDataOfGettingColumnsDetail[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0 };
- uint8_t respDataOfWriteDataFailed[] = { 0xCC, 0x01, 0x00, 0x32 };
- uint8_t respDataOfWriteDataSuccess[] = { 0xCC, 0x00, 0x00, 0x39 };
- addResponseData(responseData, sizeof(responseData), true); // device init response
- addResponseData(respDataOfGettingColumnsDetail, sizeof(respDataOfGettingColumnsDetail), false); // get column counts response
- addResponseData(respDataOfWriteDataSuccess, sizeof(respDataOfWriteDataSuccess), false); // update TV mode with Pal
- addResponseData(respDataOfWriteDataFailed, sizeof(respDataOfWriteDataFailed), false); // update charset with BF
- vcdProfile_t profile;
- memset(&profile, 0, sizeof(vcdProfile_t));
- profile.video_system = VIDEO_SYSTEM_PAL; // use auto detect
- bool r = rcdeviceOSDInit(&profile);
- EXPECT_EQ(false, r);
-}
-
-
-TEST(RCDeviceTest, TestDSAGetSettings)
-{
- // init device that have not 5 key OSD cable simulation feature
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 };
- addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // test getSettings with correct responses
- runcamDeviceSetting_t outSettingList[RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE];
- uint8_t responseDataForGetSettings1[] = { 0xCC, 0x03, 0x18, 0x00, 0x63, 0x68, 0x61, 0x72, 0x73, 0x65, 0x74, 0x00, 0x42, 0x46, 0x00, 0x01, 0x43, 0x4F, 0x4C, 0x55, 0x4D, 0x4E, 0x53, 0x00, 0x33, 0x30, 0x00, 0x22 };
- uint8_t responseDataForGetSettings2[] = { 0xCC, 0x02, 0x23, 0x02, 0x54, 0x56, 0x5F, 0x4D, 0x4F, 0x44, 0x45, 0x00, 0x4E, 0x54, 0x53, 0x43, 0x00, 0x03, 0x53, 0x44, 0x43, 0x41, 0x52, 0x44, 0x5F, 0x43, 0x41, 0x50, 0x41, 0x43, 0x49, 0x54, 0x59, 0x00, 0x30, 0x2F, 0x30, 0x00, 0x9B };
- uint8_t responseDataForGetSettings3[] = { 0xCC, 0x01, 0x33, 0x04, 0x52, 0x45, 0x4D, 0x41, 0x49, 0x4E, 0x49, 0x4E, 0x47, 0x5F, 0x52, 0x45, 0x43, 0x4F, 0x52, 0x44, 0x49, 0x4E, 0x47, 0x5F, 0x54, 0x49, 0x4D, 0x45, 0x00, 0x31, 0x00, 0x05, 0x52, 0x45, 0x53, 0x4F, 0x4C, 0x55, 0x54, 0x49, 0x4F, 0x4E, 0x00, 0x31, 0x30, 0x38, 0x30, 0x70, 0x36, 0x30, 0x66, 0x70, 0x73, 0x00, 0x95 };
- uint8_t responseDataForGetSettings4[] = { 0xCC, 0x00, 0x1F, 0x06, 0x43, 0x41, 0x4D, 0x45, 0x52, 0x41, 0x5F, 0x54, 0x49, 0x4D, 0x45, 0x00, 0x32, 0x30, 0x31, 0x37, 0x31, 0x30, 0x31, 0x30, 0x54, 0x31, 0x37, 0x30, 0x35, 0x31, 0x34, 0x2E, 0x30, 0x00, 0x7C };
- addResponseData(responseDataForGetSettings1, sizeof(responseDataForGetSettings1), true);
- addResponseData(responseDataForGetSettings2, sizeof(responseDataForGetSettings2), false);
- addResponseData(responseDataForGetSettings3, sizeof(responseDataForGetSettings3), false);
- addResponseData(responseDataForGetSettings4, sizeof(responseDataForGetSettings4), false);
- result = runcamDeviceGetSettings(camDevice, 0, (runcamDeviceSetting_t*)outSettingList, RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE);
- uint8_t i = 0;
- printf("get settings:\n");
- while (i < RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE && outSettingList[i].name[0] != 0) {
- printf("%d\t%-20s\t%-20s\n", outSettingList[i].id, outSettingList[i].name, outSettingList[i].value);
- i++;
- }
- EXPECT_EQ(result, true);
- EXPECT_STREQ("charset", outSettingList[0].name);
- EXPECT_STREQ("BF", outSettingList[0].value);
- EXPECT_STREQ("COLUMNS", outSettingList[1].name);
- EXPECT_STREQ("30", outSettingList[1].value);
- EXPECT_STREQ("TV_MODE", outSettingList[2].name);
- EXPECT_STREQ("NTSC", outSettingList[2].value);
- EXPECT_STREQ("SDCARD_CAPACITY", outSettingList[3].name);
- EXPECT_STREQ("0/0", outSettingList[3].value);
- EXPECT_STREQ("REMAINING_RECORDING_1", outSettingList[4].name);
- EXPECT_STREQ("1", outSettingList[4].value);
- EXPECT_STREQ("RESOLUTION", outSettingList[5].name);
- EXPECT_STREQ("1080p60fps", outSettingList[5].value);
- EXPECT_STREQ("CAMERA_TIME", outSettingList[6].name);
- EXPECT_STREQ("20171010T170514.0", outSettingList[6].value);
- clearResponseBuff();
-
- // test get settings with incorrect response
- uint8_t responseDataForGetSettings1WithIncorrectCRC[] = { 0xCC, 0x03, 0x18, 0x00, 0x63, 0x68, 0x61, 0x72, 0x73, 0x65, 0x74, 0x00, 0x42, 0x46, 0x00, 0x01, 0x43, 0x4F, 0x4C, 0x55, 0x4D, 0x4E, 0x53, 0x00, 0x33, 0x30, 0x00, 0xA2 };
- addResponseData(responseDataForGetSettings1WithIncorrectCRC, sizeof(responseDataForGetSettings1WithIncorrectCRC), true);
- result = runcamDeviceGetSettings(camDevice, 0, (runcamDeviceSetting_t*)outSettingList, RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // test get settings with incomplete response again
- uint8_t responseDataForGetSettings1WithIncorrectLength[] = { 0xCC, 0x03, 0x18, 0x00, 0x63, 0x68, 0x61, 0x72, 0x73, 0x01, 0x43, 0x4F, 0x4C, 0x55, 0x4D, 0x4E, 0x53, 0x00, 0x33, 0x30, 0x00, 0xA2 };
- addResponseData(responseDataForGetSettings1, sizeof(responseDataForGetSettings1), true);
- addResponseData(responseDataForGetSettings1WithIncorrectLength, sizeof(responseDataForGetSettings1WithIncorrectLength), false);
- result = runcamDeviceGetSettings(camDevice, 0, (runcamDeviceSetting_t*)outSettingList, RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // test get settings with incorrect length response again
- uint8_t responseDataForGetSettings1WithIncorrectLength2[] = { 0xCC, 0x03, 0x3D, 0x00, 0x63, 0x68, 0x61, 0x72, 0x73, 0x65, 0x74, 0x00, 0x42, 0x46, 0x00, 0x01, 0x43, 0x4F, 0x4C, 0x55, 0x4D, 0x4E, 0x53, 0x00, 0x33, 0x30, 0x00, 0xA2 };
- addResponseData(responseDataForGetSettings1, sizeof(responseDataForGetSettings1), true);
- addResponseData(responseDataForGetSettings1WithIncorrectLength2, sizeof(responseDataForGetSettings1WithIncorrectLength2), false);
- result = runcamDeviceGetSettings(camDevice, 0, (runcamDeviceSetting_t*)outSettingList, RCDEVICE_PROTOCOL_MAX_MENUITEM_PER_PAGE);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-}
-
-TEST(RCDeviceTest, TestDSATextSelectionAccess)
-{
- // init device that have not 5 key OSD cable simulation feature
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 };
- addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // get setting detail that type is text selection
- runcamDeviceSettingDetail_t charsetDetail;
- uint8_t responseDataForCharSetDetail1[] = { 0xCC, 0x00, 0x08, 0x09, 0x00, 0x42, 0x46, 0x3B, 0x43, 0x46, 0x00, 0x1C };
- addResponseData(responseDataForCharSetDetail1, sizeof(responseDataForCharSetDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetDetail);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, charsetDetail.value);
- EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_TEXT_SELECTION, charsetDetail.type);
- if (result) {
- printf("setting detail:\n");
- printf("\tcurrentValue = %d\n", charsetDetail.value);
- printf("\tavailable selections = ");
- bool foundCurValue = false;
- for (uint32_t i = 0; i < RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS; i++) {
- if (charsetDetail.textSelections[i].text[0] == 0) {
- break;
- }
-
- if (i == charsetDetail.value) {
- printf("%s[*], ", charsetDetail.textSelections[i].text);
- foundCurValue = true;
- } else {
- printf("%s, ", charsetDetail.textSelections[i].text);
- }
- }
- printf("\n");
- EXPECT_EQ(foundCurValue, true);
- }
- clearResponseBuff();
-
- // get setting detail that type is text selection with multiple chunk
- uint8_t responseDataForCharSetDetailChunk1[] = { 0xcc, 0x02, 0x3a, 0x09, 0x02, 0x4f, 0x4e, 0x45, 0x3b, 0x54, 0x57, 0x4f, 0x3b, 0x54, 0x48, 0x52, 0x45, 0x45, 0x3b, 0x46, 0x4f, 0x55, 0x52, 0x3b, 0x46, 0x49, 0x56, 0x45, 0x3b, 0x53, 0x49, 0x58, 0x3b, 0x53, 0x45, 0x56, 0x45, 0x4e, 0x3b, 0x45, 0x49, 0x47, 0x48, 0x54, 0x3b, 0x4e, 0x49, 0x4e, 0x45, 0x3b, 0x54, 0x45, 0x4e, 0x3b, 0x45, 0x6c, 0x65, 0x76, 0x65, 0x6e, 0x3b, 0x8e };
- uint8_t responseDataForCharSetDetailChunk2[] = { 0xcc, 0x01, 0x38, 0x54, 0x77, 0x65, 0x6c, 0x76, 0x65, 0x3b, 0x20, 0x74, 0x68, 0x69, 0x72, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x66, 0x6f, 0x75, 0x72, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x66, 0x69, 0x66, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x73, 0x69, 0x78, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x73, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x96 };
- uint8_t responseDataForCharSetDetailChunk3[] = { 0xcc, 0x00, 0x34, 0x45, 0x69, 0x67, 0x68, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x4e, 0x69, 0x6e, 0x65, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x74, 0x77, 0x65, 0x6e, 0x74, 0x79, 0x3b, 0x20, 0x74, 0x77, 0x65, 0x6e, 0x74, 0x79, 0x20, 0x6f, 0x6e, 0x65, 0x3b, 0x20, 0x74, 0x77, 0x65, 0x6e, 0x74, 0x79, 0x20, 0x74, 0x77, 0x6f, 0x3b, 0x00, 0x31 };
- addResponseData(responseDataForCharSetDetailChunk1, sizeof(responseDataForCharSetDetailChunk1), true);
- addResponseData(responseDataForCharSetDetailChunk2, sizeof(responseDataForCharSetDetailChunk2), false);
- addResponseData(responseDataForCharSetDetailChunk3, sizeof(responseDataForCharSetDetailChunk3), false);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetDetail);
- EXPECT_EQ(result, true);
- EXPECT_EQ(2, charsetDetail.value);
- EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_TEXT_SELECTION, charsetDetail.type);
- if (result) {
- printf("setting detail:\n");
- printf("\tcurrentValue = %d\n", charsetDetail.value);
- printf("\tavailable selections = ");
- bool foundCurValue = false;
- for (uint32_t i = 0; i < RCDEVICE_PROTOCOL_MAX_TEXT_SELECTIONS; i++) {
- if (charsetDetail.textSelections[i].text[0] == 0) {
- break;
- }
-
- if (i == charsetDetail.value) {
- printf("%s[*], ", charsetDetail.textSelections[i].text);
- foundCurValue = true;
- } else {
- printf("%s, ", charsetDetail.textSelections[i].text);
- }
- }
- printf("\n");
- EXPECT_EQ(foundCurValue, true);
- }
- clearResponseBuff();
-
- // test get setting detail that type is textselection, but the response is incorrect
- uint8_t incorrectResponseDataForCharSetDetail1[] = { 0xCC, 0x00, 0x08, 0x09, 0x00, 0x42, 0x46, 0x3B, 0x43, 0x46, 0x00, 0x1A };
- addResponseData(incorrectResponseDataForCharSetDetail1, sizeof(incorrectResponseDataForCharSetDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // test get setting detail that type is textselection, but the response length is incorrect
- uint8_t incorrectResponseDataForCharSetDetail2[] = { 0xCC, 0x00, 0x08, 0x09, 0x00, 0x42 };
- addResponseData(incorrectResponseDataForCharSetDetail2, sizeof(incorrectResponseDataForCharSetDetail2), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // test get setting detail that type is textselection multiple chunk, but the response length is incorrect
- uint8_t incorrectResponseDataForCharSetDetailChunk2[] = { 0xcc, 0x01, 0x38, 0x54, 0x77, 0x65, 0x6c, 0x76, 0x65, 0x3b, 0x20, 0x74, 0x68, 0x69, 0x72, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x66, 0x6f, 0x75, 0x72, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x66, 0x69, 0x66, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x73, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x20, 0x73, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x65, 0x65, 0x6e, 0x3b, 0x96 };
- addResponseData(responseDataForCharSetDetailChunk1, sizeof(responseDataForCharSetDetailChunk1), true);
- addResponseData(incorrectResponseDataForCharSetDetailChunk2, sizeof(incorrectResponseDataForCharSetDetailChunk2), false);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // test update setting
- runcamDeviceWriteSettingResponse_t updateSettingResponse;
- uint8_t responweDataForUpdateCharSet1[] = { 0xCC, 0x00, 0x00, 0x39 };
- addResponseData(responweDataForUpdateCharSet1, sizeof(responweDataForUpdateCharSet1), true);
- uint8_t newValue = 1;
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &newValue, sizeof(uint8_t), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, updateSettingResponse.resultCode);
- EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-
- // test update setting with response that needUpdatemenuItems's value is 1
- uint8_t responweDataForUpdateCharSet2[] = { 0xCC, 0x00, 0x01, 0xec };
- addResponseData(responweDataForUpdateCharSet2, sizeof(responweDataForUpdateCharSet2), true);
- newValue = 1;
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &newValue, sizeof(uint8_t), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, updateSettingResponse.resultCode);
- EXPECT_EQ(1, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-
- // test update setting with response that resultCode's value is 1
- uint8_t responweDataForUpdateCharSet3[] = { 0xCC, 0x01, 0x00, 0x32 };
- addResponseData(responweDataForUpdateCharSet3, sizeof(responweDataForUpdateCharSet3), true);
- newValue = 1;
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_CHARSET, &newValue, sizeof(uint8_t), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(1, updateSettingResponse.resultCode);
- EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-}
-
-TEST(RCDeviceTest, TestDSAUint8AccessProtocol)
-{
- // init device that have not 5 key OSD cable simulation feature
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 };
- addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // get setting detail that type is uint8
- runcamDeviceSettingDetail_t charsetDetail;
- uint8_t responseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0 };
- addResponseData(responseDataForColumnsDetail1, sizeof(responseDataForColumnsDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, true);
- EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_UINT8, charsetDetail.type);
- EXPECT_EQ(30, charsetDetail.value);
- EXPECT_EQ(30, charsetDetail.minValue);
- EXPECT_EQ(30, charsetDetail.maxValue);
- EXPECT_EQ(1, charsetDetail.stepSize);
- clearResponseBuff();
-
- // get setting detail that type is uint8, but the response length is incorrect
- uint8_t incorrectResponseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E };
- addResponseData(incorrectResponseDataForColumnsDetail1, sizeof(incorrectResponseDataForColumnsDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // get setting detail that type is uint8, but the response crc is incorrect
- uint8_t incorrectResponseDataForColumnsDetail2[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xC2 };
- addResponseData(incorrectResponseDataForColumnsDetail2, sizeof(incorrectResponseDataForColumnsDetail2), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // get setting detail that type is uint8, but the response crc is incorrect
- uint8_t incorrectResponseDataForColumnsDetail3[] = { 0xCC, 0x00, 0x05, 0x00, 0x1E, 0x1E, 0x1E, 0x01, 0xB0, 0xAA, 0xBB, 0xCC };
- addResponseData(incorrectResponseDataForColumnsDetail3, sizeof(incorrectResponseDataForColumnsDetail3), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // test update setting
- runcamDeviceWriteSettingResponse_t updateSettingResponse;
- uint8_t responweDataForUpdateCharSet1[] = { 0xCC, 0x00, 0x00, 0x39 };
- addResponseData(responweDataForUpdateCharSet1, sizeof(responweDataForUpdateCharSet1), true);
- uint8_t newValue = 1;
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &newValue, sizeof(uint8_t), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, updateSettingResponse.resultCode);
- EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-
- // test update setting with response that needUpdatemenuItems's value is 1
- uint8_t responweDataForUpdateCharSet2[] = { 0xCC, 0x00, 0x01, 0xec };
- addResponseData(responweDataForUpdateCharSet2, sizeof(responweDataForUpdateCharSet2), true);
- newValue = 1;
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &newValue, sizeof(uint8_t), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, updateSettingResponse.resultCode);
- EXPECT_EQ(1, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-
- // test update setting with response that resultCode's value is 1
- uint8_t responweDataForUpdateCharSet3[] = { 0xCC, 0x01, 0x00, 0x32 };
- addResponseData(responweDataForUpdateCharSet3, sizeof(responweDataForUpdateCharSet3), true);
- newValue = 1;
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &newValue, sizeof(uint8_t), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(1, updateSettingResponse.resultCode);
- EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-}
-
-TEST(RCDeviceTest, TestDSAUint16AccessProtocol)
-{
- // init device that have not 5 key OSD cable simulation feature
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 };
- addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // get setting detail that type is uint16
- runcamDeviceSettingDetail_t charsetDetail;
- uint8_t responseDataForColumnsDetail1[] = { 0xcc, 0x00, 0x09, 0x02, 0x1e, 0x3a, 0x1e, 0x00, 0xff, 0xff, 0x64, 0x00, 0x6c };
- addResponseData(responseDataForColumnsDetail1, sizeof(responseDataForColumnsDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, true);
- EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_UINT16, charsetDetail.type);
- EXPECT_EQ(14878, charsetDetail.value);
- EXPECT_EQ(30, charsetDetail.minValue);
- EXPECT_EQ(65535, charsetDetail.maxValue);
- EXPECT_EQ(100, charsetDetail.stepSize);
- clearResponseBuff();
-
- // get setting detail that type is uint16, but the response length is incorrect
- uint8_t incorrectResponseDataForColumnsDetail1[] = { 0xcc, 0x00, 0x09, 0x02, 0x1e, 0x3a, 0x1e, 0x00, 0xff, 0xff };
- addResponseData(incorrectResponseDataForColumnsDetail1, sizeof(incorrectResponseDataForColumnsDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // get setting detail that type is uint16, but the response crc is incorrect
- uint8_t incorrectResponseDataForColumnsDetail2[] = { 0xcc, 0x00, 0x09, 0x02, 0x1e, 0x3a, 0x1e, 0x00, 0xff, 0xff, 0x64, 0x00, 0x7c };
- addResponseData(incorrectResponseDataForColumnsDetail2, sizeof(incorrectResponseDataForColumnsDetail2), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // get setting detail that type is uint16, but the response has some randome data at trailing
- uint8_t incorrectResponseDataForColumnsDetail3[] = { 0xcc, 0x00, 0x09, 0x02, 0x1e, 0x3a, 0x1e, 0x00, 0xff, 0xff, 0x64, 0x00, 0x6c, 0xAA, 0xBB, 0xCC };
- addResponseData(incorrectResponseDataForColumnsDetail3, sizeof(incorrectResponseDataForColumnsDetail3), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // test update setting
- runcamDeviceWriteSettingResponse_t updateSettingResponse;
- uint8_t responweDataForUpdateCharSet1[] = { 0xCC, 0x00, 0x00, 0x39 };
- addResponseData(responweDataForUpdateCharSet1, sizeof(responweDataForUpdateCharSet1), true);
- uint16_t newValue = 55;
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)&newValue, sizeof(uint16_t), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, updateSettingResponse.resultCode);
- EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-
- // test update setting with response that needUpdatemenuItems's value is 1
- uint8_t responweDataForUpdateCharSet2[] = { 0xCC, 0x00, 0x01, 0xec };
- addResponseData(responweDataForUpdateCharSet2, sizeof(responweDataForUpdateCharSet2), true);
- newValue = 190;
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)&newValue, sizeof(uint16_t), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, updateSettingResponse.resultCode);
- EXPECT_EQ(1, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-
- // test update setting with response that resultCode's value is 1
- uint8_t responweDataForUpdateCharSet3[] = { 0xCC, 0x01, 0x00, 0x32 };
- addResponseData(responweDataForUpdateCharSet3, sizeof(responweDataForUpdateCharSet3), true);
- newValue = 10241;
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)&newValue, sizeof(uint16_t), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(1, updateSettingResponse.resultCode);
- EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-}
-
-TEST(RCDeviceTest, TestDSAFloatAccessProtocol)
-{
- // init device that have not 5 key OSD cable simulation feature
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 };
- addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // get setting detail that type is float
- runcamDeviceSettingDetail_t charsetDetail;
- uint8_t responseDataForColumnsDetail1[] = { 0xcc, 0x00, 0x12, 0x08, 0x1e, 0x33, 0x00, 0x00, 0x3a, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0x04, 0xf4, 0x01, 0x00, 0x00, 0xc9 };
- addResponseData(responseDataForColumnsDetail1, sizeof(responseDataForColumnsDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, true);
- EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_FLOAT, charsetDetail.type);
- EXPECT_EQ(13086, charsetDetail.value);
- EXPECT_EQ(58, charsetDetail.minValue);
- EXPECT_EQ(4294967295, charsetDetail.maxValue);
- EXPECT_EQ(4, charsetDetail.decimalPoint);
- EXPECT_EQ(500, charsetDetail.stepSize);
- clearResponseBuff();
-
- // get setting detail that type is float, but the response length is incorrect
- uint8_t incorrectResponseDataForColumnsDetail1[] = { 0xcc, 0x00, 0x07, 0x08, 0x1e, 0x3a, 0xff, 0x0a };
- addResponseData(incorrectResponseDataForColumnsDetail1, sizeof(incorrectResponseDataForColumnsDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // get setting detail that type is float, but the response crc is incorrect
- uint8_t incorrectResponseDataForColumnsDetail2[] = { 0xcc, 0x00, 0x07, 0x08, 0x1e, 0x3a, 0xff, 0x0a, 0x00, 0x01, 0xf2 };
- addResponseData(incorrectResponseDataForColumnsDetail2, sizeof(incorrectResponseDataForColumnsDetail2), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // get setting detail that type is float, but the response has some randome data at trailing
- uint8_t incorrectResponseDataForColumnsDetail3[] = { 0xcc, 0x00, 0x12, 0x08, 0x1e, 0x33, 0x00, 0x00, 0x3a, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0x04, 0xf4, 0x01, 0x00, 0x00, 0xc9, 0xAA, 0xBB, 0xCC };
- addResponseData(incorrectResponseDataForColumnsDetail3, sizeof(incorrectResponseDataForColumnsDetail3), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // test update setting
- runcamDeviceWriteSettingResponse_t updateSettingResponse;
- uint8_t responweDataForUpdateCharSet1[] = { 0xCC, 0x00, 0x00, 0x39 };
- addResponseData(responweDataForUpdateCharSet1, sizeof(responweDataForUpdateCharSet1), true);
- uint16_t newValue[5] = { 0x1E, 0x1E, 0x00, 0x00, 0x2 }; // the data struct is UINT32_T + UINT8(Decimal Point)
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, updateSettingResponse.resultCode);
- EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-
- // test update setting with response that needUpdatemenuItems's value is 1
- uint8_t responweDataForUpdateCharSet2[] = { 0xCC, 0x00, 0x01, 0xec };
- addResponseData(responweDataForUpdateCharSet2, sizeof(responweDataForUpdateCharSet2), true);
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, updateSettingResponse.resultCode);
- EXPECT_EQ(1, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-
- // test update setting with response that resultCode's value is 1
- uint8_t responweDataForUpdateCharSet3[] = { 0xCC, 0x01, 0x00, 0x32 };
- addResponseData(responweDataForUpdateCharSet3, sizeof(responweDataForUpdateCharSet3), true);
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(1, updateSettingResponse.resultCode);
- EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-}
-
-TEST(RCDeviceTest, TestDSAStringAccessProtocol)
-{
- // init device that have not 5 key OSD cable simulation feature
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 };
- addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // get setting detail that type is string
- runcamDeviceSettingDetail_t charsetDetail;
- uint8_t responseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x14, 0x0A, 0x31, 0x39, 0x37, 0x32, 0x30, 0x32, 0x31, 0x36, 0x54, 0x31, 0x39, 0x31, 0x35, 0x33, 0x32, 0x2E, 0x30, 0x00, 0x14, 0x20 };
- addResponseData(responseDataForColumnsDetail1, sizeof(responseDataForColumnsDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, true);
- EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_STRING, charsetDetail.type);
- EXPECT_STREQ("19720216T191532.0", charsetDetail.stringValue);
- EXPECT_EQ(20, charsetDetail.maxStringSize);
- clearResponseBuff();
-
- // get setting detail that type is string, but the response length is incorrect
- uint8_t incorrectResponseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x14, 0x0A, 0x31, 0x39, 0x37, 0x32, 0x30, 0x32, 0x31, 0x36, 0x54, 0x31, 0x39, 0x31, 0x35, 0x33, 0x32, 0x2E, 0x30 };
- addResponseData(incorrectResponseDataForColumnsDetail1, sizeof(incorrectResponseDataForColumnsDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // get setting detail that type is string, but the response crc is incorrect
- uint8_t incorrectResponseDataForColumnsDetail2[] = { 0xCC, 0x00, 0x14, 0x0A, 0x31, 0x39, 0x37, 0x32, 0x30, 0x32, 0x31, 0x36, 0x54, 0x31, 0x39, 0x31, 0x35, 0x33, 0x32, 0x2E, 0x30, 0x00, 0x14, 0x21 };
- addResponseData(incorrectResponseDataForColumnsDetail2, sizeof(incorrectResponseDataForColumnsDetail2), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // get setting detail that type is string, but the response has some randome data at trailing
- uint8_t incorrectResponseDataForColumnsDetail3[] = { 0xCC, 0x00, 0x14, 0x0A, 0x31, 0x39, 0x37, 0x32, 0x30, 0x32, 0x31, 0x36, 0x54, 0x31, 0x39, 0x31, 0x35, 0x33, 0x32, 0x2E, 0x30, 0x00, 0x14, 0x20, 0xAA, 0xBB, 0xCC };
- addResponseData(incorrectResponseDataForColumnsDetail3, sizeof(incorrectResponseDataForColumnsDetail3), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // test update setting
- runcamDeviceWriteSettingResponse_t updateSettingResponse;
- uint8_t responweDataForUpdateCharSet1[] = { 0xCC, 0x00, 0x00, 0x39 };
- addResponseData(responweDataForUpdateCharSet1, sizeof(responweDataForUpdateCharSet1), true);
- uint16_t newValue[] = { 0x31, 0x39, 0x37, 0x32, 0x30, 0x32, 0x31, 0x36, 0x54, 0x31, 0x39, 0x31, 0x35, 0x33, 0x32, 0x2E, 0x30, 0x00 }; // the data struct is UINT32_T + UINT8(Decimal Point)
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, updateSettingResponse.resultCode);
- EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-
- // test update setting with response that needUpdatemenuItems's value is 1
- uint8_t responweDataForUpdateCharSet2[] = { 0xCC, 0x00, 0x01, 0xec };
- addResponseData(responweDataForUpdateCharSet2, sizeof(responweDataForUpdateCharSet2), true);
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(0, updateSettingResponse.resultCode);
- EXPECT_EQ(1, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-
- // test update setting with response that resultCode's value is 1
- uint8_t responweDataForUpdateCharSet3[] = { 0xCC, 0x01, 0x00, 0x32 };
- addResponseData(responweDataForUpdateCharSet3, sizeof(responweDataForUpdateCharSet3), true);
- result = runcamDeviceWriteSetting(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, (uint8_t*)newValue, sizeof(newValue), &updateSettingResponse);
- EXPECT_EQ(result, true);
- EXPECT_EQ(1, updateSettingResponse.resultCode);
- EXPECT_EQ(0, updateSettingResponse.needUpdateMenuItems);
- clearResponseBuff();
-}
-
-TEST(RCDeviceTest, TestDSAInfoAccessProtocol)
-{
- // init device that have not 5 key OSD cable simulation feature
- memset(&testData, 0, sizeof(testData));
- testData.isRunCamSplitOpenPortSupported = true;
- testData.isRunCamSplitPortConfigurated = true;
- testData.isAllowBufferReadWrite = true;
- testData.maxTimesOfRespDataAvailable = 0;
- uint8_t responseData[] = { 0xCC, 0x01, 0x3F, 0x00, 0xE5 };
- addResponseData(responseData, sizeof(responseData), true);
- bool result = rcdeviceInit();
- EXPECT_EQ(result, true);
- clearResponseBuff();
-
- // get setting detail that type is string
- runcamDeviceSettingDetail_t charsetDetail;
- uint8_t responseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x0B, 0x0C, 0x31, 0x31, 0x35, 0x2F, 0x36, 0x30, 0x38, 0x39, 0x30, 0x00, 0xFE };
- addResponseData(responseDataForColumnsDetail1, sizeof(responseDataForColumnsDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, true);
- EXPECT_EQ(RCDEVICE_PROTOCOL_SETTINGTYPE_INFO, charsetDetail.type);
- EXPECT_STREQ("115/60890", charsetDetail.stringValue);
- clearResponseBuff();
-
- // get setting detail that type is string, but the response length is incorrect
- uint8_t incorrectResponseDataForColumnsDetail1[] = { 0xCC, 0x00, 0x0B, 0x0C, 0x31, 0x31, 0x35, 0x2F, 0x36, 0x30, 0x38 };
- addResponseData(incorrectResponseDataForColumnsDetail1, sizeof(incorrectResponseDataForColumnsDetail1), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // get setting detail that type is string, but the response crc is incorrect
- uint8_t incorrectResponseDataForColumnsDetail2[] = { 0xCC, 0x00, 0x0B, 0x0C, 0x31, 0x31, 0x35, 0x2F, 0x36, 0x30, 0x38, 0x39, 0x30, 0x00, 0xFF };
- addResponseData(incorrectResponseDataForColumnsDetail2, sizeof(incorrectResponseDataForColumnsDetail2), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, false);
- clearResponseBuff();
-
- // get setting detail that type is string, but the response has some randome data at trailing
- uint8_t incorrectResponseDataForColumnsDetail3[] = { 0xCC, 0x00, 0x0B, 0x0C, 0x31, 0x31, 0x35, 0x2F, 0x36, 0x30, 0x38, 0x39, 0x30, 0x00, 0xFE, 0xAA, 0xBB, 0xCC };
- addResponseData(incorrectResponseDataForColumnsDetail3, sizeof(incorrectResponseDataForColumnsDetail3), true);
- result = runcamDeviceGetSettingDetail(camDevice, RCDEVICE_PROTOCOL_SETTINGID_DISP_COLUMNS, &charsetDetail);
- EXPECT_EQ(result, true);
- clearResponseBuff();
-}
-
extern "C" {
serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, void *callbackData, uint32_t baudRate, portMode_e mode, portOptions_e options)
{
@@ -1438,16 +721,20 @@ extern "C" {
uint8_t bufIndex = testData.indexOfCurrentRespBuf;
uint8_t leftDataLen = 0;
- if (testData.responseDataReadPos >= testData.responseBufsLen[bufIndex]) {
+ if (testData.responseDataReadPos + 1 > testData.responseBufsLen[bufIndex]) {
+ printf("no data avaliable22\n");
return 0;
} else {
- leftDataLen = testData.responseBufsLen[bufIndex] - testData.responseDataReadPos;
+ printf("testData.responseBufsLen[bufIndex]:%d, testData.responseDataReadPos:%d\n", testData.responseBufsLen[bufIndex], testData.responseDataReadPos);
+ leftDataLen = testData.responseBufsLen[bufIndex] - (testData.responseDataReadPos);
}
if (leftDataLen) {
+ printf("let data:%d\n", leftDataLen);
return leftDataLen;
}
+ printf("no data avaliable\n");
return 0;
}
@@ -1574,6 +861,7 @@ extern "C" {
UNUSED(instance); UNUSED(data); UNUSED(count);
// // reset the input buffer
+ printf("buffer reseted\n");
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf++;
if (testData.indexOfCurrentRespBuf >= testData.responseBufCount) {
@@ -1609,9 +897,7 @@ extern "C" {
return ret;
}
-
uint32_t millis(void) { return testData.millis++; }
-
void beeper(beeperMode_e mode) { UNUSED(mode); }
uint8_t armingFlags = 0;
bool cmsInMenu;