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_SERIALRX_IBUS
  31
  32 #include "common/utils.h"
  33 #include "common/time.h"
  34
  35 #include "drivers/system.h"
  36 #include "drivers/time.h"
  37
  38 #include "drivers/serial.h"
  39 #include "drivers/serial_uart.h"
  40 #include "io/serial.h"
  41
  42 #include "telemetry/telemetry.h"
  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 18
  50 //In AFHDS there is 18 channels encoded in 14 slots (each slot is 2 byte long)
  51 #define IBUS_MAX_SLOTS 14
  52 #define IBUS_BUFFSIZE 32
  53 #define IBUS_MODEL_IA6B 0
  54 #define IBUS_MODEL_IA6 1
  55 #define IBUS_FRAME_GAP 500
  56
  57 #define IBUS_TELEMETRY_PACKET_LENGTH (4)
  58 #define IBUS_SERIAL_RX_PACKET_LENGTH (32)
  59
  60 static uint8_t ibusModel;
  61 static uint8_t ibusSyncByte;
  62 static uint8_t ibusFrameSize;
  63 static uint8_t ibusChannelOffset;
  64 static uint8_t rxBytesToIgnore;
  65 static uint16_t ibusChecksum;
  66
  67 static bool ibusFrameDone = false;
  68 static uint32_t ibusChannelData[IBUS_MAX_CHANNEL];
  69
  70 static uint8_t ibus[IBUS_BUFFSIZE] = { 0, };
  71
  72
  73 static bool isValidIa6bIbusPacketLength(uint8_t length)
  74 {
  75     return (length == IBUS_TELEMETRY_PACKET_LENGTH) || (length == IBUS_SERIAL_RX_PACKET_LENGTH);
  76 }
  77
  78
  79 // Receive ISR callback
  80 static void ibusDataReceive(uint16_t c, void *rxCallbackData)
  81 {
  82     UNUSED(rxCallbackData);
  83
  84     timeUs_t ibusTime;
  85     static timeUs_t ibusTimeLast;
  86     static uint8_t ibusFramePosition;
  87
  88     ibusTime = micros();
  89
  90     if ((ibusTime - ibusTimeLast) > IBUS_FRAME_GAP) {
  91         ibusFramePosition = 0;
  92         rxBytesToIgnore = 0;
  93     } else if (rxBytesToIgnore) {
  94         rxBytesToIgnore--;
  95         return;
  96     }
  97
  98     ibusTimeLast = ibusTime;
  99
 100     if (ibusFramePosition == 0) {
 101         if (isValidIa6bIbusPacketLength(c)) {
 102             ibusModel = IBUS_MODEL_IA6B;
 103             ibusSyncByte = c;
 104             ibusFrameSize = c;
 105             ibusChannelOffset = 2;
 106             ibusChecksum = 0xFFFF;
 107         } else if ((ibusSyncByte == 0) && (c == 0x55)) {
 108             ibusModel = IBUS_MODEL_IA6;
 109             ibusSyncByte = 0x55;
 110             ibusFrameSize = 31;
 111             ibusChecksum = 0x0000;
 112             ibusChannelOffset = 1;
 113         } else if (ibusSyncByte != c) {
 114             return;
 115         }
 116     }
 117
 118     ibus[ibusFramePosition] = (uint8_t)c;
 119
 120     if (ibusFramePosition == ibusFrameSize - 1) {
 121         ibusFrameDone = true;
 122     } else {
 123         ibusFramePosition++;
 124     }
 125 }
 126
 127 uint16_t ibusCalculateChecksum(const uint8_t *ibusPacket, size_t packetLength)
 128 {
 129     uint16_t checksum = 0xFFFF;
 130     for (size_t i = 0; i < packetLength - IBUS_CHECKSUM_SIZE; i++) {
 131         checksum -= ibusPacket[i];
 132     }
 133
 134     return checksum;
 135 }
 136
 137 static bool isChecksumOkIa6(void)
 138 {
 139     uint8_t offset;
 140     uint8_t i;
 141     uint16_t chksum, rxsum;
 142     chksum = ibusChecksum;
 143     rxsum = ibus[ibusFrameSize - 2] + (ibus[ibusFrameSize - 1] << 8);
 144     for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_SLOTS; i++, offset += 2) {
 145         chksum += ibus[offset] + (ibus[offset + 1] << 8);
 146     }
 147     return chksum == rxsum;
 148 }
 149
 150 bool ibusIsChecksumOkIa6b(const uint8_t *ibusPacket, const uint8_t length)
 151 {
 152     uint16_t calculatedChecksum = ibusCalculateChecksum(ibusPacket, length);
 153
 154     // Note that there's a byte order swap to little endian here
 155     return (calculatedChecksum >> 8) == ibusPacket[length - 1]
 156            && (calculatedChecksum & 0xFF) == ibusPacket[length - 2];
 157 }
 158
 159 static bool checksumIsOk(void) {
 160     if (ibusModel == IBUS_MODEL_IA6 ) {
 161         return isChecksumOkIa6();
 162     } else {
 163         return ibusIsChecksumOkIa6b(ibus, ibusFrameSize);
 164     }
 165 }
 166
 167 static void updateChannelData(void) {
 168     uint8_t i;
 169     uint8_t offset;
 170
 171     for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_SLOTS; i++, offset += 2) {
 172         ibusChannelData[i] = ibus[offset] + ((ibus[offset + 1] & 0x0F) << 8);
 173     }
 174     //latest IBUS recievers are using prviously not used 4 bits on every channel to incresse total channel count
 175     for (i = IBUS_MAX_SLOTS, offset = ibusChannelOffset + 1; i < IBUS_MAX_CHANNEL; i++, offset += 6) {
 176         ibusChannelData[i] = ((ibus[offset] & 0xF0) >> 4) | (ibus[offset + 2] & 0xF0) | ((ibus[offset + 4] & 0xF0) << 4);
 177     }
 178 }
 179
 180 static uint8_t ibusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 181 {
 182     UNUSED(rxRuntimeConfig);
 183
 184     uint8_t frameStatus = RX_FRAME_PENDING;
 185
 186     if (!ibusFrameDone) {
 187         return frameStatus;
 188     }
 189
 190     ibusFrameDone = false;
 191
 192     if (checksumIsOk()) {
 193         if (ibusModel == IBUS_MODEL_IA6 || ibusSyncByte == 0x20) {
 194             updateChannelData();
 195             frameStatus = RX_FRAME_COMPLETE;
 196         }
 197         else
 198         {
 199 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
 200             rxBytesToIgnore = respondToIbusRequest(ibus);
 201 #endif
 202         }
 203     }
 204
 205     return frameStatus;
 206 }
 207
 208 static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 209 {
 210     UNUSED(rxRuntimeConfig);
 211     return ibusChannelData[chan];
 212 }
 213
 214
 215 bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 216 {
 217     UNUSED(rxConfig);
 218     ibusSyncByte = 0;
 219
 220     rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL;
 221     rxRuntimeConfig->rcReadRawFn = ibusReadRawRC;
 222     rxRuntimeConfig->rcFrameStatusFn = ibusFrameStatus;
 223
 224     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 225     if (!portConfig) {
 226         return false;
 227     }
 228
 229 #ifdef USE_TELEMETRY
 230     bool portShared = isSerialPortShared(portConfig, FUNCTION_RX_SERIAL, FUNCTION_TELEMETRY_IBUS);
 231 #else
 232     bool portShared = false;
 233 #endif
 234
 235     rxBytesToIgnore = 0;
 236     serialPort_t *ibusPort = openSerialPort(portConfig->identifier,
 237         FUNCTION_RX_SERIAL,
 238         ibusDataReceive,
 239         NULL,
 240         IBUS_BAUDRATE,
 241         portShared ? MODE_RXTX : MODE_RX,
 242         SERIAL_NOT_INVERTED | ((tristateWithDefaultOffIsActive(rxConfig->halfDuplex) || portShared) ? SERIAL_BIDIR : 0)
 243         );
 244
 245 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
 246     if (portShared) {
 247         initSharedIbusTelemetry(ibusPort);
 248     }
 249 #endif
 250
 251     return ibusPort != NULL;
 252 }
 253
 254 #endif // USE_SERIALRX_IBUS