Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : #pragma once
16 :
17 : #include "AP_RCTelemetry_config.h"
18 :
19 : #if HAL_CRSF_TELEM_ENABLED
20 :
21 : #include <AP_OSD/AP_OSD.h>
22 : #include <AP_CRSF/AP_CRSF_Protocol.h>
23 : #include <AP_RCProtocol/AP_RCProtocol_CRSF.h>
24 : #include "AP_RCTelemetry.h"
25 : #include <AP_HAL/utility/sparse-endian.h>
26 :
27 : class AP_OSD_ParamSetting;
28 :
29 : class AP_CRSF_Telem : public AP_RCTelemetry {
30 : public:
31 : AP_CRSF_Telem();
32 : ~AP_CRSF_Telem() override;
33 :
34 : /* Do not allow copies */
35 : CLASS_NO_COPY(AP_CRSF_Telem);
36 :
37 : // init - perform required initialisation
38 : virtual bool init() override;
39 :
40 : static AP_CRSF_Telem *get_singleton(void);
41 : void queue_message(MAV_SEVERITY severity, const char *text) override;
42 :
43 : static const uint8_t PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE = 50U;
44 : static const uint8_t PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE = 9U;
45 : static const uint8_t CRSF_RX_DEVICE_PING_MAX_RETRY = 50U;
46 :
47 : // Import shared types from AP_CRSF namespace
48 : using HeartbeatFrame = AP_CRSF_Protocol::HeartbeatFrame;
49 : using CommandFrame = AP_CRSF_Protocol::CommandFrame;
50 : using ParameterPingFrame = AP_CRSF_Protocol::ParameterPingFrame;
51 : using ParameterDeviceInfoFrame = AP_CRSF_Protocol::ParameterDeviceInfoFrame;
52 :
53 : // Broadcast frame definitions courtesy of TBS
54 : struct PACKED GPSFrame { // curious fact, calling this GPS makes sizeof(GPS) return 1!
55 : int32_t latitude; // ( degree / 10`000`000 )
56 : int32_t longitude; // (degree / 10`000`000 )
57 : uint16_t groundspeed; // ( km/h / 100 )
58 : uint16_t gps_heading; // ( degree / 100 )
59 : uint16_t altitude; // ( meter - 1000m offset )
60 : uint8_t satellites; // in use ( counter )
61 : };
62 :
63 : struct PACKED BatteryFrame {
64 : uint16_t voltage; // ( mV * 100 )
65 : uint16_t current; // ( mA * 100 )
66 : uint8_t capacity[3]; // ( mAh )
67 : uint8_t remaining; // ( percent )
68 : };
69 :
70 : struct PACKED BaroVarioFrame {
71 : uint16_t altitude_packed; // Altitude above start (calibration) point.
72 : int8_t vertical_speed_packed; // vertical speed.
73 : };
74 :
75 : struct PACKED VarioFrame {
76 : int16_t v_speed; // vertical speed cm/s
77 : };
78 :
79 : struct PACKED VTXFrame {
80 : #if __BYTE_ORDER != __LITTLE_ENDIAN
81 : #error "Only supported on little-endian architectures"
82 : #endif
83 : uint8_t origin; // address
84 : // status
85 : uint8_t is_in_pitmode : 1;
86 : uint8_t is_in_user_frequency_mode : 1;
87 : uint8_t unused : 2;
88 : uint8_t is_vtx_available : 1;
89 : uint8_t smart_audio_ver : 3; // SmartAudio_V1 = 0, SmartAudio_V2 = 1
90 : // band / channel
91 : uint8_t channel : 3; // 1x-8x
92 : uint8_t band : 5; // A, B, E, AirWave, Race
93 : uint16_t user_frequency;
94 : uint8_t power : 4; // 25mW = 0, 200mW = 1, 500mW = 2, 800mW = 3
95 : uint8_t pitmode : 4; // off = 0, In_Band = 1, Out_Band = 2;
96 : };
97 :
98 : struct PACKED VTXTelemetryFrame {
99 : uint8_t origin; // address
100 : uint8_t power; // power in dBm
101 : uint16_t frequency; // frequency in Mhz
102 : uint8_t pitmode; // disable 0, enable 1
103 : };
104 :
105 : struct PACKED AttitudeFrame {
106 : int16_t pitch_angle; // ( rad * 10000 )
107 : int16_t roll_angle; // ( rad * 10000 )
108 : int16_t yaw_angle; // ( rad * 10000 )
109 : };
110 :
111 : struct PACKED FlightModeFrame {
112 : char flight_mode[16]; // ( Null-terminated string )
113 : };
114 :
115 : enum ParameterType : uint8_t
116 : {
117 : UINT8 = 0,
118 : INT8 = 1,
119 : UINT16 = 2,
120 : INT16 = 3,
121 : FLOAT = 8,
122 : TEXT_SELECTION = 9,
123 : STRING = 10,
124 : FOLDER = 11,
125 : INFO = 12,
126 : COMMAND = 13,
127 : OUT_OF_RANGE = 127
128 : };
129 :
130 : // CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY
131 : struct PACKED ParameterSettingsHeader {
132 : uint8_t destination;
133 : uint8_t origin;
134 : uint8_t param_num;
135 : };
136 :
137 : struct PACKED ParameterSettingsEntryHeader : public ParameterSettingsHeader {
138 : uint8_t chunks_left;
139 : };
140 :
141 : // CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY
142 : struct PACKED ParameterSettingsEntry {
143 : ParameterSettingsEntryHeader header;
144 : uint8_t payload[56]; // largest possible frame is 60
145 : };
146 :
147 : // CRSF_FRAMETYPE_PARAMETER_READ
148 : struct PACKED ParameterSettingsReadFrame : public ParameterSettingsHeader {
149 : uint8_t param_chunk;
150 : };
151 :
152 : // CRSF_FRAMETYPE_PARAMETER_WRITE
153 : struct PACKED ParameterSettingsWriteFrame : public ParameterSettingsHeader {
154 : uint8_t payload[57]; // largest possible frame is 60
155 : };
156 :
157 : struct ParameterPayload {
158 : uint8_t payload_length;
159 : uint8_t payload[57];
160 : };
161 :
162 : // Generic pending parameter request, used internally
163 : struct PendingParameterRequest : public ParameterSettingsReadFrame {
164 : ParameterPayload payload;
165 : } _param_request;
166 :
167 : const static uint8_t PARAMETER_MENU_ID = 1; // id of the parameter menu
168 :
169 : #if AP_CRSF_SCRIPTING_ENABLED
170 : // scripted CRSF menus
171 : // menus follow the predefined ardupilot parameter menu
172 : // to avoid a lot of id shuffling at most 10 menus each with at most 20 parameters are allowed
173 : // menu indexes are SCRIPTED_MENU_START_ID -> SCRIPTED_MENU_START_ID + 10
174 : // parameter indexes are SCRIPTED_MENU_START_ID + 10 + menu_id * MAX_SCRIPTED_MENU_SIZE
175 : const static uint8_t MAX_SCRIPTED_MENUS = 10U;
176 : const static uint8_t MAX_SCRIPTED_MENU_SIZE = 20U;
177 : const static uint8_t MAX_SCRIPTED_PARAMETERS = 255U;
178 : const static uint8_t MAX_SCRIPTED_PARAMETER_SIZE = 255U;
179 : const static uint8_t MAX_SCRIPTED_MENU_NAME_LEN = 16;
180 : const static uint8_t SCRIPTED_MENU_START_ID = AP_OSD_ParamScreen::NUM_PARAMS * AP_OSD_NUM_PARAM_SCREENS + 2;
181 :
182 : // 8-bit parameter ids must be unique within the whole menu structure
183 : // each parameter has an id, length and packed data
184 : // to avoid heavy flash usage in the CRSF protocol implementation, the data encoding is
185 : // managed in lua
186 : struct ScriptedEntry {
187 : uint8_t id; // indexed from the menu id + 1 to menu id + MAX_SCRIPTED_MENU_SIZE
188 : uint8_t parent_id;
189 : };
190 :
191 : struct ScriptedParameter : public ScriptedEntry {
192 : uint16_t length;
193 : const char* data;
194 : };
195 :
196 : // each menu contains a number of parameters and has a name
197 : struct ScriptedMenu : public ScriptedEntry {
198 : friend class AP_CRSF_Telem;
199 :
200 : uint8_t num_params;
201 : const char* name;
202 : ScriptedParameter* params;
203 : ScriptedMenu* next_menu; // linked list of menus to make addition/removal/modification easy
204 :
205 : // public API called from lua
206 : ScriptedMenu* add_menu(const char* menu_name, uint8_t size, uint8_t parent_menu);
207 : ScriptedParameter* add_parameter(uint8_t length, const char* data);
208 :
209 : // private API
210 : ScriptedMenu(const char* menu_name, uint8_t size, uint8_t parent_menu);
211 : ~ScriptedMenu();
212 : ScriptedParameter* find_parameter(uint8_t param_num);
213 : ScriptedMenu* find_menu(uint8_t param_num);
214 : bool remove_menu(uint8_t param_num);
215 :
216 : void dump_structure(uint8_t indent);
217 2096 : ScriptedMenu() {}
218 : };
219 :
220 : enum ScriptedParameterEvents : uint8_t {
221 : PARAMETER_READ = 1<<0,
222 : PARAMETER_WRITE = 1<<1
223 : };
224 :
225 : ScriptedMenu scripted_menus;
226 :
227 : typedef ParameterPayload ScriptedPayload;
228 :
229 : struct ScriptedParameterWrite {
230 : ScriptedParameterEvents type;
231 : uint8_t destination;
232 : uint8_t origin;
233 : uint8_t param_num;
234 : uint8_t param_chunk;
235 : ScriptedParameter* param;
236 : ScriptedPayload payload;
237 : };
238 :
239 : // scripted menus represent a complicated dance between the transmitter, flight controller and lua
240 : // lua-in-the-loop requires mediation via outbound_params whereas direct responses can pass straight to
241 : // ready_params. This is made more complicated by the protocol not being request-response - requests to
242 : // read can be interleaved with ongoing requests to write
243 : ObjectBuffer<ScriptedParameterWrite> inbound_params{8}; // parameter requests waiting to be processed
244 : ObjectBuffer<ScriptedParameterWrite> outbound_params{8};// parameter repsonses waiting to be processed
245 : ObjectBuffer<ScriptedParameterWrite> ready_params{8}; // parameter responses ready to be scheduled
246 : HAL_Semaphore scr_sem; // semaphore guarding access to the inbound and outbound queues
247 : // access so that submenus can serialize access from scripting
248 0 : HAL_Semaphore& get_semaphore() { return scr_sem; }
249 :
250 : void clear_menus();
251 : bool process_scripted_param_write(ParameterSettingsWriteFrame* write, uint8_t length);
252 : bool process_scripted_param_read(ParameterSettingsReadFrame* read);
253 : void dump_menu_structure();
254 :
255 : // public API called from lua
256 : // add a scripted sub-menu to this menu
257 : ScriptedMenu* add_menu(const char* name);
258 : // get a menu event filtered by event type, this adds the event to the outbound queue for processing
259 : uint8_t get_menu_event(uint8_t menu_events, uint8_t& param_id, ScriptedPayload& payload);
260 : // peek for a menu event filtered by event type, this makes no changes to the outbound queue
261 : // returns the type of event at the head of the queue and the CRSF payload data from that event
262 : uint8_t peek_menu_event(uint8_t& param_id, ScriptedPayload& payload, uint8_t& events);
263 : // pop a menu event from the inbound queue and add to the outbound queue for processing via lua
264 : void pop_menu_event();
265 : // send a new response from the first item in the outbound queueu
266 : bool send_write_response(uint8_t length, const char* data);
267 : // send a generic response from the first item in the outbound queue
268 : bool send_response();
269 : #endif
270 :
271 : // Frame to hold passthrough telemetry
272 : struct PACKED PassthroughSinglePacketFrame {
273 : uint8_t sub_type;
274 : uint16_t appid;
275 : uint32_t data;
276 : };
277 :
278 : // Frame to hold passthrough telemetry
279 : struct PACKED PassthroughMultiPacketFrame {
280 : uint8_t sub_type;
281 : uint8_t size;
282 : struct PACKED PassthroughTelemetryPacket {
283 : uint16_t appid;
284 : uint32_t data;
285 : } packets[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE];
286 : };
287 :
288 : // Frame to hold status text message
289 : struct PACKED StatusTextFrame {
290 : uint8_t sub_type;
291 : uint8_t severity;
292 : char text[PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE]; // ( Null-terminated string )
293 : };
294 :
295 : // ardupilot frametype container
296 : union PACKED APCustomTelemFrame {
297 : PassthroughSinglePacketFrame single_packet_passthrough;
298 : PassthroughMultiPacketFrame multi_packet_passthrough;
299 : StatusTextFrame status_text;
300 : };
301 :
302 :
303 : union PACKED BroadcastFrame {
304 : GPSFrame gps;
305 : HeartbeatFrame heartbeat;
306 : BaroVarioFrame baro_vario;
307 : VarioFrame vario;
308 : BatteryFrame battery;
309 : VTXFrame vtx;
310 : AttitudeFrame attitude;
311 : FlightModeFrame flightmode;
312 : APCustomTelemFrame custom_telem;
313 : };
314 :
315 : union PACKED ExtendedFrame {
316 : CommandFrame command;
317 : ParameterPingFrame ping;
318 : ParameterDeviceInfoFrame info;
319 : ParameterSettingsEntry param_entry;
320 : ParameterSettingsReadFrame param_read;
321 : ParameterSettingsWriteFrame param_write;
322 : };
323 :
324 : union PACKED TelemetryPayload {
325 : BroadcastFrame bcast;
326 : ExtendedFrame ext;
327 : };
328 :
329 : // get the protocol string
330 0 : const char* get_protocol_string() const { return AP::crsf()->get_protocol_string(_crsf_version.protocol); }
331 :
332 : // is the current protocol ELRS?
333 0 : bool is_elrs() const { return _crsf_version.protocol == AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_ELRS; }
334 : // is the current protocol Tracer?
335 0 : bool is_tracer() const { return _crsf_version.protocol == AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_TRACER; }
336 :
337 : // Process a frame from the CRSF protocol decoder
338 : static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data, uint8_t length);
339 : // get next telemetry data for external consumers of SPort data
340 : static bool get_telem_data(AP_RCProtocol_CRSF::Frame* frame, bool is_tx_active);
341 : // start bind request
342 0 : void start_bind() { _bind_request_pending = true; }
343 36177494 : bool bind_in_progress() { return _bind_request_pending;}
344 :
345 : private:
346 :
347 : enum SensorType {
348 : HEARTBEAT,
349 : PARAMETERS,
350 : BARO_VARIO,
351 : VARIO,
352 : ATTITUDE,
353 : VTX_PARAMETERS,
354 : BATTERY,
355 : GPS,
356 : FLIGHT_MODE,
357 : PASSTHROUGH,
358 : STATUS_TEXT,
359 : GENERAL_COMMAND,
360 : VERSION_PING,
361 : DEVICE_PING,
362 : NUM_SENSORS
363 : };
364 :
365 : // passthrough WFQ scheduler
366 : bool is_packet_ready(uint8_t idx, bool queue_empty) override;
367 : void process_packet(uint8_t idx) override;
368 : void adjust_packet_weight(bool queue_empty) override;
369 : void setup_custom_telemetry();
370 : void update_custom_telemetry_rates(const AP_RCProtocol_CRSF::RFMode rf_mode);
371 :
372 : void calc_parameter_ping();
373 : void calc_heartbeat();
374 : void calc_battery();
375 : uint16_t get_altitude_packed();
376 : int8_t get_vertical_speed_packed();
377 : void calc_baro_vario();
378 : void calc_vario();
379 : void calc_gps();
380 : void calc_attitude();
381 : void calc_flight_mode();
382 : void calc_device_info();
383 : void calc_device_ping(uint8_t destination);
384 : void calc_command_response();
385 : void calc_bind();
386 : void calc_parameter();
387 : #if AP_CRSF_SCRIPTING_ENABLED
388 : bool calc_scripted_parameter();
389 : #endif
390 : #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
391 : void calc_text_selection( AP_OSD_ParamSetting* param, uint8_t chunk);
392 : #endif
393 : void process_pending_requests();
394 : void update_vtx_params();
395 : void get_single_packet_passthrough_telem_data();
396 : void get_multi_packet_passthrough_telem_data(uint8_t size = PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE);
397 : void calc_status_text();
398 : bool process_rf_mode_changes();
399 : uint8_t get_custom_telem_frame_id() const;
400 : AP_RCProtocol_CRSF::RFMode get_rf_mode() const;
401 : uint16_t get_telemetry_rate() const;
402 : bool is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const;
403 :
404 : void process_vtx_frame(VTXFrame* vtx);
405 : void process_vtx_telem_frame(VTXTelemetryFrame* vtx);
406 : void process_ping_frame(ParameterPingFrame* ping);
407 : void process_param_read_frame(ParameterSettingsReadFrame* read);
408 : void process_param_write_frame(ParameterSettingsWriteFrame* write, uint8_t length);
409 : void process_device_info_frame(ParameterDeviceInfoFrame* info);
410 : void process_command_frame(CommandFrame* command);
411 :
412 : // setup ready for passthrough operation
413 : void setup_wfq_scheduler(void) override;
414 :
415 : // setup the scheduler for parameters download
416 : void enter_scheduler_params_mode();
417 : void exit_scheduler_params_mode();
418 : bool should_enter_scheduler_params_mode() const;
419 : void disable_tx_entries();
420 : void enable_tx_entries();
421 :
422 : // get next telemetry data for external consumers
423 : bool _get_telem_data(AP_RCProtocol_CRSF::Frame* data, bool is_tx_active);
424 : bool _process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data, uint8_t length);
425 :
426 : TelemetryPayload _telem;
427 : uint8_t _telem_size;
428 : uint8_t _telem_type;
429 : AP_RCProtocol_CRSF::RFMode _telem_rf_mode;
430 : // reporting telemetry rate
431 : uint32_t _telem_last_report_ms;
432 : uint16_t _telem_last_avg_rate;
433 : // do we need to report the initial state
434 : bool _telem_bootstrap_msg_pending;
435 :
436 : bool _telem_is_high_speed;
437 : bool _telem_pending;
438 : bool _enable_telemetry;
439 : // used to limit telemetry when in a failsafe condition
440 : bool _is_tx_active;
441 :
442 : struct {
443 : uint8_t destination = AP_CRSF_Protocol::CRSF_ADDRESS_BROADCAST;
444 : uint8_t frame_type;
445 : } _pending_request;
446 :
447 : struct {
448 : uint8_t minor;
449 : uint8_t major;
450 : uint8_t retry_count;
451 : bool use_rf_mode;
452 : AP_RCProtocol_CRSF::ProtocolType protocol;
453 : bool pending = true;
454 : uint32_t last_request_info_ms;
455 : } _crsf_version;
456 :
457 : struct {
458 : bool init_done;
459 : uint32_t params_mode_start_ms;
460 : bool params_mode_active;
461 : } _custom_telem;
462 :
463 : struct {
464 : bool pending;
465 : bool valid;
466 : uint8_t port_id;
467 : } _baud_rate_request;
468 :
469 : bool _bind_request_pending;
470 :
471 : // vtx state
472 : bool _vtx_freq_update; // update using the frequency method or not
473 : bool _vtx_dbm_update; // update using the dbm method or not
474 : bool _vtx_freq_change_pending; // a vtx command has been issued but not confirmed by a vtx broadcast frame
475 : bool _vtx_power_change_pending;
476 : bool _vtx_options_change_pending;
477 :
478 : bool _noted_lq_as_rssi_active;
479 :
480 : static AP_CRSF_Telem *singleton;
481 : };
482 :
483 : namespace AP {
484 : AP_CRSF_Telem *crsf_telem();
485 : };
486 :
487 : #endif
|
