src/main/rx/xbus.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 <stdlib.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_SERIALRX_XBUS
  28
  29 #include "common/crc.h"
  30
  31 #include "drivers/time.h"
  32
  33 #include "io/serial.h"
  34
  35 #ifdef USE_TELEMETRY
  36 #include "telemetry/telemetry.h"
  37 #endif
  38
  39 #include "pg/rx.h"
  40
  41 #include "rx/rx.h"
  42 #include "rx/xbus.h"
  43
  44 //
  45 // Serial driver for JR's XBus (MODE B) receiver
  46 //
  47
  48 #define XBUS_CHANNEL_COUNT 12
  49 #define XBUS_RJ01_CHANNEL_COUNT 12
  50
  51 // Frame is: ID(1 byte) + 12*channel(2 bytes) + CRC(2 bytes) = 27
  52 #define XBUS_FRAME_SIZE_A1 27
  53 #define XBUS_FRAME_SIZE_A2 35
  54
  55
  56 #define XBUS_RJ01_FRAME_SIZE 33
  57 #define XBUS_RJ01_MESSAGE_LENGTH 30
  58 #define XBUS_RJ01_OFFSET_BYTES 3
  59
  60 #define XBUS_BAUDRATE 115200
  61 #define XBUS_RJ01_BAUDRATE 250000
  62 #define XBUS_MAX_FRAME_TIME 8000
  63
  64 // NOTE!
  65 // This is actually based on ID+LENGTH (nibble each)
  66 // 0xA - Multiplex ID (also used by JR, no idea why)
  67 // 0x1 - 12 channels
  68 // 0x2 - 16 channels
  69 // However, the JR XG14 that is used for test at the moment
  70 // does only use 0xA1 as its output. This is why the implementation
  71 // is based on these numbers only. Maybe update this in the future?
  72 #define XBUS_START_OF_FRAME_BYTE_A1 (0xA1)      //12 channels
  73 #define XBUS_START_OF_FRAME_BYTE_A2 (0xA2)      //16 channels transfare, but only 12 channels use for
  74
  75 // Pulse length convertion from [0...4095] to µs:
  76 //      800µs  -> 0x000
  77 //      1500µs -> 0x800
  78 //      2200µs -> 0xFFF
  79 // Total range is: 2200 - 800 = 1400 <==> 4095
  80 // Use formula: 800 + value * 1400 / 4096 (i.e. a shift by 12)
  81 #define XBUS_CONVERT_TO_USEC(V) (800 + ((V * 1400) >> 12))
  82
  83 static bool xBusFrameReceived = false;
  84 static bool xBusDataIncoming = false;
  85 static uint8_t xBusFramePosition;
  86 static uint8_t xBusFrameLength;
  87 static uint8_t xBusChannelCount;
  88 static uint8_t xBusProvider;
  89
  90
  91 // Use max values for ram areas
  92 static volatile uint8_t xBusFrame[XBUS_FRAME_SIZE_A2];  //size 35 for 16 channels in xbus_Mode_B
  93 static uint16_t xBusChannelData[XBUS_RJ01_CHANNEL_COUNT];
  94
  95 // Full RJ01 message CRC calculations
  96 static uint8_t xBusRj01CRC8(uint8_t inData, uint8_t seed)
  97 {
  98     for (uint8_t bitsLeft = 8; bitsLeft > 0; bitsLeft--) {
  99         const uint8_t temp = ((seed ^ inData) & 0x01);
 100
 101         if (temp == 0) {
 102             seed >>= 1;
 103         } else {
 104             seed ^= 0x18;
 105             seed >>= 1;
 106             seed |= 0x80;
 107         }
 108
 109         inData >>= 1;
 110     }
 111
 112     return seed;
 113 }
 114
 115
 116 static void xBusUnpackModeBFrame(uint8_t offsetBytes)
 117 {
 118     // Calculate the CRC of the incoming frame
 119     // Calculate on all bytes except the final two CRC bytes
 120     const uint16_t inCrc = crc16_ccitt_update(0, (uint8_t*)&xBusFrame[offsetBytes], xBusFrameLength - 2);
 121
 122     // Get the received CRC
 123     const uint16_t crc = (((uint16_t)xBusFrame[offsetBytes + xBusFrameLength - 2]) << 8) + ((uint16_t)xBusFrame[offsetBytes + xBusFrameLength - 1]);
 124
 125     if (crc == inCrc) {
 126         // Unpack the data, we have a valid frame, only 12 channel unpack also when receive 16 channel
 127         for (int i = 0; i < xBusChannelCount; i++) {
 128
 129             const uint8_t frameAddr = offsetBytes + 1 + i * 2;
 130             uint16_t value = ((uint16_t)xBusFrame[frameAddr]) << 8;
 131             value = value + ((uint16_t)xBusFrame[frameAddr + 1]);
 132
 133             // Convert to internal format
 134             xBusChannelData[i] = XBUS_CONVERT_TO_USEC(value);
 135         }
 136
 137         xBusFrameReceived = true;
 138     }
 139 }
 140
 141 static void xBusUnpackRJ01Frame(void)
 142 {
 143     // Calculate the CRC of the incoming frame
 144     uint8_t outerCrc = 0;
 145     uint8_t i = 0;
 146
 147     // When using the Align RJ01 receiver with
 148     // a MODE B setting in the radio (XG14 tested)
 149     // the MODE_B -frame is packed within some
 150     // at the moment unknown bytes before and after:
 151     // 0xA1 LEN __ 0xA1 12*(High + Low) CRC1 CRC2 + __ __ CRC_OUTER
 152     // Compared to a standard MODE B frame that only
 153     // contains the "middle" package.
 154     // Hence, at the moment, the unknown header and footer
 155     // of the RJ01 MODEB packages are discarded.
 156     // However, the LAST byte (CRC_OUTER) is infact an 8-bit
 157     // CRC for the whole package, using the Dallas-One-Wire CRC
 158     // method.
 159     // So, we check both these values as well as the provided length
 160     // of the outer/full message (LEN)
 161
 162     //
 163     // Check we have correct length of message
 164     //
 165     if (xBusFrame[1] != XBUS_RJ01_MESSAGE_LENGTH)
 166     {
 167         // Unknown package as length is not ok
 168         return;
 169     }
 170
 171     //
 172     // CRC calculation & check for full message
 173     //
 174     for (i = 0; i < xBusFrameLength - 1; i++) {
 175         outerCrc = xBusRj01CRC8(outerCrc, xBusFrame[i]);
 176     }
 177
 178     if (outerCrc != xBusFrame[xBusFrameLength - 1])
 179     {
 180         // CRC does not match, skip this frame
 181         return;
 182     }
 183
 184     // Now unpack the "embedded MODE B frame"
 185     xBusUnpackModeBFrame(XBUS_RJ01_OFFSET_BYTES);
 186 }
 187
 188 // Receive ISR callback
 189 static void xBusDataReceive(uint16_t c, void *data)
 190 {
 191     UNUSED(data);
 192
 193     uint32_t now;
 194     static uint32_t xBusTimeLast, xBusTimeInterval;
 195
 196     // Check if we shall reset frame position due to time
 197     now = micros();
 198     xBusTimeInterval = now - xBusTimeLast;
 199     xBusTimeLast = now;
 200     if (xBusTimeInterval > XBUS_MAX_FRAME_TIME) {
 201         xBusFramePosition = 0;
 202         xBusDataIncoming = false;
 203     }
 204
 205     // Check if we shall start a frame?
 206     if (xBusFramePosition == 0) {
 207         if (c == XBUS_START_OF_FRAME_BYTE_A1) {
 208             xBusDataIncoming = true;
 209             xBusFrameLength = XBUS_FRAME_SIZE_A1;   //decrease framesize (when receiver change, otherwise board must reboot)
 210         } else if (c == XBUS_START_OF_FRAME_BYTE_A2) {//16channel packet
 211             xBusDataIncoming = true;
 212             xBusFrameLength = XBUS_FRAME_SIZE_A2;   //increase framesize
 213         }
 214     }
 215
 216     // Only do this if we are receiving to a frame
 217     if (xBusDataIncoming == true) {
 218         // Store in frame copy
 219         xBusFrame[xBusFramePosition] = (uint8_t)c;
 220         xBusFramePosition++;
 221     }
 222
 223     // Done?
 224     if (xBusFramePosition == xBusFrameLength) {
 225         switch (xBusProvider) {
 226         case SERIALRX_XBUS_MODE_B:
 227             xBusUnpackModeBFrame(0);
 228             FALLTHROUGH; //!!TODO - check this fall through is correct
 229         case SERIALRX_XBUS_MODE_B_RJ01:
 230             xBusUnpackRJ01Frame();
 231         }
 232         xBusDataIncoming = false;
 233         xBusFramePosition = 0;
 234     }
 235 }
 236
 237 // Indicate time to read a frame from the data...
 238 static uint8_t xBusFrameStatus(rxRuntimeState_t *rxRuntimeState)
 239 {
 240     UNUSED(rxRuntimeState);
 241
 242     if (!xBusFrameReceived) {
 243         return RX_FRAME_PENDING;
 244     }
 245
 246     xBusFrameReceived = false;
 247
 248     return RX_FRAME_COMPLETE;
 249 }
 250
 251 static float xBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 252 {
 253     uint16_t data;
 254
 255     // Deliver the data wanted
 256     if (chan >= rxRuntimeState->channelCount) {
 257         return 0;
 258     }
 259
 260     data = xBusChannelData[chan];
 261
 262     return data;
 263 }
 264
 265 bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 266 {
 267     uint32_t baudRate;
 268
 269     switch (rxRuntimeState->serialrxProvider) {
 270     case SERIALRX_XBUS_MODE_B:
 271         rxRuntimeState->channelCount = XBUS_CHANNEL_COUNT;
 272         xBusFrameReceived = false;
 273         xBusDataIncoming = false;
 274         xBusFramePosition = 0;
 275         baudRate = XBUS_BAUDRATE;
 276         xBusFrameLength = XBUS_FRAME_SIZE_A1;
 277         xBusChannelCount = XBUS_CHANNEL_COUNT;
 278         xBusProvider = SERIALRX_XBUS_MODE_B;
 279         break;
 280     case SERIALRX_XBUS_MODE_B_RJ01:
 281         rxRuntimeState->channelCount = XBUS_RJ01_CHANNEL_COUNT;
 282         xBusFrameReceived = false;
 283         xBusDataIncoming = false;
 284         xBusFramePosition = 0;
 285         baudRate = XBUS_RJ01_BAUDRATE;
 286         xBusFrameLength = XBUS_RJ01_FRAME_SIZE;
 287         xBusChannelCount = XBUS_RJ01_CHANNEL_COUNT;
 288         xBusProvider = SERIALRX_XBUS_MODE_B_RJ01;
 289         break;
 290     default:
 291         return false;
 292         break;
 293     }
 294
 295     rxRuntimeState->rxRefreshRate = 11000;
 296
 297     rxRuntimeState->rcReadRawFn = xBusReadRawRC;
 298     rxRuntimeState->rcFrameStatusFn = xBusFrameStatus;
 299
 300     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 301     if (!portConfig) {
 302         return false;
 303     }
 304
 305 #ifdef USE_TELEMETRY
 306     bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeState->serialrxProvider);
 307 #else
 308     bool portShared = false;
 309 #endif
 310
 311     serialPort_t *xBusPort = openSerialPort(portConfig->identifier,
 312         FUNCTION_RX_SERIAL,
 313         xBusDataReceive,
 314         NULL,
 315         baudRate,
 316         portShared ? MODE_RXTX : MODE_RX,
 317         (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
 318         );
 319
 320 #ifdef USE_TELEMETRY
 321     if (portShared) {
 322         telemetrySharedPort = xBusPort;
 323     }
 324 #endif
 325
 326     return xBusPort != NULL;
 327 }
 328 #endif