diff --git a/src/main/io/rcdevice.c b/src/main/io/rcdevice.c index 5dc732b4a..84d634edb 100644 --- a/src/main/io/rcdevice.c +++ b/src/main/io/rcdevice.c @@ -63,7 +63,7 @@ static uint8_t runcamDeviceGetRespLen(uint8_t command) return 0; } -static bool rcdeviceRespCtxQueuePushRespCtx(rcdeviceWaitingResponseQueue *queue, rcdeviceResponseParseContext_t *respCtx) +static bool rcdeviceRespCtxQueuePush(rcdeviceWaitingResponseQueue *queue, rcdeviceResponseParseContext_t *respCtx) { if (queue == NULL || (queue->itemCount + 1) > MAX_WAITING_RESPONSES) { return false; @@ -91,7 +91,7 @@ static rcdeviceResponseParseContext_t* rcdeviceRespCtxQueuePeekFront(rcdeviceWai return ctx; } -static rcdeviceResponseParseContext_t* rcdeviceRespCtxQueueShift(rcdeviceWaitingResponseQueue *queue) +STATIC_UNIT_TESTED rcdeviceResponseParseContext_t* rcdeviceRespCtxQueueShift(rcdeviceWaitingResponseQueue *queue) { if (queue == NULL || queue->itemCount == 0) { return NULL; @@ -165,23 +165,91 @@ static void runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_ responseCtx.parserFunc = parseFunc; responseCtx.device = device; responseCtx.protocolVer = RCDEVICE_PROTOCOL_VERSION_1_0; - memcpy(responseCtx.paramData, paramData, paramDataLen); - responseCtx.paramDataLen = paramDataLen; + if (paramData != NULL) { + memcpy(responseCtx.paramData, paramData, paramDataLen); + responseCtx.paramDataLen = paramDataLen; + } responseCtx.userInfo = userInfo; - rcdeviceRespCtxQueuePushRespCtx(&watingResponseQueue, &responseCtx); + rcdeviceRespCtxQueuePush(&watingResponseQueue, &responseCtx); // send packet runcamDeviceSendPacket(device, commandID, paramData, paramDataLen); } +static void runcamDeviceParseV1DeviceInfo(rcdeviceResponseParseContext_t *ctx) +{ + if (ctx->result != RCDEVICE_RESP_SUCCESS) { + return; + } + + runcamDevice_t *device = ctx->device; + device->info.protocolVer = 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; +} + +static uint8_t crc8HighFirst(uint8_t *ptr, uint8_t len) +{ + uint8_t crc = 0x00; + while (len--) { + crc ^= *ptr++; + for (unsigned i = 8; i > 0; --i) { + if (crc & 0x80) + crc = (crc << 1) ^ 0x31; + else + crc = (crc << 1); + } + } + return (crc); +} + +// for the rcsplits that firmware <= 1.1.0 +static void runcamSplitSendCommand(runcamDevice_t *device, uint8_t argument) +{ + if (!device->serialPort) { + return; + } + + 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 = crc8HighFirst(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); +} + static void runcamDeviceParseV2DeviceInfo(rcdeviceResponseParseContext_t *ctx) { if (ctx->result != RCDEVICE_RESP_SUCCESS) { - ctx->device->isReady = false; + runcamDeviceFlushRxBuffer(ctx->device); + + rcdeviceResponseParseContext_t responseCtx; + memset(&responseCtx, 0, sizeof(rcdeviceResponseParseContext_t)); + responseCtx.recvBuf = recvBuf; + responseCtx.command = 0xFF; + responseCtx.maxRetryTimes = rcdeviceConfig()->initDeviceAttempts; + responseCtx.expectedRespLen = 5; + responseCtx.timeout = rcdeviceConfig()->initDeviceAttemptInterval; + responseCtx.timeoutTimestamp = millis() + rcdeviceConfig()->initDeviceAttemptInterval; + responseCtx.parserFunc = runcamDeviceParseV1DeviceInfo; + responseCtx.device = ctx->device; + responseCtx.protocolVer = RCDEVICE_PROTOCOL_RCSPLIT_VERSION; + rcdeviceRespCtxQueuePush(&watingResponseQueue, &responseCtx); + + runcamSplitSendCommand(ctx->device, 0xFF); return; } runcamDevice_t *device = ctx->device; - device->info.protocolVersion = ctx->recvBuf[1]; + device->info.protocolVer = ctx->recvBuf[1]; uint8_t featureLowBits = ctx->recvBuf[2]; uint8_t featureHighBits = ctx->recvBuf[3]; @@ -216,10 +284,13 @@ void runcamDeviceInit(runcamDevice_t *device) } } + bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation) { - if (device->info.protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) { + if (device->info.protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0) { runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL, &operation, sizeof(operation)); + } else if (device->info.protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + runcamSplitSendCommand(device, operation + 1); } else { return false; } @@ -232,7 +303,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, 0, NULL, parseFunc); + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc); } // when the control was stop, must call this method to the camera to disconnect @@ -240,7 +311,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, 0, NULL, parseFunc); + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc); } // simulate button press event of 5 key osd cable with special button @@ -250,13 +321,13 @@ void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t return; } - runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), 200, 0, NULL, parseFunc); + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), 400, 2, 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, 0, NULL, parseFunc); + runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, 400, 2, NULL, parseFunc); } static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs) @@ -264,7 +335,13 @@ static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs rcdeviceResponseParseContext_t *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); + if (respCtx->protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0) { + runcamDeviceSendPacket(respCtx->device, respCtx->command, respCtx->paramData, respCtx->paramDataLen); + } else if (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) { + runcamSplitSendCommand(respCtx->device, respCtx->command); + } + + respCtx->recvRespLen = 0; respCtx->timeoutTimestamp = currentTimeMs + respCtx->timeout; respCtx->maxRetryTimes -= 1; respCtx = NULL; @@ -290,6 +367,14 @@ void rcdeviceReceive(timeUs_t currentTimeUs) rcdeviceResponseParseContext_t *respCtx = NULL; while ((respCtx = getWaitingResponse(millis())) != NULL && serialRxBytesWaiting(respCtx->device->serialPort)) { 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)) { + continue; + } + } + respCtx->recvBuf[respCtx->recvRespLen] = c; respCtx->recvRespLen += 1; @@ -301,7 +386,11 @@ void rcdeviceReceive(timeUs_t currentTimeUs) 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; } if (respCtx->parserFunc != NULL) { diff --git a/src/main/io/rcdevice.h b/src/main/io/rcdevice.h index 9f5bfae36..175077c03 100644 --- a/src/main/io/rcdevice.h +++ b/src/main/io/rcdevice.h @@ -39,6 +39,11 @@ #define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE 0x03 #define RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION 0x04 +// Old protocol defines +#define RCSPLIT_PACKET_HEADER 0x55 +#define RCSPLIT_PACKET_CMD_CTRL 0x01 +#define RCSPLIT_PACKET_TAIL 0xaa + // Feature Flag sets, it's a uint16_t flag typedef enum { RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON = (1 << 0), @@ -99,7 +104,7 @@ typedef enum { // end of Runcam Device definition typedef struct runcamDeviceInfo_s { - rcdevice_protocol_version_e protocolVersion; + rcdevice_protocol_version_e protocolVer; uint16_t features; } runcamDeviceInfo_t; @@ -110,7 +115,7 @@ typedef struct runcamDevice_s { bool isReady; } runcamDevice_t; -#define MAX_WAITING_RESPONSES 5 +#define MAX_WAITING_RESPONSES 20 typedef enum { RCDEVICE_RESP_SUCCESS = 0, diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c index a1ff320a2..fd3d95685 100644 --- a/src/main/io/rcdevice_cam.c +++ b/src/main/io/rcdevice_cam.c @@ -129,22 +129,20 @@ static void rcdeviceCameraControlProcess(void) static void rcdeviceSimulationOSDCableFailed(rcdeviceResponseParseContext_t *ctx) { + waitingDeviceResponse = false; 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); + waitingDeviceResponse = false; return; } @@ -164,7 +162,6 @@ static void rcdeviceSimulationRespHandle(rcdeviceResponseParseContext_t *ctx) rcdeviceInMenu = true; beeper(BEEPER_CAM_CONNECTION_OPEN); } else { - rcdeviceInMenu = false; beeper(BEEPER_CAM_CONNECTION_CLOSE); } } else if (operationID == RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE) { @@ -250,21 +247,19 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) return; } - if (waitingDeviceResponse) { - return; - } - if (isButtonPressed) { if (IS_MID(YAW) && IS_MID(PITCH) && IS_MID(ROLL)) { - if (rcdeviceIs5KeyEnabled()) { - rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); - waitingDeviceResponse = true; - } + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + waitingDeviceResponse = true; } } else { + if (waitingDeviceResponse) { + return; + } + rcdeviceCamSimulationKeyEvent_e key = RCDEVICE_CAM_KEY_NONE; - if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_LO(YAW)) { // Disconnect HI YAW + if (IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH) && IS_LO(YAW)) { // Disconnect Lo YAW if (rcdeviceInMenu) { key = RCDEVICE_CAM_KEY_CONNECTION_CLOSE; } @@ -289,10 +284,9 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) } if (key != RCDEVICE_CAM_KEY_NONE) { - if (rcdeviceIs5KeyEnabled()) { - rcdeviceSend5KeyOSDCableSimualtionEvent(key); - waitingDeviceResponse = true; - } + rcdeviceSend5KeyOSDCableSimualtionEvent(key); + isButtonPressed = true; + waitingDeviceResponse = true; } } } @@ -300,9 +294,12 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) void rcdeviceUpdate(timeUs_t currentTimeUs) { rcdeviceReceive(currentTimeUs); - + rcdeviceCameraControlProcess(); - rcdevice5KeySimulationProcess(currentTimeUs); + + if (rcdeviceIs5KeyEnabled()) { + rcdevice5KeySimulationProcess(currentTimeUs); + } } void rcdeviceInit(void) diff --git a/src/main/pg/rcdevice.c b/src/main/pg/rcdevice.c index 5e95f10d1..4dd60507e 100644 --- a/src/main/pg/rcdevice.c +++ b/src/main/pg/rcdevice.c @@ -25,6 +25,6 @@ PG_REGISTER_WITH_RESET_FN(rcdeviceConfig_t, rcdeviceConfig, PG_RCDEVICE_CONFIG, void pgResetFn_rcdeviceConfig(rcdeviceConfig_t *rcdeviceConfig) { - rcdeviceConfig->initDeviceAttempts = 4; + rcdeviceConfig->initDeviceAttempts = 6; rcdeviceConfig->initDeviceAttemptInterval = 1000; } \ No newline at end of file diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc index cb8c072ed..0a40e15c4 100644 --- a/src/test/unit/rcdevice_unittest.cc +++ b/src/test/unit/rcdevice_unittest.cc @@ -76,6 +76,7 @@ extern "C" { int minTimeout = 180; void rcdeviceSend5KeyOSDCableSimualtionEvent(rcdeviceCamSimulationKeyEvent_e key); + rcdeviceResponseParseContext_t* rcdeviceRespCtxQueueShift(rcdeviceWaitingResponseQueue *queue); } #define MAX_RESPONSES_COUNT 10 @@ -97,6 +98,13 @@ typedef struct testData_s { } testData_t; static testData_t testData; +extern rcdeviceWaitingResponseQueue watingResponseQueue; + +static void resetRCDeviceStatus() +{ + isButtonPressed = false; + rcdeviceInMenu = false; +} static void clearResponseBuff() { @@ -104,6 +112,10 @@ static void clearResponseBuff() testData.responseBufCount = 0; memset(testData.responseBufsLen, 0, MAX_RESPONSES_COUNT); memset(testData.responesBufs, 0, MAX_RESPONSES_COUNT * 60); + + while (rcdeviceRespCtxQueueShift(&watingResponseQueue)) { + + } } static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlushSerial) @@ -117,6 +129,9 @@ static void addResponseData(uint8_t *data, uint8_t dataLen, bool withDataForFlus TEST(RCDeviceTest, TestRCSplitInitWithoutPortConfigurated) { runcamDevice_t device; + + resetRCDeviceStatus(); + watingResponseQueue.headPos = 0; watingResponseQueue.tailPos = 0; watingResponseQueue.itemCount = 0; @@ -129,6 +144,8 @@ TEST(RCDeviceTest, TestRCSplitInitWithoutOpenPortConfigurated) { runcamDevice_t device; + resetRCDeviceStatus(); + watingResponseQueue.headPos = 0; watingResponseQueue.tailPos = 0; watingResponseQueue.itemCount = 0; @@ -144,6 +161,8 @@ TEST(RCDeviceTest, TestInitDevice) { runcamDevice_t device; + resetRCDeviceStatus(); + // test correct response watingResponseQueue.headPos = 0; watingResponseQueue.tailPos = 0; @@ -169,6 +188,8 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) { runcamDevice_t device; + resetRCDeviceStatus(); + // test correct response data with incorrect len watingResponseQueue.headPos = 0; watingResponseQueue.tailPos = 0; @@ -235,6 +256,8 @@ TEST(RCDeviceTest, TestInitDeviceWithInvalidResponse) TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready) { + resetRCDeviceStatus(); + // test correct response watingResponseQueue.headPos = 0; watingResponseQueue.tailPos = 0; @@ -295,6 +318,8 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceUnready) TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) { + resetRCDeviceStatus(); + // test correct response memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; @@ -355,6 +380,8 @@ TEST(RCDeviceTest, TestWifiModeChangeWithDeviceReady) TEST(RCDeviceTest, TestWifiModeChangeCombine) { + resetRCDeviceStatus(); + memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -437,6 +464,8 @@ TEST(RCDeviceTest, TestWifiModeChangeCombine) TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) { + resetRCDeviceStatus(); + memset(&testData, 0, sizeof(testData)); testData.isRunCamSplitOpenPortSupported = true; testData.isRunCamSplitPortConfigurated = true; @@ -627,12 +656,14 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationProtocol) TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) { + resetRCDeviceStatus(); + // 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); + rcdeviceUpdate(millis() * 1000); EXPECT_EQ(false, rcdeviceInMenu); // init device that have not 5 key OSD cable simulation feature @@ -656,7 +687,7 @@ TEST(RCDeviceTest, Test5KeyOSDCableSimulationWithout5KeyFeatureSupport) // open connection, rcdeviceInMenu will be false if the codes is right uint8_t responseDataOfOpenConnection[] = { 0xCC, 0x11, 0xe7 }; addResponseData(responseDataOfOpenConnection, sizeof(responseDataOfOpenConnection), false); - rcdeviceUpdate(0); + rcdeviceUpdate(millis() * 1000); EXPECT_EQ(false, rcdeviceInMenu); clearResponseBuff(); }