diff --git a/src/main/io/rcdevice.c b/src/main/io/rcdevice.c index a96a91fcf8..5a51951878 100644 --- a/src/main/io/rcdevice.c +++ b/src/main/io/rcdevice.c @@ -30,6 +30,8 @@ #include "drivers/time.h" +#include "flight/imu.h" + #include "io/serial.h" #include "pg/rcdevice.h" @@ -38,6 +40,24 @@ #ifdef USE_RCDEVICE +typedef enum { + RCDEVICE_STATE_WAITING_HEADER = 0, + RCDEVICE_STATE_WAITING_COMMAND, + RCDEVICE_STATE_WAITING_DATA_LENGTH, + RCDEVICE_STATE_WAITING_DATA, + RCDEVICE_STATE_WAITING_CRC, +} RCDEVICE_PARSER_STATE; + + +typedef struct { + uint8_t state; + uint8_t expectedDataLength; + runcamDeviceRequest_t request; + timeUs_t lastRecvDataTimestamp; + uint8_t crc; + uint8_t isParseDone; +} runcamDeviceRequestParseContext_t; + typedef struct runcamDeviceExpectedResponseLength_s { uint8_t command; uint8_t reponseLength; @@ -52,6 +72,8 @@ static runcamDeviceExpectedResponseLength_t expectedResponsesLength[] = { rcdeviceWaitingResponseQueue waitingResponseQueue; static uint8_t recvBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; // all the response contexts using same recv buffer +static runcamDeviceRequestParseContext_t requestParserContext; +static runcamDevice_t *currentDevice; static uint8_t runcamDeviceGetRespLen(uint8_t command) { @@ -280,6 +302,8 @@ void runcamDeviceInit(runcamDevice_t *device) runcamDeviceGetDeviceInfo(device); } } + + currentDevice = device; } bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation) @@ -358,10 +382,22 @@ static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs return respCtx; } +runcamDeviceRequest_t* rcdeviceGetRequest() +{ + if (requestParserContext.isParseDone) { + // reset the parse done state, then we can handle next request from rcdevice + requestParserContext.isParseDone = 0; + return &requestParserContext.request; + } + + return NULL; +} + void rcdeviceReceive(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); + // process the requests trigger by FC rcdeviceResponseParseContext_t *respCtx = NULL; while ((respCtx = getWaitingResponse(millis())) != NULL) { if (!serialRxBytesWaiting(respCtx->device->serialPort)) { @@ -409,6 +445,85 @@ void rcdeviceReceive(timeUs_t currentTimeUs) } } } + + // process the requests trigger by device + while (currentDevice != NULL) { + if (!serialRxBytesWaiting(currentDevice->serialPort)) { + break; + } + + // if lastest packet still not handled, do parse next bytes. + if (requestParserContext.isParseDone) { + break; + } + + // if it is during the packet receiving progress, check if it is already timeout(200 ms), + // if timeout, then reset the state, else the later requests can't be accept + if (requestParserContext.state != RCDEVICE_STATE_WAITING_HEADER && millis() - requestParserContext.lastRecvDataTimestamp > 200) { + memset(&requestParserContext, 0, sizeof(runcamDeviceRequestParseContext_t)); + requestParserContext.state = RCDEVICE_STATE_WAITING_COMMAND; // reset state to waiting header + } + + const uint8_t c = serialRead(currentDevice->serialPort); + switch (requestParserContext.state) { + case RCDEVICE_STATE_WAITING_HEADER: + if (c == RCDEVICE_PROTOCOL_HEADER) { + memset(&requestParserContext, 0, sizeof(runcamDeviceRequestParseContext_t)); + requestParserContext.state = RCDEVICE_STATE_WAITING_COMMAND; + } + break; + case RCDEVICE_STATE_WAITING_COMMAND: + requestParserContext.request.command = c; + // there is no payload for RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE, skip to waiting crc step + if (requestParserContext.request.command == RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE) { + requestParserContext.state = RCDEVICE_STATE_WAITING_CRC; + } else { + // for now, only RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE support, so reset the state to waiting header. + requestParserContext.state = RCDEVICE_PROTOCOL_HEADER; + } + break; + case RCDEVICE_STATE_WAITING_DATA_LENGTH: + requestParserContext.expectedDataLength = c; + requestParserContext.state = RCDEVICE_STATE_WAITING_DATA; + break; + case RCDEVICE_STATE_WAITING_DATA: + if (requestParserContext.request.dataLength < requestParserContext.expectedDataLength) { + requestParserContext.request.data[requestParserContext.request.dataLength] = c; + requestParserContext.request.dataLength++; + } + + if (requestParserContext.request.dataLength == requestParserContext.expectedDataLength) { + requestParserContext.state = RCDEVICE_STATE_WAITING_CRC; // data received done + } + break; + case RCDEVICE_STATE_WAITING_CRC: { + // verify crc + uint8_t crc = 0; + uint8_t header = RCDEVICE_PROTOCOL_HEADER; + crc = crc8_dvb_s2_update(crc, &header, 1); + crc = crc8_dvb_s2_update(crc, &requestParserContext.request.command, 1); + crc = crc8_dvb_s2_update(crc, &requestParserContext.request.data, requestParserContext.request.dataLength); + + if (crc == c) { + requestParserContext.isParseDone = 1; + } + + requestParserContext.state = RCDEVICE_STATE_WAITING_HEADER; + } + break; + } + + requestParserContext.lastRecvDataTimestamp = millis(); + } +} + +void runcamDeviceSendAttitude(runcamDevice_t *device) +{ + uint16_t buf[3]; + buf[0] = attitude.values.roll; + buf[1] = attitude.values.pitch; + buf[2] = DECIDEGREES_TO_DEGREES(attitude.values.yaw); + runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE, (uint8_t *)buf, sizeof(buf)); } #endif diff --git a/src/main/io/rcdevice.h b/src/main/io/rcdevice.h index a31f7c4f4d..80e786f462 100644 --- a/src/main/io/rcdevice.h +++ b/src/main/io/rcdevice.h @@ -38,6 +38,7 @@ #define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS 0x02 #define RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE 0x03 #define RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION 0x04 +#define RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE 0x50 // Old protocol defines #define RCSPLIT_PACKET_HEADER 0x55 @@ -53,6 +54,7 @@ typedef enum { RCDEVICE_PROTOCOL_FEATURE_START_RECORDING = (1 << 6), RCDEVICE_PROTOCOL_FEATURE_STOP_RECORDING = (1 << 7), RCDEVICE_PROTOCOL_FEATURE_CMS_MENU = (1 << 8), + RCDEVICE_PROTOCOL_FEATURE_FC_ATTITUDE = (1 << 9) } rcdevice_features_e; // Operation of Camera Button Simulation @@ -150,6 +152,12 @@ typedef struct { rcdeviceRespParseFunc parseFunc; } rcdeviceWaitingResponseQueue; +typedef struct { + uint8_t command; + uint8_t data[RCDEVICE_PROTOCOL_MAX_DATA_SIZE - 1]; + uint8_t dataLength; +} runcamDeviceRequest_t; + void runcamDeviceInit(runcamDevice_t *device); void rcdeviceReceive(timeUs_t currentTimeUs); @@ -160,4 +168,8 @@ bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation) 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); \ No newline at end of file +void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc); + +void runcamDeviceSendAttitude(runcamDevice_t *device); + +runcamDeviceRequest_t* rcdeviceGetRequest(); diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c index 697fd223aa..a31d811759 100644 --- a/src/main/io/rcdevice_cam.c +++ b/src/main/io/rcdevice_cam.c @@ -55,7 +55,7 @@ bool isButtonPressed = false; bool waitingDeviceResponse = false; -static bool isFeatureSupported(uint8_t feature) +static bool isFeatureSupported(uint16_t feature) { if (camDevice->info.features & feature || rcdeviceConfig()->feature & feature) { return true; @@ -284,6 +284,15 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) } } +static void rcdeviceProcessDeviceRequest(runcamDeviceRequest_t *request) +{ + switch (request->command) { + case RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE: + runcamDeviceSendAttitude(camDevice); + break; + } +} + void rcdeviceUpdate(timeUs_t currentTimeUs) { rcdeviceReceive(currentTimeUs); @@ -291,6 +300,13 @@ void rcdeviceUpdate(timeUs_t currentTimeUs) rcdeviceCameraControlProcess(); rcdevice5KeySimulationProcess(currentTimeUs); + + if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_FC_ATTITUDE)) { + runcamDeviceRequest_t *request = rcdeviceGetRequest(); + if (request) { + rcdeviceProcessDeviceRequest(request); + } + } } void rcdeviceInit(void) diff --git a/src/test/unit/rcdevice_unittest.cc b/src/test/unit/rcdevice_unittest.cc index e659555d21..4566674ef8 100644 --- a/src/test/unit/rcdevice_unittest.cc +++ b/src/test/unit/rcdevice_unittest.cc @@ -32,6 +32,7 @@ extern "C" { #include "fc/rc_controls.h" #include "fc/rc_modes.h" + #include "flight/imu.h" #include "drivers/serial.h" @@ -995,4 +996,5 @@ extern "C" { uint32_t resumeRefreshAt = 0; int getArmingDisableFlags(void) {return 0;} void pinioBoxTaskControl(void) {} + attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; }