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
  42 typedef struct runcamDeviceExpectedResponseLength_s {
  43     uint8_t command;
  44     uint8_t reponseLength;
  45 } runcamDeviceExpectedResponseLength_t;
  46
  47 static runcamDeviceExpectedResponseLength_t expectedResponsesLength[] = {
  48     { RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO,            5},
  49     { RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS,      2},
  50     { RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE,    2},
  51     { RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION,            3},
  52 };
  53
  54 rcdeviceWaitingResponseQueue watingResponseQueue;
  55 static uint8_t recvBuf[RCDEVICE_PROTOCOL_MAX_PACKET_SIZE]; // all the response contexts using same recv buffer
  56
  57 static uint8_t runcamDeviceGetRespLen(uint8_t command)
  58 {
  59     for (unsigned int i = 0; i < ARRAYLEN(expectedResponsesLength); i++) {
  60         if (expectedResponsesLength[i].command == command) {
  61             return expectedResponsesLength[i].reponseLength;
  62         }
  63     }
  64
  65     return 0;
  66 }
  67
  68 static bool rcdeviceRespCtxQueuePush(rcdeviceWaitingResponseQueue *queue, rcdeviceResponseParseContext_t *respCtx)
  69 {
  70     if (queue == NULL || (queue->itemCount + 1) > MAX_WAITING_RESPONSES) {
  71         return false;
  72     }
  73
  74     queue->buffer[queue->tailPos] = *respCtx;
  75
  76     int newTailPos = queue->tailPos + 1;
  77     if (newTailPos >= MAX_WAITING_RESPONSES) {
  78         newTailPos = 0;
  79     }
  80     queue->itemCount += 1;
  81     queue->tailPos = newTailPos;
  82
  83     return true;
  84 }
  85
  86 static rcdeviceResponseParseContext_t* rcdeviceRespCtxQueuePeekFront(rcdeviceWaitingResponseQueue *queue)
  87 {
  88     if (queue == NULL || queue->itemCount == 0) {
  89         return NULL;
  90     }
  91
  92     rcdeviceResponseParseContext_t *ctx = &queue->buffer[queue->headPos];
  93     return ctx;
  94 }
  95
  96 STATIC_UNIT_TESTED rcdeviceResponseParseContext_t* rcdeviceRespCtxQueueShift(rcdeviceWaitingResponseQueue *queue)
  97 {
  98     if (queue == NULL || queue->itemCount == 0) {
  99         return NULL;
 100     }
 101
 102     rcdeviceResponseParseContext_t *ctx = &queue->buffer[queue->headPos];
 103     int newHeadPos = queue->headPos + 1;
 104     if (newHeadPos >= MAX_WAITING_RESPONSES) {
 105         newHeadPos = 0;
 106     }
 107     queue->itemCount -= 1;
 108     queue->headPos = newHeadPos;
 109
 110     return ctx;
 111 }
 112
 113 // every time send packet to device, and want to get something from device,
 114 // it'd better call the method to clear the rx buffer before the packet send,
 115 // else may be the useless data in rx buffer will cause the response decoding
 116 // failed.
 117 static void runcamDeviceFlushRxBuffer(runcamDevice_t *device)
 118 {
 119     while (serialRxBytesWaiting(device->serialPort) > 0) {
 120         serialRead(device->serialPort);
 121     }
 122 }
 123
 124 // a common way to send packet to device
 125 static void runcamDeviceSendPacket(runcamDevice_t *device, uint8_t command, uint8_t *paramData, int paramDataLen)
 126 {
 127     // is this device open?
 128     if (!device->serialPort) {
 129         return;
 130     }
 131
 132     sbuf_t buf;
 133     // prepare pointer
 134     buf.ptr = device->buffer;
 135     buf.end = ARRAYEND(device->buffer);
 136
 137     sbufWriteU8(&buf, RCDEVICE_PROTOCOL_HEADER);
 138     sbufWriteU8(&buf, command);
 139
 140     if (paramData) {
 141         sbufWriteData(&buf, paramData, paramDataLen);
 142     }
 143
 144     // add crc over (all) data
 145     crc8_dvb_s2_sbuf_append(&buf, device->buffer);
 146
 147     // switch to reader
 148     sbufSwitchToReader(&buf, device->buffer);
 149
 150     // send data if possible
 151     serialWriteBuf(device->serialPort, sbufPtr(&buf), sbufBytesRemaining(&buf));
 152 }
 153
 154 // a common way to send a packet to device, and get response from the device.
 155 static void runcamDeviceSendRequestAndWaitingResp(runcamDevice_t *device, uint8_t commandID, uint8_t *paramData, uint8_t paramDataLen, timeMs_t tiemout, int maxRetryTimes, void *userInfo, rcdeviceRespParseFunc parseFunc)
 156 {
 157     runcamDeviceFlushRxBuffer(device);
 158
 159     rcdeviceResponseParseContext_t responseCtx;
 160     memset(&responseCtx, 0, sizeof(rcdeviceResponseParseContext_t));
 161     responseCtx.recvBuf = recvBuf;
 162     responseCtx.command = commandID;
 163     responseCtx.maxRetryTimes = maxRetryTimes;
 164     responseCtx.expectedRespLen = runcamDeviceGetRespLen(commandID);
 165     responseCtx.timeout = tiemout;
 166     responseCtx.timeoutTimestamp = millis() + tiemout;
 167     responseCtx.parserFunc = parseFunc;
 168     responseCtx.device = device;
 169     responseCtx.protocolVer = RCDEVICE_PROTOCOL_VERSION_1_0;
 170     if (paramData != NULL) {
 171         memcpy(responseCtx.paramData, paramData, paramDataLen);
 172         responseCtx.paramDataLen = paramDataLen;
 173     }
 174     responseCtx.userInfo = userInfo;
 175     rcdeviceRespCtxQueuePush(&watingResponseQueue, &responseCtx);
 176
 177     // send packet
 178     runcamDeviceSendPacket(device, commandID, paramData, paramDataLen);
 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.protocolVer = 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.protocolVer = RCDEVICE_PROTOCOL_RCSPLIT_VERSION;
 248         rcdeviceRespCtxQueuePush(&watingResponseQueue, &responseCtx);
 249
 250         runcamSplitSendCommand(ctx->device, 0xFF);
 251         return;
 252     }
 253     runcamDevice_t *device = ctx->device;
 254     device->info.protocolVer = 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 400ms in the first loop to wait the device prepared,
 271 // as we know, there are has some camera need about 200~400ms to initialization,
 272 // and then we can send/receive from it.
 273 void runcamDeviceInit(runcamDevice_t *device)
 274 {
 275     device->isReady = false;
 276     serialPortFunction_e portID = FUNCTION_RCDEVICE;
 277     serialPortConfig_t *portConfig = findSerialPortConfig(portID);
 278     if (portConfig != NULL) {
 279         device->serialPort = openSerialPort(portConfig->identifier, portID, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
 280
 281         if (device->serialPort != NULL) {
 282             // send RCDEVICE_PROTOCOL_COMMAND_GET_DEVICE_INFO to device to retrive
 283             // device info, e.g protocol version, supported features
 284             runcamDeviceGetDeviceInfo(device);
 285         }
 286     }
 287 }
 288
 289
 290 bool runcamDeviceSimulateCameraButton(runcamDevice_t *device, uint8_t operation)
 291 {
 292     if (device->info.protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0) {
 293         runcamDeviceSendPacket(device, RCDEVICE_PROTOCOL_COMMAND_CAMERA_CONTROL, &operation, sizeof(operation));
 294     } else if (device->info.protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) {
 295         runcamSplitSendCommand(device, operation + 1);
 296     } else {
 297         return false;
 298     }
 299
 300     return true;
 301 }
 302
 303 // every time start to control the OSD menu of camera, must call this method to
 304 // camera
 305 void runcamDeviceOpen5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
 306 {
 307     uint8_t operation = RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN;
 308     runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc);
 309 }
 310
 311 // when the control was stop, must call this method to the camera to disconnect
 312 // with camera.
 313 void runcamDeviceClose5KeyOSDCableConnection(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
 314 {
 315     uint8_t operation = RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE;
 316     runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_CONNECTION, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc);
 317 }
 318
 319 // simulate button press event of 5 key osd cable with special button
 320 void runcamDeviceSimulate5KeyOSDCableButtonPress(runcamDevice_t *device, uint8_t operation, rcdeviceRespParseFunc parseFunc)
 321 {
 322     if (operation == RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE) {
 323         return;
 324     }
 325
 326     runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_PRESS, &operation, sizeof(uint8_t), 400, 2, NULL, parseFunc);
 327 }
 328
 329 // simulate button release event of 5 key osd cable
 330 void runcamDeviceSimulate5KeyOSDCableButtonRelease(runcamDevice_t *device, rcdeviceRespParseFunc parseFunc)
 331 {
 332     runcamDeviceSendRequestAndWaitingResp(device, RCDEVICE_PROTOCOL_COMMAND_5KEY_SIMULATION_RELEASE, NULL, 0, 400, 2, NULL, parseFunc);
 333 }
 334
 335 static rcdeviceResponseParseContext_t* getWaitingResponse(timeMs_t currentTimeMs)
 336 {
 337     rcdeviceResponseParseContext_t *respCtx = rcdeviceRespCtxQueuePeekFront(&watingResponseQueue);
 338     while (respCtx != NULL && respCtx->timeoutTimestamp != 0 && currentTimeMs > respCtx->timeoutTimestamp) {
 339         if (respCtx->maxRetryTimes > 0) {
 340             if (respCtx->protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0) {
 341                 runcamDeviceSendPacket(respCtx->device, respCtx->command, respCtx->paramData, respCtx->paramDataLen);
 342             } else if (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) {
 343                 runcamSplitSendCommand(respCtx->device, respCtx->command);
 344             }
 345
 346             respCtx->recvRespLen = 0;
 347             respCtx->timeoutTimestamp = currentTimeMs + respCtx->timeout;
 348             respCtx->maxRetryTimes -= 1;
 349             respCtx = NULL;
 350             break;
 351         } else {
 352             respCtx->result = RCDEVICE_RESP_TIMEOUT;
 353             if (respCtx->parserFunc != NULL) {
 354                 respCtx->parserFunc(respCtx);
 355             }
 356
 357             // dequeue and get next waiting response context
 358             rcdeviceRespCtxQueueShift(&watingResponseQueue);
 359             respCtx = rcdeviceRespCtxQueuePeekFront(&watingResponseQueue);
 360         }
 361     }
 362
 363     return respCtx;
 364 }
 365
 366 void rcdeviceReceive(timeUs_t currentTimeUs)
 367 {
 368     UNUSED(currentTimeUs);
 369     rcdeviceResponseParseContext_t *respCtx = NULL;
 370     while ((respCtx = getWaitingResponse(millis())) != NULL && serialRxBytesWaiting(respCtx->device->serialPort)) {
 371         const uint8_t c = serialRead(respCtx->device->serialPort);
 372
 373         if (respCtx->recvRespLen == 0) {
 374             // Only start receiving packet when we found a header
 375             if ((respCtx->protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0 && c != RCDEVICE_PROTOCOL_HEADER) || (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION && c != RCSPLIT_PACKET_HEADER)) {
 376                 continue;
 377             }
 378         }
 379
 380         respCtx->recvBuf[respCtx->recvRespLen] = c;
 381         respCtx->recvRespLen += 1;
 382
 383         // if data received done, trigger callback to parse response data, and update rcdevice state
 384         if (respCtx->recvRespLen == respCtx->expectedRespLen) {
 385             // verify the crc value
 386             if (respCtx->protocolVer == RCDEVICE_PROTOCOL_VERSION_1_0) {
 387                 uint8_t crc = 0;
 388                 for (int i = 0; i < respCtx->recvRespLen; i++) {
 389                     crc = crc8_dvb_s2(crc, respCtx->recvBuf[i]);
 390                 }
 391
 392                 respCtx->result = (crc == 0) ? RCDEVICE_RESP_SUCCESS : RCDEVICE_RESP_INCORRECT_CRC;
 393             } else if (respCtx->protocolVer == RCDEVICE_PROTOCOL_RCSPLIT_VERSION) {
 394                 // do nothing, just call parserFunc
 395                 respCtx->result = RCDEVICE_RESP_SUCCESS;
 396             }
 397
 398             if (respCtx->parserFunc != NULL) {
 399                 respCtx->parserFunc(respCtx);
 400             }
 401
 402             // dequeue current response context
 403             rcdeviceRespCtxQueueShift(&watingResponseQueue);
 404         }
 405     }
 406 }
 407
 408 #endif