src/main/rx/sumh.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 /*
  19  * References:
  20  *  http://fpv-treff.de/viewtopic.php?f=18&t=1368&start=3020#p44535
  21  *  http://fpv-community.de/showthread.php?29033-MultiWii-mit-Graupner-SUMD-SUMH-und-USB-Joystick-auf-ProMicro
  22  */
  23
  24 #include <stdbool.h>
  25 #include <stdint.h>
  26 #include <stdlib.h>
  27
  28 #include "platform.h"
  29
  30 #include "build_config.h"
  31
  32 #include "drivers/system.h"
  33
  34 #include "drivers/serial.h"
  35 #include "drivers/serial_uart.h"
  36 #include "io/serial.h"
  37
  38 #include "rx/rx.h"
  39 #include "rx/sumh.h"
  40
  41 // driver for SUMH receiver using UART2
  42
  43 #define SUMH_BAUDRATE 115200
  44
  45 #define SUMH_MAX_CHANNEL_COUNT 8
  46 #define SUMH_FRAME_SIZE 21
  47
  48 static bool sumhFrameDone = false;
  49
  50 static uint8_t sumhFrame[SUMH_FRAME_SIZE];
  51 static uint32_t sumhChannels[SUMH_MAX_CHANNEL_COUNT];
  52
  53 static void sumhDataReceive(uint16_t c);
  54 static uint16_t sumhReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
  55
  56 static serialPort_t *sumhPort;
  57
  58
  59 static void sumhDataReceive(uint16_t c);
  60 static uint16_t sumhReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
  61
  62
  63
  64 bool sumhInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
  65 {
  66     UNUSED(rxConfig);
  67
  68     if (callback)
  69         *callback = sumhReadRawRC;
  70
  71     rxRuntimeConfig->channelCount = SUMH_MAX_CHANNEL_COUNT;
  72
  73     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
  74     if (!portConfig) {
  75         return false;
  76     }
  77
  78     sumhPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sumhDataReceive, SUMH_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED);
  79
  80     return sumhPort != NULL;
  81 }
  82
  83 // Receive ISR callback
  84 static void sumhDataReceive(uint16_t c)
  85 {
  86     uint32_t sumhTime;
  87     static uint32_t sumhTimeLast, sumhTimeInterval;
  88     static uint8_t sumhFramePosition;
  89
  90     sumhTime = micros();
  91     sumhTimeInterval = sumhTime - sumhTimeLast;
  92     sumhTimeLast = sumhTime;
  93     if (sumhTimeInterval > 5000) {
  94         sumhFramePosition = 0;
  95     }
  96
  97     sumhFrame[sumhFramePosition] = (uint8_t) c;
  98     if (sumhFramePosition == SUMH_FRAME_SIZE - 1) {
  99         // FIXME at this point the value of 'c' is unused and un tested, what should it be, is it important?
 100         sumhFrameDone = true;
 101     } else {
 102         sumhFramePosition++;
 103     }
 104 }
 105
 106 uint8_t sumhFrameStatus(void)
 107 {
 108     uint8_t channelIndex;
 109
 110     if (!sumhFrameDone) {
 111         return SERIAL_RX_FRAME_PENDING;
 112     }
 113
 114     sumhFrameDone = false;
 115
 116     if (!((sumhFrame[0] == 0xA8) && (sumhFrame[SUMH_FRAME_SIZE - 2] == 0))) {
 117         return SERIAL_RX_FRAME_PENDING;
 118     }
 119
 120     for (channelIndex = 0; channelIndex < SUMH_MAX_CHANNEL_COUNT; channelIndex++) {
 121         sumhChannels[channelIndex] = (((uint32_t)(sumhFrame[(channelIndex << 1) + 3]) << 8)
 122                 + sumhFrame[(channelIndex << 1) + 4]) / 6.4f - 375;
 123     }
 124     return SERIAL_RX_FRAME_COMPLETE;
 125 }
 126
 127 static uint16_t sumhReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 128 {
 129     UNUSED(rxRuntimeConfig);
 130
 131     if (chan >= SUMH_MAX_CHANNEL_COUNT) {
 132         return 0;
 133     }
 134
 135     return sumhChannels[chan];
 136 }