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 FPort2 implementation, with thanks to FrSky for
17 : specification
18 : */
19 :
20 : #include "AP_RCProtocol_FPort2.h"
21 :
22 : #if AP_RCPROTOCOL_FPORT2_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 : #include <stdio.h>
30 :
31 : extern const AP_HAL::HAL& hal;
32 :
33 : #define FRAME_LEN_16 0x18
34 : #define FRAME_LEN_8 0x0D
35 : #define FRAME_LEN_24 0x23
36 : #define FRAME_LEN_DOWNLINK 0x08
37 :
38 : #define FRAME_TYPE_CHANNEL 0xFF
39 : #define FRAME_TYPE_FC 0x1B
40 :
41 : #define MAX_CHANNELS 24
42 :
43 : #define FLAGS_FAILSAFE_BIT 3
44 : #define FLAGS_FRAMELOST_BIT 2
45 :
46 : #define CHAN_SCALE_FACTOR1 1000U
47 : #define CHAN_SCALE_FACTOR2 1600U
48 : #define CHAN_SCALE_OFFSET 875U
49 :
50 : #define FPORT2_PRIM_NULL 0x00
51 : #define FPORT2_PRIM_READ 0x30
52 : #define FPORT2_PRIM_WRITE 0x31
53 :
54 : struct PACKED FPort2_Frame {
55 : uint8_t len;
56 : uint8_t type;
57 : union {
58 : uint8_t data[36];
59 : struct PACKED {
60 : uint8_t prim;
61 : uint16_t appid;
62 : uint8_t data[4];
63 : uint8_t crc;
64 : } downlink;
65 : };
66 : };
67 :
68 : static_assert(sizeof(FPort2_Frame) == FPORT2_CONTROL_FRAME_SIZE, "FPort2_Frame incorrect size");
69 :
70 : // constructor
71 2094 : AP_RCProtocol_FPort2::AP_RCProtocol_FPort2(AP_RCProtocol &_frontend, bool _inverted) :
72 : AP_RCProtocol_Backend(_frontend),
73 2094 : inverted(_inverted)
74 2094 : {}
75 :
76 : // decode a full FPort2 control frame
77 0 : void AP_RCProtocol_FPort2::decode_control(const FPort2_Frame &frame)
78 : {
79 : uint16_t values[MAX_CHANNELS];
80 :
81 0 : decode_11bit_channels(frame.data, chan_count, values, CHAN_SCALE_FACTOR1, CHAN_SCALE_FACTOR2, CHAN_SCALE_OFFSET);
82 :
83 0 : const uint8_t b_flags = frame.data[byte_input.control_len-5];
84 0 : const uint8_t b_rssi = frame.data[byte_input.control_len-4];
85 :
86 0 : bool failsafe = ((b_flags & (1 << FLAGS_FAILSAFE_BIT)) != 0);
87 :
88 : // fport2 rssi 0-50, ardupilot rssi 0-255, scale factor 255/50=5.1
89 0 : const uint8_t scaled_rssi = MIN(b_rssi * 5.1f, 255);
90 :
91 0 : add_input(chan_count, values, failsafe, scaled_rssi);
92 0 : }
93 :
94 : /*
95 : decode a full FPort2 downlink frame
96 : */
97 0 : void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame)
98 : {
99 : #if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
100 : /*
101 : if we are getting FPORT2 over a UART then we can ask the FrSky
102 : telem library for some passthrough data to send back, enabling
103 : telemetry on the receiver via the same uart pin as we use for
104 : incoming RC frames
105 : */
106 :
107 0 : AP_HAL::UARTDriver *uart = get_UART();
108 0 : if (!uart) {
109 0 : return;
110 : }
111 :
112 : // no telemetry for 24ch fport2, timing is too tight
113 0 : if (chan_count > 16) {
114 0 : return;
115 : }
116 :
117 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
118 : // 0x30,0x31 read/write frames
119 : // allowed reply prim: 0x00,0x32
120 0 : if (frame.downlink.prim == FPORT2_PRIM_READ || frame.downlink.prim == FPORT2_PRIM_WRITE) {
121 0 : AP_Frsky_Telem::set_telem_data(frame.downlink.prim, frame.downlink.appid, le32toh_ptr(frame.downlink.data));
122 : }
123 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
124 :
125 : /*
126 : we're only interested in "flight controller" requests i.e. 0x1B
127 : Notes:
128 : need to check how to handle multiple sensor IDs (we probably should respond to 0x1E as well)
129 : with fport2 we need to account for the IDs we send sensor data from
130 : if we respond to multiple sensors like we do for sport we need to make sure GPS data
131 : is always sent with the same sensor ID or else OpenTX will see multiple sensor instances (one for each sensor we respond to)
132 : (need to double check on OpenTX if fport2 carries sensor IDs up to the OpenTX sensor tables)
133 : */
134 0 : if (frame.type != FRAME_TYPE_FC) {
135 : /*
136 : get SPort data from FRSky_Telem
137 : when we are not polled for data we prepare telemetry data for the next poll
138 : we save the data to a variable so in case we're late we'll
139 : send it in the next call, this prevents corruption of status text messages
140 : */
141 0 : if (!telem_data.available) {
142 : uint8_t packet_count;
143 0 : if (!AP_Frsky_Telem::get_telem_data(&telem_data.packet, packet_count, 1)) {
144 0 : telem_data.packet.frame = 0x00;
145 0 : telem_data.packet.appid = 0x00;
146 0 : telem_data.packet.data = 0x00;
147 : }
148 0 : telem_data.available = true;
149 : }
150 0 : return;
151 : }
152 :
153 : /*
154 : check that we haven't been too slow in responding to the new
155 : UART data. If we respond too late then we will corrupt the next
156 : incoming control frame.
157 : 16ch fport2: 4.5ms window before next control frame
158 : specs require to release the bus at least 1.5ms before next control frame (uplink frame takes 0.851ms)
159 : */
160 0 : uint64_t tend = uart->receive_time_constraint_us(1);
161 0 : uint64_t now = AP_HAL::micros64();
162 0 : uint64_t tdelay = now - tend;
163 0 : if (tdelay > 2500) {
164 : // we've been too slow in responding
165 0 : return;
166 : }
167 :
168 : // we initialize to a default null frame
169 0 : uint8_t buf[10] {};
170 :
171 0 : buf[0] = 0x08;
172 0 : buf[1] = frame.type;
173 : // do not consume telemetry data for invalid downlink frames, i.e. incoming prim == 0x00
174 0 : if (frame.downlink.prim != FPORT2_PRIM_NULL) {
175 0 : buf[2] = telem_data.packet.frame;
176 0 : buf[3] = telem_data.packet.appid & 0xFF;
177 0 : buf[4] = telem_data.packet.appid >> 8;
178 0 : memcpy(&buf[5], &telem_data.packet.data, 4);
179 : // get fresh telem_data in the next call
180 0 : telem_data.available = false;
181 : }
182 0 : buf[9] = crc_sum8_with_carry(&buf[1], 8);
183 :
184 0 : uart->write(buf, sizeof(buf));
185 : #endif
186 : }
187 :
188 : /*
189 : process a FPort2 input pulse of the given width
190 : */
191 0 : void AP_RCProtocol_FPort2::process_pulse(uint32_t width_s0, uint32_t width_s1)
192 : {
193 0 : if (have_UART()) {
194 : // if we can use a UART we would much prefer to, as it allows
195 : // us to send SPORT data out
196 0 : return;
197 : }
198 0 : uint32_t w0 = width_s0;
199 0 : uint32_t w1 = width_s1;
200 0 : if (inverted) {
201 0 : w0 = saved_width;
202 0 : w1 = width_s0;
203 0 : saved_width = width_s1;
204 : }
205 : uint8_t b;
206 0 : if (ss.process_pulse(w0, w1, b)) {
207 0 : _process_byte(ss.get_byte_timestamp_us(), b);
208 : }
209 : }
210 :
211 : // support byte input
212 0 : void AP_RCProtocol_FPort2::_process_byte(uint32_t timestamp_us, uint8_t b)
213 : {
214 0 : const bool have_frame_gap = (timestamp_us - byte_input.last_byte_us >= 2000U);
215 :
216 0 : byte_input.last_byte_us = timestamp_us;
217 :
218 0 : if (have_frame_gap) {
219 : // if we have a frame gap then this must be the start of a new
220 : // frame
221 0 : byte_input.ofs = 0;
222 : }
223 :
224 0 : if (byte_input.ofs == 0) {
225 0 : switch (b) {
226 0 : case FRAME_LEN_8:
227 0 : byte_input.control_len = 16;
228 0 : chan_count = 8;
229 0 : byte_input.is_downlink = false;
230 0 : break;
231 0 : case FRAME_LEN_16:
232 0 : byte_input.control_len = 27;
233 0 : chan_count = 16;
234 0 : byte_input.is_downlink = false;
235 0 : break;
236 0 : case FRAME_LEN_24:
237 0 : byte_input.control_len = 38;
238 0 : chan_count = 24;
239 0 : byte_input.is_downlink = false;
240 0 : break;
241 0 : case FRAME_LEN_DOWNLINK:
242 0 : byte_input.control_len = 10;
243 0 : byte_input.is_downlink = true;
244 0 : break;
245 0 : default:
246 : // definately not FPort2, missing header byte
247 0 : return;
248 : }
249 : }
250 :
251 0 : if (byte_input.ofs == 1) {
252 0 : if (!byte_input.is_downlink && b != FRAME_TYPE_CHANNEL) {
253 : // not channel data
254 0 : byte_input.ofs = 0;
255 0 : return;
256 : }
257 : }
258 :
259 0 : byte_input.buf[byte_input.ofs++] = b;
260 :
261 0 : const FPort2_Frame *frame = (const FPort2_Frame *)&byte_input.buf[0];
262 :
263 0 : if (byte_input.control_len > 2 && byte_input.ofs == byte_input.control_len) {
264 0 : if (!byte_input.is_downlink) {
265 0 : log_data(AP_RCProtocol::FPORT2, timestamp_us, byte_input.buf, byte_input.ofs);
266 0 : if (check_checksum()) {
267 0 : decode_control(*frame);
268 : }
269 : } else {
270 : // downlink packet
271 0 : if (check_checksum()) {
272 0 : decode_downlink(*frame);
273 : }
274 : }
275 0 : goto reset;
276 : }
277 0 : if (byte_input.ofs >= sizeof(byte_input.buf)) {
278 0 : goto reset;
279 : }
280 0 : return;
281 :
282 0 : reset:
283 0 : byte_input.ofs = 0;
284 : }
285 :
286 : // check checksum byte
287 0 : bool AP_RCProtocol_FPort2::check_checksum(void)
288 : {
289 0 : return crc_sum8_with_carry(&byte_input.buf[1], byte_input.control_len-1) == 0;
290 : }
291 :
292 : // support byte input
293 0 : void AP_RCProtocol_FPort2::process_byte(uint8_t b, uint32_t baudrate)
294 : {
295 0 : if (baudrate != 115200) {
296 0 : return;
297 : }
298 0 : _process_byte(AP_HAL::micros(), b);
299 : }
300 :
301 : #endif // AP_RCPROTOCOL_FPORT2_ENABLED
|
