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