src/main/rx/sumd.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_SUMD
  25
  26 #include "common/utils.h"
  27
  28 #include "drivers/time.h"
  29 #include "drivers/serial.h"
  30 #include "drivers/serial_uart.h"
  31
  32 #include "io/serial.h"
  33
  34 #include "rx/rx.h"
  35 #include "rx/sumd.h"
  36
  37 #include "telemetry/telemetry.h"
  38
  39 // driver for SUMD receiver using UART2
  40
  41 // FIXME test support for more than 8 channels, should probably work up to 12 channels
  42
  43 #define SUMD_SYNCBYTE 0xA8
  44 #define SUMD_MAX_CHANNEL 16
  45 #define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 = 17 bytes for 6 channels
  46
  47 #define SUMD_BAUDRATE 115200
  48
  49 static bool sumdFrameDone = false;
  50 static uint16_t sumdChannels[SUMD_MAX_CHANNEL];
  51 static uint16_t crc;
  52
  53 #define CRC_POLYNOME 0x1021
  54
  55 // CRC calculation, adds a 8 bit unsigned to 16 bit crc
  56 static void CRC16(uint8_t value)
  57 {
  58     uint8_t i;
  59
  60     crc = crc ^ (int16_t)value << 8;
  61     for (i = 0; i < 8; i++) {
  62     if (crc & 0x8000)
  63         crc = (crc << 1) ^ CRC_POLYNOME;
  64     else
  65         crc = (crc << 1);
  66     }
  67 }
  68
  69 static uint8_t sumd[SUMD_BUFFSIZE] = { 0, };
  70 static uint8_t sumdChannelCount;
  71
  72 // Receive ISR callback
  73 static void sumdDataReceive(uint16_t c, void *rxCallbackData)
  74 {
  75     UNUSED(rxCallbackData);
  76
  77     timeUs_t sumdTime;
  78     static timeUs_t sumdTimeLast;
  79     static uint8_t sumdIndex;
  80
  81     sumdTime = micros();
  82     if (cmpTimeUs(sumdTime, sumdTimeLast) > 4000)
  83         sumdIndex = 0;
  84     sumdTimeLast = sumdTime;
  85
  86     if (sumdIndex == 0) {
  87         if (c != SUMD_SYNCBYTE)
  88             return;
  89         else
  90         {
  91             sumdFrameDone = false; // lazy main loop didnt fetch the stuff
  92             crc = 0;
  93         }
  94     }
  95     if (sumdIndex == 2)
  96         sumdChannelCount = (uint8_t)c;
  97     if (sumdIndex < SUMD_BUFFSIZE)
  98         sumd[sumdIndex] = (uint8_t)c;
  99     sumdIndex++;
 100     if (sumdIndex < sumdChannelCount * 2 + 4)
 101         CRC16((uint8_t)c);
 102     else
 103         if (sumdIndex == sumdChannelCount * 2 + 5) {
 104             sumdIndex = 0;
 105             sumdFrameDone = true;
 106         }
 107 }
 108
 109 #define SUMD_OFFSET_CHANNEL_1_HIGH 3
 110 #define SUMD_OFFSET_CHANNEL_1_LOW 4
 111 #define SUMD_BYTES_PER_CHANNEL 2
 112
 113
 114 #define SUMD_FRAME_STATE_OK 0x01
 115 #define SUMD_FRAME_STATE_FAILSAFE 0x81
 116
 117 static uint8_t sumdFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 118 {
 119     UNUSED(rxRuntimeConfig);
 120
 121     uint8_t channelIndex;
 122
 123     uint8_t frameStatus = RX_FRAME_PENDING;
 124
 125     if (!sumdFrameDone) {
 126         return frameStatus;
 127     }
 128
 129     sumdFrameDone = false;
 130
 131     // verify CRC
 132     if (crc != ((sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) |
 133             (sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW])))
 134         return frameStatus;
 135
 136     switch (sumd[1]) {
 137         case SUMD_FRAME_STATE_FAILSAFE:
 138             frameStatus = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
 139             break;
 140         case SUMD_FRAME_STATE_OK:
 141             frameStatus = RX_FRAME_COMPLETE;
 142             break;
 143         default:
 144             return frameStatus;
 145     }
 146
 147     if (sumdChannelCount > SUMD_MAX_CHANNEL)
 148         sumdChannelCount = SUMD_MAX_CHANNEL;
 149
 150     for (channelIndex = 0; channelIndex < sumdChannelCount; channelIndex++) {
 151         sumdChannels[channelIndex] = (
 152             (sumd[SUMD_BYTES_PER_CHANNEL * channelIndex + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) |
 153             sumd[SUMD_BYTES_PER_CHANNEL * channelIndex + SUMD_OFFSET_CHANNEL_1_LOW]
 154         );
 155     }
 156     return frameStatus;
 157 }
 158
 159 static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 160 {
 161     UNUSED(rxRuntimeConfig);
 162     return sumdChannels[chan] / 8;
 163 }
 164
 165 bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 166 {
 167     UNUSED(rxConfig);
 168
 169     rxRuntimeConfig->channelCount = SUMD_MAX_CHANNEL;
 170     rxRuntimeConfig->rxRefreshRate = 11000;
 171
 172     rxRuntimeConfig->rcReadRawFn = sumdReadRawRC;
 173     rxRuntimeConfig->rcFrameStatusFn = sumdFrameStatus;
 174
 175     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 176     if (!portConfig) {
 177         return false;
 178     }
 179
 180 #ifdef USE_TELEMETRY
 181     bool portShared = telemetryCheckRxPortShared(portConfig);
 182 #else
 183     bool portShared = false;
 184 #endif
 185
 186     serialPort_t *sumdPort = openSerialPort(portConfig->identifier,
 187         FUNCTION_RX_SERIAL,
 188         sumdDataReceive,
 189         NULL,
 190         SUMD_BAUDRATE,
 191         portShared ? MODE_RXTX : MODE_RX,
 192         SERIAL_NOT_INVERTED | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
 193         );
 194
 195 #ifdef USE_TELEMETRY
 196     if (portShared) {
 197         telemetrySharedPort = sumdPort;
 198     }
 199 #endif
 200
 201     return sumdPort != NULL;
 202 }
 203 #endif // USE_SERIALRX_SUMD