src/main/rx/ibus.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 /*
  22  * Driver for IBUS (Flysky) receiver
  23  *   - initial implementation for MultiWii by Cesco/Pl¸schi
  24  *   - implementation for BaseFlight by Andreas (fiendie) Tacke
  25  *   - ported to CleanFlight by Konstantin (digitalentity) Sharlaimov
  26  */
  27
  28 #include <stdbool.h>
  29 #include <stdint.h>
  30 #include <stdlib.h>
  31
  32 #include "platform.h"
  33
  34 #ifdef USE_SERIALRX_IBUS
  35
  36 #include "pg/rx.h"
  37
  38 #include "common/utils.h"
  39
  40 #include "drivers/serial.h"
  41 #include "drivers/serial_uart.h"
  42 #include "drivers/time.h"
  43
  44 #include "io/serial.h"
  45
  46 #ifdef USE_TELEMETRY
  47 #include "telemetry/telemetry.h"
  48 #endif
  49
  50 #include "rx/rx.h"
  51 #include "rx/ibus.h"
  52 #include "telemetry/ibus.h"
  53 #include "telemetry/ibus_shared.h"
  54
  55 #define IBUS_MAX_CHANNEL 18
  56 //In AFHDS there is 18 channels encoded in 14 slots (each slot is 2 byte long)
  57 #define IBUS_MAX_SLOTS 14
  58 #define IBUS_BUFFSIZE 32
  59 #define IBUS_MODEL_IA6B 0
  60 #define IBUS_MODEL_IA6 1
  61 #define IBUS_FRAME_GAP 500
  62
  63 #define IBUS_BAUDRATE 115200
  64 #define IBUS_TELEMETRY_PACKET_LENGTH (4)
  65 #define IBUS_SERIAL_RX_PACKET_LENGTH (32)
  66
  67 static uint8_t ibusModel;
  68 static uint8_t ibusSyncByte;
  69 static uint8_t ibusFrameSize;
  70 static uint8_t ibusChannelOffset;
  71 static uint8_t rxBytesToIgnore;
  72 static uint16_t ibusChecksum;
  73
  74 static bool ibusFrameDone = false;
  75 static uint32_t ibusChannelData[IBUS_MAX_CHANNEL];
  76
  77 static uint8_t ibus[IBUS_BUFFSIZE] = { 0, };
  78 static timeDelta_t lastFrameDelta = 0;
  79
  80 static bool isValidIa6bIbusPacketLength(uint8_t length)
  81 {
  82     return (length == IBUS_TELEMETRY_PACKET_LENGTH) || (length == IBUS_SERIAL_RX_PACKET_LENGTH);
  83 }
  84
  85
  86 // Receive ISR callback
  87 static void ibusDataReceive(uint16_t c, void *data)
  88 {
  89     UNUSED(data);
  90
  91     static timeUs_t ibusTimeLast;
  92     static uint8_t ibusFramePosition;
  93     static timeUs_t lastFrameCompleteTimeUs = 0;
  94
  95     const timeUs_t ibusTime = microsISR();
  96
  97     if (cmpTimeUs(ibusTime, ibusTimeLast) > IBUS_FRAME_GAP) {
  98         ibusFramePosition = 0;
  99         rxBytesToIgnore = 0;
 100     } else if (rxBytesToIgnore) {
 101         rxBytesToIgnore--;
 102         return;
 103     }
 104
 105     ibusTimeLast = ibusTime;
 106
 107     if (ibusFramePosition == 0) {
 108         if (isValidIa6bIbusPacketLength(c)) {
 109             ibusModel = IBUS_MODEL_IA6B;
 110             ibusSyncByte = c;
 111             ibusFrameSize = c;
 112             ibusChannelOffset = 2;
 113             ibusChecksum = 0xFFFF;
 114         } else if ((ibusSyncByte == 0) && (c == 0x55)) {
 115             ibusModel = IBUS_MODEL_IA6;
 116             ibusSyncByte = 0x55;
 117             ibusFrameSize = 31;
 118             ibusChecksum = 0x0000;
 119             ibusChannelOffset = 1;
 120         } else if (ibusSyncByte != c) {
 121             return;
 122         }
 123     }
 124
 125     ibus[ibusFramePosition] = (uint8_t)c;
 126
 127     if (ibusFramePosition == ibusFrameSize - 1) {
 128         lastFrameDelta = cmpTimeUs(ibusTime, lastFrameCompleteTimeUs);
 129         lastFrameCompleteTimeUs = ibusTime;
 130         ibusFrameDone = true;
 131     } else {
 132         ibusFramePosition++;
 133     }
 134 }
 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
 151 static bool checksumIsOk(void) {
 152     if (ibusModel == IBUS_MODEL_IA6 ) {
 153         return isChecksumOkIa6();
 154     } else {
 155         return isChecksumOkIa6b(ibus, ibusFrameSize);
 156     }
 157 }
 158
 159
 160 static void updateChannelData(void) {
 161     uint8_t i;
 162     uint8_t offset;
 163     for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_SLOTS; i++, offset += 2) {
 164         ibusChannelData[i] = ibus[offset] + ((ibus[offset + 1] & 0x0F) << 8);
 165     }
 166     //latest IBUS recievers are using prviously not used 4 bits on every channel to incresse total channel count
 167     for (i = IBUS_MAX_SLOTS, offset = ibusChannelOffset + 1; i < IBUS_MAX_CHANNEL; i++, offset += 6) {
 168         ibusChannelData[i] = ((ibus[offset] & 0xF0) >> 4) | (ibus[offset + 2] & 0xF0) | ((ibus[offset + 4] & 0xF0) << 4);
 169     }
 170 }
 171
 172 static uint8_t ibusFrameStatus(rxRuntimeState_t *rxRuntimeState)
 173 {
 174     UNUSED(rxRuntimeState);
 175
 176     uint8_t frameStatus = RX_FRAME_PENDING;
 177
 178     if (!ibusFrameDone) {
 179         return frameStatus;
 180     }
 181
 182     ibusFrameDone = false;
 183
 184     if (checksumIsOk()) {
 185         if (ibusModel == IBUS_MODEL_IA6 || ibusSyncByte == IBUS_SERIAL_RX_PACKET_LENGTH) {
 186             updateChannelData();
 187             frameStatus = RX_FRAME_COMPLETE;
 188         }
 189         else
 190         {
 191 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
 192             rxBytesToIgnore = respondToIbusRequest(ibus);
 193 #endif
 194         }
 195     }
 196
 197     return frameStatus;
 198 }
 199
 200
 201 static uint16_t ibusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 202 {
 203     UNUSED(rxRuntimeState);
 204     return ibusChannelData[chan];
 205 }
 206
 207 static timeDelta_t ibusFrameDelta(void)
 208 {
 209     return lastFrameDelta;
 210 }
 211
 212 bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 213 {
 214     UNUSED(rxConfig);
 215     ibusSyncByte = 0;
 216
 217     rxRuntimeState->channelCount = IBUS_MAX_CHANNEL;
 218     rxRuntimeState->rxRefreshRate = 20000; // TODO - Verify speed
 219
 220     rxRuntimeState->rcReadRawFn = ibusReadRawRC;
 221     rxRuntimeState->rcFrameStatusFn = ibusFrameStatus;
 222     rxRuntimeState->rcFrameDeltaFn = ibusFrameDelta;
 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
 236     rxBytesToIgnore = 0;
 237     serialPort_t *ibusPort = openSerialPort(portConfig->identifier,
 238         FUNCTION_RX_SERIAL,
 239         ibusDataReceive,
 240         NULL,
 241         IBUS_BAUDRATE,
 242         portShared ? MODE_RXTX : MODE_RX,
 243         (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex || portShared ? SERIAL_BIDIR : 0)
 244         );
 245
 246 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
 247     if (portShared) {
 248         initSharedIbusTelemetry(ibusPort);
 249     }
 250 #endif
 251
 252     return ibusPort != NULL;
 253 }
 254
 255 #endif //SERIAL_RX