Line data Source code
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 : * This file is free software: you can redistribute it and/or modify it
39 : * under the terms of the GNU General Public License as published by the
40 : * Free Software Foundation, either version 3 of the License, or
41 : * (at your option) any later version.
42 : *
43 : * This file is distributed in the hope that it will be useful, but
44 : * WITHOUT ANY WARRANTY; without even the implied warranty of
45 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
46 : * See the GNU General Public License for more details.
47 : *
48 : * You should have received a copy of the GNU General Public License along
49 : * with this program. If not, see <http://www.gnu.org/licenses/>.
50 : *
51 : * Code by Andrew Tridgell and Siddharth Bharat Purohit
52 : */
53 :
54 : #include "AP_RCProtocol_SBUS.h"
55 :
56 : #include "AP_RCProtocol_config.h"
57 :
58 : #if AP_RCPROTOCOL_SBUS_ENABLED
59 :
60 : #define SBUS_FRAME_SIZE 25
61 : #define SBUS_INPUT_CHANNELS 16
62 : #define SBUS_FLAGS_BYTE 23
63 : #define SBUS_FAILSAFE_BIT 3
64 : #define SBUS_FRAMELOST_BIT 2
65 :
66 : /* define range mapping here, -+100% -> 1000..2000 */
67 : #define SBUS_RANGE_MIN 200
68 : #define SBUS_RANGE_MAX 1800
69 : #define SBUS_RANGE_RANGE (SBUS_RANGE_MAX - SBUS_RANGE_MIN)
70 :
71 : #define SBUS_TARGET_MIN 1000
72 : #define SBUS_TARGET_MAX 2000
73 : #define SBUS_TARGET_RANGE (SBUS_TARGET_MAX - SBUS_TARGET_MIN)
74 :
75 : // this is 875
76 : #define SBUS_SCALE_OFFSET (SBUS_TARGET_MIN - ((SBUS_TARGET_RANGE * SBUS_RANGE_MIN / SBUS_RANGE_RANGE)))
77 :
78 : #ifndef HAL_SBUS_FRAME_GAP
79 : #define HAL_SBUS_FRAME_GAP 2000U
80 : #endif
81 :
82 : // constructor
83 6282 : AP_RCProtocol_SBUS::AP_RCProtocol_SBUS(AP_RCProtocol &_frontend, bool _inverted, uint32_t configured_baud) :
84 : AP_RCProtocol_Backend(_frontend),
85 6282 : inverted(_inverted),
86 6282 : ss{configured_baud, SoftSerial::SERIAL_CONFIG_8E2I}
87 6282 : {}
88 :
89 : // decode a full SBUS frame
90 2560 : bool AP_RCProtocol_SBUS::sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values,
91 : bool &sbus_failsafe, uint16_t max_values)
92 : {
93 : /* check frame boundary markers to avoid out-of-sync cases */
94 2560 : if ((frame[0] != 0x0f)) {
95 0 : return false;
96 : }
97 :
98 2560 : uint16_t chancount = SBUS_INPUT_CHANNELS;
99 :
100 2560 : decode_11bit_channels((const uint8_t*)(&frame[1]), max_values, values,
101 : SBUS_TARGET_RANGE, SBUS_RANGE_RANGE, SBUS_SCALE_OFFSET);
102 :
103 : /* decode switch channels if data fields are wide enough */
104 2560 : if (max_values > 17 && SBUS_INPUT_CHANNELS > 15) {
105 0 : chancount = 18;
106 :
107 : /* channel 17 (index 16) */
108 0 : values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0))?1998:998;
109 : /* channel 18 (index 17) */
110 0 : values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1))?1998:998;
111 : }
112 :
113 : /* note the number of channels decoded */
114 2560 : *num_values = chancount;
115 :
116 : /*
117 : as SBUS is such a weak protocol we additionally check if any of
118 : the first 4 channels are at or below the minimum value of
119 : 875. We consider the frame as a failsafe in that case, which
120 : means we log the data but won't use it
121 : */
122 2560 : bool invalid_data = false;
123 12800 : for (uint8_t i=0; i<4; i++) {
124 10240 : if (values[i] <= SBUS_SCALE_OFFSET) {
125 8 : invalid_data = true;
126 : }
127 : }
128 :
129 : /* decode and handle failsafe and frame-lost flags */
130 2560 : if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
131 : /* report that we failed to read anything valid off the receiver */
132 0 : sbus_failsafe = true;
133 2560 : } else if (invalid_data) {
134 2 : sbus_failsafe = true;
135 2558 : } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
136 : /* set a special warning flag
137 : *
138 : * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
139 : * condition as fail-safe greatly reduces the reliability and range of the radio link,
140 : * e.g. by prematurely issuing return-to-launch!!! */
141 :
142 0 : sbus_failsafe = false;
143 : } else {
144 2558 : sbus_failsafe = false;
145 : }
146 :
147 2560 : return true;
148 : }
149 :
150 :
151 : /*
152 : process a SBUS input pulse of the given width
153 : */
154 0 : void AP_RCProtocol_SBUS::process_pulse(uint32_t width_s0, uint32_t width_s1)
155 : {
156 0 : uint32_t w0 = width_s0;
157 0 : uint32_t w1 = width_s1;
158 0 : if (inverted) {
159 0 : w0 = saved_width;
160 0 : w1 = width_s0;
161 0 : saved_width = width_s1;
162 : }
163 : uint8_t b;
164 0 : if (ss.process_pulse(w0, w1, b)) {
165 0 : _process_byte(ss.get_byte_timestamp_us(), b);
166 : }
167 0 : }
168 :
169 : // support byte input
170 0 : void AP_RCProtocol_SBUS::_process_byte(uint32_t timestamp_us, uint8_t b)
171 : {
172 0 : const bool have_frame_gap = (timestamp_us - byte_input.last_byte_us >= HAL_SBUS_FRAME_GAP);
173 0 : byte_input.last_byte_us = timestamp_us;
174 :
175 0 : if (have_frame_gap) {
176 : // if we have a frame gap then this must be the start of a new
177 : // frame
178 0 : byte_input.ofs = 0;
179 : }
180 0 : if (b != 0x0F && byte_input.ofs == 0) {
181 : // definately not SBUS, missing header byte
182 0 : return;
183 : }
184 0 : if (byte_input.ofs == 0 && !have_frame_gap) {
185 : // must have a frame gap before the start of a new SBUS frame
186 0 : return;
187 : }
188 :
189 0 : byte_input.buf[byte_input.ofs++] = b;
190 :
191 0 : if (byte_input.ofs == sizeof(byte_input.buf)) {
192 0 : log_data(AP_RCProtocol::SBUS, timestamp_us, byte_input.buf, byte_input.ofs);
193 : uint16_t values[SBUS_INPUT_CHANNELS];
194 0 : uint16_t num_values=0;
195 0 : bool sbus_failsafe = false;
196 0 : if (sbus_decode(byte_input.buf, values, &num_values,
197 0 : sbus_failsafe, SBUS_INPUT_CHANNELS) &&
198 0 : num_values >= MIN_RCIN_CHANNELS) {
199 0 : add_input(num_values, values, sbus_failsafe);
200 : }
201 0 : byte_input.ofs = 0;
202 : }
203 : }
204 :
205 : // support byte input
206 0 : void AP_RCProtocol_SBUS::process_byte(uint8_t b, uint32_t baudrate)
207 : {
208 : // note that if we're here we're not actually using SoftSerial,
209 : // but it does record our configured baud rate:
210 0 : if (baudrate != ss.baud()) {
211 0 : return;
212 : }
213 0 : _process_byte(AP_HAL::micros(), b);
214 : }
215 :
216 : #endif // AP_RCPROTOCOL_SBUS_ENABLED
|
