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
  58 static bool sumdFrameDone = false;
  59 static uint16_t sumdChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
  60 static uint16_t crc;
  61
  62 static uint8_t sumd[SUMD_BUFFSIZE] = { 0, };
  63 static uint8_t sumdChannelCount;
  64
  65 // Receive ISR callback
  66 static void sumdDataReceive(uint16_t c, void *data)
  67 {
  68     UNUSED(data);
  69
  70     uint32_t sumdTime;
  71     static uint32_t sumdTimeLast;
  72     static uint8_t sumdIndex;
  73
  74     sumdTime = micros();
  75     if ((sumdTime - sumdTimeLast) > 4000)
  76         sumdIndex = 0;
  77     sumdTimeLast = sumdTime;
  78
  79     if (sumdIndex == 0) {
  80         if (c != SUMD_SYNCBYTE)
  81             return;
  82         else
  83         {
  84             sumdFrameDone = false; // lazy main loop didnt fetch the stuff
  85             crc = 0;
  86         }
  87     }
  88     if (sumdIndex == 2)
  89         sumdChannelCount = (uint8_t)c;
  90     if (sumdIndex < SUMD_BUFFSIZE)
  91         sumd[sumdIndex] = (uint8_t)c;
  92     sumdIndex++;
  93     if (sumdIndex < sumdChannelCount * 2 + 4)
  94         crc = crc16_ccitt(crc, (uint8_t)c);
  95     else
  96         if (sumdIndex == sumdChannelCount * 2 + 5) {
  97             sumdIndex = 0;
  98             sumdFrameDone = true;
  99         }
 100 }
 101
 102 #define SUMD_OFFSET_CHANNEL_1_HIGH 3
 103 #define SUMD_OFFSET_CHANNEL_1_LOW 4
 104 #define SUMD_BYTES_PER_CHANNEL 2
 105
 106
 107 #define SUMDV1_FRAME_STATE_OK 0x01
 108 #define SUMDV3_FRAME_STATE_OK 0x03
 109 #define SUMD_FRAME_STATE_FAILSAFE 0x81
 110
 111 static uint8_t sumdFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 112 {
 113     UNUSED(rxRuntimeConfig);
 114
 115     uint8_t frameStatus = RX_FRAME_PENDING;
 116
 117     if (!sumdFrameDone) {
 118         return frameStatus;
 119     }
 120
 121     sumdFrameDone = false;
 122
 123     // verify CRC
 124     if (crc != ((sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) |
 125             (sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW])))
 126         return frameStatus;
 127
 128     switch (sumd[1]) {
 129         case SUMD_FRAME_STATE_FAILSAFE:
 130             frameStatus = RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
 131             break;
 132         case SUMDV1_FRAME_STATE_OK:
 133         case SUMDV3_FRAME_STATE_OK:
 134             frameStatus = RX_FRAME_COMPLETE;
 135             break;
 136         default:
 137             return frameStatus;
 138     }
 139
 140     unsigned channelsToProcess = MIN(sumdChannelCount, MAX_SUPPORTED_RC_CHANNEL_COUNT);
 141
 142     for (unsigned channelIndex = 0; channelIndex < channelsToProcess; channelIndex++) {
 143         sumdChannels[channelIndex] = (
 144             (sumd[SUMD_BYTES_PER_CHANNEL * channelIndex + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) |
 145             sumd[SUMD_BYTES_PER_CHANNEL * channelIndex + SUMD_OFFSET_CHANNEL_1_LOW]
 146         );
 147     }
 148     return frameStatus;
 149 }
 150
 151 static uint16_t sumdReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 152 {
 153     UNUSED(rxRuntimeConfig);
 154     return sumdChannels[chan] / 8;
 155 }
 156
 157 bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 158 {
 159     UNUSED(rxConfig);
 160
 161     rxRuntimeConfig->channelCount = MIN(SUMD_MAX_CHANNEL, MAX_SUPPORTED_RC_CHANNEL_COUNT);
 162     rxRuntimeConfig->rxRefreshRate = 11000;
 163
 164     rxRuntimeConfig->rcReadRawFn = sumdReadRawRC;
 165     rxRuntimeConfig->rcFrameStatusFn = sumdFrameStatus;
 166
 167     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 168     if (!portConfig) {
 169         return false;
 170     }
 171
 172 #ifdef USE_TELEMETRY
 173     bool portShared = telemetryCheckRxPortShared(portConfig);
 174 #else
 175     bool portShared = false;
 176 #endif
 177
 178     serialPort_t *sumdPort = openSerialPort(portConfig->identifier,
 179         FUNCTION_RX_SERIAL,
 180         sumdDataReceive,
 181         NULL,
 182         SUMD_BAUDRATE,
 183         portShared ? MODE_RXTX : MODE_RX,
 184         (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
 185         );
 186
 187 #ifdef USE_TELEMETRY
 188     if (portShared) {
 189         telemetrySharedPort = sumdPort;
 190     }
 191 #endif
 192
 193     return sumdPort != NULL;
 194 }
 195 #endif