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 :
16 : /*
17 : * Architectural Principles of CRSF Scripted Menu Support
18 : *
19 : * The CRSF scripted menu system provides a mechanism for Lua scripts to create and manage
20 : * custom menu hierarchies on a CRSF-compatible transmitter. The architecture is a hybrid
21 : * model where the C++ backend manages the menu structure, navigation, and response
22 : * generation for folders, while the Lua script handles the content of "leaf" parameters
23 : * (like selections, commands, and numbers).
24 : *
25 : * The entire menu system is built on two main C++ classes: `ScriptedMenu` and
26 : * `ScriptedParameter`.
27 : *
28 : * 1. Global Menu List - All `ScriptedMenu` objects, regardless of their position in the
29 : * hierarchy, are stored in a single, flat, singly-linked list. The list is anchored
30 : * at the `scripted_menus` object. This flat structure is crucial for the global lookup
31 : * functions (`find_menu` and `find_parameter`).
32 : *
33 : * 2. Parent-Child Hierarchy - The hierarchical relationship between menus is established
34 : * using a "parameter-as-a-shortcut" or "symbolic link" pattern:
35 : * - Each `ScriptedMenu` object contains a `parent_id` member that correctly stores the
36 : * ID of its parent menu. This is used when generating responses for "back" navigation.
37 : * - To make a sub-menu appear inside a parent, a special `ScriptedParameter` object is
38 : * created within the parent's `params` array.
39 : * - The `id` of this special "parameter-link" is manually set to be identical to the
40 : * `id` of the sub-menu object it points to.
41 : *
42 : * This dual-representation (an ID belonging to both a menu object and a parameter-link) is
43 : * the key to the entire architecture.
44 : *
45 : * The interaction to navigate into a sub-menu is a multi-step process that relies on
46 : * careful request routing within the C++ backend.
47 : *
48 : * 1. Transmitter Request - When a user selects a sub-menu (e.g., "Roll Tuning" with ID 62),
49 : * the transmitter sends a `AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_READ` request for that ID. It does not
50 : * know whether the ID represents a parameter or a menu.
51 : *
52 : * 2. Initial Routing (`process_scripted_param_read`) - This is the central routing function.
53 : * - It first calls `find_parameter(62)`. This function searches the *entire global list* of
54 : * menus, checking the `params` array of each one. It successfully finds the "parameter-link"
55 : * with ID 62 inside the "Gain Tuner" menu object.
56 : * - Having found a parameter, it then performs a second check: `find_menu(62)`. This also
57 : * succeeds because a `ScriptedMenu` object with ID 62 exists.
58 : * - Because *both* lookups succeeded, the function knows this is a sub-menu navigation request.
59 : * It returns `false` to delegate the request to the generic C++ handler.
60 : * - If only the `find_parameter` call had succeeded, it would be a regular "leaf" parameter,
61 : * and the function would return `true` to send the event to the Lua script for handling.
62 : *
63 : * 3. Response Generation (`calc_parameter`) -
64 : * - The delegated request is handled by `calc_parameter`. It calls `find_menu(62)` to get the
65 : * `ScriptedMenu` object for "Roll Tuning".
66 : * - It then constructs a `FOLDER` type response (`0x2B Parameter settings (entry)`).
67 : * - Critically, it packs the `parent_id` (e.g., 41) from the found menu object into the response
68 : * payload.
69 : * - It also includes the list of child parameter IDs from that menu's `params` array.
70 : * - This response is sent to the transmitter, which uses the `parent_id` for "back" navigation
71 : * and the child list to populate the new menu screen.
72 : *
73 : * This architecture ensures a clean separation of concerns: C++ handles the complex and
74 : * stateful task of menu navigation, while Lua provides the dynamic content for the final
75 : * user-interactive elements.
76 : */
77 :
78 : #include "AP_RCTelemetry_config.h"
79 :
80 : #if HAL_CRSF_TELEM_ENABLED
81 :
82 : #include "AP_CRSF_Telem.h"
83 : #include <AP_VideoTX/AP_VideoTX.h>
84 : #include <AP_HAL/utility/sparse-endian.h>
85 : #include <AP_BattMonitor/AP_BattMonitor.h>
86 : #include <AP_Common/AP_FWVersion.h>
87 : #include <AP_GPS/AP_GPS.h>
88 : #include <GCS_MAVLink/GCS.h>
89 : #include <AP_RCProtocol/AP_RCProtocol_CRSF.h>
90 : #include <AP_SerialManager/AP_SerialManager.h>
91 : #include <AP_AHRS/AP_AHRS.h>
92 : #include <AP_Notify/AP_Notify.h>
93 : #include <AP_OSD/AP_OSD.h>
94 : #include <AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h>
95 : #include <math.h>
96 : #include <stdio.h>
97 : #include <AP_HAL/AP_HAL.h>
98 :
99 : #include <AP_VideoTX/AP_VideoTX.h>
100 :
101 : #include <AP_Vehicle/AP_Vehicle_Type.h>
102 :
103 : //#define CRSF_DEBUG
104 : #ifdef CRSF_DEBUG
105 : # define debug(fmt, args...) hal.console->printf("CRSF: " fmt "\n", ##args)
106 : #else
107 : # define debug(fmt, args...) do {} while(0)
108 : #endif
109 :
110 : extern const AP_HAL::HAL& hal;
111 : const uint8_t AP_CRSF_Telem::PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE;
112 : const uint8_t AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE;
113 : const uint8_t AP_CRSF_Telem::CRSF_RX_DEVICE_PING_MAX_RETRY;
114 :
115 : AP_CRSF_Telem *AP_CRSF_Telem::singleton;
116 :
117 2094 : AP_CRSF_Telem::AP_CRSF_Telem() : AP_RCTelemetry(0)
118 : {
119 2096 : singleton = this;
120 2096 : }
121 :
122 0 : AP_CRSF_Telem::~AP_CRSF_Telem(void)
123 : {
124 0 : singleton = nullptr;
125 0 : }
126 :
127 2096 : bool AP_CRSF_Telem::init(void)
128 : {
129 : // sanity check that we are using a UART for RC input
130 2096 : if (!AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_RCIN, 0)
131 2096 : && !AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_CRSF, 0)) {
132 2095 : return false;
133 : }
134 :
135 : #if AP_VIDEOTX_ENABLED
136 : // Someone explicitly configured CRSF control for VTX; set_provider_enabled
137 : // ignores this if the user has excluded CRSF from VTX_TYPES
138 1 : if (AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_CRSF, 0)) {
139 0 : AP::vtx().set_provider_enabled(AP_VideoTX::VTXType::CRSF);
140 : }
141 : #endif
142 :
143 1 : return AP_RCTelemetry::init();
144 : }
145 :
146 : /*
147 : setup ready for passthrough telem
148 : */
149 1 : void AP_CRSF_Telem::setup_wfq_scheduler(void)
150 : {
151 : // initialize packet weights for the WFQ scheduler
152 : // priority[i] = 1/_scheduler.packet_weight[i]
153 : // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) )
154 :
155 : // CRSF telemetry rate is 150Hz (4ms) max, so these rates must fit
156 1 : add_scheduler_entry(50, 100); // heartbeat 10Hz
157 1 : add_scheduler_entry(5, 20); // parameters 50Hz (generally not active unless requested by the TX)
158 1 : add_scheduler_entry(50, 200); // baro_vario 5Hz
159 1 : add_scheduler_entry(50, 200); // vario 5Hz
160 1 : add_scheduler_entry(50, 120); // Attitude and compass 8Hz
161 1 : add_scheduler_entry(200, 1000); // VTX parameters 1Hz
162 1 : add_scheduler_entry(1300, 500); // battery 2Hz
163 1 : add_scheduler_entry(550, 280); // GPS 3Hz
164 1 : add_scheduler_entry(550, 500); // flight mode 2Hz
165 1 : add_scheduler_entry(5000, 100); // passthrough max 10Hz
166 1 : add_scheduler_entry(5000, 500); // status text max 2Hz
167 1 : add_scheduler_entry(5, 20); // command 50Hz (generally not active unless requested by the TX)
168 1 : add_scheduler_entry(5, 500); // version ping 2Hz (only active at startup)
169 1 : add_scheduler_entry(5, 100); // device ping 10Hz (only active during TX loss, also see CRSF_RX_TIMEOUT)
170 1 : disable_scheduler_entry(DEVICE_PING);
171 1 : }
172 :
173 0 : void AP_CRSF_Telem::setup_custom_telemetry()
174 : {
175 0 : if (_custom_telem.init_done) {
176 0 : return;
177 : }
178 :
179 : // we need crossfire firmware version
180 0 : if (_crsf_version.pending) {
181 0 : return;
182 : }
183 :
184 0 : if (!rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY)) {
185 0 : _custom_telem.init_done = true;
186 0 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"%s: bootstrap complete, fw %d.%02d", get_protocol_string(), _crsf_version.major, _crsf_version.minor);
187 0 : return;
188 : }
189 :
190 : // check if passthru already assigned
191 0 : const int8_t frsky_port = AP::serialmanager().find_portnum(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough,0);
192 0 : if (frsky_port != -1) {
193 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s: passthrough telemetry conflict on SERIAL%d", get_protocol_string(), frsky_port);
194 0 : _custom_telem.init_done = true;
195 0 : return;
196 : }
197 :
198 0 : AP_Frsky_SPort_Passthrough* passthrough = AP::frsky_passthrough_telem();
199 0 : if (passthrough == nullptr) {
200 0 : return;
201 : }
202 :
203 : // setup the frsky scheduler for crossfire and elrs
204 0 : passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LAT);
205 0 : passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LON);
206 0 : passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::TEXT);
207 0 : passthrough->set_scheduler_entry_min_period(AP_Frsky_SPort_Passthrough::ATTITUDE, 350); // 3Hz
208 :
209 : // setup the crossfire scheduler for custom telemetry
210 0 : set_scheduler_entry(FLIGHT_MODE, 1200, 2000); // 0.5Hz
211 0 : set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz
212 :
213 0 : _telem_rf_mode = get_rf_mode();
214 : // setup custom telemetry for current rf_mode
215 0 : update_custom_telemetry_rates(_telem_rf_mode);
216 :
217 0 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"%s: custom telem init done, fw %d.%02d", get_protocol_string(), _crsf_version.major, _crsf_version.minor);
218 :
219 0 : _custom_telem.init_done = true;
220 : }
221 :
222 0 : void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_mode)
223 : {
224 : // ignore rf mode changes if we are processing parameter packets
225 0 : if (_custom_telem.params_mode_active) {
226 0 : return;
227 : }
228 :
229 0 : if (is_high_speed_telemetry(rf_mode)) {
230 : // standard telemetry for high data rates
231 0 : set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz
232 0 : set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz
233 0 : set_scheduler_entry(BARO_VARIO, 1000, 1000); // 1Hz
234 0 : set_scheduler_entry(VARIO, 1000, 1000); // 1Hz
235 : // custom telemetry for high data rates
236 0 : set_scheduler_entry(GPS, 550, 500); // 2.0Hz
237 0 : set_scheduler_entry(PASSTHROUGH, 100, 100); // 8Hz
238 0 : set_scheduler_entry(STATUS_TEXT, 200, 750); // 1.5Hz
239 : } else {
240 : // standard telemetry for low data rates
241 0 : set_scheduler_entry(BATTERY, 1000, 2000); // 0.5Hz
242 0 : set_scheduler_entry(ATTITUDE, 1000, 3000); // 0.33Hz
243 0 : set_scheduler_entry(BARO_VARIO, 1000, 3000); // 0.33Hz
244 0 : set_scheduler_entry(VARIO, 1000, 3000); // 0.33Hz
245 0 : if (is_elrs()) {
246 : // ELRS custom telemetry for low data rates
247 0 : set_scheduler_entry(GPS, 550, 1000); // 1.0Hz
248 0 : set_scheduler_entry(PASSTHROUGH, 350, 500); // 2.0Hz
249 0 : set_scheduler_entry(STATUS_TEXT, 500, 2000); // 0.5Hz
250 : } else {
251 : // CRSF custom telemetry for low data rates
252 0 : set_scheduler_entry(GPS, 550, 1000); // 1.0Hz
253 0 : set_scheduler_entry(PASSTHROUGH, 500, 3000); // 0.3Hz
254 0 : set_scheduler_entry(STATUS_TEXT, 600, 2000); // 0.5Hz
255 : }
256 : }
257 : }
258 :
259 0 : bool AP_CRSF_Telem::process_rf_mode_changes()
260 : {
261 0 : const AP_RCProtocol_CRSF::RFMode current_rf_mode = get_rf_mode();
262 0 : uint32_t now = AP_HAL::millis();
263 :
264 : // the presence of a uart indicates that we are using CRSF for RC control
265 0 : AP_RCProtocol_CRSF* crsf = AP::crsf();
266 0 : AP_HAL::UARTDriver* uart = nullptr;
267 0 : if (crsf != nullptr) {
268 0 : uart = crsf->get_UART();
269 : }
270 :
271 0 : if (uart == nullptr) {
272 0 : return true;
273 : }
274 :
275 0 : if (!crsf->is_detected()) {
276 0 : return false;
277 : }
278 : // not ready yet
279 0 : if (!uart->is_initialized()) {
280 0 : return false;
281 : }
282 : #if !defined (STM32H7)
283 : // warn the user if their setup is sub-optimal, H7 does not need DMA on serial port
284 0 : if (_telem_bootstrap_msg_pending && !uart->is_dma_enabled()) {
285 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s: running on non-DMA serial port", get_protocol_string());
286 : }
287 : #endif
288 : // note if option was set to show LQ in place of RSSI
289 0 : bool current_lq_as_rssi_active = rc().option_is_enabled(RC_Channels::Option::USE_CRSF_LQ_AS_RSSI);
290 0 : if(_telem_bootstrap_msg_pending || _noted_lq_as_rssi_active != current_lq_as_rssi_active){
291 0 : _noted_lq_as_rssi_active = current_lq_as_rssi_active;
292 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s: RSSI now displays %s", get_protocol_string(), current_lq_as_rssi_active ? " as LQ" : "normally");
293 : }
294 0 : _telem_bootstrap_msg_pending = false;
295 :
296 0 : const bool is_high_speed = is_high_speed_telemetry(current_rf_mode);
297 0 : if ((now - _telem_last_report_ms > 5000)) {
298 : // report an RF mode change or a change in telemetry rate if we haven't done so in the last 5s
299 0 : if (!rc().option_is_enabled(RC_Channels::Option::SUPPRESS_CRSF_MESSAGE) && (_telem_rf_mode != current_rf_mode || abs(int16_t(_telem_last_avg_rate) - int16_t(_scheduler.avg_packet_rate)) > 25)) {
300 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s: Link rate %dHz, Telemetry rate %dHz",
301 : get_protocol_string(), crsf->get_link_rate(_crsf_version.protocol), get_telemetry_rate());
302 : }
303 : // tune the scheduler based on telemetry speed high/low transitions
304 0 : if (_telem_is_high_speed != is_high_speed) {
305 0 : update_custom_telemetry_rates(current_rf_mode);
306 : }
307 0 : _telem_is_high_speed = is_high_speed;
308 0 : _telem_rf_mode = current_rf_mode;
309 0 : _telem_last_avg_rate = _scheduler.avg_packet_rate;
310 0 : if (_telem_last_report_ms == 0) { // only want to show bootstrap messages once
311 0 : _telem_bootstrap_msg_pending = true;
312 : }
313 0 : _telem_last_report_ms = now;
314 : }
315 0 : return true;
316 : }
317 :
318 : // return custom frame id based on fw version
319 0 : uint8_t AP_CRSF_Telem::get_custom_telem_frame_id() const
320 : {
321 0 : if (!_crsf_version.pending &&
322 0 : ((_crsf_version.major > 4 || (_crsf_version.major == 4 && _crsf_version.minor >= 6))
323 0 : || is_elrs())) {
324 0 : return AP_CRSF_Protocol::CRSF_FRAMETYPE_AP_CUSTOM_TELEM;
325 : }
326 0 : return AP_CRSF_Protocol::CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY;
327 : }
328 :
329 0 : AP_RCProtocol_CRSF::RFMode AP_CRSF_Telem::get_rf_mode() const
330 : {
331 0 : AP_RCProtocol_CRSF* crsf = AP::crsf();
332 0 : if (crsf == nullptr) {
333 0 : return AP_RCProtocol_CRSF::RFMode::RF_MODE_UNKNOWN;
334 : }
335 :
336 0 : if (!_crsf_version.pending && _crsf_version.use_rf_mode) {
337 0 : if (is_elrs()) {
338 0 : return static_cast<AP_RCProtocol_CRSF::RFMode>(uint8_t(AP_RCProtocol_CRSF::RFMode::CRSF_RF_MAX_MODES) + crsf->get_link_status().rf_mode);
339 : }
340 0 : return static_cast<AP_RCProtocol_CRSF::RFMode>(crsf->get_link_status().rf_mode);
341 0 : } else if (is_tracer()) {
342 0 : return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ;
343 : }
344 :
345 : /*
346 : Note:
347 : - CRSF rf mode 2 on UARTS with DMA runs @160Hz
348 : - CRSF rf mode 2 on UARTS with no DMA runs @70Hz
349 : */
350 0 : if (get_avg_packet_rate() < 40U) {
351 : // no DMA CRSF rf mode 1
352 0 : return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ;
353 : }
354 0 : if (get_avg_packet_rate() > 120U) {
355 : // DMA CRSF rf mode 2
356 0 : return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
357 : }
358 0 : if (get_max_packet_rate() < 120U) {
359 : // no CRSF DMA rf mode 2
360 0 : return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
361 : }
362 0 : return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ;
363 : }
364 :
365 0 : bool AP_CRSF_Telem::is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const
366 : {
367 0 : if (_crsf_version.protocol != AP_CRSF_Protocol::ProtocolType::PROTOCOL_ELRS) {
368 0 : return rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ || rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ;
369 : }
370 0 : return get_telemetry_rate() > 30;
371 : }
372 :
373 0 : uint16_t AP_CRSF_Telem::get_telemetry_rate() const
374 : {
375 0 : if (_crsf_version.protocol != AP_CRSF_Protocol::ProtocolType::PROTOCOL_ELRS) {
376 0 : return get_avg_packet_rate();
377 : }
378 0 : AP_RCProtocol_CRSF* crsf = AP::crsf();
379 0 : if (crsf == nullptr) {
380 0 : return get_avg_packet_rate();
381 : }
382 : // ELRS sends 1 telemetry frame every n RC frames
383 : // the 1:n ratio is user selected
384 : // RC rate is measured by get_avg_packet_rate()
385 : // telemetry rate = air rate - RC rate
386 0 : return crsf->get_link_rate(_crsf_version.protocol) - get_avg_packet_rate();
387 : }
388 :
389 262929 : void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
390 : {
391 : // no need to queue status text messages when crossfire
392 : // custom telemetry is not enabled
393 262929 : if (!rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY)) {
394 262929 : return;
395 : }
396 0 : AP_RCTelemetry::queue_message(severity, text);
397 : }
398 :
399 : /*
400 : disable telemetry entries that require a transmitter to be present
401 : */
402 0 : void AP_CRSF_Telem::disable_tx_entries()
403 : {
404 0 : disable_scheduler_entry(ATTITUDE);
405 0 : disable_scheduler_entry(BARO_VARIO);
406 0 : disable_scheduler_entry(VARIO);
407 0 : disable_scheduler_entry(BATTERY);
408 0 : disable_scheduler_entry(GPS);
409 0 : disable_scheduler_entry(FLIGHT_MODE);
410 0 : disable_scheduler_entry(PASSTHROUGH);
411 0 : disable_scheduler_entry(STATUS_TEXT);
412 : // GENERAL_COMMAND and PARAMETERS will only be sent under very specific circumstances
413 0 : }
414 :
415 : /*
416 : enable telemetry entries that require a transmitter to be present
417 : */
418 0 : void AP_CRSF_Telem::enable_tx_entries()
419 : {
420 0 : enable_scheduler_entry(ATTITUDE);
421 0 : enable_scheduler_entry(BARO_VARIO);
422 0 : enable_scheduler_entry(VARIO);
423 0 : enable_scheduler_entry(BATTERY);
424 0 : enable_scheduler_entry(GPS);
425 0 : enable_scheduler_entry(FLIGHT_MODE);
426 0 : enable_scheduler_entry(PASSTHROUGH);
427 0 : enable_scheduler_entry(STATUS_TEXT);
428 :
429 0 : update_custom_telemetry_rates(_telem_rf_mode);
430 0 : }
431 :
432 0 : void AP_CRSF_Telem::enter_scheduler_params_mode()
433 : {
434 : debug("parameter passthrough enabled");
435 0 : set_scheduler_entry(HEARTBEAT, 50, 200); // heartbeat 5Hz
436 0 : disable_tx_entries();
437 0 : }
438 :
439 0 : void AP_CRSF_Telem::exit_scheduler_params_mode()
440 : {
441 : debug("parameter passthrough disabled");
442 : // setup the crossfire scheduler for custom telemetry
443 0 : set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz
444 0 : enable_tx_entries();
445 0 : }
446 :
447 0 : bool AP_CRSF_Telem::should_enter_scheduler_params_mode() const
448 : {
449 0 : if (hal.util->get_soft_armed()) {
450 0 : return false;
451 : }
452 :
453 0 : if (_pending_request.frame_type > 0 && _pending_request.frame_type != AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAM_DEVICE_INFO) {
454 0 : return true;
455 : }
456 : #if AP_CRSF_SCRIPTING_ENABLED
457 0 : if (!ready_params.is_empty()) {
458 0 : return true;
459 : }
460 : #endif
461 0 : return false;
462 : }
463 :
464 0 : void AP_CRSF_Telem::adjust_packet_weight(bool queue_empty)
465 : {
466 0 : uint32_t now_ms = AP_HAL::millis();
467 0 : setup_custom_telemetry();
468 :
469 : /*
470 : whenever we detect a pending request we configure the scheduler
471 : to allow faster parameters processing.
472 : We start a "fast parameter window" that we close after 5sec
473 : */
474 0 : bool expired = (now_ms - _custom_telem.params_mode_start_ms) > 5000;
475 0 : if (!_custom_telem.params_mode_active && should_enter_scheduler_params_mode()) {
476 : // fast window start
477 0 : _custom_telem.params_mode_start_ms = now_ms;
478 0 : _custom_telem.params_mode_active = true;
479 0 : enter_scheduler_params_mode();
480 0 : } else if (expired && _custom_telem.params_mode_active) {
481 : // fast window stop
482 0 : _custom_telem.params_mode_active = false;
483 0 : exit_scheduler_params_mode();
484 : }
485 0 : }
486 :
487 : // WFQ scheduler
488 0 : bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
489 : {
490 0 : if (!process_rf_mode_changes() && !_bind_request_pending) {
491 0 : return false;
492 : }
493 :
494 0 : switch (idx) {
495 0 : case PARAMETERS:
496 0 : if (_pending_request.frame_type > 0) {
497 0 : return true;
498 : }
499 : #if AP_CRSF_SCRIPTING_ENABLED
500 0 : if (!ready_params.is_empty()) {
501 0 : return true;
502 : }
503 : #endif
504 0 : return false;
505 0 : case VTX_PARAMETERS:
506 : #if AP_VIDEOTX_ENABLED
507 0 : return AP::vtx().have_params_changed() ||_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending;
508 : #else
509 : return false;
510 : #endif
511 0 : case PASSTHROUGH:
512 0 : return rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY);
513 0 : case STATUS_TEXT:
514 0 : return rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY) && !queue_empty;
515 0 : case GENERAL_COMMAND:
516 0 : return _baud_rate_request.pending || _bind_request_pending;
517 0 : case VERSION_PING:
518 0 : return _crsf_version.pending && AP::crsf()->is_detected(); // only send pings if protocol has been detected
519 0 : case HEARTBEAT:
520 0 : return true; // always send heartbeat if enabled
521 0 : case DEVICE_PING:
522 0 : return !_crsf_version.pending; // only send pings if version has been negotiated
523 0 : default:
524 0 : return _enable_telemetry;
525 : }
526 : }
527 :
528 : // WFQ scheduler
529 0 : void AP_CRSF_Telem::process_packet(uint8_t idx)
530 : {
531 : // send packet
532 0 : switch (idx) {
533 0 : case HEARTBEAT: // HEARTBEAT
534 0 : calc_heartbeat();
535 0 : break;
536 0 : case PARAMETERS: // update parameter settings
537 0 : process_pending_requests();
538 0 : break;
539 0 : case BARO_VARIO:
540 0 : calc_baro_vario();
541 0 : break;
542 0 : case VARIO:
543 0 : calc_vario();
544 0 : break;
545 0 : case ATTITUDE:
546 0 : calc_attitude();
547 0 : break;
548 : #if AP_VIDEOTX_ENABLED
549 0 : case VTX_PARAMETERS: // update various VTX parameters
550 0 : update_vtx_params();
551 0 : break;
552 : #endif
553 0 : case BATTERY: // BATTERY
554 0 : calc_battery();
555 0 : break;
556 0 : case GPS: // GPS
557 0 : calc_gps();
558 0 : break;
559 0 : case FLIGHT_MODE: // GPS
560 0 : calc_flight_mode();
561 0 : break;
562 0 : case PASSTHROUGH:
563 0 : if (is_high_speed_telemetry(_telem_rf_mode)) {
564 : // on fast links we have 1:1 ratio between
565 : // passthrough frames and crossfire frames
566 0 : get_single_packet_passthrough_telem_data();
567 : } else {
568 : // on slower links we pack many passthrough
569 : // frames in a single crossfire one (up to 9)
570 0 : const uint8_t size = is_elrs() ? 3 : AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE;
571 0 : get_multi_packet_passthrough_telem_data(size);
572 : }
573 0 : break;
574 0 : case STATUS_TEXT:
575 0 : calc_status_text();
576 0 : break;
577 0 : case GENERAL_COMMAND:
578 0 : if (_bind_request_pending) {
579 0 : calc_bind();
580 : } else {
581 0 : calc_command_response();
582 : }
583 0 : break;
584 0 : case VERSION_PING:
585 : // to get crossfire firmware version we send an RX device ping
586 0 : if (_crsf_version.retry_count++ > CRSF_RX_DEVICE_PING_MAX_RETRY) {
587 0 : _crsf_version.pending = false;
588 0 : _crsf_version.minor = 0;
589 0 : _crsf_version.major = 0;
590 0 : disable_scheduler_entry(VERSION_PING);
591 0 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"%s: RX device ping failed", get_protocol_string());
592 : } else {
593 0 : calc_device_ping(AP_CRSF_Protocol::CRSF_ADDRESS_CRSF_RECEIVER);
594 0 : uint32_t tnow_ms = AP_HAL::millis();
595 0 : if ((tnow_ms - _crsf_version.last_request_info_ms) > 5000) {
596 0 : _crsf_version.last_request_info_ms = tnow_ms;
597 0 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"%s: requesting RX device info", get_protocol_string());
598 : }
599 : }
600 0 : break;
601 0 : case DEVICE_PING:
602 0 : calc_device_ping(AP_CRSF_Protocol::CRSF_ADDRESS_CRSF_RECEIVER);
603 0 : break;
604 0 : default:
605 0 : break;
606 : }
607 0 : }
608 :
609 : // Process a frame from the CRSF protocol decoder
610 0 : bool AP_CRSF_Telem::_process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data, uint8_t length) {
611 0 : switch (frame_type) {
612 : // this means we are connected to an RC receiver and can send telemetry
613 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
614 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED:
615 : // the EVO sends battery frames and we should send telemetry back to populate the OSD
616 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_BATTERY_SENSOR:
617 0 : _enable_telemetry = true;
618 0 : break;
619 :
620 : #if AP_VIDEOTX_ENABLED
621 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_VTX:
622 0 : process_vtx_frame((VTXFrame*)data);
623 0 : break;
624 :
625 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_VTX_TELEM:
626 0 : process_vtx_telem_frame((VTXTelemetryFrame*)data);
627 0 : break;
628 : #endif
629 :
630 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
631 0 : process_ping_frame((ParameterPingFrame*)data);
632 0 : break;
633 :
634 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_READ:
635 0 : process_param_read_frame((ParameterSettingsReadFrame*)data);
636 0 : break;
637 :
638 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_WRITE:
639 0 : process_param_write_frame((ParameterSettingsWriteFrame*)data, length);
640 0 : break;
641 :
642 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAM_DEVICE_INFO:
643 0 : process_device_info_frame((ParameterDeviceInfoFrame*)data);
644 0 : break;
645 :
646 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_COMMAND:
647 0 : process_command_frame((CommandFrame*)data);
648 0 : break;
649 :
650 0 : default:
651 0 : break;
652 : }
653 0 : return true;
654 : }
655 :
656 : #if AP_VIDEOTX_ENABLED
657 0 : void AP_CRSF_Telem::process_vtx_frame(VTXFrame* vtx) {
658 0 : vtx->user_frequency = be16toh(vtx->user_frequency);
659 :
660 : debug("VTX: SmartAudio: %d, Avail: %d, FreqMode: %d, Band: %d, Channel: %d, Freq: %d, PitMode: %d, Pwr: %d, Pit: %d",
661 : vtx->smart_audio_ver, vtx->is_vtx_available, vtx->is_in_user_frequency_mode,
662 : vtx->band, vtx->channel, vtx->is_in_user_frequency_mode ? vtx->user_frequency : AP_VideoTX::get_frequency_mhz(vtx->band, vtx->channel),
663 : vtx->is_in_pitmode, vtx->power, vtx->pitmode);
664 0 : AP_VideoTX& apvtx = AP::vtx();
665 :
666 : // the user may have a VTX connected but not want AP to control it
667 : // (for instance because they are using myVTX on the transmitter), or may
668 : // have handed VTX control to another transport via VTX_TYPES
669 0 : if (!apvtx.get_enabled() || !apvtx.is_type_enabled(AP_VideoTX::VTXType::CRSF)) {
670 0 : return;
671 : }
672 :
673 0 : apvtx.set_provider_enabled(AP_VideoTX::VTXType::CRSF);
674 :
675 0 : apvtx.set_band(vtx->band);
676 0 : apvtx.set_channel(vtx->channel);
677 0 : if (vtx->is_in_user_frequency_mode) {
678 0 : apvtx.set_frequency_mhz(vtx->user_frequency);
679 : } else {
680 0 : apvtx.set_frequency_mhz(AP_VideoTX::get_frequency_mhz(vtx->band, vtx->channel));
681 : }
682 : // 14dBm (25mW), 20dBm (100mW), 26dBm (400mW), 29dBm (800mW)
683 0 : switch (vtx->power) {
684 0 : case 0:
685 0 : apvtx.set_power_mw(25);
686 0 : break;
687 0 : case 1:
688 0 : apvtx.set_power_mw(100);
689 0 : break;
690 0 : case 2:
691 0 : apvtx.set_power_mw(400);
692 0 : break;
693 0 : case 3:
694 0 : apvtx.set_power_mw(800);
695 0 : break;
696 : }
697 0 : if (vtx->is_in_pitmode) {
698 0 : apvtx.set_options(apvtx.get_options() | uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
699 : } else {
700 0 : apvtx.set_options(apvtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
701 : }
702 : // make sure the configured values now reflect reality
703 0 : if (!apvtx.set_defaults() && (_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending)) {
704 0 : AP::vtx().announce_vtx_settings();
705 : }
706 :
707 0 : _vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false;
708 : }
709 :
710 0 : void AP_CRSF_Telem::process_vtx_telem_frame(VTXTelemetryFrame* vtx)
711 : {
712 0 : vtx->frequency = be16toh(vtx->frequency);
713 : debug("VTXTelemetry: Freq: %d, PitMode: %d, Power: %d", vtx->frequency, vtx->pitmode, vtx->power);
714 :
715 0 : AP_VideoTX& apvtx = AP::vtx();
716 :
717 0 : if (!apvtx.get_enabled() || !apvtx.is_type_enabled(AP_VideoTX::VTXType::CRSF)) {
718 0 : return;
719 : }
720 :
721 0 : apvtx.set_provider_enabled(AP_VideoTX::VTXType::CRSF);
722 :
723 0 : apvtx.set_frequency_mhz(vtx->frequency);
724 :
725 : AP_VideoTX::VideoBand band;
726 : uint8_t channel;
727 0 : if (AP_VideoTX::get_band_and_channel(vtx->frequency, band, channel)) {
728 0 : apvtx.set_band(uint8_t(band));
729 0 : apvtx.set_channel(channel);
730 : }
731 :
732 0 : apvtx.set_power_dbm(vtx->power);
733 :
734 0 : if (vtx->pitmode) {
735 0 : apvtx.set_options(apvtx.get_options() | uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
736 : } else {
737 0 : apvtx.set_options(apvtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
738 : }
739 : // make sure the configured values now reflect reality
740 0 : if (!apvtx.set_defaults() && (_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending)) {
741 0 : AP::vtx().announce_vtx_settings();
742 : }
743 :
744 0 : _vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false;
745 : }
746 : #endif // AP_VIDEOTX_ENABLED
747 :
748 : // request for device info
749 0 : void AP_CRSF_Telem::process_ping_frame(ParameterPingFrame* ping)
750 : {
751 : debug("process_ping_frame: %d -> %d", ping->origin, ping->destination);
752 0 : if (ping->destination != 0 && ping->destination != AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
753 0 : return; // request was not for us
754 : }
755 :
756 0 : _param_request.origin = ping->origin;
757 0 : _pending_request.frame_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAM_DEVICE_INFO;
758 0 : _pending_request.destination = ping->origin;
759 : }
760 :
761 : // request for device info
762 0 : void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info)
763 : {
764 : debug("process_device_info_frame: 0x%x -> 0x%x", info->origin, info->destination);
765 0 : if (info->destination != 0 && info->destination != AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
766 0 : return; // request was not for us
767 : }
768 :
769 : // we are only interested in RC device info for firmware version detection
770 0 : if (info->origin != 0 && info->origin != AP_CRSF_Protocol::CRSF_ADDRESS_CRSF_RECEIVER) {
771 0 : return;
772 : }
773 : /*
774 : Payload size is 58:
775 : char[] Device name ( Null-terminated string, max len is 42 )
776 : uint32_t Serial number
777 : uint32_t Hardware ID
778 : uint32_t Firmware ID (0x00:0x00:0xAA:0xBB AA=major, BB=minor)
779 : uint8_t Parameters count
780 : uint8_t Parameter version number
781 : */
782 : // get the terminator of the device name string
783 0 : const uint8_t offset = strnlen((char*)info->payload,42U);
784 0 : if (strncmp((char*)info->payload, "Tracer", 6) == 0) {
785 0 : _crsf_version.protocol = AP_CRSF_Protocol::ProtocolType::PROTOCOL_TRACER;
786 0 : } else if (strncmp((char*)&info->payload[offset+1], "ELRS", 4) == 0) {
787 : // ELRS magic number is ELRS encoded in the serial number
788 : // 0x45 'E' 0x4C 'L' 0x52 'R' 0x53 'S'
789 0 : _crsf_version.protocol = AP_CRSF_Protocol::ProtocolType::PROTOCOL_ELRS;
790 : }
791 :
792 0 : if (!is_elrs()) {
793 : /*
794 : fw major ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 3rd byte of sw id = 11bytes
795 : fw minor ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 4th byte of sw id = 12bytes
796 : */
797 0 : _crsf_version.major = info->payload[offset+11];
798 0 : _crsf_version.minor = info->payload[offset+12];
799 : } else {
800 : // ELRS does not populate the version field so cook up something sensible
801 0 : _crsf_version.major = 1;
802 0 : _crsf_version.minor = 0;
803 : }
804 :
805 : // should we use rf_mode reported by link statistics?
806 0 : if (is_elrs() || (!is_tracer() && (_crsf_version.major > 3 || (_crsf_version.major == 3 && _crsf_version.minor >= 72)))) {
807 0 : _crsf_version.use_rf_mode = true;
808 : }
809 :
810 0 : _crsf_version.pending = false;
811 0 : disable_scheduler_entry(VERSION_PING);
812 : }
813 :
814 : // request for a general command
815 0 : void AP_CRSF_Telem::process_command_frame(CommandFrame* command)
816 : {
817 : debug("process_command_frame: 0x%x -> 0x%x: 0x%x", command->origin, command->destination, command->payload[0]);
818 0 : if (command->destination != 0 && command->destination != AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
819 0 : return; // request was not for us
820 : }
821 :
822 : // we are only interested in commands from the RX
823 0 : if (command->origin != 0 && command->origin != AP_CRSF_Protocol::CRSF_ADDRESS_CRSF_RECEIVER) {
824 0 : return;
825 : }
826 :
827 0 : switch (command->payload[0]) {
828 0 : case AP_CRSF_Protocol::CRSF_COMMAND_GENERAL_CRSF_SPEED_PROPOSAL: {
829 0 : uint32_t baud_rate = command->payload[2] << 24 | command->payload[3] << 16
830 0 : | command->payload[4] << 8 | command->payload[5];
831 0 : _baud_rate_request.port_id = command->payload[1];
832 0 : _baud_rate_request.valid = AP::crsf()->change_baud_rate(baud_rate);
833 0 : _baud_rate_request.pending = true;
834 : debug("requested baud rate change %lu", baud_rate);
835 0 : break;
836 : }
837 0 : default:
838 0 : break; // do nothing
839 : }
840 : }
841 :
842 0 : void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_frame)
843 : {
844 0 : if (read_frame->destination != 0 && read_frame->destination != AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
845 : debug("process_param_read_frame: %d -> %d for %d[%d]: NAK", read_frame->origin, read_frame->destination,
846 : read_frame->param_num, read_frame->param_chunk);
847 0 : return; // request was not for us
848 : }
849 : #if AP_CRSF_SCRIPTING_ENABLED
850 : // write parameter in scripted menus
851 0 : if (process_scripted_param_read(read_frame)) {
852 : debug("process_param_read_frame: %d -> %d for %d[%d]: scripted response OK", read_frame->origin, read_frame->destination,
853 : read_frame->param_num, read_frame->param_chunk);
854 0 : return;
855 : }
856 : #endif // AP_CRSF_SCRIPTING_ENABLED
857 : debug("process_param_read_frame: %d -> %d for %d[%d]: generic response", read_frame->origin, read_frame->destination,
858 : read_frame->param_num, read_frame->param_chunk);
859 0 : _param_request.origin = read_frame->origin;
860 0 : _param_request.destination = read_frame->destination;
861 0 : _param_request.param_num = read_frame->param_num;
862 0 : _param_request.param_chunk = read_frame->param_chunk;
863 0 : _param_request.payload.payload_length = 0;
864 :
865 0 : _pending_request.frame_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_READ;
866 : }
867 :
868 0 : void AP_CRSF_Telem::process_pending_requests()
869 : {
870 : #if AP_CRSF_SCRIPTING_ENABLED
871 : // handle SCRIPTED parameter requests first, as they are now self-contained
872 0 : if (_pending_request.frame_type == 0 && calc_scripted_parameter()) {
873 : // scripted request was popped from outbound_params and handled.
874 0 : _custom_telem.params_mode_start_ms = AP_HAL::millis();
875 0 : return;
876 : }
877 : #endif
878 : // handle general parameter requests
879 0 : switch (_pending_request.frame_type) {
880 : // construct a response to a ping frame
881 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAM_DEVICE_INFO:
882 0 : _custom_telem.params_mode_start_ms = AP_HAL::millis();
883 0 : calc_device_info();
884 0 : break;
885 : // construct a ping frame originating here
886 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
887 0 : calc_device_ping(_pending_request.destination);
888 0 : break;
889 0 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_READ:
890 : case AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_WRITE:
891 : // reset parameter passthrough timeout
892 0 : _custom_telem.params_mode_start_ms = AP_HAL::millis();
893 0 : calc_parameter();
894 0 : break;
895 0 : default:
896 0 : break;
897 : }
898 :
899 0 : _pending_request.frame_type = 0;
900 : }
901 :
902 : #if AP_VIDEOTX_ENABLED
903 0 : void AP_CRSF_Telem::update_vtx_params()
904 : {
905 0 : AP_VideoTX& vtx = AP::vtx();
906 :
907 : // This function does ugly things with the vtx parameters which will upset other providers
908 0 : if (!vtx.get_enabled() || !vtx.is_provider_enabled(AP_VideoTX::VTXType::CRSF)) {
909 0 : return;
910 : }
911 :
912 0 : _vtx_freq_change_pending = vtx.update_band() || vtx.update_channel() || vtx.update_frequency() || _vtx_freq_change_pending;
913 : // don't update the power if we are supposed to be in pitmode as this will take us out of pitmode
914 0 : const bool pitmode = vtx.get_configured_options() & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE);
915 0 : _vtx_power_change_pending = !pitmode && (vtx.update_power() || _vtx_power_change_pending);
916 0 : _vtx_options_change_pending = vtx.update_options() || _vtx_options_change_pending;
917 :
918 0 : if (_vtx_freq_change_pending || _vtx_power_change_pending || _vtx_options_change_pending) {
919 : // make the desired frequency match the desired band and channel
920 0 : if (_vtx_freq_change_pending) {
921 0 : if (vtx.update_band() || vtx.update_channel()) {
922 0 : vtx.update_configured_frequency();
923 : } else {
924 0 : vtx.update_configured_channel_and_band();
925 : }
926 : }
927 :
928 : debug("update_params(): freq %d->%d, chan: %d->%d, band: %d->%d, pwr: %d->%d, opts: %d->%d",
929 : vtx.get_frequency_mhz(), vtx.get_configured_frequency_mhz(),
930 : vtx.get_channel(), vtx.get_configured_channel(),
931 : vtx.get_band(), vtx.get_configured_band(),
932 : vtx.get_power_mw(), vtx.get_configured_power_mw(),
933 : vtx.get_options(), vtx.get_configured_options());
934 :
935 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_COMMAND;
936 0 : _telem.ext.command.destination = AP_CRSF_Protocol::CRSF_ADDRESS_VTX;
937 0 : _telem.ext.command.origin = AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER;
938 0 : _telem.ext.command.command_id = AP_CRSF_Protocol::CRSF_COMMAND_VTX;
939 :
940 0 : uint8_t len = 5;
941 : // prioritize option changes so that the pilot can get in and out of pitmode
942 0 : if (_vtx_options_change_pending) {
943 0 : _telem.ext.command.payload[0] = AP_CRSF_Protocol::CRSF_COMMAND_VTX_PITMODE;
944 0 : if (vtx.get_configured_options() & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE)) {
945 0 : _telem.ext.command.payload[1] = 1;
946 : } else {
947 0 : _telem.ext.command.payload[1] = 0;
948 : }
949 0 : } else if (_vtx_freq_change_pending && _vtx_freq_update) {
950 0 : _telem.ext.command.payload[0] = AP_CRSF_Protocol::CRSF_COMMAND_VTX_FREQ;
951 0 : _telem.ext.command.payload[1] = (vtx.get_frequency_mhz() & 0xFF00) >> 8;
952 0 : _telem.ext.command.payload[2] = (vtx.get_frequency_mhz() & 0xFF);
953 0 : _vtx_freq_update = false;
954 0 : len++;
955 0 : } else if (_vtx_freq_change_pending) {
956 0 : _telem.ext.command.payload[0] = AP_CRSF_Protocol::CRSF_COMMAND_VTX_CHANNEL;
957 0 : _telem.ext.command.payload[1] = vtx.get_configured_band() * VTX_MAX_CHANNELS + vtx.get_configured_channel();
958 0 : _vtx_freq_update = true;
959 0 : } else if (_vtx_power_change_pending && _vtx_dbm_update) {
960 0 : _telem.ext.command.payload[0] = AP_CRSF_Protocol::CRSF_COMMAND_VTX_POWER_DBM;
961 0 : _telem.ext.command.payload[1] = vtx.get_configured_power_dbm();
962 0 : _vtx_dbm_update = false;
963 0 : } else if (_vtx_power_change_pending) {
964 0 : _telem.ext.command.payload[0] = AP_CRSF_Protocol::CRSF_COMMAND_VTX_POWER;
965 0 : if (vtx.get_configured_power_mw() < 26) {
966 0 : vtx.set_configured_power_mw(25);
967 0 : } else if (vtx.get_configured_power_mw() < 201) {
968 0 : if (vtx.get_configured_power_mw() < 101) {
969 0 : vtx.set_configured_power_mw(100);
970 : } else {
971 0 : vtx.set_configured_power_mw(200);
972 : }
973 0 : } else if (vtx.get_configured_power_mw() < 501) {
974 0 : if (vtx.get_configured_power_mw() < 401) {
975 0 : vtx.set_configured_power_mw(400);
976 : } else {
977 0 : vtx.set_configured_power_mw(500);
978 : }
979 : } else {
980 0 : vtx.set_configured_power_mw(800);
981 : }
982 0 : _telem.ext.command.payload[1] = vtx.get_configured_power_level();
983 0 : _vtx_dbm_update = true;
984 : }
985 0 : _telem_pending = true;
986 : // calculate command crc
987 : #pragma GCC diagnostic push
988 : #if defined(__GNUC__) && __GNUC__ >= 10
989 : #pragma GCC diagnostic ignored "-Wstringop-overflow"
990 : #endif
991 0 : uint8_t* crcptr = &_telem.ext.command.destination;
992 0 : uint8_t crc = crc8_dvb(0, AP_CRSF_Protocol::CRSF_FRAMETYPE_COMMAND, 0xBA);
993 0 : for (uint8_t i = 0; i < len; i++) {
994 0 : crc = crc8_dvb(crc, crcptr[i], 0xBA);
995 : }
996 0 : crcptr[len] = crc;
997 0 : _telem_size = len + 1;
998 : #pragma GCC diagnostic pop
999 : }
1000 : }
1001 : #endif // AP_VIDEOTX_ENABLED
1002 :
1003 : // prepare parameter ping data
1004 0 : void AP_CRSF_Telem::calc_parameter_ping()
1005 : {
1006 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
1007 0 : _telem.ext.ping.destination = AP_CRSF_Protocol::CRSF_ADDRESS_VTX;
1008 0 : _telem.ext.ping.origin = AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER;
1009 0 : _telem_size = sizeof(ParameterPingFrame);
1010 0 : _telem_pending = true;
1011 0 : }
1012 :
1013 : // prepare qos data - mandatory frame that must be sent periodically
1014 0 : void AP_CRSF_Telem::calc_heartbeat()
1015 : {
1016 0 : _telem.bcast.heartbeat.origin = AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER;
1017 0 : _telem_size = sizeof(HeartbeatFrame);
1018 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_HEARTBEAT;
1019 0 : _telem_pending = true;
1020 0 : }
1021 :
1022 : // prepare battery data
1023 0 : void AP_CRSF_Telem::calc_battery()
1024 : {
1025 0 : const AP_BattMonitor &_battery = AP::battery();
1026 :
1027 0 : _telem.bcast.battery.voltage = htobe16(uint16_t(roundf(_battery.voltage(0) * 10.0f)));
1028 :
1029 : float current;
1030 0 : if (!_battery.current_amps(current, 0)) {
1031 0 : current = 0;
1032 : }
1033 0 : _telem.bcast.battery.current = htobe16(int16_t(roundf(current * 10.0f)));
1034 :
1035 : float used_mah;
1036 0 : if (!_battery.consumed_mah(used_mah, 0)) {
1037 0 : used_mah = 0;
1038 : }
1039 :
1040 0 : uint8_t percentage = 0;
1041 0 : IGNORE_RETURN(_battery.capacity_remaining_pct(percentage, 0));
1042 :
1043 0 : _telem.bcast.battery.remaining = percentage;
1044 :
1045 0 : const int32_t capacity = used_mah;
1046 0 : _telem.bcast.battery.capacity[0] = (capacity & 0xFF0000) >> 16;
1047 0 : _telem.bcast.battery.capacity[1] = (capacity & 0xFF00) >> 8;
1048 0 : _telem.bcast.battery.capacity[2] = (capacity & 0xFF);
1049 :
1050 0 : _telem_size = sizeof(BatteryFrame);
1051 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_BATTERY_SENSOR;
1052 :
1053 0 : _telem_pending = true;
1054 0 : }
1055 :
1056 0 : uint16_t AP_CRSF_Telem::get_altitude_packed()
1057 : {
1058 0 : int32_t altitude_dm = get_nav_alt_m(Location::AltFrame::ABOVE_HOME) * 10;
1059 :
1060 : enum : int32_t {
1061 : ALT_MIN_DM = 10000, // minimum altitude in dm
1062 : ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM, // altitude of precision changing in dm
1063 : ALT_MAX_DM = 0x7ffe * 10 - 5, // maximum altitude in dm
1064 : };
1065 :
1066 0 : if (altitude_dm < -ALT_MIN_DM) { // less than minimum altitude
1067 0 : return 0; // minimum
1068 : }
1069 0 : if (altitude_dm > ALT_MAX_DM) { // more than maximum
1070 0 : return 0xFFFEU; // maximum
1071 : }
1072 0 : if(altitude_dm < ALT_THRESHOLD_DM) { //dm-resolution range
1073 0 : return uint16_t(altitude_dm + ALT_MIN_DM);
1074 : }
1075 0 : return uint16_t((altitude_dm + 5) / 10) | uint16_t(0x8000); // meter-resolution range
1076 : }
1077 :
1078 0 : int8_t AP_CRSF_Telem::get_vertical_speed_packed()
1079 : {
1080 0 : float vspeed = get_vspeed_ms();
1081 0 : float vertical_speed_cms = vspeed * 100.0f;
1082 0 : const int16_t Kl = 100; // linearity constant;
1083 0 : const float Kr = 0.026f; // range constant;
1084 0 : int8_t vspeed_packed = int8_t(logf(fabsf(vertical_speed_cms)/Kl + 1)/Kr);
1085 0 : return vspeed_packed * (is_negative(vertical_speed_cms) ? -1 : 1);
1086 : }
1087 :
1088 : // prepare vario data
1089 0 : void AP_CRSF_Telem::calc_baro_vario()
1090 : {
1091 0 : _telem.bcast.baro_vario.altitude_packed = htobe16(get_altitude_packed());
1092 0 : _telem.bcast.baro_vario.vertical_speed_packed = get_vertical_speed_packed();
1093 :
1094 0 : _telem_size = sizeof(BaroVarioFrame);
1095 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_BARO_VARIO;
1096 :
1097 0 : _telem_pending = true;
1098 0 : }
1099 :
1100 : // prepare vario data
1101 0 : void AP_CRSF_Telem::calc_vario()
1102 : {
1103 0 : _telem.bcast.vario.v_speed = htobe16(int16_t(get_vspeed_ms() * 100.0f));
1104 0 : _telem_size = sizeof(VarioFrame);
1105 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_VARIO;
1106 :
1107 0 : _telem_pending = true;
1108 0 : }
1109 :
1110 : // prepare gps data
1111 0 : void AP_CRSF_Telem::calc_gps()
1112 : {
1113 0 : const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw)
1114 :
1115 0 : _telem.bcast.gps.latitude = htobe32(loc.lat);
1116 0 : _telem.bcast.gps.longitude = htobe32(loc.lng);
1117 0 : _telem.bcast.gps.groundspeed = htobe16(roundf(AP::gps().ground_speed() * 36.0f));
1118 0 : _telem.bcast.gps.altitude = htobe16(constrain_int16(loc.alt / 100, 0, 5000) + 1000);
1119 0 : _telem.bcast.gps.gps_heading = htobe16(roundf(AP::gps().ground_course() * 100.0f));
1120 0 : _telem.bcast.gps.satellites = AP::gps().num_sats();
1121 :
1122 0 : _telem_size = sizeof(AP_CRSF_Telem::GPSFrame);
1123 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_GPS;
1124 :
1125 0 : _telem_pending = true;
1126 0 : }
1127 :
1128 : // prepare attitude data
1129 0 : void AP_CRSF_Telem::calc_attitude()
1130 : {
1131 0 : AP_AHRS &_ahrs = AP::ahrs();
1132 0 : WITH_SEMAPHORE(_ahrs.get_semaphore());
1133 :
1134 0 : const int16_t INT_PI = 31415;
1135 : // units are radians * 10000
1136 0 : _telem.bcast.attitude.roll_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.get_roll_rad()) * 10000.0f), -INT_PI, INT_PI));
1137 0 : _telem.bcast.attitude.pitch_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.get_pitch_rad()) * 10000.0f), -INT_PI, INT_PI));
1138 0 : _telem.bcast.attitude.yaw_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.get_yaw_rad()) * 10000.0f), -INT_PI, INT_PI));
1139 :
1140 0 : _telem_size = sizeof(AP_CRSF_Telem::AttitudeFrame);
1141 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_ATTITUDE;
1142 :
1143 0 : _telem_pending = true;
1144 0 : }
1145 :
1146 : // prepare flight mode data
1147 0 : void AP_CRSF_Telem::calc_flight_mode()
1148 : {
1149 0 : AP_Notify * notify = AP_Notify::get_singleton();
1150 0 : if (notify) {
1151 : // Note: snprintf() always terminates the string
1152 0 : hal.util->snprintf(
1153 0 : _telem.bcast.flightmode.flight_mode,
1154 : sizeof(AP_CRSF_Telem::FlightModeFrame),
1155 : "%s%s",
1156 : notify->get_flight_mode_str(),
1157 0 : rc().option_is_enabled(RC_Channels::Option::CRSF_FM_DISARM_STAR) && !hal.util->get_soft_armed() ? "*" : ""
1158 : );
1159 : // Note: strlen(_telem.bcast.flightmode.flight_mode) is safe because called on a guaranteed null terminated string
1160 0 : _telem_size = strlen(_telem.bcast.flightmode.flight_mode) + 1; //send the terminator as well
1161 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_FLIGHT_MODE;
1162 0 : _telem_pending = true;
1163 : }
1164 0 : }
1165 :
1166 : // return device information about ArduPilot
1167 0 : void AP_CRSF_Telem::calc_device_info() {
1168 : #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
1169 0 : _telem.ext.info.destination = _param_request.origin;
1170 0 : _telem.ext.info.origin = AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER;
1171 :
1172 0 : const AP_FWVersion &fwver = AP::fwversion();
1173 : // write out the name with version, max width is 60 - 18 = the meaning of life
1174 0 : int32_t n = strlen(fwver.fw_short_string);
1175 0 : strncpy((char*)_telem.ext.info.payload, fwver.fw_short_string, 41);
1176 0 : n = MIN(n + 1, 42);
1177 :
1178 0 : put_be32_ptr(&_telem.ext.info.payload[n], // serial number
1179 0 : uint32_t(fwver.major) << 24 | uint32_t(fwver.minor) << 16 | uint32_t(fwver.patch) << 8 | uint32_t(fwver.fw_type));
1180 0 : n += 4;
1181 0 : put_be32_ptr(&_telem.ext.info.payload[n], // hardware id
1182 0 : uint32_t(fwver.vehicle_type) << 24 | uint32_t(fwver.board_type) << 16 | uint32_t(fwver.board_subtype));
1183 0 : n += 4;
1184 0 : put_be32_ptr(&_telem.ext.info.payload[n], fwver.os_sw_version); // software id
1185 0 : n += 4;
1186 : #if OSD_PARAM_ENABLED
1187 0 : _telem.ext.info.payload[n] = AP_OSD_ParamScreen::NUM_PARAMS * AP_OSD_NUM_PARAM_SCREENS; // param count
1188 : #if AP_CRSF_SCRIPTING_ENABLED
1189 0 : for (ScriptedMenu* m = &scripted_menus; m != nullptr; m = m->next_menu) {
1190 0 : _telem.ext.info.payload[n] += m->num_params;
1191 : }
1192 : #endif // AP_CRSF_SCRIPTING_ENABLED
1193 0 : n++;
1194 : #else
1195 : _telem.ext.info.payload[n++] = 0; // param count
1196 : #endif
1197 0 : _telem.ext.info.payload[n++] = 0; // param version
1198 :
1199 0 : _telem_size = n + 2;
1200 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAM_DEVICE_INFO;
1201 :
1202 0 : _pending_request.frame_type = 0;
1203 0 : _telem_pending = true;
1204 : #endif
1205 0 : }
1206 :
1207 : // send a device ping
1208 0 : void AP_CRSF_Telem::calc_device_ping(uint8_t destination) {
1209 0 : _telem.ext.ping.destination = destination;
1210 0 : _telem.ext.ping.origin = AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER;
1211 0 : _telem_size = 2;
1212 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
1213 0 : _pending_request.frame_type = 0;
1214 :
1215 0 : _telem_pending = true;
1216 0 : }
1217 :
1218 : // send a command response
1219 0 : void AP_CRSF_Telem::calc_command_response() {
1220 0 : _telem.ext.command.destination = AP_CRSF_Protocol::CRSF_ADDRESS_CRSF_RECEIVER;
1221 0 : _telem.ext.command.origin = AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER;
1222 0 : _telem.ext.command.command_id = AP_CRSF_Protocol::CRSF_COMMAND_GENERAL;
1223 0 : _telem.ext.command.payload[0] = AP_CRSF_Protocol::CRSF_COMMAND_GENERAL_CRSF_SPEED_RESPONSE;
1224 0 : _telem.ext.command.payload[1] = _baud_rate_request.port_id;
1225 0 : _telem.ext.command.payload[2] = _baud_rate_request.valid;
1226 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_COMMAND;
1227 :
1228 : // calculate command crc
1229 0 : uint8_t len = 6;
1230 0 : uint8_t* crcptr = &_telem.ext.command.destination;
1231 0 : uint8_t crc = crc8_dvb(0, AP_CRSF_Protocol::CRSF_FRAMETYPE_COMMAND, 0xBA);
1232 0 : for (uint8_t i = 0; i < len; i++) {
1233 0 : crc = crc8_dvb(crc, crcptr[i], 0xBA);
1234 : }
1235 0 : crcptr[len] = crc;
1236 0 : _telem_size = len + 1;
1237 :
1238 0 : _pending_request.frame_type = 0;
1239 0 : _baud_rate_request.pending = false;
1240 :
1241 : debug("sent baud rate response: %u", _baud_rate_request.valid);
1242 0 : _telem_pending = true;
1243 0 : }
1244 :
1245 : // send a command response
1246 0 : void AP_CRSF_Telem::calc_bind() {
1247 0 : _telem.ext.command.destination = AP_CRSF_Protocol::CRSF_ADDRESS_CRSF_RECEIVER;
1248 0 : _telem.ext.command.origin = AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER;
1249 0 : _telem.ext.command.command_id = AP_CRSF_Protocol::CRSF_COMMAND_RX;
1250 0 : _telem.ext.command.payload[0] = AP_CRSF_Protocol::CRSF_COMMAND_RX_BIND;
1251 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_COMMAND;
1252 :
1253 : // calculate command crc
1254 0 : uint8_t len = 4;
1255 0 : uint8_t* crcptr = &_telem.ext.command.destination;
1256 0 : uint8_t crc = crc8_dvb(0, AP_CRSF_Protocol::CRSF_FRAMETYPE_COMMAND, 0xBA);
1257 0 : for (uint8_t i = 0; i < len; i++) {
1258 0 : crc = crc8_dvb(crc, crcptr[i], 0xBA);
1259 : }
1260 0 : crcptr[len] = crc;
1261 0 : _telem_size = len + 1;
1262 :
1263 0 : _pending_request.frame_type = 0;
1264 0 : _bind_request_pending = false;
1265 :
1266 : debug("sent bind request");
1267 0 : _telem_pending = true;
1268 0 : }
1269 :
1270 : // return parameter information
1271 0 : void AP_CRSF_Telem::calc_parameter() {
1272 : #if OSD_PARAM_ENABLED
1273 : #if AP_CRSF_SCRIPTING_ENABLED
1274 0 : WITH_SEMAPHORE(scr_sem);
1275 : #endif
1276 :
1277 0 : _telem.ext.param_entry.header.destination = _param_request.origin;
1278 0 : _telem.ext.param_entry.header.origin = AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER;
1279 0 : size_t idx = 0;
1280 :
1281 : // root folder request
1282 0 : if (_param_request.param_num == 0) {
1283 0 : _telem.ext.param_entry.header.param_num = 0;
1284 0 : _telem.ext.param_entry.header.chunks_left = 0;
1285 0 : _telem.ext.param_entry.payload[idx++] = 0; // parent folder
1286 0 : _telem.ext.param_entry.payload[idx++] = ParameterType::FOLDER; // type
1287 0 : _telem.ext.param_entry.payload[idx++] = 'r'; // "root" name
1288 0 : _telem.ext.param_entry.payload[idx++] = 'o';
1289 0 : _telem.ext.param_entry.payload[idx++] = 'o';
1290 0 : _telem.ext.param_entry.payload[idx++] = 't';
1291 0 : _telem.ext.param_entry.payload[idx++] = 0; // null terminator
1292 :
1293 : #if AP_CRSF_SCRIPTING_ENABLED
1294 0 : if (scripted_menus.next_menu != nullptr) {
1295 0 : _telem.ext.param_entry.payload[idx++] = PARAMETER_MENU_ID; // root parameter screen ardupilot menu
1296 0 : for (ScriptedMenu* m = scripted_menus.next_menu; m != nullptr; m = m->next_menu) {
1297 0 : if (m->parent_id == 0) {
1298 0 : _telem.ext.param_entry.payload[idx++] = m->id; // scripted menus
1299 : }
1300 : }
1301 : } else
1302 : #endif // AP_CRSF_SCRIPTING_ENABLED
1303 : {
1304 : // write out all of the ids we are going to send
1305 0 : for (uint8_t i = 0; i < AP_OSD_ParamScreen::NUM_PARAMS * AP_OSD_NUM_PARAM_SCREENS; i++) {
1306 0 : _telem.ext.param_entry.payload[idx++] = PARAMETER_MENU_ID + i + 1;
1307 : }
1308 : }
1309 0 : _telem.ext.param_entry.payload[idx] = 0xFF; // terminator
1310 :
1311 0 : _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx;
1312 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
1313 0 : _pending_request.frame_type = 0;
1314 0 : _telem_pending = true;
1315 0 : return;
1316 : }
1317 :
1318 : #if AP_CRSF_SCRIPTING_ENABLED
1319 : // root folder request
1320 0 : if (scripted_menus.next_menu != nullptr && _param_request.param_num == PARAMETER_MENU_ID) {
1321 0 : _telem.ext.param_entry.header.param_num = PARAMETER_MENU_ID;
1322 0 : _telem.ext.param_entry.header.chunks_left = 0;
1323 0 : _telem.ext.param_entry.payload[idx++] = 0; // parent folder
1324 0 : _telem.ext.param_entry.payload[idx++] = ParameterType::FOLDER; // type
1325 : // name
1326 0 : strncpy((char*)&_telem.ext.param_entry.payload[idx], "Parameters", ARRAY_SIZE(_telem.ext.param_entry.payload) - idx - 1);
1327 0 : idx += strlen("Parameters");
1328 0 : _telem.ext.param_entry.payload[idx++] = 0; // null terminator
1329 : // write out all of the ids we are going to send
1330 0 : for (uint8_t i = 0; i < AP_OSD_ParamScreen::NUM_PARAMS * AP_OSD_NUM_PARAM_SCREENS; i++) {
1331 0 : _telem.ext.param_entry.payload[idx++] = PARAMETER_MENU_ID + i + 1;
1332 : }
1333 0 : _telem.ext.param_entry.payload[idx] = 0xFF; // terminator
1334 :
1335 0 : _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx;
1336 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
1337 0 : _pending_request.frame_type = 0;
1338 0 : _telem_pending = true;
1339 0 : return;
1340 : }
1341 :
1342 : // scripted menu folder request indexed as the set of parameter ids from SCRIPTED_MENU_START_ID
1343 0 : AP_CRSF_Telem::ScriptedMenu* menu = scripted_menus.find_menu(_param_request.param_num);
1344 :
1345 0 : if (menu != nullptr) {
1346 : debug("menu request %d -> %s[%d]", _param_request.param_num, menu->name, menu->num_params);
1347 :
1348 0 : _telem.ext.param_entry.header.param_num = _param_request.param_num;
1349 0 : _telem.ext.param_entry.header.chunks_left = 0; // assume menus fit in a single chunk
1350 :
1351 0 : _telem.ext.param_entry.payload[idx++] = menu->parent_id; // parent folder
1352 0 : _telem.ext.param_entry.payload[idx++] = ParameterType::FOLDER; // type
1353 : // name
1354 0 : const uint8_t namelen = ARRAY_SIZE(_telem.ext.param_entry.payload) - idx - 1;
1355 0 : strncpy((char*)&_telem.ext.param_entry.payload[idx], menu->name, namelen);
1356 0 : idx += strnlen(menu->name, namelen);
1357 0 : _telem.ext.param_entry.payload[idx++] = 0; // null terminator
1358 :
1359 : // menu entries of parameter ids
1360 0 : for (uint8_t i = 0; i < menu->num_params; i++) {
1361 0 : _telem.ext.param_entry.payload[idx++] = menu->params[i].id;
1362 : }
1363 0 : _telem.ext.param_entry.payload[idx] = 0xFF; // terminator
1364 :
1365 0 : _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx;
1366 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
1367 0 : _pending_request.frame_type = 0;
1368 0 : _telem_pending = true;
1369 0 : return;
1370 : }
1371 : #endif // AP_CRSF_SCRIPTING_ENABLED
1372 :
1373 0 : AP_OSD* osd = AP::osd();
1374 :
1375 0 : if (osd == nullptr) {
1376 0 : return;
1377 : }
1378 :
1379 0 : AP_OSD_ParamSetting* setting = osd->get_setting((_param_request.param_num - (PARAMETER_MENU_ID + 1)) / AP_OSD_ParamScreen::NUM_PARAMS,
1380 0 : (_param_request.param_num - (PARAMETER_MENU_ID + 1)) % AP_OSD_ParamScreen::NUM_PARAMS);
1381 :
1382 0 : if (setting == nullptr) {
1383 0 : return;
1384 : }
1385 :
1386 0 : _telem.ext.param_entry.header.param_num = _param_request.param_num;
1387 : #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
1388 0 : if (setting->get_custom_metadata() != nullptr) {
1389 0 : calc_text_selection(setting, _param_request.param_chunk);
1390 0 : return;
1391 : }
1392 : #endif
1393 0 : _telem.ext.param_entry.header.chunks_left = 0;
1394 0 : _telem.ext.param_entry.payload[idx++] = 0; // parent folder
1395 0 : idx++; // leave a gap for the type
1396 0 : setting->copy_name_camel_case((char*)&_telem.ext.param_entry.payload[idx], 17);
1397 0 : idx += strnlen((char*)&_telem.ext.param_entry.payload[idx], 16) + 1;
1398 :
1399 0 : switch (setting->_param_type) {
1400 0 : case AP_PARAM_INT8: {
1401 0 : AP_Int8* p = (AP_Int8*)setting->_param;
1402 0 : _telem.ext.param_entry.payload[1] = ParameterType::INT8;
1403 0 : _telem.ext.param_entry.payload[idx] = p->get(); // value
1404 0 : _telem.ext.param_entry.payload[idx+1] = int8_t(setting->_param_min.get()); // min
1405 0 : _telem.ext.param_entry.payload[idx+2] = int8_t(setting->_param_max.get()); // max
1406 0 : _telem.ext.param_entry.payload[idx+3] = int8_t(0); // default
1407 0 : idx += 4;
1408 0 : break;
1409 : }
1410 0 : case AP_PARAM_INT16: {
1411 0 : AP_Int16* p = (AP_Int16*)setting->_param;
1412 0 : _telem.ext.param_entry.payload[1] = ParameterType::INT16;
1413 0 : put_be16_ptr(&_telem.ext.param_entry.payload[idx], p->get()); // value
1414 0 : put_be16_ptr(&_telem.ext.param_entry.payload[idx+2], setting->_param_min.get()); // min
1415 0 : put_be16_ptr(&_telem.ext.param_entry.payload[idx+4], setting->_param_max.get()); // max
1416 0 : put_be16_ptr(&_telem.ext.param_entry.payload[idx+6], 0); // default
1417 0 : idx += 8;
1418 0 : break;
1419 : }
1420 0 : case AP_PARAM_INT32: {
1421 0 : AP_Int32* p = (AP_Int32*)setting->_param;
1422 0 : _telem.ext.param_entry.payload[1] = ParameterType::FLOAT;
1423 : #define FLOAT_ENCODE(f) (int32_t(roundf(f)))
1424 0 : put_be32_ptr(&_telem.ext.param_entry.payload[idx], p->get()); // value
1425 0 : put_be32_ptr(&_telem.ext.param_entry.payload[idx+4], FLOAT_ENCODE(setting->_param_min)); // min
1426 0 : put_be32_ptr(&_telem.ext.param_entry.payload[idx+8], FLOAT_ENCODE(setting->_param_max)); // max
1427 0 : put_be32_ptr(&_telem.ext.param_entry.payload[idx+12], FLOAT_ENCODE(0.0f)); // default
1428 : #undef FLOAT_ENCODE
1429 0 : _telem.ext.param_entry.payload[idx+16] = 0; // decimal point
1430 0 : put_be32_ptr(&_telem.ext.param_entry.payload[idx+17], 1); // step size
1431 0 : idx += 21;
1432 0 : break;
1433 : }
1434 0 : case AP_PARAM_FLOAT: {
1435 0 : AP_Float* p = (AP_Float*)setting->_param;
1436 0 : _telem.ext.param_entry.payload[1] = ParameterType::FLOAT;
1437 0 : uint8_t digits = 0;
1438 0 : const float incr = MAX(0.001f, setting->_param_incr); // a bug in OpenTX prevents this going any smaller
1439 :
1440 0 : for (float floatp = incr; floatp < 1.0f; floatp *= 10) {
1441 0 : digits++;
1442 : }
1443 0 : const float mult = powf(10, digits);
1444 : #define FLOAT_ENCODE(f) (int32_t(roundf(mult * f)))
1445 0 : put_be32_ptr(&_telem.ext.param_entry.payload[idx], FLOAT_ENCODE(p->get())); // value
1446 0 : put_be32_ptr(&_telem.ext.param_entry.payload[idx+4], FLOAT_ENCODE(setting->_param_min)); // min
1447 0 : put_be32_ptr(&_telem.ext.param_entry.payload[idx+8], FLOAT_ENCODE(setting->_param_max)); // max
1448 0 : put_be32_ptr(&_telem.ext.param_entry.payload[idx+12], FLOAT_ENCODE(0.0f)); // default
1449 0 : _telem.ext.param_entry.payload[idx+16] = digits; // decimal point
1450 0 : put_be32_ptr(&_telem.ext.param_entry.payload[idx+17], FLOAT_ENCODE(incr)); // step size
1451 : #undef FLOAT_ENCODE
1452 : //debug("Encoding param %f(%f -> %f, %f) as %d(%d) (%d -> %d, %d)", p->get(),
1453 : // param->_param_min.get(), param->_param_max.get(), param->_param_incr.get(),
1454 : // int(FLOAT_ENCODE(p->get())), digits, int(FLOAT_ENCODE(param->_param_min)),
1455 : // int(FLOAT_ENCODE(param->_param_max)), int(FLOAT_ENCODE(param->_param_incr)));
1456 0 : idx += 21;
1457 0 : break;
1458 : }
1459 0 : default:
1460 0 : return;
1461 : }
1462 0 : _telem.ext.param_entry.payload[idx] = 0; // units
1463 :
1464 0 : _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx;
1465 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
1466 :
1467 0 : _pending_request.frame_type = 0;
1468 0 : _telem_pending = true;
1469 : #endif // OSD_PARAM_ENABLED
1470 0 : }
1471 :
1472 : #if AP_CRSF_SCRIPTING_ENABLED
1473 : // process a completed scripted parameter request from the outbound queue
1474 0 : bool AP_CRSF_Telem::calc_scripted_parameter()
1475 : {
1476 0 : WITH_SEMAPHORE(scr_sem);
1477 : ScriptedParameterWrite spw;
1478 : // Check if there's a fully-formed scripted request to process from a Lua script.
1479 : // This is the new thread-safe path that avoids using shared global variables.
1480 0 : if (!ready_params.pop(spw)) {
1481 0 : return false;
1482 : }
1483 :
1484 : // The spw object now contains all context needed to build the response.
1485 0 : size_t idx = 0;
1486 0 : _telem.ext.param_entry.header.destination = spw.origin;
1487 0 : _telem.ext.param_entry.header.origin = AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER;
1488 0 : _telem.ext.param_entry.header.param_num = spw.param_num;
1489 :
1490 : // For a WRITE to a non-COMMAND parameter, the response type is PARAMETER_WRITE (0x2D).
1491 0 : if (spw.type == ScriptedParameterEvents::PARAMETER_WRITE &&
1492 0 : !(spw.param->length > 0 && spw.param->data[0] == ParameterType::COMMAND)) {
1493 :
1494 0 : memcpy((uint8_t*)_telem.ext.param_write.payload, spw.payload.payload, MIN(spw.payload.payload_length, 57));
1495 0 : idx += spw.payload.payload_length;
1496 :
1497 0 : _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsHeader) + idx;
1498 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_WRITE;
1499 :
1500 : } else {
1501 : // For a READ, or a WRITE to a COMMAND parameter, the response is a SETTINGS_ENTRY (0x2B).
1502 :
1503 0 : if (spw.type == ScriptedParameterEvents::PARAMETER_WRITE) { // COMMAND type
1504 0 : _telem.ext.param_entry.header.chunks_left = 0;
1505 0 : _telem.ext.param_entry.payload[idx++] = spw.param->parent_id;
1506 0 : memcpy((uint8_t*)&_telem.ext.param_entry.payload[idx], spw.payload.payload, MIN(spw.payload.payload_length, 57-idx));
1507 0 : idx += spw.payload.payload_length;
1508 : } else { // READ type
1509 0 : const uint8_t CHUNK_SIZE = 55; // This wastes a byte on chunks > 0 but makes the maths easier
1510 0 : const uint8_t chunks = ((spw.param->length) / CHUNK_SIZE) + 1; // includes the extra byte for the parent folder id
1511 0 : _telem.ext.param_entry.header.chunks_left = (chunks - 1) - spw.param_chunk;
1512 :
1513 0 : if (spw.param_chunk == 0) {
1514 0 : _telem.ext.param_entry.payload[idx++] = spw.param->parent_id; // parent folder only in first chunk
1515 : }
1516 :
1517 0 : const uint8_t chunk_len = _telem.ext.param_entry.header.chunks_left > 0 ? CHUNK_SIZE : (spw.param->length + 1) % CHUNK_SIZE;
1518 0 : memcpy((uint8_t*)&_telem.ext.param_entry.payload[idx],
1519 0 : &spw.param->data[spw.param_chunk * CHUNK_SIZE], chunk_len);
1520 0 : idx += chunk_len;
1521 : }
1522 0 : _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + idx;
1523 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
1524 : }
1525 :
1526 0 : _pending_request.frame_type = 0;
1527 0 : _telem_pending = true;
1528 0 : return true; // Scripted request handled.
1529 0 : }
1530 : #endif // AP_CRSF_SCRIPTING_ENABLED
1531 :
1532 : #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
1533 : // class that spits out a chunk of data from a larger stream of contiguous chunks
1534 : // the caller describes which chunk it needs and provides this class with all of the data
1535 : // data is not written until the start position is reached and after a whole chunk
1536 : // is accumulated the rest of the data is skipped in order to determine how many chunks
1537 : // are left to be sent
1538 : class BufferChunker {
1539 : public:
1540 0 : BufferChunker(uint8_t* buf, uint16_t chunk_size, uint16_t start_chunk) :
1541 0 : _buf(buf),
1542 0 : _idx(0),
1543 0 : _bytes(0),
1544 0 : _chunk(0),
1545 0 : _start_chunk(start_chunk),
1546 0 : _chunk_size(chunk_size)
1547 0 : { }
1548 :
1549 : // accumulate a string, writing to the underlying buffer as required
1550 0 : void put_string(const char* str, uint16_t str_len) {
1551 : // skip over data we have already written or have yet to write
1552 0 : if (_chunk != _start_chunk) {
1553 0 : if (skip_bytes(str_len)) {
1554 : // partial write
1555 0 : strncpy((char*)_buf, &str[str_len - _idx], _idx);
1556 0 : _bytes += _idx;
1557 : }
1558 0 : return;
1559 : }
1560 :
1561 0 : uint16_t rem = remaining();
1562 0 : if (rem > str_len) {
1563 0 : strncpy_noterm((char*)&_buf[_idx], str, str_len);
1564 0 : _idx += str_len;
1565 0 : _bytes += str_len;
1566 : } else {
1567 0 : strncpy_noterm((char*)&_buf[_idx], str, rem);
1568 0 : _chunk++;
1569 0 : _idx += str_len;
1570 0 : _bytes += rem;
1571 0 : _idx %= _chunk_size;
1572 : }
1573 : }
1574 :
1575 : // accumulate a byte, writing to the underlying buffer as required
1576 0 : void put_byte(uint8_t b) {
1577 0 : if (_chunk != _start_chunk) {
1578 0 : if (skip_bytes(1)) {
1579 0 : _buf[0] = b;
1580 0 : _bytes++;
1581 : }
1582 0 : return;
1583 : }
1584 0 : if (remaining() > 0) {
1585 0 : _buf[_idx++] = b;
1586 0 : _bytes++;
1587 : } else {
1588 0 : _chunk++;
1589 0 : _idx = 0;
1590 : }
1591 : }
1592 :
1593 0 : uint8_t chunks_remaining() const { return _chunk - _start_chunk; }
1594 0 : uint8_t bytes_written() const { return _bytes; }
1595 :
1596 : private:
1597 0 : uint16_t remaining() const { return _chunk_size - _bytes; }
1598 :
1599 : // skip over the requested number of bytes
1600 : // returns true if we overflow into a chunk that needs to be written
1601 0 : bool skip_bytes(uint16_t len) {
1602 0 : _idx += len;
1603 0 : if (_idx >= _chunk_size) {
1604 0 : _chunk++;
1605 0 : _idx %= _chunk_size;
1606 : // partial write
1607 0 : if (_chunk == _start_chunk && _idx > 0) {
1608 0 : return true;
1609 : }
1610 : }
1611 0 : return false;
1612 : }
1613 :
1614 : uint8_t* _buf;
1615 : uint16_t _idx;
1616 : uint16_t _bytes;
1617 : uint8_t _chunk;
1618 : const uint16_t _start_chunk;
1619 : const uint16_t _chunk_size;
1620 : };
1621 :
1622 : // provide information about a text selection, possibly over multiple chunks
1623 0 : void AP_CRSF_Telem::calc_text_selection(AP_OSD_ParamSetting* param, uint8_t chunk)
1624 : {
1625 0 : const uint8_t CHUNK_SIZE = 56;
1626 0 : const AP_OSD_ParamSetting::ParamMetadata* metadata = param->get_custom_metadata();
1627 :
1628 : // chunk the output
1629 0 : BufferChunker chunker(_telem.ext.param_entry.payload, CHUNK_SIZE, chunk);
1630 :
1631 0 : chunker.put_byte(0); // parent folder
1632 0 : chunker.put_byte(ParameterType::TEXT_SELECTION); // parameter type
1633 :
1634 : char name[17];
1635 0 : param->copy_name_camel_case(name, 17);
1636 0 : chunker.put_string(name, strnlen(name, 16)); // parameter name
1637 0 : chunker.put_byte(0); // trailing null
1638 :
1639 0 : for (uint8_t i = 0; i < metadata->values_max; i++) {
1640 0 : uint8_t len = strnlen(metadata->values[i], 16);
1641 0 : if (len == 0) {
1642 0 : chunker.put_string("---", 3);
1643 : } else {
1644 0 : chunker.put_string(metadata->values[i], len);
1645 : }
1646 0 : if (i == metadata->values_max - 1) {
1647 0 : chunker.put_byte(0);
1648 : } else {
1649 0 : chunker.put_byte(';');
1650 : }
1651 : }
1652 :
1653 0 : int32_t val = -1;
1654 0 : switch (param->_param_type) {
1655 0 : case AP_PARAM_INT8:
1656 0 : val = ((AP_Int8*)param->_param)->get();
1657 0 : break;
1658 0 : case AP_PARAM_INT16:
1659 0 : val = ((AP_Int16*)param->_param)->get();
1660 0 : break;
1661 0 : case AP_PARAM_INT32:
1662 0 : val = ((AP_Int32*)param->_param)->get();
1663 0 : break;
1664 0 : default:
1665 0 : return;
1666 : }
1667 :
1668 : // out of range values really confuse the TX
1669 0 : val = constrain_int16(val, 0, metadata->values_max - 1);
1670 0 : chunker.put_byte(val); // value
1671 0 : chunker.put_byte(0); // min
1672 0 : chunker.put_byte(metadata->values_max); // max
1673 0 : chunker.put_byte(0); // default
1674 0 : chunker.put_byte(0); // units
1675 :
1676 0 : _telem.ext.param_entry.header.chunks_left = chunker.chunks_remaining();
1677 :
1678 0 : _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + chunker.bytes_written();
1679 0 : _telem_type = AP_CRSF_Protocol::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
1680 :
1681 0 : _pending_request.frame_type = 0;
1682 0 : _telem_pending = true;
1683 : }
1684 : #endif // HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
1685 :
1686 : // write parameter information back into AP - assumes we already know the encoding for floats
1687 0 : void AP_CRSF_Telem::process_param_write_frame(ParameterSettingsWriteFrame* write_frame, uint8_t length)
1688 : {
1689 : debug("process_param_write_frame: %d -> %d", write_frame->origin, write_frame->destination);
1690 0 : if (write_frame->destination != AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
1691 0 : return; // request was not for us
1692 : }
1693 : #if AP_CRSF_SCRIPTING_ENABLED
1694 : // write paramter in scripted menus
1695 0 : if (process_scripted_param_write(write_frame, length)) {
1696 0 : return;
1697 : }
1698 : #endif
1699 : #if OSD_PARAM_ENABLED
1700 0 : AP_OSD* osd = AP::osd();
1701 :
1702 0 : if (osd == nullptr) {
1703 0 : return;
1704 : }
1705 :
1706 0 : AP_OSD_ParamSetting* param = osd->get_setting((write_frame->param_num - (PARAMETER_MENU_ID + 1)) / AP_OSD_ParamScreen::NUM_PARAMS,
1707 0 : (write_frame->param_num - (PARAMETER_MENU_ID + 1)) % AP_OSD_ParamScreen::NUM_PARAMS);
1708 :
1709 0 : if (param == nullptr) {
1710 0 : return;
1711 : }
1712 :
1713 : #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
1714 0 : bool text_selection = param->get_custom_metadata() != nullptr;
1715 : #else
1716 : bool text_selection = false;
1717 : #endif
1718 :
1719 0 : switch (param->_param_type) {
1720 0 : case AP_PARAM_INT8: {
1721 0 : AP_Int8* p = (AP_Int8*)param->_param;
1722 0 : p->set_and_save(write_frame->payload[0]);
1723 0 : break;
1724 : }
1725 0 : case AP_PARAM_INT16: {
1726 0 : AP_Int16* p = (AP_Int16*)param->_param;
1727 0 : if (text_selection) {
1728 : // if we have custom metadata then the parameter is a text selection
1729 0 : p->set_and_save(write_frame->payload[0]);
1730 : } else {
1731 0 : p->set_and_save(be16toh_ptr(write_frame->payload));
1732 : }
1733 0 : break;
1734 : }
1735 0 : case AP_PARAM_INT32: {
1736 0 : AP_Int32* p = (AP_Int32*)param->_param;
1737 0 : if (text_selection) {
1738 : // if we have custom metadata then the parameter is a text selection
1739 0 : p->set_and_save(write_frame->payload[0]);
1740 : } else {
1741 0 : p->set_and_save(be32toh_ptr(write_frame->payload));
1742 : }
1743 0 : break;
1744 : }
1745 0 : case AP_PARAM_FLOAT: {
1746 0 : AP_Float* p = (AP_Float*)param->_param;
1747 0 : const int32_t val = be32toh_ptr(write_frame->payload);
1748 0 : uint8_t digits = 0;
1749 0 : const float incr = MAX(0.001f, param->_param_incr); // a bug in OpenTX prevents this going any smaller
1750 :
1751 0 : for (float floatp = incr; floatp < 1.0f; floatp *= 10) {
1752 0 : digits++;
1753 : }
1754 0 : p->set_and_save(float(val) / powf(10, digits));
1755 0 : break;
1756 : }
1757 0 : default:
1758 0 : break;
1759 : }
1760 : #endif // OSD_PARAM_ENABLED
1761 : }
1762 :
1763 : #if AP_CRSF_SCRIPTING_ENABLED
1764 :
1765 : // get a menu event filtered by event type, this adds the event to the outbound queue for processing
1766 : // called from lua
1767 0 : uint8_t AP_CRSF_Telem::get_menu_event(uint8_t menu_events, uint8_t& param_id, ScriptedPayload& payload)
1768 : {
1769 0 : WITH_SEMAPHORE(scr_sem);
1770 :
1771 0 : uint8_t events = 0;
1772 : ScriptedParameterWrite spw;
1773 0 : if (inbound_params.pop(spw)) {
1774 : // not listening for the event, send a response directly
1775 0 : if ((spw.type & menu_events) == 0) {
1776 0 : ready_params.push(spw);
1777 0 : return 0;
1778 : }
1779 0 : switch (spw.type) {
1780 0 : case ScriptedParameterEvents::PARAMETER_WRITE:
1781 0 : events |= PARAMETER_WRITE;
1782 0 : payload = spw.payload;
1783 0 : param_id = spw.param->id;
1784 0 : break;
1785 0 : case ScriptedParameterEvents::PARAMETER_READ:
1786 0 : events |= PARAMETER_READ;
1787 0 : payload.payload_length = 0;
1788 0 : param_id = spw.param->id;
1789 0 : break;
1790 0 : default:
1791 0 : break;
1792 : }
1793 : // respond with parameter entry
1794 0 : outbound_params.push(spw);
1795 : debug("get_menu_event(%u) -> %s", param_id, spw.type == ScriptedParameterEvents::PARAMETER_WRITE ? "WRITE" : "READ");
1796 : }
1797 :
1798 0 : return events;
1799 0 : }
1800 :
1801 : // peek for a menu event filtered by event type, this makes no changes to the outbound queue
1802 : // called from lua
1803 0 : uint8_t AP_CRSF_Telem::peek_menu_event(uint8_t& param_id, ScriptedPayload& payload, uint8_t& events)
1804 : {
1805 0 : WITH_SEMAPHORE(scr_sem);
1806 :
1807 : ScriptedParameterWrite spw;
1808 0 : if (!inbound_params.peek(spw)) {
1809 0 : events = 0;
1810 0 : param_id = 0;
1811 0 : payload.payload_length = 0;
1812 0 : return 0;
1813 : }
1814 :
1815 0 : switch (spw.type) {
1816 0 : case ScriptedParameterEvents::PARAMETER_WRITE:
1817 0 : payload = spw.payload;
1818 0 : break;
1819 0 : case ScriptedParameterEvents::PARAMETER_READ:
1820 0 : payload.payload_length = 0;
1821 0 : break;
1822 0 : default:
1823 0 : break;
1824 : }
1825 0 : events = spw.type;
1826 0 : param_id = spw.param->id;
1827 :
1828 : debug("peek_menu_event(%u) -> %s, size: %u", param_id, events == ScriptedParameterEvents::PARAMETER_WRITE ? "WRITE" : "READ", payload.payload_length);
1829 :
1830 0 : return inbound_params.available();
1831 0 : }
1832 :
1833 : // pop a menu event from the inbound queue and add to the outbound queue for processing via lua
1834 : // called from lua
1835 0 : void AP_CRSF_Telem::pop_menu_event()
1836 : {
1837 : ScriptedParameterWrite spw;
1838 : // Pop the event from the queue. We must check if it's empty in case of a race condition
1839 : // where another script handled the event between our peek and pop.
1840 0 : if (inbound_params.pop(spw)) {
1841 : debug("pop_menu_event(%u) -> %s", spw.param->id, spw.type == ScriptedParameterEvents::PARAMETER_WRITE ? "WRITE" : "READ");
1842 : // respond with parameter entry
1843 0 : outbound_params.push(spw);
1844 : } else {
1845 : debug("pop_menu_event(): EMPTY");
1846 : }
1847 0 : }
1848 :
1849 : // send a new response from the first item in the outbound queue
1850 : // called from lua
1851 0 : bool AP_CRSF_Telem::send_write_response(uint8_t length, const char* data)
1852 : {
1853 0 : WITH_SEMAPHORE(scr_sem);
1854 :
1855 : ScriptedParameterWrite spw;
1856 0 : if (!outbound_params.pop(spw)) {
1857 : // another script might have handled it
1858 0 : return false;
1859 : }
1860 :
1861 0 : switch (spw.type) {
1862 0 : case ScriptedParameterEvents::PARAMETER_READ:
1863 : // For a READ response, Lua provides the full data payload to be sent.
1864 : // We update the parameter's data buffer directly.
1865 0 : if (length > spw.param->length) {
1866 0 : spw.param->data = (const char*)mem_realloc((char*)spw.param->data, 0, length);
1867 : }
1868 0 : if (spw.param->data == nullptr) {
1869 : // failed to realloc
1870 0 : return false;
1871 : }
1872 0 : memcpy((char*)spw.param->data, data, length);
1873 0 : spw.param->length = length;
1874 0 : break;
1875 0 : case ScriptedParameterEvents::PARAMETER_WRITE:
1876 : // For a WRITE response, Lua provides the data for the response packet.
1877 : // This is stored in the payload field of the queued request.
1878 0 : memcpy(spw.payload.payload, data, MIN(length, sizeof(spw.payload.payload)));
1879 0 : spw.payload.payload_length = MIN(length, sizeof(spw.payload.payload));
1880 0 : break;
1881 : }
1882 :
1883 : // Update the object in the queue with the new data
1884 0 : ready_params.push(spw);
1885 :
1886 : // The scheduler will now be triggered by the is_packet_ready() check on ready_params
1887 : // and process this request in calc_parameter().
1888 0 : return true;
1889 0 : }
1890 :
1891 : // send a generic response from the first item in the outbound queue
1892 : // called from lua
1893 0 : bool AP_CRSF_Telem::send_response()
1894 : {
1895 : ScriptedParameterWrite spw;
1896 0 : if (outbound_params.pop(spw)) {
1897 : // Update the object in the queue with the new data
1898 0 : ready_params.push(spw);
1899 0 : return true;
1900 : }
1901 0 : return false;
1902 : }
1903 :
1904 0 : AP_CRSF_Telem::ScriptedParameter* AP_CRSF_Telem::ScriptedMenu::find_parameter(uint8_t param_num)
1905 : {
1906 : // This function is called on the root, so loop through all menus.
1907 0 : for (ScriptedMenu* m = next_menu; m != nullptr; m = m->next_menu) {
1908 : // Search all parameters within the current menu 'm'.
1909 0 : for (uint8_t i = 0; i < m->num_params; i++) {
1910 0 : if (m->params[i].id == param_num) {
1911 : // Found the parameter (or menu link).
1912 0 : return &m->params[i];
1913 : }
1914 : }
1915 : }
1916 :
1917 : // Parameter was not found in any menu.
1918 0 : return nullptr;
1919 : }
1920 :
1921 : // called from lua
1922 0 : AP_CRSF_Telem::ScriptedParameter* AP_CRSF_Telem::ScriptedMenu::add_parameter(uint8_t length, const char* data)
1923 : {
1924 0 : WITH_SEMAPHORE(AP::crsf_telem()->get_semaphore());
1925 :
1926 0 : ScriptedParameter* new_params = (ScriptedParameter*)mem_realloc(params,
1927 0 : sizeof(ScriptedParameter) * num_params,
1928 0 : sizeof(ScriptedParameter) * (num_params+1));
1929 0 : if (new_params == nullptr) {
1930 0 : return nullptr;
1931 : }
1932 0 : num_params++;
1933 0 : params = new_params;
1934 0 : ScriptedParameter& param = params[num_params-1];
1935 0 : param.id = id + num_params;
1936 0 : param.parent_id = id;
1937 0 : param.length = length;
1938 0 : if (length > 0) {
1939 0 : param.data = (const char*)malloc(length);
1940 :
1941 0 : if (param.data == nullptr) {
1942 0 : num_params--;
1943 0 : return nullptr;
1944 : }
1945 :
1946 0 : memcpy((char*)param.data, data, length);
1947 : }
1948 : debug("adding menu item %d", param.id);
1949 :
1950 0 : return ¶m;
1951 0 : }
1952 :
1953 0 : AP_CRSF_Telem::ScriptedMenu* AP_CRSF_Telem::ScriptedMenu::find_menu(uint8_t param_num)
1954 : {
1955 : // find the parameter to write
1956 0 : for (ScriptedMenu* m = next_menu; m != nullptr; m = m->next_menu) {
1957 0 : if (m->id == param_num) {
1958 0 : return m;
1959 : }
1960 : }
1961 0 : return nullptr;
1962 : }
1963 :
1964 0 : bool AP_CRSF_Telem::ScriptedMenu::remove_menu(uint8_t param_num)
1965 : {
1966 0 : WITH_SEMAPHORE(AP::crsf_telem()->get_semaphore());
1967 :
1968 : // find the parameter to write
1969 0 : ScriptedMenu* prev = this;
1970 0 : for (ScriptedMenu* m = next_menu; m != nullptr; m = m->next_menu) {
1971 0 : if (m->id == param_num) {
1972 0 : prev->next_menu = m->next_menu;
1973 0 : delete m;
1974 0 : return true;
1975 : }
1976 : }
1977 0 : return false;
1978 0 : }
1979 :
1980 0 : AP_CRSF_Telem::ScriptedMenu::~ScriptedMenu()
1981 : {
1982 0 : if (params != nullptr) {
1983 0 : for (uint8_t i = 0; i < num_params; i++) {
1984 0 : free((char*)params[i].data);
1985 : }
1986 0 : free(params);
1987 : }
1988 0 : if (name != nullptr) {
1989 0 : free((char*)name);
1990 : }
1991 0 : }
1992 :
1993 : // add a scripted sub-menu to this menu
1994 : // called from lua
1995 0 : AP_CRSF_Telem::ScriptedMenu* AP_CRSF_Telem::ScriptedMenu::add_menu(const char* menu_name, uint8_t size, uint8_t parent_menu)
1996 : {
1997 0 : WITH_SEMAPHORE(AP::crsf_telem()->get_semaphore());
1998 :
1999 : // find the menu to create
2000 0 : ScriptedMenu* tail = this;
2001 0 : for (; tail->next_menu != nullptr; tail = tail->next_menu) {
2002 : }
2003 :
2004 0 : ScriptedMenu* menu = NEW_NOTHROW ScriptedMenu(menu_name, size, parent_menu);
2005 0 : if (menu == nullptr) {
2006 0 : return nullptr;
2007 : }
2008 :
2009 0 : menu->id = tail->id == 0 ? SCRIPTED_MENU_START_ID : tail->id + MAX_SCRIPTED_MENU_SIZE + 1;
2010 0 : tail->next_menu = menu;
2011 :
2012 : // dummy parameter for a submenu
2013 0 : if (parent_menu != 0) {
2014 0 : ScriptedParameter* param = add_parameter(0, nullptr);
2015 0 : param->id = menu->id;
2016 : }
2017 0 : return menu;
2018 0 : }
2019 :
2020 0 : AP_CRSF_Telem::ScriptedMenu::ScriptedMenu(const char* menu_name, uint8_t size, uint8_t parent_menu)
2021 : {
2022 0 : parent_id = parent_menu;
2023 0 : name = (const char*)malloc(MAX_SCRIPTED_MENU_NAME_LEN+1);
2024 0 : strncpy((char*)name, menu_name, MAX_SCRIPTED_MENU_NAME_LEN);
2025 : // num_params = size;
2026 0 : params = (ScriptedParameter*)calloc(size, sizeof(ScriptedParameter));
2027 0 : }
2028 :
2029 0 : bool AP_CRSF_Telem::process_scripted_param_write(ParameterSettingsWriteFrame* write_frame, uint8_t length)
2030 : {
2031 0 : if (write_frame->param_num < SCRIPTED_MENU_START_ID) {
2032 0 : return false;
2033 : }
2034 :
2035 0 : WITH_SEMAPHORE(scr_sem); // not called from lua, but interacting with data structures that might change
2036 :
2037 : // find the parameter to write
2038 0 : ScriptedParameter* param = scripted_menus.find_parameter(write_frame->param_num);
2039 :
2040 0 : if (param == nullptr || length < 3) {
2041 0 : return false;
2042 : }
2043 0 : ScriptedParameterWrite spw {
2044 : ScriptedParameterEvents::PARAMETER_WRITE,
2045 0 : write_frame->destination,
2046 0 : write_frame->origin,
2047 0 : write_frame->param_num,
2048 : 0,
2049 : param
2050 0 : };
2051 :
2052 0 : spw.payload.payload_length = uint8_t(length - 3U); // remove destination, origin and param number from payload length
2053 0 : memcpy(spw.payload.payload, write_frame->payload, spw.payload.payload_length);
2054 0 : inbound_params.push(spw); // payload size in frame
2055 :
2056 0 : return true;
2057 0 : }
2058 :
2059 0 : bool AP_CRSF_Telem::process_scripted_param_read(ParameterSettingsReadFrame* read_frame)
2060 : {
2061 0 : if (read_frame->param_num < SCRIPTED_MENU_START_ID) {
2062 0 : return false;
2063 : }
2064 :
2065 0 : WITH_SEMAPHORE(scr_sem); // not called from lua, but interacting with data structures that might change
2066 :
2067 0 : if (scripted_menus.find_menu(read_frame->param_num) != nullptr) {
2068 : // This is a request for a menu folder, not a leaf parameter.
2069 : // The exception is if it's a multi-chunk read for a parameter that happens
2070 : // to have the same ID as a menu (which is the case for submenu links).
2071 : // If it's a chunked read, we must handle it as a scripted param read.
2072 0 : if (read_frame->param_chunk == 0) {
2073 0 : return false;
2074 : }
2075 : }
2076 :
2077 : // First, check if the requested ID corresponds to a parameter link.
2078 0 : ScriptedParameter* param = scripted_menus.find_parameter(read_frame->param_num);
2079 :
2080 0 : if (param != nullptr) {
2081 : // We found a parameter. NOW, we must check if this parameter's ID
2082 : // ALSO corresponds to a menu object. If it does, it's a sub-menu link.
2083 0 : if (scripted_menus.find_menu(read_frame->param_num) != nullptr) {
2084 : // This is a sub-menu link. Return false to let the C++ 'calc_parameter'
2085 : // function handle it as a FOLDER request.
2086 0 : return false;
2087 : }
2088 :
2089 0 : ScriptedParameterWrite spw{
2090 : ScriptedParameterEvents::PARAMETER_READ,
2091 0 : read_frame->destination,
2092 0 : read_frame->origin,
2093 0 : read_frame->param_num,
2094 0 : read_frame->param_chunk,
2095 : param
2096 0 : };
2097 :
2098 0 : if (read_frame->param_chunk > 0) {
2099 0 : ready_params.push(spw); // handle directly - no need to involve lua
2100 : } else {
2101 : // This is a regular parameter (not a menu link). Handle it in Lua.
2102 0 : inbound_params.push(spw);
2103 : }
2104 0 : return true;
2105 : }
2106 :
2107 : // If it's not a parameter at all, check if it's a top-level menu request.
2108 0 : if (scripted_menus.find_menu(read_frame->param_num) != nullptr) {
2109 0 : return false; // Delegate to C++ handler.
2110 : }
2111 :
2112 0 : return false;
2113 0 : }
2114 :
2115 : // add a scripted top-level menu
2116 : // called from lua
2117 0 : AP_CRSF_Telem::ScriptedMenu* AP_CRSF_Telem::add_menu(const char* name)
2118 : {
2119 0 : WITH_SEMAPHORE(scr_sem);
2120 :
2121 : // create a new menu
2122 0 : ScriptedMenu* new_menu = scripted_menus.add_menu(name, 2, 0);
2123 : debug("adding menu %d", new_menu->id);
2124 0 : return new_menu;
2125 0 : }
2126 :
2127 : // called from lua
2128 56 : void AP_CRSF_Telem::clear_menus()
2129 : {
2130 56 : WITH_SEMAPHORE(scr_sem);
2131 :
2132 : #ifdef CRSF_DEBUG
2133 : dump_menu_structure();
2134 : #endif
2135 56 : for (ScriptedMenu* m = scripted_menus.next_menu; m != nullptr; ) {
2136 0 : ScriptedMenu* next = m->next_menu;
2137 0 : delete m;
2138 0 : m = next;
2139 : }
2140 56 : scripted_menus.next_menu = nullptr;
2141 56 : }
2142 :
2143 : #ifdef CRSF_DEBUG
2144 : void AP_CRSF_Telem::dump_menu_structure()
2145 : {
2146 : for (ScriptedMenu* m = scripted_menus.next_menu; m != nullptr; m = m->next_menu) {
2147 : if (m->parent_id == 0) {
2148 : m->dump_structure(0);
2149 : }
2150 : }
2151 : }
2152 :
2153 : void AP_CRSF_Telem::ScriptedMenu::dump_structure(uint8_t indent)
2154 : {
2155 : char indent_str[] = " ";
2156 : indent_str[indent] = '\0';
2157 : hal.console->printf("%s[\'%s\': id: %d, parent: %d\n", indent_str, name, id, parent_id);
2158 : for (uint8_t i = 0; i<num_params; i++) {
2159 : if (params[i].length == 0) {
2160 : ScriptedMenu* menu = find_menu(params[i].id);
2161 : if (menu != nullptr) {
2162 : menu->dump_structure(indent+1);
2163 : } else {
2164 : hal.console->printf("No menu %d!\n", params[i].id);
2165 : }
2166 : } else {
2167 : hal.console->printf("%s \'%s\': id:%d, parent:%d, len:%d\n", indent_str, ¶ms[i].data[1], params[i].id, params[i].parent_id, params[i].length);
2168 : }
2169 : }
2170 : hal.console->printf("%s]\n", indent_str);
2171 : }
2172 :
2173 : #endif
2174 :
2175 : #endif //AP_CRSF_SCRIPTING_ENABLED
2176 :
2177 : // get status text data
2178 0 : void AP_CRSF_Telem::calc_status_text()
2179 : {
2180 0 : if (!_statustext.available) {
2181 0 : WITH_SEMAPHORE(_statustext.sem);
2182 : // check link speed
2183 0 : if (_crsf_version.protocol != AP_CRSF_Protocol::ProtocolType::PROTOCOL_ELRS
2184 0 : && !is_high_speed_telemetry(_telem_rf_mode)) {
2185 : // keep only warning/error/critical/alert/emergency status text messages
2186 0 : bool got_message = false;
2187 0 : while (_statustext.queue.pop(_statustext.next)) {
2188 0 : if (_statustext.next.severity <= MAV_SEVERITY_WARNING) {
2189 0 : got_message = true;
2190 0 : break;
2191 : }
2192 : }
2193 0 : if (!got_message) {
2194 0 : return;
2195 : }
2196 0 : } else if (!_statustext.queue.pop(_statustext.next)) {
2197 0 : return;
2198 : }
2199 0 : _statustext.available = true;
2200 0 : }
2201 :
2202 0 : _telem_type = get_custom_telem_frame_id();
2203 0 : _telem.bcast.custom_telem.status_text.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_STATUS_TEXT;
2204 0 : _telem.bcast.custom_telem.status_text.severity = _statustext.next.severity;
2205 : // Note: snprintf() always terminates the string
2206 0 : hal.util->snprintf(_telem.bcast.custom_telem.status_text.text, AP_CRSF_Telem::PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE, "%s", _statustext.next.text);
2207 : // frame size = sub_type(1) + severity(1) + strlen(text) + terminator
2208 : // Note: strlen(_telem.bcast.custom_telem.status_text.text) is safe because called on a guaranteed null terminated string
2209 0 : _telem_size = 2 + strlen(_telem.bcast.custom_telem.status_text.text) + 1;
2210 0 : _telem_pending = true;
2211 0 : _statustext.available = false;
2212 : }
2213 :
2214 : /*
2215 : Get 1 packet of passthrough telemetry data
2216 : */
2217 0 : void AP_CRSF_Telem::get_single_packet_passthrough_telem_data()
2218 : {
2219 0 : _telem_pending = false;
2220 : uint8_t packet_count;
2221 : AP_Frsky_SPort::sport_packet_t packet;
2222 0 : if (!AP_Frsky_Telem::get_telem_data(&packet, packet_count, 1)) {
2223 0 : return;
2224 : }
2225 0 : _telem.bcast.custom_telem.single_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH;
2226 0 : _telem.bcast.custom_telem.single_packet_passthrough.appid = packet.appid;
2227 0 : _telem.bcast.custom_telem.single_packet_passthrough.data = packet.data;
2228 0 : _telem_size = sizeof(AP_CRSF_Telem::PassthroughSinglePacketFrame);
2229 0 : _telem_type = get_custom_telem_frame_id();
2230 0 : _telem_pending = true;
2231 : }
2232 :
2233 : /*
2234 : Get up to PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packets of passthrough telemetry data (for slow links)
2235 : Note: we have 2 distinct frame types (single packet vs multi packet) because
2236 : whenever possible we use smaller frames for they have a higher "chance"
2237 : of being transmitted by the crossfire RX scheduler.
2238 : */
2239 0 : void AP_CRSF_Telem::get_multi_packet_passthrough_telem_data(uint8_t size)
2240 : {
2241 0 : size = MIN(size, AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE);
2242 0 : _telem_pending = false;
2243 0 : uint8_t count = 0;
2244 0 : AP_Frsky_SPort::sport_packet_t buffer[AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE] {};
2245 : // we request a PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packet array, i.e. 9 packets
2246 0 : if (!AP_Frsky_Telem::get_telem_data(buffer, count, size)) {
2247 0 : return;
2248 : }
2249 0 : _telem.bcast.custom_telem.multi_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH;
2250 0 : for (uint8_t idx=0; idx<count; idx++) {
2251 0 : _telem.bcast.custom_telem.multi_packet_passthrough.packets[idx].appid = buffer[idx].appid;
2252 0 : _telem.bcast.custom_telem.multi_packet_passthrough.packets[idx].data = buffer[idx].data;
2253 : }
2254 0 : _telem.bcast.custom_telem.multi_packet_passthrough.size = count;
2255 0 : _telem_size = 2 + sizeof(AP_CRSF_Telem::PassthroughMultiPacketFrame::PassthroughTelemetryPacket)*count; //subtype + size + 6*count
2256 0 : _telem_type = get_custom_telem_frame_id();
2257 0 : _telem_pending = true;
2258 : }
2259 :
2260 : /*
2261 : fetch CRSF frame data
2262 : if is_tx_active is true then this will be a request for telemetry after receiving an RC frame
2263 : */
2264 0 : bool AP_CRSF_Telem::_get_telem_data(AP_RCProtocol_CRSF::Frame* data, bool is_tx_active)
2265 : {
2266 0 : memset(&_telem, 0, sizeof(TelemetryPayload));
2267 : // update telemetry tasks if we either lost or regained the transmitter
2268 0 : if (_is_tx_active != is_tx_active) {
2269 0 : if (is_tx_active) {
2270 0 : disable_scheduler_entry(DEVICE_PING);
2271 0 : enable_tx_entries();
2272 : } else {
2273 0 : disable_tx_entries();
2274 0 : enable_scheduler_entry(DEVICE_PING);
2275 : }
2276 0 : _is_tx_active = is_tx_active;
2277 : }
2278 :
2279 0 : run_wfq_scheduler();
2280 0 : if (!_telem_pending) {
2281 0 : return false;
2282 : }
2283 0 : memcpy(data->payload, &_telem, _telem_size);
2284 0 : data->device_address = AP_CRSF_Protocol::CRSF_ADDRESS_FLIGHT_CONTROLLER; // sync byte
2285 0 : data->length = _telem_size + 2;
2286 0 : data->type = _telem_type;
2287 :
2288 0 : _telem_pending = false;
2289 0 : return true;
2290 : }
2291 :
2292 : /*
2293 : fetch data for an external transport, such as CRSF
2294 : */
2295 0 : bool AP_CRSF_Telem::process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data, uint8_t length)
2296 : {
2297 0 : if (!get_singleton()) {
2298 0 : return false;
2299 : }
2300 0 : return singleton->_process_frame(frame_type, data, length);
2301 : }
2302 :
2303 : /*
2304 : fetch data for an external transport, such as CRSF
2305 : */
2306 0 : bool AP_CRSF_Telem::get_telem_data(AP_RCProtocol_CRSF::Frame* data, bool is_tx_active)
2307 : {
2308 0 : if (!get_singleton()) {
2309 0 : return false;
2310 : }
2311 0 : return singleton->_get_telem_data(data, is_tx_active);
2312 : }
2313 :
2314 36440478 : AP_CRSF_Telem *AP_CRSF_Telem::get_singleton(void) {
2315 36440478 : if (!singleton && !hal.util->get_soft_armed()) {
2316 : // if telem data is requested when we are disarmed and don't
2317 : // yet have a AP_CRSF_Telem object then try to allocate one
2318 2096 : NEW_NOTHROW AP_CRSF_Telem();
2319 : // initialize the passthrough scheduler
2320 2096 : if (singleton) {
2321 2096 : singleton->init();
2322 : }
2323 : }
2324 36440478 : return singleton;
2325 : }
2326 :
2327 : namespace AP {
2328 36440478 : AP_CRSF_Telem *crsf_telem() {
2329 36440478 : return AP_CRSF_Telem::get_singleton();
2330 : }
2331 : };
2332 :
2333 : #endif // HAL_CRSF_TELEM_ENABLED
2334 :
2335 :
|
