APM:Libraries
sbus.cpp
Go to the documentation of this file.
1 /*
2  SBUS decoder, based on src/modules/px4iofirmware/sbus.c from PX4Firmware
3  modified for use in AP_HAL_* by Andrew Tridgell
4  */
5 /****************************************************************************
6  *
7  * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * 1. Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * 2. Redistributions in binary form must reproduce the above copyright
16  * notice, this list of conditions and the following disclaimer in
17  * the documentation and/or other materials provided with the
18  * distribution.
19  * 3. Neither the name PX4 nor the names of its contributors may be
20  * used to endorse or promote products derived from this software
21  * without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
30  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
31  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  ****************************************************************************/
37 
38 #include <stdint.h>
39 
40 #include "sbus.h"
41 
42 #define SBUS_FRAME_SIZE 25
43 #define SBUS_INPUT_CHANNELS 16
44 #define SBUS_FLAGS_BYTE 23
45 #define SBUS_FAILSAFE_BIT 3
46 #define SBUS_FRAMELOST_BIT 2
47 
48 /* define range mapping here, -+100% -> 1000..2000 */
49 #define SBUS_RANGE_MIN 200.0f
50 #define SBUS_RANGE_MAX 1800.0f
51 
52 #define SBUS_TARGET_MIN 1000.0f
53 #define SBUS_TARGET_MAX 2000.0f
54 
55 /* pre-calculate the floating point stuff as far as possible at compile time */
56 #define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
57 #define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
58 
59 /*
60  * S.bus decoder matrix.
61  *
62  * Each channel value can come from up to 3 input bytes. Each row in the
63  * matrix describes up to three bytes, and each entry gives:
64  *
65  * - byte offset in the data portion of the frame
66  * - right shift applied to the data byte
67  * - mask for the data byte
68  * - left shift applied to the result into the channel value
69  */
70 struct sbus_bit_pick {
71  uint8_t byte;
72  uint8_t rshift;
73  uint8_t mask;
74  uint8_t lshift;
75 };
76 static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
77  /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
78  /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
79  /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
80  /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
81  /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
82  /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} },
83  /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
84  /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} },
85  /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
86  /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
87  /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
88  /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
89  /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
90  /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} },
91  /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
92  /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
93 };
94 
95 
96 bool
97 sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values,
98  bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
99 {
100  /* check frame boundary markers to avoid out-of-sync cases */
101  if ((frame[0] != 0x0f)) {
102  return false;
103  }
104 
105  switch (frame[24]) {
106  case 0x00:
107  /* this is S.BUS 1 */
108  break;
109  case 0x03:
110  /* S.BUS 2 SLOT0: RX battery and external voltage */
111  break;
112  case 0x83:
113  /* S.BUS 2 SLOT1 */
114  break;
115  case 0x43:
116  case 0xC3:
117  case 0x23:
118  case 0xA3:
119  case 0x63:
120  case 0xE3:
121  break;
122  default:
123  /* we expect one of the bits above, but there are some we don't know yet */
124  break;
125  }
126 
127  unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ?
128  SBUS_INPUT_CHANNELS : max_values;
129 
130  /* use the decoder matrix to extract channel data */
131  for (unsigned channel = 0; channel < chancount; channel++) {
132  unsigned value = 0;
133 
134  for (unsigned pick = 0; pick < 3; pick++) {
135  const struct sbus_bit_pick *decode = &sbus_decoder[channel][pick];
136 
137  if (decode->mask != 0) {
138  unsigned piece = frame[1 + decode->byte];
139  piece >>= decode->rshift;
140  piece &= decode->mask;
141  piece <<= decode->lshift;
142 
143  value |= piece;
144  }
145  }
146 
147 
148  /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */
149  values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET;
150  }
151 
152  /* decode switch channels if data fields are wide enough */
153  if (max_values > 17 && chancount > 15) {
154  chancount = 18;
155 
156  /* channel 17 (index 16) */
157  values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998;
158  /* channel 18 (index 17) */
159  values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998;
160  }
161 
162  /* note the number of channels decoded */
163  *num_values = chancount;
164 
165  /* decode and handle failsafe and frame-lost flags */
166  if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
167  /* report that we failed to read anything valid off the receiver */
168  *sbus_failsafe = true;
169  *sbus_frame_drop = true;
170  }
171  else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
172  /* set a special warning flag
173  *
174  * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
175  * condition as fail-safe greatly reduces the reliability and range of the radio link,
176  * e.g. by prematurely issuing return-to-launch!!! */
177 
178  *sbus_failsafe = false;
179  *sbus_frame_drop = true;
180  } else {
181  *sbus_failsafe = false;
182  *sbus_frame_drop = false;
183  }
184 
185  return true;
186 }
#define SBUS_FRAMELOST_BIT
Definition: sbus.cpp:46
uint8_t byte
Definition: sbus.cpp:71
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3]
Definition: sbus.cpp:76
#define SBUS_FAILSAFE_BIT
Definition: sbus.cpp:45
uint8_t rshift
Definition: sbus.cpp:72
bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
Definition: sbus.cpp:97
#define SBUS_SCALE_FACTOR
Definition: sbus.cpp:56
uint8_t mask
Definition: sbus.cpp:73
#define f(i)
#define SBUS_FLAGS_BYTE
Definition: sbus.cpp:44
float value
#define SBUS_SCALE_OFFSET
Definition: sbus.cpp:57
uint8_t lshift
Definition: sbus.cpp:74
#define SBUS_INPUT_CHANNELS
Definition: sbus.cpp:43