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 SERIAL_RX
  31
  32 #include "build/build_config.h"
  33
  34 #include "drivers/system.h"
  35
  36 #include "drivers/serial.h"
  37 #include "drivers/serial_uart.h"
  38 #include "io/serial.h"
  39
  40 #ifdef TELEMETRY
  41 #include "telemetry/telemetry.h"
  42 #endif
  43
  44 #include "rx/rx.h"
  45 #include "rx/ibus.h"
  46
  47 #define IBUS_MAX_CHANNEL 14
  48 #define IBUS_BUFFSIZE 32
  49 #define IBUS_MODEL_IA6B 0
  50 #define IBUS_MODEL_IA6 1
  51 #define IBUS_FRAME_GAP 500
  52
  53 #define IBUS_BAUDRATE 115200
  54
  55 static uint8_t ibusModel;
  56 static uint8_t ibusSyncByte;
  57 static uint8_t ibusFrameSize;
  58 static uint8_t ibusChannelOffset;
  59 static uint16_t ibusChecksum;
  60
  61 static bool ibusFrameDone = false;
  62 static uint32_t ibusChannelData[IBUS_MAX_CHANNEL];
  63
  64     ibusSyncByte = 0;
  65
  66 static uint8_t ibus[IBUS_BUFFSIZE] = { 0, };
  67
  68 // Receive ISR callback
  69 static void ibusDataReceive(uint16_t c)
  70 {
  71     uint32_t ibusTime;
  72     static uint32_t ibusTimeLast;
  73     static uint8_t ibusFramePosition;
  74
  75     ibusTime = micros();
  76
  77     if ((ibusTime - ibusTimeLast) > IBUS_FRAME_GAP)
  78         ibusFramePosition = 0;
  79
  80     ibusTimeLast = ibusTime;
  81
  82     if (ibusFramePosition == 0) {
  83         if (ibusSyncByte == 0) {
  84             // detect the frame type based on the STX byte.
  85             if (c == 0x55) {
  86                 ibusModel = IBUS_MODEL_IA6;
  87                 ibusSyncByte = 0x55;
  88                 ibusFrameSize = 31;
  89                 ibusChecksum = 0x0000;
  90                 ibusChannelOffset = 1;
  91             } else if (c == 0x20) {
  92                 ibusModel = IBUS_MODEL_IA6B;
  93                 ibusSyncByte = 0x20;
  94                 ibusFrameSize = 32;
  95                 ibusChannelOffset = 2;
  96                 ibusChecksum = 0xFFFF;
  97             } else
  98                 return;
  99         } else if (ibusSyncByte != c) {
 100             return;
 101         }
 102     }
 103
 104     ibus[ibusFramePosition] = (uint8_t)c;
 105
 106     if (ibusFramePosition == ibusFrameSize - 1) {
 107         ibusFrameDone = true;
 108     } else {
 109         ibusFramePosition++;
 110     }
 111 }
 112
 113 uint8_t ibusFrameStatus(void)
 114 {
 115     uint8_t i, offset;
 116     uint8_t frameStatus = RX_FRAME_PENDING;
 117     uint16_t chksum, rxsum;
 118
 119     if (!ibusFrameDone) {
 120         return frameStatus;
 121     }
 122
 123     ibusFrameDone = false;
 124
 125     chksum = ibusChecksum;
 126     rxsum = ibus[ibusFrameSize - 2] + (ibus[ibusFrameSize - 1] << 8);
 127     if (ibusModel == IBUS_MODEL_IA6) {
 128         for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2)
 129             chksum += ibus[offset] + (ibus[offset + 1] << 8);
 130     } else {
 131         for (i = 0; i < 30; i++)
 132             chksum -= ibus[i];
 133     }
 134
 135     if (chksum == rxsum) {
 136         for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_CHANNEL; i++, offset += 2) {
 137             ibusChannelData[i] = ibus[offset] + (ibus[offset + 1] << 8);
 138         }
 139         frameStatus = RX_FRAME_COMPLETE;
 140     }
 141
 142     return frameStatus;
 143 }
 144
 145 static uint16_t ibusReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 146 {
 147     UNUSED(rxRuntimeConfig);
 148     return ibusChannelData[chan];
 149 }
 150
 151 bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 152 {
 153     UNUSED(rxConfig);
 154
 155     rxRuntimeConfig->channelCount = IBUS_MAX_CHANNEL;
 156     rxRuntimeConfig->rxRefreshRate = 20000; // TODO - Verify speed
 157
 158     rxRuntimeConfig->rcReadRawFunc = ibusReadRawRC;
 159     rxRuntimeConfig->rcFrameStatusFunc = ibusFrameStatus;
 160
 161     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 162     if (!portConfig) {
 163         return false;
 164     }
 165
 166 #ifdef TELEMETRY
 167     bool portShared = telemetryCheckRxPortShared(portConfig);
 168 #else
 169     bool portShared = false;
 170 #endif
 171
 172     serialPort_t *ibusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, ibusDataReceive, IBUS_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
 173
 174 #ifdef TELEMETRY
 175     if (portShared) {
 176         telemetrySharedPort = ibusPort;
 177     }
 178 #endif
 179
 180     return ibusPort != NULL;
 181 }
 182 #endif