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