src/main/io/rcdevice.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #include "common/crc.h"
  28 #include "common/maths.h"
  29 #include "common/streambuf.h"
  30
  31 #include "drivers/time.h"
  32
  33 #include "flight/imu.h"
  34
  35 #include "io/serial.h"
  36
  37 #include "pg/rcdevice.h"
  38
  39 #include "rcdevice.h"
  40
  41 #ifdef USE_RCDEVICE
  42
  43 typedef enum {
  44     RCDEVICE_STATE_WAITING_HEADER = 0,
  45     RCDEVICE_STATE_WAITING_COMMAND,
  46     RCDEVICE_STATE_WAITING_DATA_LENGTH,
  47     RCDEVICE_STATE_WAITING_DATA,
  48     RCDEVICE_STATE_WAITING_CRC,
  49 } RCDEVICE_PARSER_STATE;
  50
  51
  52 typedef struct {
  53     uint8_t state;
  54     uint8_t expectedDataLength;
  55     runcamDeviceRequest_t request;
  56     timeUs_t lastRecvDataTimestamp;
  57     uint8_t crc;
  58     uint8_t isParseDone;
  59 } runcamDeviceRequestParseContext_t;
  60
  61 typedef struct runcamDeviceExpectedResponseLength_s {
  62     uint8_t command;
  63     uint8_t reponseLength;
  64 } runcamDeviceExpectedResponseLength_t;
  65
  66 static runcamDeviceExpectedResponseLength_t expectedResponsesLength[] = {
  67     { RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO,            5},
  68     { RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS,      2},
  69     { RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE,    2},
  70     { RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION,            3},
  71 };
  72
  73 rcdeviceWaitingResponseQueue waitingResponseQueue;
  74 static uint8_t recvBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; // all the response contexts using same recv buffer
  75 static runcamDeviceRequestParseContext_t requestParserContext;
  76 static runcamDevice_t *currentDevice;
  77
  78 static uint8_t runcamDeviceGetRespLen(uint8_t command)
  79 {
  80     for (unsigned int i = 0; i < ARRAYLEN(expectedResponsesLength); i++) {
  81         if (expectedResponsesLength[i].command == command) {
  82             return expectedResponsesLength[i].reponseLength;
  83         }
  84     }
  85
  86     return 0;
  87 }
  88
  89 static bool rcdeviceRespCtxQueuePush(rcdeviceWaitingResponseQueue *queue, rcdeviceResponseParseContext_t *respCtx)
  90 {
  91     if (queue == NULL || (queue->itemCount + 1) > MAX_WAITING_RESPONSES) {
  92         return false;
  93     }
  94
  95     queue->buffer[queue->tailPos] = *respCtx;
  96
  97     int newTailPos = queue->tailPos + 1;
  98     if (newTailPos >= MAX_WAITING_RESPONSES) {
  99         newTailPos = 0;
 100     }
 101     queue->itemCount += 1;
 102     queue->tailPos = newTailPos;
 103
 104     return true;
 105 }
 106
 107 static rcdeviceResponseParseContext_t* rcdeviceRespCtxQueuePeekFront(rcdeviceWaitingResponseQueue *queue)
 108 {
 109     if (queue == NULL || queue->itemCount == 0) {
 110         return NULL;
 111     }
 112
 113     rcdeviceResponseParseContext_t *ctx = &queue->buffer[queue->headPos];
 114     return ctx;
 115 }
 116
 117 STATIC_UNIT_TESTED rcdeviceResponseParseContext_t* rcdeviceRespCtxQueueShift(rcdeviceWaitingResponseQueue *queue)
 118 {
 119     if (queue == NULL || queue->itemCount == 0) {
 120         return NULL;
 121     }
 122
 123     rcdeviceResponseParseContext_t *ctx = &queue->buffer[queue->headPos];
 124     int newHeadPos = queue->headPos + 1;
 125     if (newHeadPos >= MAX_WAITING_RESPONSES) {
 126         newHeadPos = 0;
 127     }
 128     queue->itemCount -= 1;
 129     queue->headPos = newHeadPos;
 130
 131     return ctx;
 132 }
 133
 134 // every time send packet to device, and want to get something from device,
 135 // it'd better call the method to clear the rx buffer before the packet send,
 136 // else may be the useless data in rx buffer will cause the response decoding
 137 // failed.
 138 static void runcamDeviceFlushRxBuffer(runcamDevice_t *device)
 139 {
 140     while (serialRxBytesWaiting(device->serialPort) > 0) {
 141         serialRead(device->serialPort);
 142     }
 143 }
 144
 145 // a common way to send packet to device
 146 static void runcamDeviceSendPacket(runcamDevice_t *device, uint8_t command, uint8_t *paramData, int paramDataLen)
 147 {
 148     // is this device open?
 149     if (!device->serialPort) {
 150         return;
 151     }
 152
 153     sbuf_t buf;
 154     // prepare pointer
 155     buf.ptr = device->buffer;
 156     buf.end = ARRAYEND(device->buffer);
 157
 158     sbufWriteU8(&buf, RCDEVICE_PROTOCOL_HEADER);
 159     sbufWriteU8(&buf, command);
 160
 161     if (paramData) {
 162         sbufWriteData(&buf, paramData, paramDataLen);
 163     }
 164
 165     // add crc over (all) data
 166     crc8_dvb_s2_sbuf_append(&buf, device->buffer);
 167
 168     // switch to reader
 169     sbufSwitchToReader(&buf, device->buffer);
 170
 171     // send data if possible
 172     serialWriteBuf(device->serialPort, sbufPtr(&buf), sbufBytesRemaining(&buf));
 173 }
 174
 175 // a common way to send a packet to device, and get response from the device.
 176 static void runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_t commandID, uint8_t *paramData, uint8_t paramDataLen, timeMs_t tiemout, int maxRetryTimes, void *userInfo, rcdeviceRespParseFunc parseFunc)
 177 {
 178     runcamDeviceFlushRxBuffer(device);
 179
 180     rcdeviceResponseParseContext_t responseCtx;
 181     memset(&responseCtx, 0, sizeof(rcdeviceResponseParseContext_t));
 182     responseCtx.recvBuf = recvBuf;
 183     responseCtx.command = commandID;
 184     responseCtx.maxRetryTimes = maxRetryTimes;
 185     responseCtx.expectedRespLen = runcamDeviceGetRespLen(commandID);
 186     responseCtx.timeout = tiemout;
 187     responseCtx.timeoutTimestamp = millis() + tiemout;
 188     responseCtx.parserFunc = parseFunc;
 189     responseCtx.device = device;
 190     responseCtx.protocolVersion = RCDEVICE_PROTOCOL_VERSION_1_0;
 191     if (paramData != NULL) {
 192         memcpy(responseCtx.paramData, paramData, paramDataLen);
 193         responseCtx.paramDataLen = paramDataLen;
 194     }
 195
 196     responseCtx.userInfo = userInfo;
 197     if (rcdeviceRespCtxQueuePush(&waitingResponseQueue, &responseCtx)) {
 198         // send packet
 199         runcamDeviceSendPacket(device, commandID, paramData, paramDataLen);
 200     }
 201 }
 202
 203 static void runcamDeviceParseV1DeviceInfo(rcdeviceResponseParseContext_t *ctx)
 204 {
 205     if (ctx->result != RCDEVICE_RESP_SUCCESS) {
 206         return;
 207     }
 208
 209     runcamDevice_t *device = ctx->device;
 210     device->info.protocolVersion = RCDEVICE_PROTOCOL_RCSPLIT_VERSION;
 211     device->info.features = RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON | RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON | RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE;
 212     device->isReady = true;
 213 }
 214
 215 static uint8_t crc8HighFirst(uint8_t *ptr, uint8_t len)
 216 {
 217     uint8_t crc = 0x00;
 218     while (len--) {
 219         crc ^= *ptr++;
 220         for (unsigned i = 8; i > 0; --i) {
 221             if (crc & 0x80)
 222                 crc = (crc << 1) ^ 0x31;
 223             else
 224                 crc = (crc << 1);
 225         }
 226     }
 227     return (crc);
 228 }
 229
 230 // for the rcsplits that firmware <= 1.1.0
 231 static void runcamSplitSendCommand(runcamDevice_t *device, uint8_t argument)
 232 {
 233     if (!device->serialPort) {
 234         return;
 235     }
 236
 237     uint8_t uart_buffer[5] = {0};
 238     uint8_t crc = 0;
 239
 240     uart_buffer[0] = RCSPLIT_PACKET_HEADER;
 241     uart_buffer[1] = RCSPLIT_PACKET_CMD_CTRL;
 242     uart_buffer[2] = argument;
 243     uart_buffer[3] = RCSPLIT_PACKET_TAIL;
 244     crc = crc8HighFirst(uart_buffer, 4);
 245
 246     // build up a full request [header]+[command]+[argument]+[crc]+[tail]
 247     uart_buffer[3] = crc;
 248     uart_buffer[4] = RCSPLIT_PACKET_TAIL;
 249
 250     // write to device
 251     serialWriteBuf(device->serialPort, uart_buffer, 5);
 252 }
 253
 254 static void runcamDeviceParseV2DeviceInfo(rcdeviceResponseParseContext_t *ctx)
 255 {
 256     if (ctx->result != RCDEVICE_RESP_SUCCESS) {
 257         runcamDeviceFlushRxBuffer(ctx->device);
 258
 259         rcdeviceResponseParseContext_t responseCtx;
 260         memset(&responseCtx, 0, sizeof(rcdeviceResponseParseContext_t));
 261         responseCtx.recvBuf = recvBuf;
 262         responseCtx.command = 0xFF;
 263         responseCtx.maxRetryTimes = rcdeviceConfig()->initDeviceAttempts;
 264         responseCtx.expectedRespLen = 5;
 265         responseCtx.timeout = rcdeviceConfig()->initDeviceAttemptInterval;
 266         responseCtx.timeoutTimestamp = millis() + rcdeviceConfig()->initDeviceAttemptInterval;
 267         responseCtx.parserFunc = runcamDeviceParseV1DeviceInfo;
 268         responseCtx.device = ctx->device;
 269         responseCtx.protocolVersion = RCDEVICE_PROTOCOL_RCSPLIT_VERSION;
 270         rcdeviceRespCtxQueuePush(&waitingResponseQueue, &responseCtx);
 271
 272         runcamSplitSendCommand(ctx->device, 0xFF);
 273         return;
 274     }
 275     runcamDevice_t *device = ctx->device;
 276     device->info.protocolVersion = ctx->recvBuf[1];
 277
 278     uint8_t featureLowBits = ctx->recvBuf[2];
 279     uint8_t featureHighBits = ctx->recvBuf[3];
 280     device->info.features = (featureHighBits << 8) | featureLowBits;
 281     device->isReady = true;
 282 }
 283
 284 // get the device info(firmware version, protocol version and features, see the
 285 // definition of runcamDeviceInfo_t to know more)
 286 static void runcamDeviceGetDeviceInfo(runcamDevice_t *device)
 287 {
 288     runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO, NULL, 0, rcdeviceConfig()->initDeviceAttemptInterval, rcdeviceConfig()->initDeviceAttempts, NULL, runcamDeviceParseV2DeviceInfo);
 289 }
 290
 291 // init the runcam device, it'll search the UART port with FUNCTION_RCDEVICE id
 292 // this function will delay 3 seconds to wait the device prepared(special for runcam split)
 293 void runcamDeviceInit(runcamDevice_t *device)
 294 {
 295     device->isReady = false;
 296     serialPortFunction_e portID = FUNCTION_RCDEVICE;
 297     const serialPortConfig_t *portConfig = findSerialPortConfig(portID);
 298     if (portConfig != NULL) {
 299         device->serialPort = openSerialPort(portConfig->identifier, portID, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
 300         device->info.protocolVersion = rcdeviceConfig()->protocolVersion;
 301         if (device->serialPort != NULL) {
 302             runcamDeviceGetDeviceInfo(device);
 303         }
 304     }
 305
 306     currentDevice = device;
 307 }
 308
 309 bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation)
 310 {
 311     if (device->info.protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) {
 312         runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL, &operation, sizeof(operation));
 313     } else if (device->info.protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) {
 314         runcamSplitSendCommand(device, operation + 1);
 315     } else {
 316         return false;
 317     }
 318
 319     return true;
 320 }
 321
 322 // every time start to control the OSD menu of camera, must call this method to
 323 // camera
 324 void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
 325 {
 326     uint8_t operation = RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN;
 327     runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc);
 328 }
 329
 330 // when the control was stop, must call this method to the camera to disconnect
 331 // with camera.
 332 void runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
 333 {
 334     uint8_t operation = RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE;
 335     runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc);
 336 }
 337
 338 // simulate button press event of 5 key osd cable with special button
 339 void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation, rcdeviceRespParseFunc parseFunc)
 340 {
 341     if (operation == RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE) {
 342         return;
 343     }
 344
 345     runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc);
 346 }
 347
 348 // simulate button release event of 5 key osd cable
 349 void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
 350 {
 351     runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, 400, 2, NULL, parseFunc);
 352 }
 353
 354 static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs)
 355 {
 356     rcdeviceResponseParseContext_t *respCtx = rcdeviceRespCtxQueuePeekFront(&waitingResponseQueue);
 357     while (respCtx != NULL && respCtx->timeoutTimestamp != 0 && currentTimeMs > respCtx->timeoutTimestamp) {
 358         if (respCtx->maxRetryTimes > 0) {
 359             if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) {
 360                 runcamDeviceSendPacket(respCtx->device, respCtx->command, respCtx->paramData, respCtx->paramDataLen);
 361             } else if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) {
 362                 runcamSplitSendCommand(respCtx->device, respCtx->command);
 363             }
 364
 365             respCtx->recvRespLen = 0;
 366             respCtx->timeoutTimestamp = currentTimeMs + respCtx->timeout;
 367             respCtx->maxRetryTimes -= 1;
 368             respCtx = NULL;
 369             break;
 370         } else {
 371             respCtx->result = RCDEVICE_RESP_TIMEOUT;
 372             if (respCtx->parserFunc != NULL) {
 373                 respCtx->parserFunc(respCtx);
 374             }
 375
 376             // dequeue and get next waiting response context
 377             rcdeviceRespCtxQueueShift(&waitingResponseQueue);
 378             respCtx = rcdeviceRespCtxQueuePeekFront(&waitingResponseQueue);
 379         }
 380     }
 381
 382     return respCtx;
 383 }
 384
 385 runcamDeviceRequest_t* rcdeviceGetRequest()
 386 {
 387     if (requestParserContext.isParseDone) {
 388         // reset the parse done state, then we can handle next request from rcdevice
 389         requestParserContext.isParseDone = 0;
 390         return &requestParserContext.request;
 391     }
 392
 393     return NULL;
 394 }
 395
 396 void rcdeviceReceive(timeUs_t currentTimeUs)
 397 {
 398     UNUSED(currentTimeUs);
 399
 400     // process the requests trigger by FC
 401     rcdeviceResponseParseContext_t *respCtx = NULL;
 402     while ((respCtx = getWaitingResponse(millis())) != NULL) {
 403         if (!serialRxBytesWaiting(respCtx->device->serialPort)) {
 404             break;
 405         }
 406
 407         const uint8_t c = serialRead(respCtx->device->serialPort);
 408         if (respCtx->recvRespLen == 0) {
 409             // Only start receiving packet when we found a header
 410             if ((respCtx->protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0 && c != RCDEVICE_PROTOCOL_HEADER) || (respCtx->protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION && c != RCSPLIT_PACKET_HEADER)) {
 411                 continue;
 412             }
 413         }
 414
 415         respCtx->recvBuf[respCtx->recvRespLen] = c;
 416         respCtx->recvRespLen += 1;
 417
 418         // if data received done, trigger callback to parse response data, and update rcdevice state
 419         if (respCtx->recvRespLen == respCtx->expectedRespLen) {
 420             if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_VERSION_1_0) {
 421                 uint8_t crc = 0;
 422                 for (int i = 0; i < respCtx->recvRespLen; i++) {
 423                     crc = crc8_dvb_s2(crc, respCtx->recvBuf[i]);
 424                 }
 425
 426                 respCtx->result = (crc == 0) ? RCDEVICE_RESP_SUCCESS : RCDEVICE_RESP_INCORRECT_CRC;
 427             } else if (respCtx->protocolVersion == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) {
 428                 if (respCtx->recvBuf[0] == RCSPLIT_PACKET_HEADER && respCtx->recvBuf[1] == RCSPLIT_PACKET_CMD_CTRL && respCtx->recvBuf[2] == 0xFF && respCtx->recvBuf[4] == RCSPLIT_PACKET_TAIL) {
 429                     uint8_t crcFromPacket = respCtx->recvBuf[3];
 430                     respCtx->recvBuf[3] = respCtx->recvBuf[4]; // move packet tail field to crc field, and calc crc with first 4 bytes
 431                     uint8_t crc = crc8HighFirst(respCtx->recvBuf, 4);
 432
 433                     respCtx->result = crc == crcFromPacket ? RCDEVICE_RESP_SUCCESS : RCDEVICE_RESP_INCORRECT_CRC;
 434                 } else {
 435                     respCtx->result = RCDEVICE_RESP_INCORRECT_CRC;
 436                 }
 437             }
 438
 439             if (respCtx->parserFunc != NULL) {
 440                 respCtx->parserFunc(respCtx);
 441             }
 442
 443             if (respCtx->result == RCDEVICE_RESP_SUCCESS) {
 444                 rcdeviceRespCtxQueueShift(&waitingResponseQueue);
 445             }
 446         }
 447     }
 448
 449     // process the requests trigger by device
 450     while (currentDevice != NULL) {
 451         if (!serialRxBytesWaiting(currentDevice->serialPort)) {
 452             break;
 453         }
 454
 455         // if lastest packet still not handled, do parse next bytes.
 456         if (requestParserContext.isParseDone) {
 457             break;
 458         }
 459
 460         // if it is during the packet receiving progress, check if it is already timeout(200 ms),
 461         // if timeout, then reset the state, else the later requests can't  be accept
 462         if (requestParserContext.state != RCDEVICE_STATE_WAITING_HEADER && millis() - requestParserContext.lastRecvDataTimestamp > 200) {
 463             memset(&requestParserContext, 0, sizeof(runcamDeviceRequestParseContext_t));
 464             requestParserContext.state = RCDEVICE_STATE_WAITING_COMMAND; // reset state to waiting header
 465         }
 466
 467         const uint8_t c = serialRead(currentDevice->serialPort);
 468         switch (requestParserContext.state) {
 469             case RCDEVICE_STATE_WAITING_HEADER:
 470                 if (c == RCDEVICE_PROTOCOL_HEADER) {
 471                     memset(&requestParserContext, 0, sizeof(runcamDeviceRequestParseContext_t));
 472                     requestParserContext.state = RCDEVICE_STATE_WAITING_COMMAND;
 473                 }
 474                 break;
 475             case RCDEVICE_STATE_WAITING_COMMAND:
 476                 requestParserContext.request.command = c;
 477                 // there is no payload for RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE, skip to waiting crc step
 478                 if (requestParserContext.request.command == RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE) {
 479                     requestParserContext.state = RCDEVICE_STATE_WAITING_CRC;
 480                 } else {
 481                     // for now, only RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE support, so reset the state to waiting header.
 482                     requestParserContext.state = RCDEVICE_PROTOCOL_HEADER;
 483                 }
 484                 break;
 485             case RCDEVICE_STATE_WAITING_DATA_LENGTH:
 486                 requestParserContext.expectedDataLength = c;
 487                 requestParserContext.state = RCDEVICE_STATE_WAITING_DATA;
 488                 break;
 489             case RCDEVICE_STATE_WAITING_DATA:
 490                 if (requestParserContext.request.dataLength < requestParserContext.expectedDataLength) {
 491                     requestParserContext.request.data[requestParserContext.request.dataLength] = c;
 492                     requestParserContext.request.dataLength++;
 493                 }
 494
 495                 if (requestParserContext.request.dataLength == requestParserContext.expectedDataLength) {
 496                     requestParserContext.state = RCDEVICE_STATE_WAITING_CRC; // data received done
 497                 }
 498                 break;
 499             case RCDEVICE_STATE_WAITING_CRC: {
 500                 // verify crc
 501                 uint8_t crc = 0;
 502                 uint8_t header = RCDEVICE_PROTOCOL_HEADER;
 503                 crc = crc8_dvb_s2_update(crc, &header, 1);
 504                 crc = crc8_dvb_s2_update(crc, &requestParserContext.request.command, 1);
 505                 crc = crc8_dvb_s2_update(crc, &requestParserContext.request.data, requestParserContext.request.dataLength);
 506
 507                 if (crc == c) {
 508                     requestParserContext.isParseDone = 1;
 509                 }
 510
 511                 requestParserContext.state = RCDEVICE_STATE_WAITING_HEADER;
 512             }
 513                 break;
 514         }
 515
 516         requestParserContext.lastRecvDataTimestamp = millis();
 517     }
 518 }
 519
 520 void runcamDeviceSendAttitude(runcamDevice_t *device)
 521 {
 522     uint16_t buf[3];
 523     buf[0] = attitude.values.roll;
 524     buf[1] = attitude.values.pitch;
 525     buf[2] = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 526     runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_REQUEST_FC_ATTITUDE, (uint8_t *)buf, sizeof(buf));
 527 }
 528
 529 #endif