diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index 68d6765e7e..3c7bc0093c 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -82,6 +82,7 @@
#include "pg/vcd.h"
#include "pg/usb.h"
#include "pg/sdio.h"
+#include "pg/rcdevice.h"
#include "rx/rx.h"
#include "rx/cc2500_frsky_common.h"
@@ -1091,6 +1092,11 @@ const clivalue_t valueTable[] = {
#ifdef USE_FLASH
{ "flash_spi_bus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, SPIDEV_COUNT }, PG_FLASH_CONFIG, offsetof(flashConfig_t, spiDevice) },
#endif
+// RCDEVICE
+#ifdef USE_RCDEVICE
+ { "rcdevice_init_dev_attempts", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_RCDEVICE_CONFIG, offsetof(rcdeviceConfig_t, initDeviceAttempts) },
+ { "rcdevice_init_dev_attempt_interval", VAR_UINT32 | MASTER_VALUE, .config.minmax = { 500, 5000 }, PG_RCDEVICE_CONFIG, offsetof(rcdeviceConfig_t, initDeviceAttemptInterval) }
+#endif
};
const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);
diff --git a/src/main/io/rcdevice.c b/src/main/io/rcdevice.c
index 3b1deb37ca..5dc732b4a6 100644
--- a/src/main/io/rcdevice.c
+++ b/src/main/io/rcdevice.c
@@ -30,10 +30,13 @@
#include "io/serial.h"
+#include "pg/rcdevice.h"
+
#include "rcdevice.h"
#ifdef USE_RCDEVICE
+
typedef struct runcamDeviceExpectedResponseLength_s {
uint8_t command;
uint8_t reponseLength;
@@ -147,7 +150,7 @@ 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 void runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_t commandID, uint8_t *paramData, uint8_t paramDataLen, timeUs_t tiemout, int maxRetryTimes, void *userInfo, rcdeviceRespParseFunc parseFunc)
+static void runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_t commandID, uint8_t *paramData, uint8_t paramDataLen, timeMs_t tiemout, int maxRetryTimes, void *userInfo, rcdeviceRespParseFunc parseFunc)
{
runcamDeviceFlushRxBuffer(device);
@@ -190,7 +193,7 @@ static void runcamDeviceParseV2DeviceInfo(rcdeviceResponseParseContext_t *ctx)
// definition of runcamDeviceInfo_t to know more)
static void runcamDeviceGetDeviceInfo(runcamDevice_t *device)
{
- runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, NULL, 0, 5000, 0, NULL, runcamDeviceParseV2DeviceInfo);
+ runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, NULL, 0, rcdeviceConfig()->initDeviceAttemptInterval, rcdeviceConfig()->initDeviceAttempts, NULL, runcamDeviceParseV2DeviceInfo);
}
// init the runcam device, it'll search the UART port with FUNCTION_RCDEVICE id
@@ -229,7 +232,7 @@ bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation)
void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
{
uint8_t operation = RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN;
- runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 200, 1, NULL, parseFunc);
+ runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 200, 0, NULL, parseFunc);
}
// when the control was stop, must call this method to the camera to disconnect
@@ -237,7 +240,7 @@ void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceResp
void runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
{
uint8_t operation = RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE;
- runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 200, 1, NULL, parseFunc);
+ runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 200, 0, NULL, parseFunc);
}
// simulate button press event of 5 key osd cable with special button
@@ -247,36 +250,34 @@ void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t
return;
}
- runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), 200, 1, NULL, parseFunc);
+ runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), 200, 0, NULL, parseFunc);
}
// simulate button release event of 5 key osd cable
void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
{
- runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, 200, 1, NULL, parseFunc);
+ runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, 200, 0, NULL, parseFunc);
}
-static rcdeviceResponseParseContext_t* getWaitingResponse(timeUs_t currentTimeUs)
+static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs)
{
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);
+ while (respCtx != NULL && respCtx->timeoutTimestamp != 0 && currentTimeMs > respCtx->timeoutTimestamp) {
+ if (respCtx->maxRetryTimes > 0) {
+ runcamDeviceSendPacket(respCtx->device, respCtx->command, respCtx->paramData, respCtx->paramDataLen);
+ respCtx->timeoutTimestamp = currentTimeMs + 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);
}
}
diff --git a/src/main/io/rcdevice.h b/src/main/io/rcdevice.h
index 2ddc5009dd..9f5bfae36a 100644
--- a/src/main/io/rcdevice.h
+++ b/src/main/io/rcdevice.h
@@ -125,8 +125,8 @@ struct rcdeviceResponseParseContext_s {
uint8_t expectedRespLen; // total length of response data
uint8_t recvRespLen; // length of the data received
uint8_t *recvBuf; // response data buffer
- timeUs_t timeout;
- timeUs_t timeoutTimestamp; // if zero, it's means keep waiting for the response
+ timeMs_t timeout;
+ timeMs_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];
diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c
index 2c29876807..a1ff320a28 100644
--- a/src/main/io/rcdevice_cam.c
+++ b/src/main/io/rcdevice_cam.c
@@ -25,7 +25,7 @@
#include "pg/rx.h"
-#include "drivers/time.h"
+#include "common/time.h"
#include "cms/cms.h"
@@ -147,7 +147,7 @@ static void rcdeviceSimulationRespHandle(rcdeviceResponseParseContext_t *ctx)
rcdeviceSimulationOSDCableFailed(ctx);
return;
}
-
+
switch (ctx->command) {
case RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE:
isButtonPressed = false;
@@ -156,6 +156,7 @@ static void rcdeviceSimulationRespHandle(rcdeviceResponseParseContext_t *ctx)
{
// the high 4 bits is the operationID that we sent
// the low 4 bits is the result code
+ isButtonPressed = true;
uint8_t operationID = ctx->paramData[0];
bool errorCode = (ctx->recvBuf[1] & 0x0F);
if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN) {
diff --git a/src/main/pg/pg_ids.h b/src/main/pg/pg_ids.h
index 275b0a8e69..8b762af70c 100644
--- a/src/main/pg/pg_ids.h
+++ b/src/main/pg/pg_ids.h
@@ -133,7 +133,8 @@
#define PG_SPI_PREINIT_OPU_CONFIG 536
#define PG_RX_SPI_CONFIG 537
#define PG_BOARD_CONFIG 538
-#define PG_BETAFLIGHT_END 538
+#define PG_RCDEVICE_CONFIG 539
+#define PG_BETAFLIGHT_END 539
// OSD configuration (subject to change)
diff --git a/src/main/pg/rcdevice.c b/src/main/pg/rcdevice.c
new file mode 100644
index 0000000000..5e95f10d12
--- /dev/null
+++ b/src/main/pg/rcdevice.c
@@ -0,0 +1,30 @@
+/*
+ * 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 "pg/pg_ids.h"
+#include "pg/rcdevice.h"
+
+PG_REGISTER_WITH_RESET_FN(rcdeviceConfig_t, rcdeviceConfig, PG_RCDEVICE_CONFIG, 0);
+
+void pgResetFn_rcdeviceConfig(rcdeviceConfig_t *rcdeviceConfig)
+{
+ rcdeviceConfig->initDeviceAttempts = 4;
+ rcdeviceConfig->initDeviceAttemptInterval = 1000;
+}
\ No newline at end of file
diff --git a/src/main/pg/rcdevice.h b/src/main/pg/rcdevice.h
new file mode 100644
index 0000000000..5b7a67d971
--- /dev/null
+++ b/src/main/pg/rcdevice.h
@@ -0,0 +1,31 @@
+/*
+ * 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
+
+#include "pg/pg.h"
+#include "common/time.h"
+
+typedef struct rcdeviceConfig_s {
+ uint8_t initDeviceAttempts;
+ timeMs_t initDeviceAttemptInterval;
+} rcdeviceConfig_t;
+
+PG_DECLARE(rcdeviceConfig_t, rcdeviceConfig);
\ No newline at end of file
diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc
index d1e5c98996..00a1bdf785 100644
--- a/src/test/unit/rcdevice_unittest.cc
+++ b/src/test/unit/rcdevice_unittest.cc
@@ -47,6 +47,7 @@ extern "C" {
#include "pg/pg_ids.h"
#include "pg/vcd.h"
#include "pg/rx.h"
+ #include "pg/rcdevice.h"
#include "rx/rx.h"
@@ -57,6 +58,7 @@ extern "C" {
extern bool isButtonPressed;
extern bool rcdeviceInMenu;
extern rcdeviceWaitingResponseQueue watingResponseQueue;
+ PG_REGISTER_WITH_RESET_FN(rcdeviceConfig_t, rcdeviceConfig, PG_RCDEVICE_CONFIG, 0);
bool unitTestIsSwitchActivited(boxId_e boxId)
{
uint8_t adjustBoxID = boxId - BOXCAMERA1;
@@ -64,8 +66,14 @@ extern "C" {
return switchState.isActivated;
}
+ void pgResetFn_rcdeviceConfig(rcdeviceConfig_t *rcdeviceConfig)
+{
+ rcdeviceConfig->initDeviceAttempts = 4;
+ rcdeviceConfig->initDeviceAttemptInterval = 1000;
+}
+
uint32_t millis(void);
- int minTimeout = 400;
+ int minTimeout = 180;
}
#define MAX_RESPONSES_COUNT 10
@@ -146,11 +154,11 @@ TEST(RCDeviceTest, TestInitDevice)
addResponseData(responseData, sizeof(responseData), true);
runcamDeviceInit(&device);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf = 0;
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(device.isReady, true);
}
@@ -171,12 +179,11 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse)
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD, 0x33 };
addResponseData(responseData, sizeof(responseData), true);
runcamDeviceInit(&device);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf = 0;
- printf("call receiver func again\n");
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
EXPECT_EQ(device.isReady, true);
clearResponseBuff();
testData.millis += minTimeout;
@@ -185,12 +192,11 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse)
uint8_t responseDataWithInvalidCRC[] = { 0xCC, 0x01, 0x37, 0x00, 0xBE };
addResponseData(responseDataWithInvalidCRC, sizeof(responseDataWithInvalidCRC), true);
runcamDeviceInit(&device);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf = 0;
- printf("call receiver func again11\n");
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
EXPECT_EQ(device.isReady, false);
clearResponseBuff();
testData.millis += minTimeout;
@@ -199,12 +205,11 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse)
uint8_t incompleteResponseData[] = { 0xCC, 0x01, 0x37 };
addResponseData(incompleteResponseData, sizeof(incompleteResponseData), true);
runcamDeviceInit(&device);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf = 0;
- printf("call receiver func again2222\n");
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(device.isReady, false);
clearResponseBuff();
@@ -216,12 +221,11 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse)
testData.isRunCamSplitPortConfigurated = true;
testData.isAllowBufferReadWrite = true;
runcamDeviceInit(&device);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf = 0;
- printf("call receiver func again3333\n");
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
EXPECT_EQ(device.isReady, false);
clearResponseBuff();
testData.millis += minTimeout;
@@ -241,11 +245,11 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready)
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBC }; // wrong response
addResponseData(responseData, sizeof(responseData), true);
rcdeviceInit();
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf = 0;
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(camDevice->isReady, false);
@@ -299,13 +303,12 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady)
addResponseData(responseData, sizeof(responseData), true);
camDevice->info.features = 15;
rcdeviceInit();
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf = 0;
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
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
@@ -358,11 +361,11 @@ TEST(RCDeviceTest, TestWifiModeChangeCombine)
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
addResponseData(responseData, sizeof(responseData), true);
rcdeviceInit();
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf = 0;
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, camDevice->isReady);
@@ -440,22 +443,20 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
addResponseData(responseData, sizeof(responseData), true);
rcdeviceInit();
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf = 0;
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, camDevice->isReady);
clearResponseBuff();
- printf("pass init device\n");
// test timeout of open connection
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN);
- printf("waiting open connection \n");
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += 3000;
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(false, rcdeviceInMenu);
clearResponseBuff();
@@ -463,10 +464,8 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
// open connection with correct response
uint8_t responseDataOfOpenConnection[] = { 0xCC, 0x11, 0xe7 };
addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), true);
- printf("waiting open connection 222222\n");
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN);
- printf("start to recei vdedata\n");
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, rcdeviceInMenu);
clearResponseBuff();
@@ -475,7 +474,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t incorrectResponseDataOfOpenConnection1[] = { 0xCC, 0x11, 0xe7, 0x55 };
addResponseData(incorrectResponseDataOfOpenConnection1, sizeof(incorrectResponseDataOfOpenConnection1), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, rcdeviceInMenu);
clearResponseBuff();
@@ -484,16 +483,16 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t incorrectResponseDataOfOpenConnection2[] = { 0xCC, 0x10, 0x42 };
addResponseData(incorrectResponseDataOfOpenConnection2, sizeof(incorrectResponseDataOfOpenConnection2), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_OPEN);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, rcdeviceInMenu); // when crc wrong won't change the menu state
clearResponseBuff();
// test timeout of close connection
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += 3000;
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, rcdeviceInMenu); // close menu timeout won't change the menu state
clearResponseBuff();
@@ -502,7 +501,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t responseDataOfCloseConnection[] = { 0xCC, 0x21, 0x11 };
addResponseData(responseDataOfCloseConnection, sizeof(responseDataOfCloseConnection), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(false, rcdeviceInMenu);
clearResponseBuff();
@@ -510,8 +509,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
// 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());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, rcdeviceInMenu);
clearResponseBuff();
@@ -519,7 +517,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t responseDataOfCloseConnection1[] = { 0xCC, 0x21, 0x11, 0xC1 };
addResponseData(responseDataOfCloseConnection1, sizeof(responseDataOfCloseConnection1), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(false, rcdeviceInMenu);
clearResponseBuff();
@@ -527,8 +525,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
// 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());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, rcdeviceInMenu);
clearResponseBuff();
@@ -536,16 +533,24 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t responseDataOfCloseConnection2[] = { 0xCC, 0x21, 0xA1 };
addResponseData(responseDataOfCloseConnection2, sizeof(responseDataOfCloseConnection2), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_CONNECTION_CLOSE);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, rcdeviceInMenu);
clearResponseBuff();
+ // release button first
+ uint8_t responseDataOfSimulation4[] = { 0xCC, 0xA5 };
+ addResponseData(responseDataOfSimulation4, sizeof(responseDataOfSimulation4), true);
+ rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
+ rcdeviceReceive(millis() * 1000);
+ testData.millis += minTimeout;
+ EXPECT_EQ(false, isButtonPressed);
+ clearResponseBuff();
+
// simulate press button with no response
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER);
- rcdeviceReceive(millis());
testData.millis += 2000;
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(false, isButtonPressed);
clearResponseBuff();
@@ -554,16 +559,15 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t responseDataOfSimulation1[] = { 0xCC, 0xA5 };
addResponseData(responseDataOfSimulation1, sizeof(responseDataOfSimulation1), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
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());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(false, isButtonPressed);
clearResponseBuff();
@@ -571,7 +575,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t responseDataOfSimulation2[] = { 0xCC, 0xA5, 0x22 };
addResponseData(responseDataOfSimulation2, sizeof(responseDataOfSimulation2), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, isButtonPressed);
clearResponseBuff();
@@ -580,16 +584,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t responseDataOfSimulation3[] = { 0xCC, 0xB5, 0x22 };
addResponseData(responseDataOfSimulation3, sizeof(responseDataOfSimulation3), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER);
- rcdeviceReceive(millis());
- testData.millis += minTimeout;
- EXPECT_EQ(true, isButtonPressed);
- clearResponseBuff();
-
- // simulate release button event
- rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
- rcdeviceReceive(millis());
- testData.millis += 1200;
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, isButtonPressed);
clearResponseBuff();
@@ -597,7 +592,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
// simulate release button with correct response
addResponseData(responseDataOfSimulation4, sizeof(responseDataOfSimulation4), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(false, isButtonPressed);
clearResponseBuff();
@@ -605,7 +600,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
// simulate release button with correct response but wrong data length
addResponseData(responseDataOfSimulation1, sizeof(responseDataOfSimulation1), true); // press first
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_ENTER);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(true, isButtonPressed);
clearResponseBuff();
@@ -613,7 +608,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t responseDataOfSimulation5[] = { 0xCC, 0xA5, 0xFF };
addResponseData(responseDataOfSimulation5, sizeof(responseDataOfSimulation5), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(false, isButtonPressed);
clearResponseBuff();
@@ -622,7 +617,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol)
uint8_t responseDataOfSimulation6[] = { 0xCC, 0x31, 0xFF };
addResponseData(responseDataOfSimulation6, sizeof(responseDataOfSimulation6), true);
rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE);
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += minTimeout;
EXPECT_EQ(false, isButtonPressed);
clearResponseBuff();
@@ -647,11 +642,11 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport)
uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD };
addResponseData(responseData, sizeof(responseData), true);
rcdeviceInit();
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += 200;
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf = 0;
- rcdeviceReceive(millis());
+ rcdeviceReceive(millis() * 1000);
testData.millis += 200;
EXPECT_EQ(camDevice->isReady, true);
clearResponseBuff();
@@ -722,19 +717,15 @@ extern "C" {
uint8_t bufIndex = testData.indexOfCurrentRespBuf;
uint8_t leftDataLen = 0;
if (testData.responseDataReadPos + 1 > testData.responseBufsLen[bufIndex]) {
- printf("no data avaliable22\n");
return 0;
} else {
- 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;
}
@@ -860,8 +851,7 @@ extern "C" {
{
UNUSED(instance); UNUSED(data); UNUSED(count);
- // // reset the input buffer
- printf("buffer reseted\n");
+ // reset the input buffer
testData.responseDataReadPos = 0;
testData.indexOfCurrentRespBuf++;
if (testData.indexOfCurrentRespBuf >= testData.responseBufCount) {