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
  80 // Receive ISR callback
  81 static void sumdDataReceive(uint16_t c, void *data)
  82 {
  83     UNUSED(data);
  84
  85     static timeUs_t sumdTimeLast;
  86     static uint8_t sumdIndex;
  87
  88     const timeUs_t now = microsISR();
  89     if (cmpTimeUs(now, sumdTimeLast) > SUMD_TIME_NEEDED_PER_FRAME) {
  90         sumdIndex = 0;
  91     }
  92     sumdTimeLast = now;
  93
  94     if (sumdIndex == SUMD_SYNC_BYTE_INDEX) {
  95         if (c != SUMD_SYNCBYTE) {
  96             return;
  97         } else {
  98             sumdFrameDone = false; // lazy main loop didnt fetch the stuff
  99             crc = 0;
 100         }
 101     } else if (sumdIndex == SUMD_CHANNEL_COUNT_INDEX) {
 102         sumdChannelCount = (uint8_t)c;
 103     }
 104
 105     if (sumdIndex < SUMD_BUFFSIZE) {
 106         sumd[sumdIndex] = (uint8_t)c;
 107     }
 108
 109     sumdIndex++;
 110     if (sumdIndex <= sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH) {
 111         crc = crc16_ccitt(crc, (uint8_t)c);
 112     } else if (sumdIndex == sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH + SUMD_CRC_LENGTH) {
 113         lastFrameTimeUs = now;
 114         sumdIndex = 0;
 115         sumdFrameDone = true;
 116     }
 117 }
 118
 119 static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
 120 {
 121     UNUSED(rxRuntimeState);
 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
 135         switch (sumd[1]) {
 136         case SUMD_FRAME_STATE_FAILSAFE:
 137             frameStatus = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
 138             break;
 139         case SUMDV1_FRAME_STATE_OK:
 140         case SUMDV3_FRAME_STATE_OK:
 141             frameStatus = RX_FRAME_COMPLETE;
 142             break;
 143         }
 144
 145         if (frameStatus & RX_FRAME_COMPLETE) {
 146             const unsigned channelsToProcess = MIN(sumdChannelCount, MAX_SUPPORTED_RC_CHANNEL_COUNT);
 147
 148             for (unsigned channelIndex = 0; channelIndex < channelsToProcess; channelIndex++) {
 149                 sumdChannels[channelIndex] = (
 150                     (sumd[SUMD_BYTES_PER_CHANNEL * channelIndex + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) |
 151                     sumd[SUMD_BYTES_PER_CHANNEL * channelIndex + SUMD_OFFSET_CHANNEL_1_LOW]
 152                 );
 153             }
 154         }
 155     }
 156
 157     if (!(frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED))) {
 158         rxRuntimeState->lastRcFrameTimeUs = lastFrameTimeUs;
 159     }
 160
 161     return frameStatus;
 162 }
 163
 164 static float sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 165 {
 166     UNUSED(rxRuntimeState);
 167     return (float)sumdChannels[chan] / 8;
 168 }
 169
 170 bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 171 {
 172     UNUSED(rxConfig);
 173
 174     rxRuntimeState->channelCount = MIN(SUMD_MAX_CHANNEL, MAX_SUPPORTED_RC_CHANNEL_COUNT);
 175     rxRuntimeState->rxRefreshRate = 11000;
 176
 177     rxRuntimeState->rcReadRawFn = sumdReadRawRC;
 178     rxRuntimeState->rcFrameStatusFn = sumdFrameStatus;
 179     rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
 180
 181     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 182     if (!portConfig) {
 183         return false;
 184     }
 185
 186 #ifdef USE_TELEMETRY
 187     bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeState->serialrxProvider);
 188 #else
 189     bool portShared = false;
 190 #endif
 191
 192     serialPort_t *sumdPort = openSerialPort(portConfig->identifier,
 193         FUNCTION_RX_SERIAL,
 194         sumdDataReceive,
 195         NULL,
 196         SUMD_BAUDRATE,
 197         portShared ? MODE_RXTX : MODE_RX,
 198         (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
 199         );
 200
 201 #ifdef USE_TELEMETRY
 202     if (portShared) {
 203         telemetrySharedPort = sumdPort;
 204     }
 205 #endif
 206
 207     return sumdPort != NULL;
 208 }
 209 #endif