src/main/rx/sbus_channels.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_SERIAL_RX
  25
  26 #include "common/utils.h"
  27
  28 #include "rx/rx.h"
  29 #include "rx/sbus_channels.h"
  30
  31 #define DEBUG_SBUS_FRAME_INTERVAL 3
  32
  33 #define SBUS_FLAG_CHANNEL_17        (1 << 0)
  34 #define SBUS_FLAG_CHANNEL_18        (1 << 1)
  35
  36 #define SBUS_DIGITAL_CHANNEL_MIN 173
  37 #define SBUS_DIGITAL_CHANNEL_MAX 1812
  38
  39 uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels)
  40 {
  41     uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
  42     sbusChannelData[0] = channels->chan0;
  43     sbusChannelData[1] = channels->chan1;
  44     sbusChannelData[2] = channels->chan2;
  45     sbusChannelData[3] = channels->chan3;
  46     sbusChannelData[4] = channels->chan4;
  47     sbusChannelData[5] = channels->chan5;
  48     sbusChannelData[6] = channels->chan6;
  49     sbusChannelData[7] = channels->chan7;
  50     sbusChannelData[8] = channels->chan8;
  51     sbusChannelData[9] = channels->chan9;
  52     sbusChannelData[10] = channels->chan10;
  53     sbusChannelData[11] = channels->chan11;
  54     sbusChannelData[12] = channels->chan12;
  55     sbusChannelData[13] = channels->chan13;
  56     sbusChannelData[14] = channels->chan14;
  57     sbusChannelData[15] = channels->chan15;
  58
  59     if (channels->flags & SBUS_FLAG_CHANNEL_17) {
  60         sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX;
  61     } else {
  62         sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN;
  63     }
  64
  65     if (channels->flags & SBUS_FLAG_CHANNEL_18) {
  66         sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
  67     } else {
  68         sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
  69     }
  70
  71     if (channels->flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
  72         // internal failsafe enabled and rx failsafe flag set
  73         // RX *should* still be sending valid channel data (repeated), so use it.
  74         return RX_FRAME_DROPPED | RX_FRAME_FAILSAFE;
  75     }
  76
  77     if (channels->flags & SBUS_FLAG_SIGNAL_LOSS) {
  78         // The received data is a repeat of the last valid data so can be considered complete.
  79         return RX_FRAME_DROPPED;
  80     }
  81
  82     return RX_FRAME_COMPLETE;
  83 }
  84
  85 static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
  86 {
  87     // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
  88     // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
  89     return (5 * rxRuntimeConfig->channelData[chan] / 8) + 880;
  90 }
  91
  92 void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
  93 {
  94     rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
  95     for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
  96         rxRuntimeConfig->channelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
  97     }
  98 }
  99 #endif