rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #6810 from azolyoung/fix_rcsplit_t_control_bug 

fix rcdevice can't work
This commit is contained in:
Michael Keller 2018-10-06 15:38:20 +13:00 committed by GitHub
parent 1c5568cd8d 18085eafdd
commit 811364c2ec
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 161 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
src/main/io/rcdevice.c
View File

@ -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) {

9
src/main/io/rcdevice.h
View File

@ -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,

39
src/main/io/rcdevice_cam.c
View File

@ -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)

2
src/main/pg/rcdevice.c
View File

@ -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;
}

35
src/test/unit/rcdevice_unittest.cc
View File

@ -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();
}