diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index c2afdfc52..18b4ece85 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1350,6 +1350,8 @@ const clivalue_t valueTable[] = { #ifdef USE_RCDEVICE { "rcdevice_init_dev_attempts", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10 }, PG_RCDEVICE_CONFIG, offsetof(rcdeviceConfig_t, initDeviceAttempts) }, { "rcdevice_init_dev_attempt_interval", VAR_UINT32 | MASTER_VALUE, .config.u32Max = 5000, PG_RCDEVICE_CONFIG, offsetof(rcdeviceConfig_t, initDeviceAttemptInterval) }, + { "rcdevice_protocol_version", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_RCDEVICE_CONFIG, offsetof(rcdeviceConfig_t, protocolVersion) }, + { "rcdevice_feature", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = {0, 65535}, PG_RCDEVICE_CONFIG, offsetof(rcdeviceConfig_t, feature) }, #endif // PG_GYRO_DEVICE_CONFIG diff --git a/src/main/io/rcdevice.c b/src/main/io/rcdevice.c index 70bad974c..842521a6b 100644 --- a/src/main/io/rcdevice.c +++ b/src/main/io/rcdevice.c @@ -38,7 +38,6 @@ #ifdef USE_RCDEVICE - typedef struct runcamDeviceExpectedResponseLength_s { uint8_t command; uint8_t reponseLength; @@ -51,7 +50,7 @@ static runcamDeviceExpectedResponseLength_t expectedResponsesLength[] = { { RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, 3}, }; -rcdeviceWaitingResponseQueue watingResponseQueue; +rcdeviceWaitingResponseQueue waitingResponseQueue; static uint8_t recvBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; // all the response contexts using same recv buffer static uint8_t runcamDeviceGetRespLen(uint8_t command) @@ -166,16 +165,17 @@ static void runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_ responseCtx.timeoutTimestamp = millis() + tiemout; responseCtx.parserFunc = parseFunc; responseCtx.device = device; - responseCtx.protocolVer = RCDEVICE_PROTOCOL_VERSION_1_0; + responseCtx.protocolVersion = RCDEVICE_PROTOCOL_VERSION_1_0; if (paramData != NULL) { memcpy(responseCtx.paramData, paramData, paramDataLen); responseCtx.paramDataLen = paramDataLen; } - responseCtx.userInfo = userInfo; - rcdeviceRespCtxQueuePush(&watingResponseQueue, &responseCtx); - // send packet - runcamDeviceSendPacket(device, commandID, paramData, paramDataLen); + responseCtx.userInfo = userInfo; + if (rcdeviceRespCtxQueuePush(&waitingResponseQueue, &responseCtx)) { + // send packet + runcamDeviceSendPacket(device, commandID, paramData, paramDataLen); + } } static void runcamDeviceParseV1DeviceInfo(rcdeviceResponseParseContext_t *ctx) @@ -185,7 +185,7 @@ static void runcamDeviceParseV1DeviceInfo(rcdeviceResponseParseContext_t *ctx) } runcamDevice_t *device = ctx->device; - device->info.protocolVer = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; + device->info.protocolVersion = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; device->info.features = RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON | RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON | RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE; device->isReady = true; } @@ -244,14 +244,14 @@ static void runcamDeviceParseV2DeviceInfo(rcdeviceResponseParseContext_t *ctx) responseCtx.timeoutTimestamp = millis() + rcdeviceConfig()->initDeviceAttemptInterval; responseCtx.parserFunc = runcamDeviceParseV1DeviceInfo; responseCtx.device = ctx->device; - responseCtx.protocolVer = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; - rcdeviceRespCtxQueuePush(&watingResponseQueue, &responseCtx); + responseCtx.protocolVersion = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; + rcdeviceRespCtxQueuePush(&waitingResponseQueue, &responseCtx); runcamSplitSendCommand(ctx->device, 0xFF); return; } runcamDevice_t *device = ctx->device; - device->info.protocolVer = ctx->recvBuf[1]; + device->info.protocolVersion = ctx->recvBuf[1]; uint8_t featureLowBits = ctx->recvBuf[2]; uint8_t featureHighBits = ctx->recvBuf[3]; @@ -267,9 +267,7 @@ static void runcamDeviceGetDeviceInfo(runcamDevice_t *device) } // 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. +// this function will delay 3 seconds to wait the device prepared(special for runcam split) void runcamDeviceInit(runcamDevice_t *device) { device->isReady = false; @@ -277,21 +275,18 @@ void runcamDeviceInit(runcamDevice_t *device) serialPortConfig_t *portConfig = findSerialPortConfig(portID); if (portConfig != NULL) { device->serialPort = openSerialPort(portConfig->identifier, portID, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED); - + device->info.protocolVersion = rcdeviceConfig()->protocolVersion; if (device->serialPort != NULL) { - // send RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO to device to retrive - // device info, e.g protocol version, supported features runcamDeviceGetDeviceInfo(device); } } } - bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation) { - if (device->info.protocolVer == 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 if (device->info.protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + } else if (device->info.protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { runcamSplitSendCommand(device, operation + 1); } else { return false; @@ -334,12 +329,12 @@ void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdev static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs) { - rcdeviceResponseParseContext_t *respCtx = rcdeviceRespCtxQueuePeekFront(&watingResponseQueue); + rcdeviceResponseParseContext_t *respCtx = rcdeviceRespCtxQueuePeekFront(&waitingResponseQueue); while (respCtx != NULL && respCtx->timeoutTimestamp != 0 && currentTimeMs > respCtx->timeoutTimestamp) { if (respCtx->maxRetryTimes > 0) { - if (respCtx->protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0) { + if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) { runcamDeviceSendPacket(respCtx->device, respCtx->command, respCtx->paramData, respCtx->paramDataLen); - } else if (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + } else if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { runcamSplitSendCommand(respCtx->device, respCtx->command); } @@ -355,8 +350,8 @@ static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs } // dequeue and get next waiting response context - rcdeviceRespCtxQueueShift(&watingResponseQueue); - respCtx = rcdeviceRespCtxQueuePeekFront(&watingResponseQueue); + rcdeviceRespCtxQueueShift(&waitingResponseQueue); + respCtx = rcdeviceRespCtxQueuePeekFront(&waitingResponseQueue); } } @@ -366,41 +361,52 @@ static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs 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); + rcdeviceResponseParseContext_t *respCtx = NULL; + while ((respCtx = getWaitingResponse(millis())) != NULL) { + if (!serialRxBytesWaiting(respCtx->device->serialPort)) { + break; + } + + const uint8_t c = serialRead(respCtx->device->serialPort); if (respCtx->recvRespLen == 0) { // Only start receiving packet when we found a header - if ((respCtx->protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0 && c != RCDEVICE_PROTOCOL_HEADER) || (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION && c != RCSPLIT_PACKET_HEADER)) { + if ((respCtx->protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0 && c != RCDEVICE_PROTOCOL_HEADER) || (respCtx->protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION && c != RCSPLIT_PACKET_HEADER)) { continue; } } - + 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_VERSION_1_0) { + if (respCtx->protocolVersion == 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; - } else if (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { - // do nothing, just call parserFunc - respCtx->result = RCDEVICE_RESP_SUCCESS; + } else if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + if (respCtx->recvBuf[0] == RCSPLIT_PACKET_HEADER && respCtx->recvBuf[1] == RCSPLIT_PACKET_CMD_CTRL && respCtx->recvBuf[2] == 0xFF && respCtx->recvBuf[4] == RCSPLIT_PACKET_TAIL) { + 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 = crc8HighFirst(respCtx->recvBuf, 4); + + respCtx->result = crc == crcFromPacket ? RCDEVICE_RESP_SUCCESS : RCDEVICE_RESP_INCORRECT_CRC; + } else { + respCtx->result = RCDEVICE_RESP_INCORRECT_CRC; + } } if (respCtx->parserFunc != NULL) { respCtx->parserFunc(respCtx); } - // dequeue current response context - rcdeviceRespCtxQueueShift(&watingResponseQueue); + if (respCtx->result == RCDEVICE_RESP_SUCCESS) { + rcdeviceRespCtxQueueShift(&waitingResponseQueue); + } } } } diff --git a/src/main/io/rcdevice.h b/src/main/io/rcdevice.h index 175077c03..a31f7c4f4 100644 --- a/src/main/io/rcdevice.h +++ b/src/main/io/rcdevice.h @@ -104,7 +104,7 @@ typedef enum { // end of Runcam Device definition typedef struct runcamDeviceInfo_s { - rcdevice_protocol_version_e protocolVer; + rcdevice_protocol_version_e protocolVersion; uint16_t features; } runcamDeviceInfo_t; @@ -115,7 +115,7 @@ typedef struct runcamDevice_s { bool isReady; } runcamDevice_t; -#define MAX_WAITING_RESPONSES 20 +#define MAX_WAITING_RESPONSES 1 typedef enum { RCDEVICE_RESP_SUCCESS = 0, @@ -136,7 +136,7 @@ struct rcdeviceResponseParseContext_s { runcamDevice_t *device; uint8_t paramData[RCDEVICE_PROTOCOL_MAX_DATA_SIZE]; uint8_t paramDataLen; - uint8_t protocolVer; + uint8_t protocolVersion; int maxRetryTimes; void *userInfo; rcdeviceResponseStatus_e result; diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c index fd3d95685..0f836b510 100644 --- a/src/main/io/rcdevice_cam.c +++ b/src/main/io/rcdevice_cam.c @@ -38,6 +38,8 @@ #include "rx/rx.h" +#include "pg/rcdevice.h" + #ifdef USE_RCDEVICE #define IS_HI(X) (rcData[X] > FIVE_KEY_CABLE_JOYSTICK_MAX) @@ -55,7 +57,7 @@ bool waitingDeviceResponse = false; static bool isFeatureSupported(uint8_t feature) { - if (camDevice->info.features & feature) { + if (camDevice->info.features & feature || rcdeviceConfig()->feature & feature) { return true; } @@ -67,15 +69,6 @@ bool rcdeviceIsEnabled(void) return camDevice->serialPort != NULL; } -static bool rcdeviceIs5KeyEnabled(void) -{ - if (camDevice->serialPort != NULL && isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE)) { - return true; - } - - return false; -} - static void rcdeviceCameraControlProcess(void) { for (boxId_e i = BOXCAMERA1; i <= BOXCAMERA3; i++) { @@ -297,9 +290,7 @@ void rcdeviceUpdate(timeUs_t currentTimeUs) rcdeviceCameraControlProcess(); - if (rcdeviceIs5KeyEnabled()) { - rcdevice5KeySimulationProcess(currentTimeUs); - } + rcdevice5KeySimulationProcess(currentTimeUs); } void rcdeviceInit(void) diff --git a/src/main/io/rcdevice_cam.h b/src/main/io/rcdevice_cam.h index f8fc92470..1e60b8ee1 100644 --- a/src/main/io/rcdevice_cam.h +++ b/src/main/io/rcdevice_cam.h @@ -43,3 +43,5 @@ void rcdeviceInit(void); void rcdeviceUpdate(timeUs_t currentTimeUs); bool rcdeviceIsEnabled(void); + +void rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key); diff --git a/src/main/pg/rcdevice.c b/src/main/pg/rcdevice.c index 15610d846..78fed2c8d 100644 --- a/src/main/pg/rcdevice.c +++ b/src/main/pg/rcdevice.c @@ -29,4 +29,6 @@ void pgResetFn_rcdeviceConfig(rcdeviceConfig_t *rcdeviceConfig) { rcdeviceConfig->initDeviceAttempts = 6; rcdeviceConfig->initDeviceAttemptInterval = 1000; -} + rcdeviceConfig->feature = 0; + rcdeviceConfig->protocolVersion = 0; +} \ No newline at end of file diff --git a/src/main/pg/rcdevice.h b/src/main/pg/rcdevice.h index 5b7a67d97..671d20f64 100644 --- a/src/main/pg/rcdevice.h +++ b/src/main/pg/rcdevice.h @@ -26,6 +26,10 @@ typedef struct rcdeviceConfig_s { uint8_t initDeviceAttempts; timeMs_t initDeviceAttemptInterval; + + // sometimes FC can't get featureInfo from devie(still no idea), so user can set it manaually. + uint32_t feature; + uint8_t protocolVersion; } rcdeviceConfig_t; PG_DECLARE(rcdeviceConfig_t, rcdeviceConfig); \ No newline at end of file diff --git a/src/test/Makefile b/src/test/Makefile index af4dc166a..603dc3abd 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -323,6 +323,7 @@ rcdevice_unittest_SRC := \ $(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/io/rcdevice.c \ $(USER_DIR)/io/rcdevice_cam.c \ + $(USER_DIR)/pg/pg.c \ pid_unittest_SRC := \ $(USER_DIR)/common/filter.c \ diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc index 8669640d5..2f8c7abbf 100644 --- a/src/test/unit/rcdevice_unittest.cc +++ b/src/test/unit/rcdevice_unittest.cc @@ -58,7 +58,7 @@ extern "C" { extern runcamDevice_t *camDevice; extern bool isButtonPressed; extern bool rcdeviceInMenu; - extern rcdeviceWaitingResponseQueue watingResponseQueue; + extern rcdeviceWaitingResponseQueue waitingResponseQueue; PG_REGISTER_WITH_RESET_FN(rcdeviceConfig_t, rcdeviceConfig, PG_RCDEVICE_CONFIG, 0); bool unitTestIsSwitchActivited(boxId_e boxId) { @@ -67,11 +67,15 @@ extern "C" { return switchState.isActivated; } + void pgResetFn_rcdeviceConfig(rcdeviceConfig_t *rcdeviceConfig) -{ - rcdeviceConfig->initDeviceAttempts = 4; - rcdeviceConfig->initDeviceAttemptInterval = 1000; -} + { + rcdeviceConfig->initDeviceAttempts = 4; + rcdeviceConfig->initDeviceAttemptInterval = 1000; + + rcdeviceConfig->feature = 0; + rcdeviceConfig->protocolVersion = 0; + } uint32_t millis(void); int minTimeout = 180; @@ -99,13 +103,7 @@ typedef struct testData_s { } testData_t; static testData_t testData; -extern rcdeviceWaitingResponseQueue watingResponseQueue; - -static void resetRCDeviceStatus() -{ - isButtonPressed = false; - rcdeviceInMenu = false; -} +extern rcdeviceWaitingResponseQueue waitingResponseQueue; static void clearResponseBuff() { @@ -114,11 +112,21 @@ static void clearResponseBuff() memset(testData.responseBufsLen, 0, MAX_RESPONSES_COUNT); memset(testData.responesBufs, 0, MAX_RESPONSES_COUNT * 60); - while (rcdeviceRespCtxQueueShift(&watingResponseQueue)) { + while (rcdeviceRespCtxQueueShift(&waitingResponseQueue)) { } } +static void resetRCDeviceStatus() +{ + isButtonPressed = false; + rcdeviceInMenu = false; + PG_RESET(rcdeviceConfig); + clearResponseBuff(); +} + + + static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlushSerial) { UNUSED(withDataForFlushSerial); @@ -133,9 +141,9 @@ TEST(RCDeviceTest, TestRCSplitInitWithoutPortConfigurated) resetRCDeviceStatus(); - watingResponseQueue.headPos = 0; - watingResponseQueue.tailPos = 0; - watingResponseQueue.itemCount = 0; + waitingResponseQueue.headPos = 0; + waitingResponseQueue.tailPos = 0; + waitingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); runcamDeviceInit(&device); EXPECT_EQ(false, device.isReady); @@ -147,9 +155,9 @@ TEST(RCDeviceTest, TestRCSplitInitWithoutOpenPortConfigurated) resetRCDeviceStatus(); - watingResponseQueue.headPos = 0; - watingResponseQueue.tailPos = 0; - watingResponseQueue.itemCount = 0; + waitingResponseQueue.headPos = 0; + waitingResponseQueue.tailPos = 0; + waitingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = false; testData.isRunCamSplitPortConfigurated = true; @@ -165,22 +173,26 @@ TEST(RCDeviceTest, TestInitDevice) resetRCDeviceStatus(); // test correct response - watingResponseQueue.headPos = 0; - watingResponseQueue.tailPos = 0; - watingResponseQueue.itemCount = 0; + waitingResponseQueue.headPos = 0; + waitingResponseQueue.tailPos = 0; + waitingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; testData.isAllowBufferReadWrite = true; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; - addResponseData(responseData, sizeof(responseData), true); + runcamDeviceInit(&device); + testData.millis += 3001; + rcdeviceReceive(millis() * 1000); + addResponseData(responseData, sizeof(responseData), true); rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; testData.responseDataReadPos = 0; testData.indexOfCurrentRespBuf = 0; rcdeviceReceive(millis() * 1000); + rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; EXPECT_EQ(device.isReady, true); } @@ -192,9 +204,9 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) resetRCDeviceStatus(); // test correct response data with incorrect len - watingResponseQueue.headPos = 0; - watingResponseQueue.tailPos = 0; - watingResponseQueue.itemCount = 0; + waitingResponseQueue.headPos = 0; + waitingResponseQueue.tailPos = 0; + waitingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -203,6 +215,7 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD, 0x33 }; addResponseData(responseData, sizeof(responseData), true); runcamDeviceInit(&device); + testData.millis += 3001; rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; testData.responseDataReadPos = 0; @@ -216,6 +229,7 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) uint8_t responseDataWithInvalidCRC[] = { 0xCC, 0x01, 0x37, 0x00, 0xBE }; addResponseData(responseDataWithInvalidCRC, sizeof(responseDataWithInvalidCRC), true); runcamDeviceInit(&device); + testData.millis += 3001; rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; testData.responseDataReadPos = 0; @@ -229,6 +243,7 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) uint8_t incompleteResponseData[] = { 0xCC, 0x01, 0x37 }; addResponseData(incompleteResponseData, sizeof(incompleteResponseData), true); runcamDeviceInit(&device); + testData.millis += 3001; rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; testData.responseDataReadPos = 0; @@ -245,6 +260,7 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) testData.isRunCamSplitPortConfigurated = true; testData.isAllowBufferReadWrite = true; runcamDeviceInit(&device); + testData.millis += 3001; rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; testData.responseDataReadPos = 0; @@ -260,9 +276,9 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready) resetRCDeviceStatus(); // test correct response - watingResponseQueue.headPos = 0; - watingResponseQueue.tailPos = 0; - watingResponseQueue.itemCount = 0; + waitingResponseQueue.headPos = 0; + waitingResponseQueue.tailPos = 0; + waitingResponseQueue.itemCount = 0; memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -271,6 +287,7 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready) uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBC }; // wrong response addResponseData(responseData, sizeof(responseData), true); rcdeviceInit(); + testData.millis += 3001; rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; testData.responseDataReadPos = 0; @@ -314,6 +331,12 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready) // runn process loop rcdeviceUpdate(0); + // remove all request from queue + for (int i = 0; i < 10; i++) { + testData.millis += 500000; + rcdeviceReceive(millis()); + } + EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1)); EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA2)); EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3)); @@ -331,12 +354,16 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) testData.maxTimesOfRespDataAvailable = 0; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); + camDevice->info.features = 15; rcdeviceInit(); + testData.millis += 3001; rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; testData.responseDataReadPos = 0; testData.indexOfCurrentRespBuf = 0; + + rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; EXPECT_EQ(camDevice->isReady, true); @@ -381,6 +408,12 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA1)); EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2)); EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3)); + + // remove all request from queue + for (int i = 0; i < 10; i++) { + testData.millis += 500000; + rcdeviceReceive(millis()); + } } TEST(RCDeviceTest, TestWifiModeChangeCombine) @@ -395,6 +428,7 @@ TEST(RCDeviceTest, TestWifiModeChangeCombine) uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); rcdeviceInit(); + testData.millis += 3001; rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; testData.responseDataReadPos = 0; @@ -467,6 +501,12 @@ TEST(RCDeviceTest, TestWifiModeChangeCombine) EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA1)); EXPECT_EQ(true, unitTestIsSwitchActivited(BOXCAMERA2)); EXPECT_EQ(false, unitTestIsSwitchActivited(BOXCAMERA3)); + + // remove all request from queue + for (int i = 0; i < 10; i++) { + testData.millis += 500000; + rcdeviceReceive(millis()); + } } TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) @@ -481,6 +521,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; addResponseData(responseData, sizeof(responseData), true); rcdeviceInit(); + testData.millis += 3001; rcdeviceReceive(millis() * 1000); testData.millis += minTimeout; testData.responseDataReadPos = 0; @@ -659,6 +700,12 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) testData.millis += minTimeout; EXPECT_EQ(false, isButtonPressed); clearResponseBuff(); + + // remove all request from queue + for (int i = 0; i < 300; i++) { + testData.millis += 500000; + rcdeviceReceive(millis()); + } } TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) @@ -672,7 +719,12 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) rcData[YAW] = FIVE_KEY_JOYSTICK_MAX; // Yaw High rcdeviceUpdate(millis() * 1000); EXPECT_EQ(false, rcdeviceInMenu); - + // remove all request from queue + for (int i = 0; i < 10; i++) { + testData.millis += 500000; + rcdeviceReceive(millis()); + } + // init device that have not 5 key OSD cable simulation feature memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; @@ -680,12 +732,14 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) testData.isAllowBufferReadWrite = true; testData.maxTimesOfRespDataAvailable = 0; uint8_t responseData[] = { 0xCC, 0x01, 0x37, 0x00, 0xBD }; - addResponseData(responseData, sizeof(responseData), true); + rcdeviceInit(); + testData.millis += 3001; rcdeviceReceive(millis() * 1000); testData.millis += 200; testData.responseDataReadPos = 0; testData.indexOfCurrentRespBuf = 0; + addResponseData(responseData, sizeof(responseData), true); rcdeviceReceive(millis() * 1000); testData.millis += 200; EXPECT_EQ(camDevice->isReady, true); @@ -697,6 +751,12 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) rcdeviceUpdate(millis() * 1000); EXPECT_EQ(false, rcdeviceInMenu); clearResponseBuff(); + + // remove all request from queue + for (int i = 0; i < 10; i++) { + testData.millis += 500000; + rcdeviceReceive(millis()); + } } extern "C" {