src/main/rx/sumd.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_SUMD
  28
  29 #include "common/crc.h"
  30 #include "common/utils.h"
  31 #include "common/maths.h"
  32
  33 #include "drivers/time.h"
  34
  35 #include "io/serial.h"
  36
  37 #ifdef USE_TELEMETRY
  38 #include "telemetry/telemetry.h"
  39 #endif
  40
  41 #include "pg/rx.h"
  42
  43 #include "rx/rx.h"
  44 #include "rx/sumd.h"
  45
  46 // driver for SUMD receiver using UART2
  47
  48 // Support for SUMD and SUMD V3
  49 // Tested with 16 channels, SUMD supports up to 16(*), SUMD V3 up to 32 (MZ-32) channels, but limit to MAX_SUPPORTED_RC_CHANNEL_COUNT (currently 8, BF 3.4)
  50 // * According to the original SUMD V1 documentation, SUMD V1 already supports up to 32 Channels?!?
  51
  52 #define SUMD_SYNCBYTE 0xA8
  53 #define SUMD_MAX_CHANNEL 32
  54 #define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 = 17 bytes for 6 channels
  55
  56 #define SUMD_BAUDRATE 115200
  57 #define SUMD_TIME_NEEDED_PER_FRAME 4000
  58
  59 #define SUMD_OFFSET_CHANNEL_1_HIGH 3
  60 #define SUMD_OFFSET_CHANNEL_1_LOW 4
  61 #define SUMD_BYTES_PER_CHANNEL 2
  62 #define SUMD_SYNC_BYTE_INDEX 0
  63 #define SUMD_CHANNEL_COUNT_INDEX 2
  64
  65 #define SUMD_HEADER_LENGTH 3
  66 #define SUMD_CRC_LENGTH 2
  67
  68 #define SUMDV1_FRAME_STATE_OK 0x01
  69 #define SUMDV3_FRAME_STATE_OK 0x03
  70 #define SUMD_FRAME_STATE_FAILSAFE 0x81
  71
  72 static bool sumdFrameDone = false;
  73 static uint16_t sumdChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
  74 static uint16_t crc;
  75
  76 static uint8_t sumd[SUMD_BUFFSIZE] = { 0, };
  77 static uint8_t sumdChannelCount;
  78 static timeUs_t lastFrameTimeUs = 0;
  79 static timeUs_t lastRcFrameTimeUs = 0;
  80
  81 // Receive ISR callback
  82 static void sumdDataReceive(uint16_t c, void *data)
  83 {
  84     UNUSED(data);
  85
  86     static timeUs_t sumdTimeLast;
  87     static uint8_t sumdIndex;
  88
  89     const timeUs_t now = microsISR();
  90     if (cmpTimeUs(now, sumdTimeLast) > SUMD_TIME_NEEDED_PER_FRAME) {
  91         sumdIndex = 0;
  92     }
  93     sumdTimeLast = now;
  94
  95     if (sumdIndex == SUMD_SYNC_BYTE_INDEX) {
  96         if (c != SUMD_SYNCBYTE) {
  97             return;
  98         } else {
  99             sumdFrameDone = false; // lazy main loop didnt fetch the stuff
 100             crc = 0;
 101         }
 102     } else if (sumdIndex == SUMD_CHANNEL_COUNT_INDEX) {
 103         sumdChannelCount = (uint8_t)c;
 104     }
 105
 106     if (sumdIndex < SUMD_BUFFSIZE) {
 107         sumd[sumdIndex] = (uint8_t)c;
 108     }
 109
 110     sumdIndex++;
 111     if (sumdIndex <= sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH) {
 112         crc = crc16_ccitt(crc, (uint8_t)c);
 113     } else if (sumdIndex == sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH + SUMD_CRC_LENGTH) {
 114         lastFrameTimeUs = now;
 115         sumdIndex = 0;
 116         sumdFrameDone = true;
 117     }
 118 }
 119
 120 static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
 121 {
 122     UNUSED(rxRuntimeState);
 123
 124     uint8_t frameStatus = RX_FRAME_PENDING;
 125
 126     if (!sumdFrameDone) {
 127         return frameStatus;
 128     }
 129
 130     sumdFrameDone = false;
 131
 132     // verify CRC
 133     if (crc == ((sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) |
 134             (sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW]))) {
 135
 136         switch (sumd[1]) {
 137         case SUMD_FRAME_STATE_FAILSAFE:
 138             frameStatus = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
 139             break;
 140         case SUMDV1_FRAME_STATE_OK:
 141         case SUMDV3_FRAME_STATE_OK:
 142             frameStatus = RX_FRAME_COMPLETE;
 143             break;
 144         }
 145
 146         if (frameStatus & RX_FRAME_COMPLETE) {
 147             const unsigned channelsToProcess = MIN(sumdChannelCount, MAX_SUPPORTED_RC_CHANNEL_COUNT);
 148
 149             for (unsigned channelIndex = 0; channelIndex < channelsToProcess; channelIndex++) {
 150                 sumdChannels[channelIndex] = (
 151                     (sumd[SUMD_BYTES_PER_CHANNEL * channelIndex + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) |
 152                     sumd[SUMD_BYTES_PER_CHANNEL * channelIndex + SUMD_OFFSET_CHANNEL_1_LOW]
 153                 );
 154             }
 155         }
 156     }
 157
 158     if (!(frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED))) {
 159         lastRcFrameTimeUs = lastFrameTimeUs;
 160     }
 161
 162     return frameStatus;
 163 }
 164
 165 static float sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 166 {
 167     UNUSED(rxRuntimeState);
 168     return (float)sumdChannels[chan] / 8;
 169 }
 170
 171 static timeUs_t sumdFrameTimeUsFn(void)
 172 {
 173     return lastRcFrameTimeUs;
 174 }
 175
 176 bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 177 {
 178     UNUSED(rxConfig);
 179
 180     rxRuntimeState->channelCount = MIN(SUMD_MAX_CHANNEL, MAX_SUPPORTED_RC_CHANNEL_COUNT);
 181     rxRuntimeState->rxRefreshRate = 11000;
 182
 183     rxRuntimeState->rcReadRawFn = sumdReadRawRC;
 184     rxRuntimeState->rcFrameStatusFn = sumdFrameStatus;
 185     rxRuntimeState->rcFrameTimeUsFn = sumdFrameTimeUsFn;
 186
 187     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 188     if (!portConfig) {
 189         return false;
 190     }
 191
 192 #ifdef USE_TELEMETRY
 193     bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeState->serialrxProvider);
 194 #else
 195     bool portShared = false;
 196 #endif
 197
 198     serialPort_t *sumdPort = openSerialPort(portConfig->identifier,
 199         FUNCTION_RX_SERIAL,
 200         sumdDataReceive,
 201         NULL,
 202         SUMD_BAUDRATE,
 203         portShared ? MODE_RXTX : MODE_RX,
 204         (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
 205         );
 206
 207 #ifdef USE_TELEMETRY
 208     if (portShared) {
 209         telemetrySharedPort = sumdPort;
 210     }
 211 #endif
 212
 213     return sumdPort != NULL;
 214 }
 215 #endif