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