src/main/rx/ibus.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  * Driver for IBUS (Flysky) receiver
  19  *   - initial implementation for MultiWii by Cesco/Pl¸schi
  20  *   - implementation for BaseFlight by Andreas (fiendie) Tacke
  21  *   - ported to CleanFlight by Konstantin (digitalentity) Sharlaimov
  22  */
  23
  24 #include <stdbool.h>
  25 #include <stdint.h>
  26 #include <stdlib.h>
  27
  28 #include "platform.h"
  29
  30 #ifdef USE_SERIAL_RX
  31
  32 #include "common/utils.h"
  33
  34 #include "drivers/serial.h"
  35 #include "drivers/serial_uart.h"
  36 #include "drivers/time.h"
  37
  38 #include "io/serial.h"
  39
  40 #ifdef USE_TELEMETRY
  41 #include "telemetry/telemetry.h"
  42 #endif
  43
  44 #include "rx/rx.h"
  45 #include "rx/ibus.h"
  46 #include "telemetry/ibus.h"
  47 #include "telemetry/ibus_shared.h"
  48
  49 #define IBUS_MAX_CHANNEL 14
  50 #define IBUS_BUFFSIZE 32
  51 #define IBUS_MODEL_IA6B 0
  52 #define IBUS_MODEL_IA6 1
  53 #define IBUS_FRAME_GAP 500
  54
  55 #define IBUS_BAUDRATE 115200
  56 #define IBUS_TELEMETRY_PACKET_LENGTH (4)
  57 #define IBUS_SERIAL_RX_PACKET_LENGTH (32)
  58
  59 static uint8_t ibusModel;
  60 static uint8_t ibusSyncByte;
  61 static uint8_t ibusFrameSize;
  62 static uint8_t ibusChannelOffset;
  63 static uint8_t rxBytesToIgnore;
  64 static uint16_t ibusChecksum;
  65
  66 static bool ibusFrameDone = false;
  67 static uint32_t ibusChannelData[IBUS_MAX_CHANNEL];
  68
  69 static uint8_t ibus[IBUS_BUFFSIZE] = { 0, };
  70
  71
  72 static bool isValidIa6bIbusPacketLength(uint8_t length)
  73 {
  74     return (length == IBUS_TELEMETRY_PACKET_LENGTH) || (length == IBUS_SERIAL_RX_PACKET_LENGTH);
  75 }
  76
  77
  78 // Receive ISR callback
  79 static void ibusDataReceive(uint16_t c, void *data)
  80 {
  81     UNUSED(data);
  82
  83     uint32_t ibusTime;
  84     static uint32_t ibusTimeLast;
  85     static uint8_t ibusFramePosition;
  86
  87     ibusTime = micros();
  88
  89     if ((ibusTime - ibusTimeLast) > IBUS_FRAME_GAP) {
  90         ibusFramePosition = 0;
  91         rxBytesToIgnore = 0;
  92     } else if (rxBytesToIgnore) {
  93         rxBytesToIgnore--;
  94         return;
  95     }
  96
  97     ibusTimeLast = ibusTime;
  98
  99     if (ibusFramePosition == 0) {
 100         if (isValidIa6bIbusPacketLength(c)) {
 101             ibusModel = IBUS_MODEL_IA6B;
 102             ibusSyncByte = c;
 103             ibusFrameSize = c;
 104             ibusChannelOffset = 2;
 105             ibusChecksum = 0xFFFF;
 106         } else if ((ibusSyncByte == 0) && (c == 0x55)) {
 107             ibusModel = IBUS_MODEL_IA6;
 108             ibusSyncByte = 0x55;
 109             ibusFrameSize = 31;
 110             ibusChecksum = 0x0000;
 111             ibusChannelOffset = 1;
 112         } else if (ibusSyncByte != c) {
 113             return;
 114         }
 115     }
 116
 117     ibus[ibusFramePosition] = (uint8_t)c;
 118
 119     if (ibusFramePosition == ibusFrameSize - 1) {
 120         ibusFrameDone = true;
 121     } else {
 122         ibusFramePosition++;
 123     }
 124 }
 125
 126
 127 static bool isChecksumOkIa6(void)
 128 {
 129     uint8_t offset;
 130     uint8_t i;
 131     uint16_t chksum, rxsum;
 132     chksum = ibusChecksum;
 133     rxsum = ibus[ibusFrameSize - 2] + (ibus[ibusFrameSize - 1] << 8);
 134     for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) {
 135         chksum += ibus[offset] + (ibus[offset + 1] << 8);
 136     }
 137     return chksum == rxsum;
 138 }
 139
 140
 141 static bool checksumIsOk(void) {
 142     if (ibusModel == IBUS_MODEL_IA6 ) {
 143         return isChecksumOkIa6();
 144     } else {
 145         return isChecksumOkIa6b(ibus, ibusFrameSize);
 146     }
 147 }
 148
 149
 150 static void updateChannelData(void) {
 151     uint8_t i;
 152     uint8_t offset;
 153
 154     for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) {
 155         ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8);
 156     }
 157 }
 158
 159 static uint8_t ibusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 160 {
 161     UNUSED(rxRuntimeConfig);
 162
 163     uint8_t frameStatus = RX_FRAME_PENDING;
 164
 165     if (!ibusFrameDone) {
 166         return frameStatus;
 167     }
 168
 169     ibusFrameDone = false;
 170
 171     if (checksumIsOk()) {
 172         if (ibusModel == IBUS_MODEL_IA6 || ibusSyncByte == 0x20) {
 173             updateChannelData();
 174             frameStatus = RX_FRAME_COMPLETE;
 175         }
 176         else
 177         {
 178 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
 179             rxBytesToIgnore = respondToIbusRequest(ibus);
 180 #endif
 181         }
 182     }
 183
 184     return frameStatus;
 185 }
 186
 187
 188 static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 189 {
 190     UNUSED(rxRuntimeConfig);
 191     return ibusChannelData[chan];
 192 }
 193
 194
 195 bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 196 {
 197     UNUSED(rxConfig);
 198     ibusSyncByte = 0;
 199
 200     rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL;
 201     rxRuntimeConfig->rxRefreshRate = 20000; // TODO - Verify speed
 202
 203     rxRuntimeConfig->rcReadRawFn = ibusReadRawRC;
 204     rxRuntimeConfig->rcFrameStatusFn = ibusFrameStatus;
 205
 206     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 207     if (!portConfig) {
 208         return false;
 209     }
 210
 211 #ifdef USE_TELEMETRY
 212     // bool portShared = telemetryCheckRxPortShared(portConfig);
 213     bool portShared = isSerialPortShared(portConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS);
 214 #else
 215     bool portShared = false;
 216 #endif
 217
 218
 219     rxBytesToIgnore = 0;
 220     serialPort_t *ibusPort = openSerialPort(portConfig->identifier,
 221         FUNCTION_RX_SERIAL,
 222         ibusDataReceive,
 223         NULL,
 224         IBUS_BAUDRATE,
 225         portShared ? MODE_RXTX : MODE_RX,
 226         (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex || portShared ? SERIAL_BIDIR : 0)
 227         );
 228
 229 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
 230     if (portShared) {
 231         initSharedIbusTelemetry(ibusPort);
 232     }
 233 #endif
 234
 235     return ibusPort != NULL;
 236 }
 237
 238 #endif //SERIAL_RX