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 : /*
17 : * Crossfire constants provided by Team Black Sheep under terms of the 2-Clause BSD License
18 : */
19 : #pragma once
20 :
21 : #include "AP_RCProtocol_config.h"
22 : #include <AP_RCTelemetry/AP_RCTelemetry_config.h>
23 :
24 : #if AP_RCPROTOCOL_CRSF_ENABLED
25 :
26 : #include "AP_RCProtocol.h"
27 : #include <AP_CRSF/AP_CRSF_Protocol.h>
28 : #include <AP_Math/AP_Math.h>
29 : #include <RC_Channel/RC_Channel.h>
30 : #include "SoftSerial.h"
31 : #include <AP_OSD/AP_OSD_config.h>
32 :
33 : #define CRSF_FRAME_LENGTH_MIN 2 // min value for _frame.length
34 : #define CRSF_BAUDRATE 416666U
35 : #define ELRS_BAUDRATE 420000U
36 : #define CRSF_TX_TIMEOUT 500000U // the period after which the transmitter is considered disconnected (matches copters failsafe)
37 : #define CRSF_RX_TIMEOUT 150000U // the period after which the receiver is considered disconnected (>ping frequency)
38 :
39 : class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
40 : public:
41 : AP_RCProtocol_CRSF(AP_RCProtocol &_frontend);
42 : virtual ~AP_RCProtocol_CRSF();
43 : void process_byte(uint8_t byte, uint32_t baudrate) override;
44 : void process_handshake(uint32_t baudrate) override;
45 : void update(void) override;
46 : #if HAL_CRSF_TELEM_ENABLED
47 : void start_bind(void) override;
48 : bool bind_in_progress(void);
49 : #endif
50 : // support for CRSF v3
51 : bool change_baud_rate(uint32_t baudrate);
52 : // bootstrap baudrate
53 0 : uint32_t get_bootstrap_baud_rate() const {
54 : #if AP_RC_CHANNEL_ENABLED
55 0 : return rc().option_is_enabled(RC_Channels::Option::ELRS_420KBAUD) ? ELRS_BAUDRATE : CRSF_BAUDRATE;
56 : #else
57 : return CRSF_BAUDRATE;
58 : #endif
59 : }
60 :
61 : // is the receiver active, used to detect power loss and baudrate changes
62 0 : bool is_rx_active() const override {
63 : // later versions of CRSFv3 will send link rate frames every 200ms
64 : // but only before an initial failsafe
65 0 : return _last_rx_frame_time_us != 0 && AP_HAL::micros() - _last_rx_frame_time_us < CRSF_RX_TIMEOUT;
66 : }
67 :
68 : // is the transmitter active, used to adjust telemetry data
69 0 : bool is_tx_active() const {
70 : // this is the same as the Copter failsafe timeout
71 0 : return _last_tx_frame_time_us != 0 && AP_HAL::micros() - _last_tx_frame_time_us < CRSF_TX_TIMEOUT;
72 : }
73 :
74 : // get singleton instance
75 0 : static AP_RCProtocol_CRSF* get_singleton() {
76 0 : return _singleton;
77 : }
78 :
79 : // Import shared types from AP_CRSF namespace
80 : using FrameType = AP_CRSF_Protocol::FrameType;
81 : using CommandID = AP_CRSF_Protocol::CommandID;
82 : using DeviceAddress = AP_CRSF_Protocol::DeviceAddress;
83 : using CommandGeneral = AP_CRSF_Protocol::CommandGeneral;
84 : using CommandVTX = AP_CRSF_Protocol::CommandVTX;
85 : using CommandRX = AP_CRSF_Protocol::CommandRX;
86 : using CustomTelemSubTypeID = AP_CRSF_Protocol::CustomTelemSubTypeID;
87 : using ProtocolType = AP_CRSF_Protocol::ProtocolType;
88 : using Frame = AP_CRSF_Protocol::Frame;
89 : using SubsetChannelsFrame = AP_CRSF_Protocol::SubsetChannelsFrame;
90 : using LinkStatisticsTXFrame = AP_CRSF_Protocol::LinkStatisticsTXFrame;
91 :
92 : // Commands for CRSF_COMMAND_FC
93 : enum CommandFC {
94 : CRSF_COMMAND_FC_DISARM = 0x01,
95 : CRSF_COMMAND_SCALE_CHANNEL = 0x02,
96 : };
97 :
98 : // Commands for CRSF_COMMAND_BLUETOOTH
99 : enum CommandBluetooth {
100 : CRSF_COMMAND_BLUETOOTH_RESET = 0x01,
101 : CRSF_COMMAND_BLUETOOTH_ENABLE = 0x02,
102 : CRSF_COMMAND_BLUETOOTH_ECHO = 0x64,
103 : };
104 :
105 : // Commands for CRSF_COMMAND_OSD
106 : enum CommandOSD {
107 : CRSF_COMMAND_OSD_SEND = 0x01,
108 : };
109 :
110 : // Commands for CRSF_COMMAND_LED
111 : enum CommandLED {
112 : CRSF_COMMAND_LED_SET_DEFAULT = 0x01,
113 : CRSF_COMMAND_LED_COLOR = 0x02,
114 : CRSF_COMMAND_LED_PULSE = 0x03,
115 : CRSF_COMMAND_LED_BLINK = 0x04,
116 : CRSF_COMMAND_LED_SHIFT = 0x05,
117 : };
118 :
119 : enum ExtendedFrameOffset {
120 : CRSF_EXTENDED_FRAME_LENGTH_OFFSET = 1,
121 : CRSF_EXTENDED_FRAME_TYPE_OFFSET = 2,
122 : CRSF_EXTENDED_FRAME_DESTINATION_OFFSET = 3,
123 : CRSF_EXTENDED_FRAME_ORIGIN_OFFSET = 4,
124 : CRSF_EXTENDED_FRAME_PAYLOAD_OFFSET = 5,
125 : };
126 :
127 : struct LinkStatisticsFrame {
128 : uint8_t uplink_rssi_ant1; // ( dBm * -1 )
129 : uint8_t uplink_rssi_ant2; // ( dBm * -1 )
130 : uint8_t uplink_status; // Package success rate / Link quality ( % )
131 : int8_t uplink_snr; // ( db )
132 : uint8_t active_antenna; // Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
133 : uint8_t rf_mode; // ( enum 4fps = 0 , 50fps, 150hz)
134 : uint8_t uplink_tx_power; // ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
135 : uint8_t downlink_rssi; // ( dBm * -1 )
136 : uint8_t downlink_status; // Downlink package success rate / Link quality ( % )
137 : int8_t downlink_dnr; // ( db )
138 : } PACKED;
139 :
140 : struct LinkStatisticsRXFrame {
141 : uint8_t rssi_db; // RSSI(dBm*-1)
142 : uint8_t rssi_percent; // RSSI in percent
143 : uint8_t link_quality; // Package success rate / Link quality ( % )
144 : int8_t snr; // SNR(dB)
145 : uint8_t rf_power_db; // rf power in dBm
146 : } PACKED;
147 :
148 : // Source for ELRS RF modes: https://www.expresslrs.org/info/signal-health/#rf-mode-indexes-rfmd
149 : enum RFMode {
150 : CRSF_RF_MODE_4HZ = 0,
151 : CRSF_RF_MODE_50HZ,
152 : CRSF_RF_MODE_150HZ,
153 : CRSF_RF_MODE_250HZ,
154 : CRSF_RF_MAX_MODES = 4,
155 : ELRS_RF_MODE_4HZ = 4,
156 : ELRS_RF_MODE_25HZ,
157 : ELRS_RF_MODE_50HZ,
158 : ELRS_RF_MODE_100HZ,
159 : ELRS_RF_MODE_100HZ_FULL,
160 : ELRS_RF_MODE_150HZ,
161 : ELRS_RF_MODE_200HZ,
162 : ELRS_RF_MODE_250HZ,
163 : ELRS_RF_MODE_333HZ_FULL,
164 : ELRS_RF_MODE_500HZ,
165 : ELRS_RF_MODE_D250HZ,
166 : ELRS_RF_MODE_D500HZ,
167 : ELRS_RF_MODE_F500HZ,
168 : ELRS_RF_MODE_F1000HZ,
169 : ELRS_RF_MODE_D50HZ,
170 : RF_MODE_MAX_MODES,
171 : RF_MODE_UNKNOWN,
172 : };
173 :
174 : #if AP_OSD_LINK_STATS_EXTENSIONS_ENABLED
175 : // These power levels are valid for both Crossfire and ELRS systems
176 : static constexpr uint16_t tx_powers[] = { 0, 10, 25, 100, 500, 1000, 2000, 250, 50 };
177 : #endif
178 :
179 : struct LinkStatus {
180 : int16_t rssi = -1;
181 : int16_t link_quality = -1;
182 : uint8_t rf_mode;
183 : #if AP_OSD_LINK_STATS_EXTENSIONS_ENABLED
184 : // Add the extra data fields to be used by the OSD panels
185 : int16_t tx_power = -1;
186 : int8_t rssi_dbm = -1;
187 : int8_t snr = INT8_MIN;
188 : int8_t active_antenna = -1;
189 : #endif
190 : };
191 :
192 :
193 : // this will be used by AP_CRSF_Telem to access link status data
194 : // from within AP_RCProtocol_CRSF thread so no need for cross-thread synch
195 0 : const volatile LinkStatus& get_link_status() const {
196 0 : return _link_status;
197 : }
198 :
199 : // return the link rate as defined by the LinkStatistics
200 : uint16_t get_link_rate(ProtocolType protocol) const;
201 :
202 : // return the protocol string
203 : const char* get_protocol_string(ProtocolType protocol) const;
204 :
205 : private:
206 : Frame _frame;
207 : uint8_t *_frame_bytes = (uint8_t*)&_frame;
208 : Frame _telemetry_frame;
209 : uint8_t _frame_ofs;
210 :
211 2094 : const uint8_t MAX_CHANNELS = MIN((uint8_t)CRSF_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS);
212 :
213 : static AP_RCProtocol_CRSF* _singleton;
214 :
215 : void _process_byte(uint8_t byte);
216 : bool check_frame(uint32_t timestamp_us);
217 : void skip_to_next_frame(uint32_t timestamp_us);
218 : bool decode_crsf_packet();
219 : bool process_telemetry(bool check_constraint = true);
220 : void process_link_stats_frame(const void* data);
221 : void process_link_stats_rx_frame(const void* data);
222 : void process_link_stats_tx_frame(const void* data);
223 : // crsf v3 decoding
224 : void decode_variable_bit_channels(const uint8_t* data, uint8_t frame_length, uint8_t nchannels, uint16_t *values);
225 :
226 : void write_frame(Frame* frame);
227 : void start_uart();
228 4 : AP_HAL::UARTDriver* get_current_UART() { return (_uart ? _uart : get_available_UART()); }
229 :
230 : uint16_t _channels[CRSF_MAX_CHANNELS]; /* buffer for extracted RC channel data as pulsewidth in microseconds */
231 :
232 : uint32_t _last_frame_time_us;
233 : uint32_t _last_tx_frame_time_us;
234 : uint32_t _last_uart_start_time_ms;
235 : uint32_t _last_rx_frame_time_us;
236 : uint32_t _start_frame_time_us;
237 : bool telem_available;
238 : uint32_t _new_baud_rate;
239 : bool _crsf_v3_active;
240 :
241 : bool _use_lq_for_rssi;
242 : int16_t derive_scaled_lq_value(uint8_t uplink_lq);
243 :
244 : volatile struct LinkStatus _link_status;
245 :
246 : static const uint16_t RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES];
247 :
248 : AP_HAL::UARTDriver *_uart;
249 : };
250 :
251 : namespace AP {
252 : AP_RCProtocol_CRSF* crsf();
253 : };
254 :
255 : #endif // AP_RCPROTOCOL_CRSF_ENABLED
|
