Merge pull request #10214 from azolyoung/add_fc_attitude_for_rcdevice

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

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);
+void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc);
+
+void runcamDeviceSendAttitude(runcamDevice_t *device);
+
+runcamDeviceRequest_t* rcdeviceGetRequest();

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)

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