Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : FRSky FPort implementation, with thanks to BetaFlight for
17 : specification and code reference
18 : */
19 :
20 : #include "AP_RCProtocol_FPort.h"
21 :
22 : #if AP_RCPROTOCOL_FPORT_ENABLED
23 :
24 : #include <AP_Vehicle/AP_Vehicle_Type.h>
25 : #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
26 : #include <RC_Channel/RC_Channel.h>
27 : #include <AP_Math/AP_Math.h>
28 : #include <AP_Math/crc.h>
29 :
30 : extern const AP_HAL::HAL& hal;
31 :
32 : #define FRAME_HEAD 0x7E
33 : #define FRAME_DLE 0x7D
34 : #define FRAME_XOR 0x20
35 : #define FRAME_LEN_CONTROL 0x19
36 : #define FRAME_LEN_DOWNLINK 0x08
37 : #define MIN_FRAME_SIZE 12
38 : #define MAX_CHANNELS 16
39 :
40 : #define FLAGS_FAILSAFE_BIT 3
41 : #define FLAGS_FRAMELOST_BIT 2
42 :
43 : #define CHAN_SCALE_FACTOR1 1000U
44 : #define CHAN_SCALE_FACTOR2 1600U
45 : #define CHAN_SCALE_OFFSET 875U
46 :
47 : #define FPORT_TYPE_CONTROL 0
48 : #define FPORT_TYPE_DOWNLINK 1
49 : #define FPORT_PRIM_NULL 0x00
50 : #define FPORT_PRIM_DATA 0x10
51 : #define FPORT_PRIM_READ 0x30
52 : #define FPORT_PRIM_WRITE 0x31
53 :
54 : #define MAX_FPORT_CONSECUTIVE_FRAMES 2
55 :
56 : struct PACKED FPort_Frame {
57 : uint8_t header; // 0x7E
58 : uint8_t len; // 0x19 for control, 0x08 for downlink
59 : uint8_t type;
60 : union {
61 : struct PACKED {
62 : uint8_t data[22]; // 16 11-bit channels
63 : uint8_t flags;
64 : uint8_t rssi;
65 : uint8_t crc;
66 : uint8_t end;
67 : } control;
68 : struct PACKED {
69 : uint8_t prim;
70 : uint16_t appid;
71 : uint8_t data[4];
72 : uint8_t crc;
73 : uint8_t end;
74 : } downlink;
75 : };
76 : };
77 :
78 : static_assert(sizeof(FPort_Frame) == FPORT_CONTROL_FRAME_SIZE, "FPort_Frame incorrect size");
79 :
80 : // constructor
81 2094 : AP_RCProtocol_FPort::AP_RCProtocol_FPort(AP_RCProtocol &_frontend, bool _inverted) :
82 : AP_RCProtocol_Backend(_frontend),
83 2094 : inverted(_inverted)
84 2094 : {}
85 :
86 : // decode a full FPort control frame
87 0 : void AP_RCProtocol_FPort::decode_control(const FPort_Frame &frame)
88 : {
89 : uint16_t values[MAX_CHANNELS];
90 :
91 0 : decode_11bit_channels(frame.control.data, MAX_CHANNELS, values, CHAN_SCALE_FACTOR1, CHAN_SCALE_FACTOR2, CHAN_SCALE_OFFSET);
92 :
93 0 : bool failsafe = ((frame.control.flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
94 :
95 : // fport rssi 0-50, ardupilot rssi 0-255, scale factor 255/50=5.1
96 0 : const uint8_t scaled_rssi = MIN(frame.control.rssi * 5.1f, 255);
97 :
98 0 : add_input(MAX_CHANNELS, values, failsafe, scaled_rssi);
99 0 : }
100 :
101 : /*
102 : decode a full FPort downlink frame
103 : */
104 0 : void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
105 : {
106 : #if !APM_BUILD_TYPE(APM_BUILD_iofirmware) && AP_FRSKY_SPORT_TELEM_ENABLED
107 0 : switch (frame.downlink.prim) {
108 0 : case FPORT_PRIM_DATA:
109 : // we've seen at least one 0x10 frame
110 0 : rx_driven_frame_rate = true;
111 0 : break;
112 0 : case FPORT_PRIM_NULL:
113 0 : if (rx_driven_frame_rate) {
114 0 : return;
115 : }
116 : // with 0x00 and no rx control we have a constraint
117 : // on max consecutive frames
118 0 : if (consecutive_telemetry_frame_count >= MAX_FPORT_CONSECUTIVE_FRAMES) {
119 0 : consecutive_telemetry_frame_count = 0;
120 0 : return;
121 : } else {
122 0 : consecutive_telemetry_frame_count++;
123 : }
124 0 : break;
125 0 : case FPORT_PRIM_READ:
126 : case FPORT_PRIM_WRITE:
127 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
128 0 : AP_Frsky_Telem::set_telem_data(frame.downlink.prim, frame.downlink.appid, le32toh_ptr(frame.downlink.data));
129 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
130 : // do not respond to 0x30 and 0x31
131 0 : return;
132 : }
133 :
134 : /*
135 : if we are getting FPORT over a UART then we can ask the FrSky
136 : telem library for some passthrough data to send back, enabling
137 : telemetry on the receiver via the same uart pin as we use for
138 : incoming RC frames
139 : */
140 :
141 0 : AP_HAL::UARTDriver *uart = get_UART();
142 0 : if (!uart) {
143 0 : return;
144 : }
145 :
146 : /*
147 : get SPort data from FRSky_Telem or send a null frame.
148 : We save the data to a variable so in case we're late we'll
149 : send it in the next call, this prevents corruption of
150 : status text messages
151 : */
152 0 : if (!telem_data.available) {
153 : uint8_t packet_count;
154 0 : if (!AP_Frsky_Telem::get_telem_data(&telem_data.packet, packet_count, 1)) {
155 : // nothing to send, send a null frame
156 0 : telem_data.packet.frame = 0x00;
157 0 : telem_data.packet.appid = 0x00;
158 0 : telem_data.packet.data = 0x00;
159 : }
160 0 : telem_data.available = true;
161 : }
162 : /*
163 : check that we haven't been too slow in responding to the new
164 : UART data. If we respond too late then we will corrupt the next
165 : incoming control frame
166 : */
167 0 : uint64_t tend = uart->receive_time_constraint_us(1);
168 0 : uint64_t now = AP_HAL::micros64();
169 0 : uint64_t tdelay = now - tend;
170 0 : if (tdelay > 2500) {
171 : // we've been too slow in responding
172 0 : return;
173 : }
174 : uint8_t buf[10];
175 :
176 0 : buf[0] = 0x08;
177 0 : buf[1] = 0x81;
178 0 : buf[2] = telem_data.packet.frame;
179 0 : buf[3] = telem_data.packet.appid & 0xFF;
180 0 : buf[4] = telem_data.packet.appid >> 8;
181 0 : memcpy(&buf[5], &telem_data.packet.data, 4);
182 0 : buf[9] = crc_sum8_with_carry(&buf[0], 9);
183 :
184 : // perform byte stuffing per FPort spec
185 0 : uint8_t len = 0;
186 : uint8_t buf2[sizeof(buf)*2+1];
187 :
188 0 : if (rc().option_is_enabled(RC_Channels::Option::FPORT_PAD)) {
189 : // this padding helps on some uarts that have hw pullups
190 0 : buf2[len++] = 0xff;
191 : }
192 :
193 0 : for (uint8_t i=0; i<sizeof(buf); i++) {
194 0 : uint8_t c = buf[i];
195 0 : if (c == FRAME_DLE || buf[i] == FRAME_HEAD) {
196 0 : buf2[len++] = FRAME_DLE;
197 0 : buf2[len++] = c ^ FRAME_XOR;
198 : } else {
199 0 : buf2[len++] = c;
200 : }
201 : }
202 0 : uart->write(buf2, len);
203 : // get fresh telem_data in the next call
204 0 : telem_data.available = false;
205 : #endif
206 : }
207 :
208 : /*
209 : process a FPort input pulse of the given width
210 : */
211 0 : void AP_RCProtocol_FPort::process_pulse(uint32_t width_s0, uint32_t width_s1)
212 : {
213 0 : if (have_UART()) {
214 : // if we can use a UART we would much prefer to, as it allows
215 : // us to send SPORT data out
216 0 : return;
217 : }
218 0 : uint32_t w0 = width_s0;
219 0 : uint32_t w1 = width_s1;
220 0 : if (inverted) {
221 0 : w0 = saved_width;
222 0 : w1 = width_s0;
223 0 : saved_width = width_s1;
224 : }
225 : uint8_t b;
226 0 : if (ss.process_pulse(w0, w1, b)) {
227 0 : _process_byte(ss.get_byte_timestamp_us(), b);
228 : }
229 : }
230 :
231 : // support byte input
232 0 : void AP_RCProtocol_FPort::_process_byte(uint32_t timestamp_us, uint8_t b)
233 : {
234 0 : const bool have_frame_gap = (timestamp_us - byte_input.last_byte_us >= 2000U);
235 :
236 0 : byte_input.last_byte_us = timestamp_us;
237 :
238 0 : if (have_frame_gap) {
239 : // if we have a frame gap then this must be the start of a new
240 : // frame
241 0 : byte_input.ofs = 0;
242 0 : byte_input.got_DLE = false;
243 : }
244 0 : if (b != FRAME_HEAD && byte_input.ofs == 0) {
245 : // definately not FPort, missing header byte
246 0 : return;
247 : }
248 :
249 : // handle byte-stuffing decode
250 0 : if (byte_input.got_DLE) {
251 0 : b ^= FRAME_XOR;
252 0 : byte_input.got_DLE = false;
253 0 : } else if (b == FRAME_DLE) {
254 0 : byte_input.got_DLE = true;
255 0 : return;
256 : }
257 :
258 0 : byte_input.buf[byte_input.ofs++] = b;
259 :
260 0 : const FPort_Frame *frame = (const FPort_Frame *)&byte_input.buf[0];
261 :
262 0 : if (byte_input.ofs == 2) {
263 : // check for valid lengths
264 0 : if (frame->len != FRAME_LEN_CONTROL &&
265 0 : frame->len != FRAME_LEN_DOWNLINK) {
266 : // invalid, reset
267 0 : goto reset;
268 : }
269 : }
270 :
271 0 : if (byte_input.ofs == 3) {
272 : // check for valid lengths
273 0 : if ((frame->type == FPORT_TYPE_CONTROL && frame->len != FRAME_LEN_CONTROL) ||
274 0 : (frame->type == FPORT_TYPE_DOWNLINK && frame->len != FRAME_LEN_DOWNLINK)) {
275 0 : goto reset;
276 : }
277 0 : if (frame->type != FPORT_TYPE_CONTROL && frame->type != FPORT_TYPE_DOWNLINK) {
278 : // invalid type
279 0 : goto reset;
280 : }
281 : }
282 :
283 0 : if (frame->type == FPORT_TYPE_CONTROL && byte_input.ofs == FRAME_LEN_CONTROL + 4) {
284 0 : log_data(AP_RCProtocol::FPORT, timestamp_us, byte_input.buf, byte_input.ofs);
285 0 : if (check_checksum()) {
286 0 : decode_control(*frame);
287 : }
288 0 : goto reset;
289 0 : } else if (frame->type == FPORT_TYPE_DOWNLINK && byte_input.ofs == FRAME_LEN_DOWNLINK + 4) {
290 0 : log_data(AP_RCProtocol::FPORT, timestamp_us, byte_input.buf, byte_input.ofs);
291 0 : if (check_checksum()) {
292 0 : decode_downlink(*frame);
293 : }
294 0 : goto reset;
295 : }
296 0 : if (byte_input.ofs == sizeof(byte_input.buf)) {
297 0 : goto reset;
298 : }
299 0 : return;
300 :
301 0 : reset:
302 0 : byte_input.ofs = 0;
303 0 : byte_input.got_DLE = false;
304 : }
305 :
306 : // check checksum byte
307 0 : bool AP_RCProtocol_FPort::check_checksum(void)
308 : {
309 0 : const uint8_t len = byte_input.buf[1]+2;
310 0 : return crc_sum8_with_carry(&byte_input.buf[1], len) == 0x00;
311 : }
312 :
313 : // support byte input
314 0 : void AP_RCProtocol_FPort::process_byte(uint8_t b, uint32_t baudrate)
315 : {
316 0 : if (baudrate != 115200) {
317 0 : return;
318 : }
319 0 : _process_byte(AP_HAL::micros(), b);
320 : }
321 :
322 : #endif // AP_RCPROTOCOL_FPORT_ENABLED
|
