src/main/io/rcdevice.c

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