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