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 #include "common/maths.h"
  28
  29 #include "build/debug.h"
  30
  31 #include "rx/sbus_channels.h"
  32
  33
  34 #define SBUS_DIGITAL_CHANNEL_MIN 173
  35 #define SBUS_DIGITAL_CHANNEL_MAX 1812
  36
  37 STATIC_ASSERT(SBUS_FRAME_SIZE == sizeof(sbusFrame_t), SBUS_FRAME_SIZE_doesnt_match_sbusFrame_t);
  38
  39 uint8_t sbus26ChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels, bool highChannels)
  40 {
  41     uint8_t offset = highChannels ? 16 : 0;
  42     uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
  43     sbusChannelData[0 + offset] = channels->chan0;
  44     sbusChannelData[1 + offset] = channels->chan1;
  45     sbusChannelData[2 + offset] = channels->chan2;
  46     sbusChannelData[3 + offset] = channels->chan3;
  47     sbusChannelData[4 + offset] = channels->chan4;
  48     sbusChannelData[5 + offset] = channels->chan5;
  49     sbusChannelData[6 + offset] = channels->chan6;
  50     sbusChannelData[7 + offset] = channels->chan7;
  51     sbusChannelData[8 + offset] = channels->chan8;
  52     sbusChannelData[9 + offset] = channels->chan9;
  53     sbusChannelData[10 + offset] = channels->chan10;
  54     sbusChannelData[11 + offset] = channels->chan11;
  55     sbusChannelData[12 + offset] = channels->chan12;
  56     sbusChannelData[13 + offset] = channels->chan13;
  57     sbusChannelData[14 + offset] = channels->chan14;
  58     sbusChannelData[15 + offset] = channels->chan15;
  59
  60     if (!highChannels) {
  61         if (channels->flags & SBUS_FLAG_CHANNEL_DG1) {
  62             sbusChannelData[32] = SBUS_DIGITAL_CHANNEL_MAX;
  63         } else {
  64             sbusChannelData[32] = SBUS_DIGITAL_CHANNEL_MIN;
  65         }
  66
  67         if (channels->flags & SBUS_FLAG_CHANNEL_DG2) {
  68             sbusChannelData[33] = SBUS_DIGITAL_CHANNEL_MAX;
  69         } else {
  70             sbusChannelData[33] = SBUS_DIGITAL_CHANNEL_MIN;
  71         }
  72     }
  73
  74     if (channels->flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
  75         // internal failsafe enabled and rx failsafe flag set
  76         // RX *should* still be sending valid channel data, so use it.
  77         return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
  78     }
  79
  80     if (channels->flags & SBUS_FLAG_SIGNAL_LOSS) {
  81         // The received data is a repeat of the last valid data so can be considered complete.
  82         return RX_FRAME_COMPLETE | RX_FRAME_DROPPED;
  83     }
  84
  85     return RX_FRAME_COMPLETE;
  86 }
  87
  88 uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels)
  89 {
  90     uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
  91     sbusChannelData[0] = channels->chan0;
  92     sbusChannelData[1] = channels->chan1;
  93     sbusChannelData[2] = channels->chan2;
  94     sbusChannelData[3] = channels->chan3;
  95     sbusChannelData[4] = channels->chan4;
  96     sbusChannelData[5] = channels->chan5;
  97     sbusChannelData[6] = channels->chan6;
  98     sbusChannelData[7] = channels->chan7;
  99     sbusChannelData[8] = channels->chan8;
 100     sbusChannelData[9] = channels->chan9;
 101     sbusChannelData[10] = channels->chan10;
 102     sbusChannelData[11] = channels->chan11;
 103     sbusChannelData[12] = channels->chan12;
 104     sbusChannelData[13] = channels->chan13;
 105     sbusChannelData[14] = channels->chan14;
 106     sbusChannelData[15] = channels->chan15;
 107
 108     if (channels->flags & SBUS_FLAG_CHANNEL_DG1) {
 109         sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX;
 110     } else {
 111         sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN;
 112     }
 113
 114     if (channels->flags & SBUS_FLAG_CHANNEL_DG2) {
 115         sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
 116     } else {
 117         sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
 118     }
 119
 120     if (channels->flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
 121         // internal failsafe enabled and rx failsafe flag set
 122         // RX *should* still be sending valid channel data, so use it.
 123         return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
 124     }
 125
 126     if (channels->flags & SBUS_FLAG_SIGNAL_LOSS) {
 127         // The received data is a repeat of the last valid data so can be considered complete.
 128         return RX_FRAME_COMPLETE | RX_FRAME_DROPPED;
 129     }
 130
 131     return RX_FRAME_COMPLETE;
 132 }
 133
 134 uint16_t sbusDecodeChannelValue(uint16_t sbusValue, bool safeValuesOnly)
 135 {
 136     // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
 137     // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
 138     if (safeValuesOnly) {
 139         // Clip channel values to more or less safe values (988 .. 2012)
 140         return (5 * constrain(sbusValue, SBUS_DIGITAL_CHANNEL_MIN, SBUS_DIGITAL_CHANNEL_MAX) / 8) + 880;
 141     }
 142     else {
 143         // Use full range of values (11 bit, channel values in range 880 .. 2159)
 144         return (5 * constrain(sbusValue, 0, 2047) / 8) + 880;
 145     }
 146 }
 147
 148 uint16_t sbusEncodeChannelValue(uint16_t rcValue)
 149 {
 150     return constrain((((int)rcValue - 880) * 8 + 4) / 5, SBUS_DIGITAL_CHANNEL_MIN, SBUS_DIGITAL_CHANNEL_MAX);
 151 }
 152
 153 static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 154 {
 155     return sbusDecodeChannelValue(rxRuntimeConfig->channelData[chan], false);
 156 }
 157
 158 void sbusChannelsInit(rxRuntimeConfig_t *rxRuntimeConfig)
 159 {
 160     rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
 161     for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
 162         rxRuntimeConfig->channelData[b] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408;
 163     }
 164 }
 165 #endif