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 : #include <AP_AHRS/AP_AHRS.h>
17 : #include <AP_Baro/AP_Baro.h>
18 : #include <AP_Airspeed/AP_Airspeed.h>
19 : #include <AP_BattMonitor/AP_BattMonitor.h>
20 : #include <AP_Compass/AP_Compass.h>
21 : #include <AP_ESC_Telem/AP_ESC_Telem.h>
22 : #include <RC_Channel/RC_Channel.h>
23 : #include <AP_Common/AP_FWVersion.h>
24 : #include <AP_GPS/AP_GPS.h>
25 : #include <AP_Notify/AP_Notify.h>
26 : #include <AP_OpticalFlow/AP_OpticalFlow.h>
27 : #include <AP_RangeFinder/AP_RangeFinder.h>
28 : #include <AP_RSSI/AP_RSSI.h>
29 : #include <AP_RTC/AP_RTC.h>
30 : #include <AP_VideoTX/AP_VideoTX.h>
31 : #include <GCS_MAVLink/GCS.h>
32 : #include <AP_Vehicle/AP_Vehicle.h>
33 :
34 : #include "AP_MSP.h"
35 : #include "AP_MSP_Telem_Backend.h"
36 :
37 : #include <ctype.h>
38 : #include <stdio.h>
39 :
40 : #if HAL_MSP_ENABLED
41 :
42 : extern const AP_HAL::HAL& hal;
43 : constexpr uint8_t AP_MSP_Telem_Backend::arrows[8];
44 :
45 : using namespace MSP;
46 :
47 6 : AP_MSP_Telem_Backend::AP_MSP_Telem_Backend(AP_HAL::UARTDriver *uart) : AP_RCTelemetry(MSP_TIME_SLOT_MAX)
48 : {
49 6 : _msp_port.uart = uart;
50 6 : }
51 :
52 : /*
53 : Scheduler helper
54 : */
55 6 : void AP_MSP_Telem_Backend::setup_wfq_scheduler(void)
56 : {
57 : // initialize packet weights for the WFQ scheduler
58 : // priority[i] = 1/_scheduler.packet_weight[i]
59 : // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) )
60 :
61 6 : set_scheduler_entry(EMPTY_SLOT, 50, 50); // nothing to send
62 6 : set_scheduler_entry(NAME, 200, 200); // 5Hz 12 chars string used for general purpose text messages
63 6 : set_scheduler_entry(STATUS, 500, 500); // 2Hz flightmode
64 6 : set_scheduler_entry(CONFIG, 200, 200); // 5Hz OSD item positions
65 6 : set_scheduler_entry(RAW_GPS, 250, 250); // 4Hz GPS lat/lon
66 6 : set_scheduler_entry(COMP_GPS, 250, 250); // 4Hz home direction and distance
67 6 : set_scheduler_entry(ATTITUDE, 200, 200); // 5Hz attitude
68 6 : set_scheduler_entry(ALTITUDE, 250, 250); // 4Hz altitude(cm) and velocity(cm/s)
69 6 : set_scheduler_entry(ANALOG, 250, 250); // 4Hz rssi + batt
70 : #if AP_BATTERY_ENABLED
71 6 : set_scheduler_entry(BATTERY_STATE, 500, 500); // 2Hz battery
72 : #endif
73 : #if HAL_WITH_ESC_TELEM
74 6 : set_scheduler_entry(ESC_SENSOR_DATA, 500, 500); // 2Hz ESC telemetry
75 : #endif
76 6 : set_scheduler_entry(RTC_DATETIME, 1000, 1000); // 1Hz RTC
77 : #if AP_MSP_VIDEOTX_ENABLED
78 6 : set_scheduler_entry(VTX_PARAMETERS, 1000, 1000); // 1Hz VTX
79 : #endif
80 6 : }
81 :
82 : /*
83 : * init - perform required initialisation
84 : */
85 6 : bool AP_MSP_Telem_Backend::init()
86 : {
87 6 : enable_warnings();
88 6 : return AP_RCTelemetry::init();
89 : }
90 :
91 3 : bool AP_MSP_Telem_Backend::init_uart()
92 : {
93 3 : if (_msp_port.uart != nullptr) {
94 : // re-init port here for use in this thread
95 3 : _msp_port.uart->begin(0);
96 3 : return true;
97 : }
98 0 : return false;
99 : }
100 :
101 9158 : void AP_MSP_Telem_Backend::process_outgoing_data()
102 : {
103 9158 : if (is_scheduler_enabled()) {
104 2373 : AP_RCTelemetry::run_wfq_scheduler();
105 : }
106 9158 : }
107 :
108 : /*
109 : Scheduler helper
110 : */
111 3116 : bool AP_MSP_Telem_Backend::is_packet_ready(uint8_t idx, bool queue_empty)
112 : {
113 3116 : switch (idx) {
114 1368 : case EMPTY_SLOT: // empty slot
115 : case NAME: // used for status_text messages
116 : case STATUS: // flightmode
117 : case CONFIG: // OSD config
118 : case RAW_GPS: // lat,lon, speed
119 : case COMP_GPS: // home dir,dist
120 : case ATTITUDE: // Attitude
121 : case ALTITUDE: // Altitude and Vario
122 : case ANALOG: // Rssi, Battery, mAh, Current
123 : #if AP_BATTERY_ENABLED
124 : case BATTERY_STATE: // voltage, capacity, current, mAh
125 : #endif
126 : #if HAL_WITH_ESC_TELEM
127 : case ESC_SENSOR_DATA: // esc temp + rpm
128 : #endif
129 : case RTC_DATETIME: // RTC
130 1368 : return true;
131 : #if AP_MSP_VIDEOTX_ENABLED
132 1748 : case VTX_PARAMETERS: // VTX control
133 1748 : return AP::vtx().have_params_changed();
134 : #endif
135 0 : default:
136 0 : return false;
137 : }
138 : }
139 :
140 : /*
141 : Invoked at each scheduler step
142 : */
143 1232 : void AP_MSP_Telem_Backend::process_packet(uint8_t idx)
144 : {
145 1232 : if (idx == EMPTY_SLOT) {
146 436 : return;
147 : }
148 :
149 : #if AP_MSP_VIDEOTX_ENABLED
150 : // the VTX config is pushed on change rather than every tick, and only once
151 : // the air unit has completed its boot handshake
152 849 : if (msp_packet_type_map[idx] == MSP_VTX_CONFIG && !vtx_should_push_config()) {
153 53 : return;
154 : }
155 : #endif
156 :
157 796 : uint8_t out_buf[MSP_PORT_OUTBUF_SIZE] {};
158 :
159 796 : msp_packet_t reply = {
160 : .buf = { .ptr = out_buf, .end = MSP_ARRAYEND(out_buf), },
161 796 : .cmd = (int16_t)msp_packet_type_map[idx],
162 : .flags = 0,
163 : .result = 0,
164 796 : };
165 796 : uint8_t *out_buf_head = reply.buf.ptr;
166 :
167 796 : msp_process_out_command(msp_packet_type_map[idx], nullptr, &reply.buf);
168 796 : uint32_t len = reply.buf.ptr - &out_buf[0];
169 796 : sbuf_switch_to_reader(&reply.buf, out_buf_head); // change streambuf direction
170 :
171 : // the negotiated version is only known once the peer has sent us something;
172 : // betaflight pushes the VTX config as MSP v2 native, so match that for an
173 : // MSP VTX which otherwise ignores the unsolicited v1 frame
174 796 : msp_version_e version = _msp_port.msp_version;
175 : #if AP_MSP_VIDEOTX_ENABLED
176 796 : if (msp_packet_type_map[idx] == MSP_VTX_CONFIG) {
177 0 : version = MSP_V2_NATIVE;
178 : }
179 : #endif
180 796 : if (len > 0) {
181 : // don't send zero length packets
182 796 : msp_serial_encode(&_msp_port, &reply, version);
183 : }
184 :
185 796 : _msp_port.c_state = MSP_IDLE;
186 : }
187 :
188 : #if AP_BATTERY_ENABLED
189 126 : uint8_t AP_MSP_Telem_Backend::calc_cell_count(const float battery_voltage)
190 : {
191 126 : return floorf((battery_voltage / CELLFULL) + 1);
192 : }
193 : #endif
194 :
195 84 : float AP_MSP_Telem_Backend::get_vspeed_ms(void) const
196 : {
197 : {
198 : // release semaphore as soon as possible
199 84 : AP_AHRS &_ahrs = AP::ahrs();
200 84 : Vector3f v {};
201 84 : WITH_SEMAPHORE(_ahrs.get_semaphore());
202 84 : if (_ahrs.get_velocity_NED(v)) {
203 49 : return -v.z;
204 : }
205 84 : }
206 35 : AP_Baro &_baro = AP::baro();
207 35 : WITH_SEMAPHORE(_baro.get_semaphore());
208 35 : return _baro.get_climb_rate();
209 35 : }
210 :
211 4407 : void AP_MSP_Telem_Backend::update_home_pos(home_state_t &home_state)
212 : {
213 4407 : AP_AHRS &_ahrs = AP::ahrs();
214 4407 : WITH_SEMAPHORE(_ahrs.get_semaphore());
215 4407 : Location loc;
216 : float alt;
217 4407 : if (_ahrs.get_location(loc) && _ahrs.home_is_set()) {
218 2095 : const Location &home_loc = _ahrs.get_home();
219 2095 : home_state.home_distance_m = home_loc.get_distance(loc);
220 2095 : home_state.home_bearing_cd = loc.get_bearing_to(home_loc);
221 : } else {
222 2312 : home_state.home_distance_m = 0;
223 2312 : home_state.home_bearing_cd = 0;
224 : }
225 4407 : _ahrs.get_relative_position_D_home(alt);
226 4407 : home_state.rel_altitude_cm = -alt * 100;
227 4407 : home_state.home_is_set = _ahrs.home_is_set();
228 4407 : }
229 :
230 : #if AP_GPS_ENABLED
231 4323 : void AP_MSP_Telem_Backend::update_gps_state(gps_state_t &gps_state)
232 : {
233 4323 : AP_GPS& gps = AP::gps();
234 :
235 4323 : memset(&gps_state, 0, sizeof(gps_state));
236 :
237 4323 : WITH_SEMAPHORE(gps.get_semaphore());
238 4323 : gps_state.fix_type = gps.status() >= AP_GPS_FixType::FIX_3D? 2:0;
239 4323 : gps_state.num_sats = gps.num_sats();
240 :
241 4323 : if (gps_state.fix_type > 0) {
242 3112 : const Location &loc = AP::gps().location(); //get gps instance 0
243 3112 : gps_state.lat = loc.lat;
244 3112 : gps_state.lon = loc.lng;
245 3112 : gps_state.alt_m = loc.alt/100; // 1m resolution
246 3112 : gps_state.speed_cms = gps.ground_speed() * 100;
247 3112 : gps_state.ground_course_dd = gps.ground_course_cd() / 10;
248 : }
249 4323 : }
250 : #endif
251 :
252 : #if AP_BATTERY_ENABLED
253 126 : void AP_MSP_Telem_Backend::update_battery_state(battery_state_t &battery_state)
254 : {
255 126 : memset(&battery_state, 0, sizeof(battery_state));
256 :
257 126 : const AP_BattMonitor &_battery = AP::battery();
258 126 : if (!_battery.current_amps(battery_state.batt_current_a)) {
259 0 : battery_state.batt_current_a = 0;
260 : }
261 126 : if (!_battery.consumed_mah(battery_state.batt_consumed_mah)) {
262 0 : battery_state.batt_consumed_mah = 0;
263 : }
264 126 : battery_state.batt_voltage_v =_battery.voltage();
265 126 : battery_state.batt_capacity_mah = _battery.pack_capacity_mah();
266 :
267 126 : const AP_Notify& notify = AP::notify();
268 126 : if (notify.flags.failsafe_battery) {
269 0 : battery_state.batt_state = MSP_BATTERY_CRITICAL;
270 : } else {
271 126 : battery_state.batt_state = MSP_BATTERY_OK;
272 : }
273 : // detect cellcount and update only if we get a higher values, we do not want to update it while discharging
274 126 : uint8_t cc = calc_cell_count(battery_state.batt_voltage_v);
275 126 : if (cc > battery_state.batt_cellcount) {
276 126 : battery_state.batt_cellcount = cc;
277 : }
278 126 : }
279 : #endif // AP_BATTERY_ENABLED
280 :
281 0 : void AP_MSP_Telem_Backend::update_airspeed(airspeed_state_t &airspeed_state)
282 : {
283 0 : AP_AHRS &ahrs = AP::ahrs();
284 0 : WITH_SEMAPHORE(ahrs.get_semaphore());
285 0 : airspeed_state.airspeed_have_estimate = ahrs.airspeed_EAS(airspeed_state.airspeed_estimate_ms);
286 0 : if (!airspeed_state.airspeed_have_estimate) {
287 0 : airspeed_state.airspeed_estimate_ms = 0.0;
288 : }
289 0 : }
290 :
291 : /*
292 : MSP OSDs can display up to MSP_TXT_VISIBLE_CHARS chars (UTF8 characters are supported)
293 : We display the flight mode string either with or without wind state
294 : */
295 17 : void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, uint8_t size, bool wind_enabled)
296 : {
297 : #if OSD_ENABLED
298 17 : AP_OSD *osd = AP::osd();
299 17 : if (osd == nullptr) {
300 0 : return;
301 : }
302 : #endif
303 17 : AP_Notify *notify = AP_Notify::get_singleton();
304 17 : if (notify == nullptr) {
305 0 : return;
306 : }
307 : // clear
308 17 : memset(flight_mode_str, 0, size);
309 :
310 17 : if (wind_enabled) {
311 : /*
312 : Wind is rendered next to the current flight mode, for the direction we use an UTF8 arrow (bytes 0xE286[nn])
313 : example: MANU 4m/s ↗
314 : */
315 0 : AP_AHRS &ahrs = AP::ahrs();
316 0 : Vector3f v;
317 : {
318 0 : WITH_SEMAPHORE(ahrs.get_semaphore());
319 : // use the estimate even if it is not marked valid, to
320 : // preserve existing behaviour
321 0 : IGNORE_RETURN(ahrs.get_wind(v));
322 0 : }
323 0 : bool invert_wind = false;
324 : #if OSD_ENABLED
325 0 : AP_MSP *msp = AP::msp();
326 0 : if (msp == nullptr) {
327 0 : return;
328 : }
329 :
330 0 : invert_wind = osd->screen[msp->_msp_status.current_screen].check_option(AP_OSD::OPTION_INVERTED_WIND);
331 : #endif
332 0 : if (invert_wind) {
333 0 : v = -v;
334 : }
335 0 : uint8_t units = OSD_UNIT_METRIC;
336 : #if OSD_ENABLED
337 0 : units = osd->units == AP_OSD::UNITS_IMPERIAL ? OSD_UNIT_IMPERIAL : OSD_UNIT_METRIC;
338 : #endif
339 : // if needed convert m/s to ft/s
340 0 : const float v_length = (units == OSD_UNIT_METRIC) ? v.length() : v.length() * 3.28084;
341 0 : const char* unit = (units == OSD_UNIT_METRIC) ? "m/s" : "f/s";
342 :
343 0 : if (v_length > 1.0f) {
344 : #if AP_VEHICLE_ENABLED
345 : float roll_rad, pitch_rad, yaw_rad;
346 0 : AP::vehicle()->get_osd_attitude_rad(roll_rad, pitch_rad, yaw_rad);
347 : #else
348 : float yaw_rad = ahrs.get_yaw_rad();
349 : #endif // AP_VEHICLE_ENABLED
350 :
351 0 : const int32_t angle = wrap_360_cd(rad_to_cd(atan2f(v.y, v.x)) - rad_to_cd(yaw_rad));
352 0 : const int32_t interval = 36000 / ARRAY_SIZE(arrows);
353 0 : uint8_t arrow = arrows[((angle + interval / 2) / interval) % ARRAY_SIZE(arrows)];
354 0 : snprintf(flight_mode_str, size, "%s %d%s%c%c%c", notify->get_flight_mode_str(), (uint8_t)roundf(v_length), unit, 0xE2, 0x86, arrow);
355 : } else {
356 0 : snprintf(flight_mode_str, size, "%s ---%s", notify->get_flight_mode_str(), unit);
357 : }
358 : } else {
359 : /*
360 : Flight mode is rendered with simple mode flags
361 : examples:
362 : MANU
363 : MANU [S]
364 : MANU [SS]
365 : */
366 : #if HAL_GCS_ENABLED
367 17 : const char* simple_mode_str = gcs().simple_input_active() ? " [S]" : (gcs().supersimple_input_active() ? " [SS]" : "");
368 17 : snprintf(flight_mode_str, size, "%s%s", notify->get_flight_mode_str(), simple_mode_str);
369 : #else
370 : snprintf(flight_mode_str, size, "%s", notify->get_flight_mode_str());
371 : #endif
372 : }
373 : }
374 :
375 6 : void AP_MSP_Telem_Backend::enable_warnings()
376 : {
377 6 : AP_MSP *msp = AP::msp();
378 6 : if (msp == nullptr) {
379 0 : return;
380 : }
381 6 : BIT_SET(msp->_osd_config.enabled_warnings, OSD_WARNING_FAIL_SAFE);
382 : #if AP_BATTERY_ENABLED
383 6 : BIT_SET(msp->_osd_config.enabled_warnings, OSD_WARNING_BATTERY_CRITICAL);
384 : #endif
385 : }
386 :
387 9158 : void AP_MSP_Telem_Backend::process_incoming_data()
388 : {
389 9158 : if (_msp_port.uart == nullptr) {
390 0 : return;
391 : }
392 :
393 9158 : uint32_t numc = MIN(_msp_port.uart->available(), 1024U);
394 :
395 9158 : if (numc > 0) {
396 : // Process incoming bytes
397 8217 : while (numc-- > 0) {
398 8116 : const uint8_t c = _msp_port.uart->read();
399 8116 : msp_parse_received_data(&_msp_port, c);
400 :
401 8116 : if (_msp_port.c_state == MSP_COMMAND_RECEIVED) {
402 217 : msp_process_received_command();
403 : }
404 : }
405 : }
406 : }
407 :
408 : /*
409 : send an MSP packet
410 : */
411 21035 : void AP_MSP_Telem_Backend::msp_send_packet(uint16_t cmd, MSP::msp_version_e msp_version, const void *p, uint16_t size, bool is_request)
412 : {
413 : uint8_t out_buf[MSP_PORT_OUTBUF_SIZE];
414 :
415 21035 : msp_packet_t pkt = {
416 : .buf = { .ptr = out_buf, .end = MSP_ARRAYEND(out_buf), },
417 21035 : .cmd = (int16_t)cmd,
418 : .flags = 0,
419 : .result = 0,
420 21035 : };
421 :
422 21035 : sbuf_write_data(&pkt.buf, p, size);
423 21035 : sbuf_switch_to_reader(&pkt.buf, &out_buf[0]);
424 21035 : msp_serial_encode(&_msp_port, &pkt, msp_version, is_request);
425 21035 : }
426 :
427 : /*
428 : ported from betaflight/src/main/msp/msp_serial.c
429 : */
430 217 : void AP_MSP_Telem_Backend::msp_process_received_command()
431 : {
432 : uint8_t out_buf[MSP_PORT_OUTBUF_SIZE];
433 :
434 217 : msp_packet_t reply = {
435 : .buf = { .ptr = out_buf, .end = MSP_ARRAYEND(out_buf), },
436 : .cmd = -1,
437 : .flags = 0,
438 : .result = 0,
439 217 : };
440 217 : uint8_t *out_buf_head = reply.buf.ptr;
441 :
442 217 : msp_packet_t command = {
443 217 : .buf = { .ptr = _msp_port.in_buf, .end = _msp_port.in_buf + _msp_port.data_size, },
444 217 : .cmd = (int16_t)_msp_port.cmd_msp,
445 217 : .flags = _msp_port.cmd_flags,
446 : .result = 0,
447 217 : };
448 :
449 217 : const MSPCommandResult status = msp_process_command(&command, &reply);
450 :
451 217 : if (status != MSP_RESULT_NO_REPLY) {
452 141 : sbuf_switch_to_reader(&reply.buf, out_buf_head); // change streambuf direction
453 141 : msp_serial_encode(&_msp_port, &reply, _msp_port.msp_version);
454 : }
455 :
456 217 : _msp_port.c_state = MSP_IDLE;
457 217 : }
458 :
459 : /*
460 : ported from inav/src/main/fc/fc_msp.c
461 : pull-mode telemetry
462 : */
463 217 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_command(msp_packet_t *cmd, msp_packet_t *reply)
464 : {
465 217 : MSPCommandResult ret = MSP_RESULT_ACK;
466 217 : sbuf_t *dst = &reply->buf;
467 217 : sbuf_t *src = &cmd->buf;
468 217 : const uint16_t cmd_msp = cmd->cmd;
469 : // initialize reply by default
470 217 : reply->cmd = cmd->cmd;
471 :
472 217 : if (MSP2_IS_SENSOR_MESSAGE(cmd_msp)) {
473 76 : ret = msp_process_sensor_command(cmd_msp, src);
474 : } else {
475 141 : ret = msp_process_out_command(cmd_msp, src, dst);
476 : }
477 :
478 : // Process DONT_REPLY flag
479 217 : if (cmd->flags & MSP_FLAG_DONT_REPLY) {
480 0 : ret = MSP_RESULT_NO_REPLY;
481 : }
482 :
483 217 : reply->result = ret;
484 217 : return ret;
485 : }
486 :
487 937 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_command(uint16_t cmd_msp, sbuf_t *src, sbuf_t *dst)
488 : {
489 937 : switch (cmd_msp) {
490 0 : case MSP_API_VERSION:
491 0 : return msp_process_out_api_version(dst);
492 0 : case MSP_FC_VARIANT:
493 0 : return msp_process_out_fc_variant(dst);
494 0 : case MSP_FC_VERSION:
495 0 : return msp_process_out_fc_version(dst);
496 0 : case MSP_BOARD_INFO:
497 0 : return msp_process_out_board_info(dst);
498 0 : case MSP_BUILD_INFO:
499 0 : return msp_process_out_build_info(dst);
500 104 : case MSP_NAME:
501 104 : return msp_process_out_name(dst);
502 104 : case MSP_OSD_CONFIG:
503 104 : return msp_process_out_osd_config(dst);
504 0 : case MSP_OSD_CANVAS:
505 0 : return msp_process_out_osd_canvas(dst);
506 42 : case MSP_STATUS:
507 : case MSP_STATUS_EX:
508 42 : return msp_process_out_status(dst);
509 84 : case MSP_RAW_GPS:
510 84 : return msp_process_out_raw_gps(dst);
511 84 : case MSP_COMP_GPS:
512 84 : return msp_process_out_comp_gps(dst);
513 105 : case MSP_ATTITUDE:
514 105 : return msp_process_out_attitude(dst);
515 84 : case MSP_ALTITUDE:
516 84 : return msp_process_out_altitude(dst);
517 84 : case MSP_ANALOG:
518 84 : return msp_process_out_analog(dst);
519 : #if AP_BATTERY_ENABLED
520 42 : case MSP_BATTERY_STATE:
521 42 : return msp_process_out_battery_state(dst);
522 : #endif
523 0 : case MSP_UID:
524 0 : return msp_process_out_uid(dst);
525 : #if HAL_WITH_ESC_TELEM
526 42 : case MSP_ESC_SENSOR_DATA:
527 42 : return msp_process_out_esc_sensor_data(dst);
528 : #endif
529 21 : case MSP_RTC:
530 21 : return msp_process_out_rtc(dst);
531 0 : case MSP_RC:
532 0 : return msp_process_out_rc(dst);
533 : #if AP_MSP_VIDEOTX_ENABLED
534 125 : case MSP_VTX_CONFIG:
535 : // reply with our config whether this is our proactive push (src == null)
536 : // or a GET request from the VTX/goggle (src != null), which polls it
537 125 : return msp_process_out_vtx_config(src, dst);
538 10 : case MSP_SET_VTX_CONFIG:
539 10 : return msp_process_in_vtx_config(src, dst);
540 6 : case MSP_SET_VTXTABLE_POWERLEVEL:
541 6 : return msp_process_in_vtxtable_powerlevel(src, dst);
542 : #endif // AP_MSP_VIDEOTX_ENABLED
543 0 : default:
544 : // MSP always requires an ACK even for unsupported messages
545 0 : return MSP_RESULT_ACK;
546 : }
547 : }
548 :
549 76 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_sensor_command(uint16_t cmd_msp, sbuf_t *src)
550 : {
551 : MSP_UNUSED(src);
552 :
553 76 : switch (cmd_msp) {
554 : #if HAL_MSP_RANGEFINDER_ENABLED
555 0 : case MSP2_SENSOR_RANGEFINDER: {
556 0 : const MSP::msp_rangefinder_data_message_t *pkt = (const MSP::msp_rangefinder_data_message_t *)src->ptr;
557 0 : msp_handle_rangefinder(*pkt);
558 : }
559 0 : break;
560 : #endif
561 : #if HAL_MSP_OPTICALFLOW_ENABLED
562 0 : case MSP2_SENSOR_OPTIC_FLOW: {
563 0 : const MSP::msp_opflow_data_message_t *pkt = (const MSP::msp_opflow_data_message_t *)src->ptr;
564 0 : msp_handle_opflow(*pkt);
565 : }
566 0 : break;
567 : #endif
568 : #if HAL_MSP_GPS_ENABLED
569 76 : case MSP2_SENSOR_GPS: {
570 76 : const MSP::msp_gps_data_message_t *pkt = (const MSP::msp_gps_data_message_t *)src->ptr;
571 76 : msp_handle_gps(*pkt);
572 : }
573 76 : break;
574 : #endif
575 : #if AP_COMPASS_MSP_ENABLED
576 0 : case MSP2_SENSOR_COMPASS: {
577 0 : const MSP::msp_compass_data_message_t *pkt = (const MSP::msp_compass_data_message_t *)src->ptr;
578 0 : msp_handle_compass(*pkt);
579 : }
580 0 : break;
581 : #endif
582 : #if AP_BARO_MSP_ENABLED
583 0 : case MSP2_SENSOR_BAROMETER: {
584 0 : const MSP::msp_baro_data_message_t *pkt = (const MSP::msp_baro_data_message_t *)src->ptr;
585 0 : msp_handle_baro(*pkt);
586 : }
587 0 : break;
588 : #endif
589 : #if AP_AIRSPEED_MSP_ENABLED && AP_AIRSPEED_ENABLED
590 0 : case MSP2_SENSOR_AIRSPEED: {
591 0 : const MSP::msp_airspeed_data_message_t *pkt = (const MSP::msp_airspeed_data_message_t *)src->ptr;
592 0 : msp_handle_airspeed(*pkt);
593 : }
594 0 : break;
595 : #endif
596 : }
597 :
598 76 : return MSP_RESULT_NO_REPLY;
599 : }
600 :
601 : #if HAL_MSP_OPTICALFLOW_ENABLED
602 0 : void AP_MSP_Telem_Backend::msp_handle_opflow(const MSP::msp_opflow_data_message_t &pkt)
603 : {
604 0 : AP_OpticalFlow *optflow = AP::opticalflow();
605 0 : if (optflow == nullptr) {
606 0 : return;
607 : }
608 0 : optflow->handle_msp(pkt);
609 : }
610 : #endif
611 :
612 : #if HAL_MSP_RANGEFINDER_ENABLED
613 0 : void AP_MSP_Telem_Backend::msp_handle_rangefinder(const MSP::msp_rangefinder_data_message_t &pkt)
614 : {
615 0 : RangeFinder *rangefinder = AP::rangefinder();
616 0 : if (rangefinder == nullptr) {
617 0 : return;
618 : }
619 0 : rangefinder->handle_msp(pkt);
620 : }
621 : #endif
622 :
623 : #if HAL_MSP_GPS_ENABLED
624 76 : void AP_MSP_Telem_Backend::msp_handle_gps(const MSP::msp_gps_data_message_t &pkt)
625 : {
626 76 : AP::gps().handle_msp(pkt);
627 76 : }
628 : #endif
629 :
630 : #if AP_COMPASS_MSP_ENABLED
631 0 : void AP_MSP_Telem_Backend::msp_handle_compass(const MSP::msp_compass_data_message_t &pkt)
632 : {
633 0 : AP::compass().handle_msp(pkt);
634 0 : }
635 : #endif
636 :
637 : #if AP_BARO_MSP_ENABLED
638 0 : void AP_MSP_Telem_Backend::msp_handle_baro(const MSP::msp_baro_data_message_t &pkt)
639 : {
640 0 : AP::baro().handle_msp(pkt);
641 0 : }
642 : #endif
643 :
644 : #if AP_AIRSPEED_MSP_ENABLED && AP_AIRSPEED_ENABLED
645 0 : void AP_MSP_Telem_Backend::msp_handle_airspeed(const MSP::msp_airspeed_data_message_t &pkt)
646 : {
647 0 : auto *airspeed = AP::airspeed();
648 0 : if (airspeed) {
649 0 : airspeed->handle_msp(pkt);
650 : }
651 0 : }
652 : #endif
653 :
654 42 : uint32_t AP_MSP_Telem_Backend::get_osd_flight_mode_bitmask(void)
655 : {
656 : // Note: we only set the BOXARM bit (bit 0) which is the same for BF, INAV and DJI VTX
657 : // When armed we simply return 1 (1 == 1 << 0)
658 : // VTXs that drop to low power when disarmed watch this bit, so the
659 : // ForceVTXHighPower option keeps it set to hold the VTX at full power
660 42 : const AP_MSP *msp = AP::msp();
661 42 : if (hal.util->get_soft_armed()
662 42 : || (msp != nullptr && msp->is_option_enabled(AP_MSP::Option::VTX_HIGH_POWER))) {
663 0 : return 1U;
664 : }
665 42 : return 0U;
666 : }
667 :
668 0 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_api_version(sbuf_t *dst)
669 : {
670 : const struct {
671 : uint8_t proto;
672 : uint8_t major;
673 : uint8_t minor;
674 0 : } api_version {
675 : proto : MSP_PROTOCOL_VERSION,
676 : major : API_VERSION_MAJOR,
677 : minor : API_VERSION_MINOR
678 : };
679 :
680 0 : sbuf_write_data(dst, &api_version, sizeof(api_version));
681 0 : return MSP_RESULT_ACK;
682 : }
683 :
684 0 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_fc_version(sbuf_t *dst)
685 : {
686 : const struct {
687 : uint8_t major;
688 : uint8_t minor;
689 : uint8_t patch;
690 0 : } fc_version {
691 : major : FC_VERSION_MAJOR,
692 : minor : FC_VERSION_MINOR,
693 : patch : FC_VERSION_PATCH_LEVEL
694 : };
695 :
696 0 : sbuf_write_data(dst, &fc_version, sizeof(fc_version));
697 0 : return MSP_RESULT_ACK;
698 : }
699 :
700 0 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_fc_variant(sbuf_t *dst)
701 : {
702 0 : sbuf_write_data(dst, "ARDU", FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
703 0 : return MSP_RESULT_ACK;
704 : }
705 :
706 84 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_raw_gps(sbuf_t *dst)
707 : {
708 : #if OSD_ENABLED
709 84 : AP_OSD *osd = AP::osd();
710 84 : if (osd == nullptr) {
711 0 : return MSP_RESULT_ERROR;
712 : }
713 : #endif
714 84 : gps_state_t gps_state {};
715 : #if AP_GPS_ENABLED
716 84 : update_gps_state(gps_state);
717 : #endif
718 :
719 : // handle airspeed override
720 84 : bool airspeed_en = false;
721 : #if OSD_ENABLED
722 84 : AP_MSP *msp = AP::msp();
723 84 : if (msp == nullptr) {
724 0 : return MSP_RESULT_ERROR;
725 : }
726 :
727 84 : airspeed_en = osd->screen[msp->_msp_status.current_screen].aspeed.enabled;
728 : #endif
729 84 : if (airspeed_en) {
730 : airspeed_state_t airspeed_state;
731 0 : update_airspeed(airspeed_state);
732 0 : gps_state.speed_cms = airspeed_state.airspeed_estimate_ms * 100; // airspeed in cm/s
733 : }
734 :
735 84 : sbuf_write_data(dst, &gps_state, sizeof(gps_state));
736 84 : return MSP_RESULT_ACK;
737 : }
738 :
739 84 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_comp_gps(sbuf_t *dst)
740 : {
741 : home_state_t home_state;
742 84 : update_home_pos(home_state);
743 :
744 : // no need to apply yaw compensation, the DJI air unit will do it for us :-)
745 84 : uint16_t angle_deg = home_state.home_bearing_cd * 0.01;
746 84 : if (home_state.home_distance_m < 2) {
747 : //avoid fast rotating arrow at small distances
748 84 : angle_deg = 0;
749 : }
750 :
751 : const struct PACKED {
752 : uint16_t dist_home_m;
753 : uint16_t home_angle_deg;
754 : uint8_t toggle_gps;
755 84 : } gps {
756 84 : dist_home_m : uint16_t(constrain_int32(home_state.home_distance_m, 0, 0xFFFF)),
757 : home_angle_deg : angle_deg,
758 : toggle_gps : 1
759 84 : };
760 :
761 84 : sbuf_write_data(dst, &gps, sizeof(gps));
762 84 : return MSP_RESULT_ACK;
763 : }
764 :
765 : // Autoscroll message is the same as in minimosd-extra.
766 : // Thanks to night-ghost for the approach.
767 104 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_name(sbuf_t *dst)
768 : {
769 : #if OSD_ENABLED
770 104 : AP_OSD *osd = AP::osd();
771 104 : if (osd == nullptr) {
772 0 : return MSP_RESULT_ERROR;
773 : }
774 : #endif
775 104 : AP_MSP *msp = AP::msp();
776 104 : if (msp == nullptr) {
777 0 : return MSP_RESULT_ERROR;
778 : }
779 104 : AP_Notify * notify = AP_Notify::get_singleton();
780 104 : if (notify) {
781 104 : uint16_t msgtime_ms = 10000; //default is 10 secs
782 : #if OSD_ENABLED
783 104 : msgtime_ms = AP::osd()->msgtime_s * 1000;
784 : #endif
785 : // text message is visible for _msp.msgtime_s but only if
786 : // a flight mode change did not steal focus
787 104 : const uint32_t visible_time_ms = AP_HAL::millis() - notify->get_text_updated_millis();
788 104 : if (visible_time_ms < msgtime_ms && !msp->_msp_status.flight_mode_focus) {
789 : char buffer[NOTIFY_TEXT_BUFFER_SIZE];
790 87 : strncpy(buffer, notify->get_text(), ARRAY_SIZE(buffer));
791 87 : const uint8_t len = strnlen(buffer, ARRAY_SIZE(buffer));
792 :
793 2552 : for (uint8_t i=0; i<len; i++) {
794 : //normalize whitespace
795 2465 : if (isspace(buffer[i])) {
796 289 : buffer[i] = ' ';
797 : } else {
798 : //converted to uppercase,
799 2176 : buffer[i] = toupper(buffer[i]);
800 : }
801 : }
802 :
803 87 : int8_t start_position = 0;
804 : //scroll if required
805 : //scroll pattern: wait, scroll to the left, wait, scroll to the right
806 87 : if (len > MSP_TXT_VISIBLE_CHARS) {
807 87 : const uint8_t chars_to_scroll = len - MSP_TXT_VISIBLE_CHARS;
808 87 : const uint8_t total_cycles = 2*message_scroll_delay + 2*chars_to_scroll;
809 87 : const uint8_t current_cycle = (visible_time_ms / message_scroll_time_ms) % total_cycles;
810 :
811 : //calculate scroll start_position
812 87 : if (current_cycle < total_cycles/2) {
813 : //move to the left
814 75 : start_position = current_cycle - message_scroll_delay;
815 : } else {
816 : //move to the right
817 12 : start_position = total_cycles - current_cycle;
818 : }
819 87 : start_position = constrain_int16(start_position, 0, chars_to_scroll);
820 87 : uint8_t end_position = start_position + MSP_TXT_VISIBLE_CHARS;
821 :
822 : //ensure array boundaries
823 87 : start_position = MIN(start_position, int8_t(ARRAY_SIZE(buffer)-1));
824 87 : end_position = MIN(end_position, int8_t(ARRAY_SIZE(buffer)-1));
825 :
826 : //trim invisible part
827 87 : buffer[end_position] = 0;
828 : }
829 :
830 87 : sbuf_write_data(dst, buffer + start_position, strlen(buffer + start_position)); // max MSP_TXT_VISIBLE_CHARS chars general text...
831 87 : } else {
832 17 : bool wind_en = false;
833 : char flight_mode_str[MSP_TXT_BUFFER_SIZE];
834 : #if OSD_ENABLED
835 17 : wind_en = osd->screen[msp->_msp_status.current_screen].wind.enabled;
836 : #endif
837 17 : update_flight_mode_str(flight_mode_str, ARRAY_SIZE(flight_mode_str), wind_en);
838 17 : sbuf_write_data(dst, flight_mode_str, ARRAY_SIZE(flight_mode_str)); // rendered as up to MSP_TXT_VISIBLE_CHARS chars with UTF8 support
839 : }
840 : }
841 104 : return MSP_RESULT_ACK;
842 : }
843 :
844 42 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_status(sbuf_t *dst)
845 : {
846 :
847 : struct PACKED {
848 : uint16_t task_delta_time;
849 : uint16_t i2c_error_count;
850 : uint16_t sensor_status;
851 : uint32_t flight_mode_flags;
852 : uint8_t pid_profile;
853 : uint16_t system_load;
854 : uint16_t gyro_cycle_time;
855 : uint8_t box_mode_flags;
856 : uint8_t arming_disable_flags_count;
857 : uint32_t arming_disable_flags;
858 : uint8_t extra_flags;
859 42 : } status {};
860 :
861 42 : status.flight_mode_flags = get_osd_flight_mode_bitmask();
862 42 : status.arming_disable_flags_count = 1;
863 42 : status.arming_disable_flags = !AP::notify().flags.armed;
864 :
865 42 : sbuf_write_data(dst, &status, sizeof(status));
866 42 : return MSP_RESULT_ACK;
867 : }
868 :
869 0 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_osd_canvas(sbuf_t *dst)
870 : {
871 : // report the canvas matching the current screen's text resolution so an HD
872 : // goggle sizes its OSD grid correctly (TXT_RES 0:30x16, 1:50x18, 2:60x22)
873 : struct PACKED {
874 : uint8_t cols;
875 : uint8_t rows;
876 0 : } canvas { 30, 16 };
877 : #if OSD_ENABLED
878 0 : const AP_OSD *osd = AP::osd();
879 0 : const AP_MSP *msp = AP::msp();
880 0 : if (osd != nullptr && msp != nullptr) {
881 0 : switch (osd->screen[msp->_msp_status.current_screen].get_txt_resolution()) {
882 0 : case 1:
883 0 : canvas = { 50, 18 };
884 0 : break;
885 0 : case 2:
886 0 : canvas = { 60, 22 };
887 0 : break;
888 0 : default:
889 0 : break;
890 : }
891 : }
892 : #endif
893 0 : sbuf_write_data(dst, &canvas, sizeof(canvas));
894 0 : return MSP_RESULT_ACK;
895 : }
896 :
897 104 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_osd_config(sbuf_t *dst)
898 : {
899 : #if OSD_ENABLED
900 104 : AP_OSD *osd = AP::osd();
901 104 : if (osd == nullptr) {
902 0 : return MSP_RESULT_ERROR;
903 : }
904 : #endif
905 104 : const AP_MSP *msp = AP::msp();
906 104 : if (msp == nullptr) {
907 0 : return MSP_RESULT_ERROR;
908 : }
909 : struct PACKED {
910 : uint8_t flags;
911 : uint8_t video_system;
912 : uint8_t units;
913 : uint8_t rssi_alarm;
914 : uint16_t capacity_alarm;
915 : uint8_t unused_0;
916 : uint8_t item_count;
917 : uint16_t alt_alarm;
918 : uint16_t items_position[OSD_ITEM_COUNT];
919 : uint8_t stats_items_count;
920 : uint16_t stats_items[OSD_STAT_COUNT] ;
921 : uint8_t timers_count;
922 : uint16_t timers[OSD_TIMER_COUNT];
923 : uint16_t enabled_warnings_old;
924 : uint8_t warnings_count_new;
925 : uint32_t enabled_warnings_new;
926 : uint8_t available_profiles;
927 : uint8_t selected_profile;
928 : uint8_t osd_stick_overlay;
929 104 : } osd_config {};
930 :
931 : // Configuration
932 104 : osd_config.units = OSD_UNIT_METRIC;
933 : #if OSD_ENABLED
934 104 : osd_config.units = osd->units == AP_OSD::UNITS_METRIC ? OSD_UNIT_METRIC : OSD_UNIT_IMPERIAL;
935 : #endif
936 : // Alarms
937 104 : osd_config.rssi_alarm = msp->_osd_config.rssi_alarm;
938 104 : osd_config.capacity_alarm = msp->_osd_config.cap_alarm;
939 104 : osd_config.alt_alarm = msp->_osd_config.alt_alarm;
940 : // Reuse old timer alarm (U16) as OSD_ITEM_COUNT
941 104 : osd_config.item_count = OSD_ITEM_COUNT;
942 : // Element position and visibility
943 104 : uint16_t pos = 0; // default is hide this element
944 6136 : for (uint8_t i = 0; i < OSD_ITEM_COUNT; i++) {
945 6032 : pos = 0; // 0 is hide this item
946 6032 : if (msp->_osd_item_settings[i] != nullptr) { // ok supported
947 2704 : if (msp->_osd_item_settings[i]->enabled) { // ok enabled
948 : // let's check if we need to hide this dynamically
949 2080 : if (!BIT_IS_SET_64(osd_hidden_items_bitmask, i)) {
950 1846 : pos = MSP_OSD_POS(msp->_osd_item_settings[i]);
951 : }
952 : }
953 : }
954 6032 : osd_config.items_position[i] = pos;
955 : }
956 : // Post flight statistics
957 104 : osd_config.stats_items_count = OSD_STAT_COUNT; // stats items count
958 : // Timers
959 104 : osd_config.timers_count = OSD_TIMER_COUNT; // timers
960 : // Enabled warnings
961 : // API < 1.41
962 : // Send low word first for backwards compatibility
963 104 : osd_config.enabled_warnings_old = (uint16_t)(msp->_osd_config.enabled_warnings & 0xFFFF);
964 : // API >= 1.41
965 : // Send the warnings count and 32bit enabled warnings flags.
966 : // Add currently active OSD profile (0 indicates OSD profiles not available).
967 : // Add OSD stick overlay mode (0 indicates OSD stick overlay not available).
968 104 : osd_config.warnings_count_new = OSD_WARNING_COUNT;
969 104 : osd_config.enabled_warnings_new = msp->_osd_config.enabled_warnings;
970 : // If the feature is not available there is only 1 profile and it's always selected
971 104 : osd_config.available_profiles = 1;
972 104 : osd_config.selected_profile = 1;
973 :
974 104 : sbuf_write_data(dst, &osd_config, sizeof(osd_config));
975 104 : return MSP_RESULT_ACK;
976 : }
977 :
978 105 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_attitude(sbuf_t *dst)
979 : {
980 : #if AP_VEHICLE_ENABLED
981 : float roll_rad, pitch_rad, yaw_rad;
982 105 : AP::vehicle()->get_osd_attitude_rad(roll_rad, pitch_rad, yaw_rad);
983 : #else
984 : AP_AHRS &ahrs = AP::ahrs();
985 : WITH_SEMAPHORE(ahrs.get_semaphore());
986 : float roll_rad = ahrs.get_roll_rad();
987 : float pitch_rad = ahrs.get_pitch_rad();
988 : float yaw_rad = ahrs.get_yaw_rad();
989 : #endif // AP_VEHICLE_ENABLED
990 :
991 : const struct PACKED {
992 : int16_t roll;
993 : int16_t pitch;
994 : int16_t yaw;
995 : } attitude {
996 105 : roll : int16_t(degrees(roll_rad) * 10), // degress to decidegrees
997 210 : pitch : int16_t(degrees(pitch_rad) * 10), // degress to decidegrees
998 105 : yaw : int16_t(wrap_360(degrees(yaw_rad)))
999 105 : };
1000 :
1001 105 : sbuf_write_data(dst, &attitude, sizeof(attitude));
1002 105 : return MSP_RESULT_ACK;
1003 : }
1004 :
1005 84 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_altitude(sbuf_t *dst)
1006 : {
1007 : home_state_t home_state;
1008 84 : update_home_pos(home_state);
1009 :
1010 : const struct PACKED {
1011 : int32_t rel_altitude_cm; // relative altitude cm
1012 : int16_t vspeed_cms; // climb rate cm/s
1013 : } altitude {
1014 84 : rel_altitude_cm : home_state.rel_altitude_cm,
1015 168 : vspeed_cms : int16_t(get_vspeed_ms() * 100)
1016 84 : };
1017 :
1018 84 : sbuf_write_data(dst, &altitude, sizeof(altitude));
1019 84 : return MSP_RESULT_ACK;
1020 : }
1021 :
1022 84 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_analog(sbuf_t *dst)
1023 : {
1024 : #if AP_BATTERY_ENABLED
1025 : battery_state_t battery_state;
1026 84 : update_battery_state(battery_state);
1027 :
1028 : float rssi;
1029 : const struct PACKED {
1030 : uint8_t voltage_dv;
1031 : uint16_t mah;
1032 : uint16_t rssi;
1033 : int16_t current_ca;
1034 : uint16_t voltage_cv;
1035 : } analog {
1036 84 : voltage_dv : (uint8_t)constrain_int16(battery_state.batt_voltage_v * 10, 0, 255), // battery voltage V to dV
1037 84 : mah : (uint16_t)constrain_int32(battery_state.batt_consumed_mah, 0, 0xFFFF), // milliamp hours drawn from battery
1038 84 : rssi : uint16_t(get_rssi(rssi) ? constrain_float(rssi,0,1) * 1023 : 0), // rssi 0-1 to 0-1023)
1039 84 : current_ca : (int16_t)constrain_int32(battery_state.batt_current_a * 100, -0x8000, 0x7FFF), // current A to cA (0.01 steps, range is -320A to 320A)
1040 84 : voltage_cv : (uint16_t)constrain_int32(battery_state.batt_voltage_v * 100,0,0xFFFF) // battery voltage in 0.01V steps
1041 168 : };
1042 : #else
1043 : float rssi;
1044 : const struct PACKED {
1045 : uint8_t voltage_dv;
1046 : uint16_t mah;
1047 : uint16_t rssi;
1048 : int16_t current_ca;
1049 : uint16_t voltage_cv;
1050 : } analog {
1051 : voltage_dv : 0,
1052 : mah : 0,
1053 : rssi : uint16_t(get_rssi(rssi) ? constrain_float(rssi,0,1) * 1023 : 0), // rssi 0-1 to 0-1023)
1054 : current_ca : 0,
1055 : voltage_cv : 0
1056 : };
1057 : #endif
1058 84 : sbuf_write_data(dst, &analog, sizeof(analog));
1059 84 : return MSP_RESULT_ACK;
1060 : }
1061 :
1062 : #if AP_BATTERY_ENABLED
1063 42 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_battery_state(sbuf_t *dst)
1064 : {
1065 42 : const AP_MSP *msp = AP::msp();
1066 42 : if (msp == nullptr) {
1067 0 : return MSP_RESULT_ERROR;
1068 : }
1069 : battery_state_t battery_state;
1070 42 : update_battery_state(battery_state);
1071 :
1072 : const struct PACKED {
1073 : uint8_t cellcount;
1074 : uint16_t capacity_mah;
1075 : uint8_t voltage_dv;
1076 : uint16_t mah;
1077 : int16_t current_ca;
1078 : uint8_t state;
1079 : uint16_t voltage_cv;
1080 : } battery {
1081 42 : cellcount : (uint8_t)constrain_int16((msp->_cellcount > 0 ? msp->_cellcount : battery_state.batt_cellcount), 0, 255), // cell count 0 indicates battery not detected.
1082 42 : capacity_mah : (uint16_t)battery_state.batt_capacity_mah, // in mAh
1083 42 : voltage_dv : (uint8_t)constrain_int16(battery_state.batt_voltage_v * 10, 0, 255), // battery voltage V to dV
1084 84 : mah : (uint16_t)MIN(battery_state.batt_consumed_mah, 0xFFFF), // milliamp hours drawn from battery
1085 42 : current_ca : (int16_t)constrain_int32(battery_state.batt_current_a * 100, -0x8000, 0x7FFF), // current A to cA (0.01 steps, range is -320A to 320A)
1086 42 : state : (uint8_t)battery_state.batt_state, // BATTERY: OK=0, CRITICAL=2
1087 42 : voltage_cv : (uint16_t)constrain_int32(battery_state.batt_voltage_v * 100, 0, 0x7FFF) // battery voltage in 0.01V steps
1088 42 : };
1089 :
1090 42 : sbuf_write_data(dst, &battery, sizeof(battery));
1091 42 : return MSP_RESULT_ACK;
1092 : }
1093 : #endif
1094 :
1095 0 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(sbuf_t *dst)
1096 : {
1097 : #if HAL_WITH_ESC_TELEM
1098 0 : AP_ESC_Telem& telem = AP::esc_telem();
1099 0 : if (telem.get_last_telem_data_ms(0)) {
1100 : struct PACKED {
1101 : uint8_t num_motors;
1102 : struct PACKED {
1103 : uint8_t temp;
1104 : uint16_t rpm;
1105 : } data[ESC_TELEM_MAX_ESCS];
1106 0 : } esc_sensor {};
1107 :
1108 0 : esc_sensor.num_motors = telem.get_num_active_escs();
1109 0 : for (uint8_t i = 0; i < esc_sensor.num_motors; i++) {
1110 0 : int16_t temp = 0;
1111 0 : float rpm = 0.0f;
1112 0 : IGNORE_RETURN(telem.get_rpm(i, rpm));
1113 0 : IGNORE_RETURN(telem.get_temperature(i, temp));
1114 0 : esc_sensor.data[i].temp = uint8_t(temp * 0.01f);
1115 0 : esc_sensor.data[i].rpm = uint16_t(rpm * 0.1f);
1116 : }
1117 0 : sbuf_write_data(dst, &esc_sensor, 1 + 3*esc_sensor.num_motors);
1118 : }
1119 : #endif
1120 0 : return MSP_RESULT_ACK;
1121 : }
1122 :
1123 21 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rtc(sbuf_t *dst)
1124 : {
1125 21 : tm localtime_tm {}; // year is relative to 1900
1126 21 : uint64_t time_usec = 0;
1127 : #if AP_RTC_ENABLED
1128 21 : if (AP::rtc().get_utc_usec(time_usec)) { // may fail, leaving time_unix at 0
1129 12 : const time_t time_sec = time_usec / 1000000;
1130 12 : struct tm tmd {};
1131 12 : localtime_tm = *gmtime_r(&time_sec, &tmd);
1132 : }
1133 : #endif
1134 : const struct PACKED {
1135 : uint16_t year;
1136 : uint8_t mon;
1137 : uint8_t mday;
1138 : uint8_t hour;
1139 : uint8_t min;
1140 : uint8_t sec;
1141 : uint16_t millis;
1142 : } rtc {
1143 21 : year : uint16_t(localtime_tm.tm_year + 1900), // tm_year is relative to year 1900
1144 21 : mon : uint8_t(localtime_tm.tm_mon + 1), // MSP requires [1-12] months
1145 21 : mday : uint8_t(localtime_tm.tm_mday),
1146 21 : hour : uint8_t(localtime_tm.tm_hour),
1147 21 : min : uint8_t(localtime_tm.tm_min),
1148 21 : sec : uint8_t(localtime_tm.tm_sec),
1149 21 : millis : uint16_t((time_usec / 1000U) % 1000U)
1150 21 : };
1151 :
1152 21 : sbuf_write_data(dst, &rtc, sizeof(rtc));
1153 21 : return MSP_RESULT_ACK;
1154 : }
1155 :
1156 : #if AP_RC_CHANNEL_ENABLED
1157 0 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst)
1158 : {
1159 0 : float roll = rc().get_roll_channel().norm_input_dz();
1160 0 : float pitch = -rc().get_pitch_channel().norm_input_dz();
1161 0 : float yaw = rc().get_yaw_channel().norm_input_dz();
1162 0 : float throttle = rc().get_throttle_channel().norm_input_dz();
1163 :
1164 : const struct PACKED {
1165 : uint16_t a;
1166 : uint16_t e;
1167 : uint16_t r;
1168 : uint16_t t;
1169 : } rc {
1170 : // send only 4 channels, MSP order is AERT
1171 0 : a : uint16_t(roll*500+1500), // A
1172 0 : e : uint16_t(pitch*500+1500), // E
1173 0 : r : uint16_t(yaw*500+1500), // R
1174 0 : t : uint16_t(throttle*1000+1000) // T
1175 0 : };
1176 :
1177 0 : sbuf_write_data(dst, &rc, sizeof(rc));
1178 0 : return MSP_RESULT_ACK;
1179 : }
1180 : #endif // AP_RC_CHANNEL_ENABLED
1181 :
1182 : #if AP_MSP_VIDEOTX_ENABLED
1183 : // set the configured pitmode option without disturbing the other option bits
1184 10 : void AP_MSP_Telem_Backend::msp_vtx_set_pitmode(bool pitmode)
1185 : {
1186 10 : AP::vtx().set_option_enabled(AP_VideoTX::VideoOptions::VTX_PITMODE, pitmode);
1187 10 : }
1188 :
1189 : // fallback power plan used until the connected VTX declares its own table via
1190 : // MSP_SET_VTXTABLE_POWERLEVEL; the index is one based (0 means "unset"), so
1191 : // index 1 == 25mW. 14dBm (25mW), 20dBm (100mW), 26dBm (400mW), 29dBm (800mW)
1192 : static const uint16_t msp_vtx_default_power_mw[] { 25, 100, 400, 800 };
1193 :
1194 : // apply a one-based betaflight power index, leaving power unchanged for 0
1195 10 : void AP_MSP_Telem_Backend::msp_vtx_set_power_index(uint8_t index)
1196 : {
1197 10 : AP_VideoTX& vtx = AP::vtx();
1198 : // prefer the table the VTX declared for itself, fall back to the default
1199 10 : uint16_t mw = vtx.get_power_mw_for_index(index);
1200 10 : if (mw == 0 && index >= 1 && index <= ARRAY_SIZE(msp_vtx_default_power_mw)) {
1201 4 : mw = msp_vtx_default_power_mw[index - 1];
1202 : }
1203 10 : if (mw > 0) {
1204 10 : vtx.set_configured_power_mw(mw);
1205 : }
1206 10 : }
1207 :
1208 : // convert the configured power back to the one-based index, 0 if unmatched
1209 125 : uint8_t AP_MSP_Telem_Backend::msp_vtx_get_power_index() const
1210 : {
1211 125 : const AP_VideoTX& vtx = AP::vtx();
1212 125 : const uint16_t mw = vtx.get_configured_power_mw();
1213 125 : const uint8_t index = vtx.get_power_index_for_mw(mw);
1214 125 : if (index > 0) {
1215 27 : return index;
1216 : }
1217 490 : for (uint8_t i = 0; i < ARRAY_SIZE(msp_vtx_default_power_mw); i++) {
1218 392 : if (mw == msp_vtx_default_power_mw[i]) {
1219 0 : return i + 1;
1220 : }
1221 : }
1222 98 : return 0;
1223 : }
1224 :
1225 : // store a frequency along with the matching band/channel; all setters are
1226 : // set-and-save-if-changed so repeated identical messages cause no flash writes
1227 0 : void AP_MSP_Telem_Backend::msp_vtx_set_frequency(uint16_t freq_mhz)
1228 : {
1229 0 : AP_VideoTX& vtx = AP::vtx();
1230 0 : vtx.set_configured_frequency_mhz(freq_mhz);
1231 : AP_VideoTX::VideoBand band;
1232 : uint8_t channel;
1233 0 : if (AP_VideoTX::get_band_and_channel(freq_mhz, band, channel)) {
1234 0 : vtx.set_configured_band(uint8_t(band));
1235 0 : vtx.set_configured_channel(channel);
1236 : }
1237 0 : }
1238 :
1239 : // store a (zero based) band/channel along with the matching frequency
1240 12 : void AP_MSP_Telem_Backend::msp_vtx_set_band_and_channel(uint8_t band, uint8_t channel)
1241 : {
1242 12 : AP_VideoTX& vtx = AP::vtx();
1243 12 : vtx.set_configured_band(band);
1244 12 : vtx.set_configured_channel(channel);
1245 12 : vtx.set_configured_frequency_mhz(AP_VideoTX::get_frequency_mhz(band, channel));
1246 12 : }
1247 :
1248 10 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_in_vtx_config(sbuf_t *src, sbuf_t *dst)
1249 : {
1250 10 : AP_VideoTX& vtx = AP::vtx();
1251 : // the user may have a VTX connected but not want AP to control it
1252 : // (for instance because they are using myVTX on the transmitter)
1253 10 : if (!vtx.get_enabled()) {
1254 0 : return MSP_RESULT_ERROR;
1255 : }
1256 :
1257 : // MSP_SET_VTX_CONFIG is variable length: clients send anything from the
1258 : // legacy 4-byte payload up to the full 1.42 message, so every field beyond
1259 : // the first must be guarded by the remaining byte count (betaflight msp.c)
1260 10 : if (sbuf_bytes_remaining(src) < 2) {
1261 0 : return MSP_RESULT_ERROR;
1262 : }
1263 :
1264 : // the leading u16 is overloaded: values <= VTX_MSP_BANDCHAN_CHKVAL encode
1265 : // band and channel (band = v/8, channel = v%8, both zero based internally),
1266 : // larger values are a direct frequency in MHz
1267 10 : const uint16_t VTX_MSP_BANDCHAN_CHKVAL = (7 << 3) + 7;
1268 10 : const uint16_t freq_or_bandchan = sbuf_read_u16(src);
1269 10 : if (freq_or_bandchan <= VTX_MSP_BANDCHAN_CHKVAL) {
1270 10 : msp_vtx_set_band_and_channel(freq_or_bandchan / VTX_MAX_CHANNELS, freq_or_bandchan % VTX_MAX_CHANNELS);
1271 : } else {
1272 0 : msp_vtx_set_frequency(freq_or_bandchan);
1273 : }
1274 :
1275 10 : if (sbuf_bytes_remaining(src) >= 2) {
1276 10 : msp_vtx_set_power_index(sbuf_read_u8(src));
1277 10 : msp_vtx_set_pitmode(sbuf_read_u8(src) != 0);
1278 : }
1279 :
1280 : // lowPowerDisarm - not supported, consume to stay aligned
1281 10 : if (sbuf_bytes_remaining(src) >= 1) {
1282 2 : sbuf_read_u8(src);
1283 : }
1284 :
1285 : // API 1.42 - pitModeFreq - not supported, consume to stay aligned
1286 10 : if (sbuf_bytes_remaining(src) >= 2) {
1287 2 : sbuf_read_u16(src);
1288 : }
1289 :
1290 : // API 1.42 - standalone (non-encoded) band/channel/frequency; betaflight
1291 : // sends band/channel one based with band == 0 meaning "use raw frequency".
1292 : // When present this supersedes the legacy encoding decoded above.
1293 10 : if (sbuf_bytes_remaining(src) >= 4) {
1294 2 : const uint8_t band = sbuf_read_u8(src);
1295 2 : const uint8_t channel = sbuf_read_u8(src);
1296 2 : const uint16_t freq = sbuf_read_u16(src);
1297 2 : if (band > 0) {
1298 2 : msp_vtx_set_band_and_channel(band - 1, channel > 0 ? channel - 1 : 0);
1299 : } else {
1300 0 : msp_vtx_set_frequency(freq);
1301 : }
1302 : }
1303 :
1304 10 : vtx.set_provider_enabled(AP_VideoTX::VTXType::MSP);
1305 : // the VTX has pushed its own config, so the boot handshake is complete and
1306 : // we may now advertise readiness and push config changes back to it
1307 10 : _vtx_config_received = true;
1308 : // make sure the configured values now reflect reality
1309 10 : if (!vtx.set_defaults()) {
1310 8 : vtx.announce_vtx_settings();
1311 : }
1312 : // seed the push snapshot with the VTX's own values so we only push once the
1313 : // user actually changes something, not immediately after the handshake
1314 10 : _vtx_pushed = {
1315 10 : vtx.get_configured_band(),
1316 10 : vtx.get_configured_channel(),
1317 10 : vtx.get_configured_frequency_mhz(),
1318 10 : vtx.get_configured_power_mw(),
1319 10 : vtx.get_configured_pitmode(),
1320 : true
1321 : };
1322 10 : _vtx_config_push_remaining = 0;
1323 :
1324 10 : return MSP_RESULT_ACK;
1325 : }
1326 :
1327 : // push the VTX config on change, repeating MSP_VTX_CONFIG_PUSH_COUNT times for
1328 : // delivery robustness. Returns true when the caller should emit a config frame
1329 : // this tick. We only control the VTX after its boot handshake; the air unit
1330 : // does not report its state back, so receipt cannot be confirmed (unlike Tramp)
1331 53 : bool AP_MSP_Telem_Backend::vtx_should_push_config()
1332 : {
1333 53 : AP_VideoTX& vtx = AP::vtx();
1334 53 : if (!vtx.get_enabled() || !_vtx_config_received) {
1335 47 : return false;
1336 : }
1337 :
1338 6 : const uint8_t band = vtx.get_configured_band();
1339 6 : const uint8_t channel = vtx.get_configured_channel();
1340 6 : const uint16_t freq_mhz = vtx.get_configured_frequency_mhz();
1341 6 : const uint16_t power_mw = vtx.get_configured_power_mw();
1342 6 : const bool pitmode = vtx.get_configured_pitmode();
1343 :
1344 6 : if (!_vtx_pushed.valid
1345 6 : || band != _vtx_pushed.band
1346 6 : || channel != _vtx_pushed.channel
1347 6 : || freq_mhz != _vtx_pushed.freq_mhz
1348 6 : || power_mw != _vtx_pushed.power_mw
1349 6 : || pitmode != _vtx_pushed.pitmode) {
1350 : // configuration changed, (re)start a push burst
1351 0 : _vtx_pushed = { band, channel, freq_mhz, power_mw, pitmode, true };
1352 0 : _vtx_config_push_remaining = MSP_VTX_CONFIG_PUSH_COUNT;
1353 0 : vtx.set_configuration_finished(false);
1354 : }
1355 :
1356 6 : if (_vtx_config_push_remaining == 0) {
1357 6 : return false;
1358 : }
1359 0 : if (--_vtx_config_push_remaining == 0) {
1360 : // optimistically consider the VTX configured once the burst completes
1361 0 : vtx.set_configuration_finished(true);
1362 : }
1363 0 : return true;
1364 : }
1365 :
1366 125 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_vtx_config(sbuf_t *src, sbuf_t *dst)
1367 : {
1368 125 : AP_VideoTX& vtx = AP::vtx();
1369 :
1370 125 : if (!vtx.get_enabled()) {
1371 0 : return MSP_RESULT_ERROR;
1372 : }
1373 :
1374 : // band/channel are one based on the wire (band == 0 means raw frequency),
1375 : // zero based internally
1376 125 : const uint8_t VTXDEV_MSP = 5; // betaflight vtxDevType_e
1377 : struct PACKED {
1378 : uint8_t type;
1379 : uint8_t band;
1380 : uint8_t channel;
1381 : uint8_t power;
1382 : uint8_t pitmode;
1383 : uint16_t freq;
1384 : uint8_t deviceIsReady;
1385 : uint8_t lowPowerDisarm;
1386 : // API version 1.42
1387 : uint16_t pitModeFreq;
1388 : uint8_t table;
1389 : uint8_t table_bands;
1390 : uint8_t table_channels;
1391 : uint8_t table_powerLevels;
1392 125 : } vtx_config {
1393 : // band/channel are one based on the wire (band == 0 means raw frequency),
1394 : // zero based internally. Keep the frequency consistent with band/channel:
1395 : // the user may change VTX_CHANNEL without VTX_FREQ, and a stale frequency
1396 : // would contradict the band/channel a betaflight-style VTX keys off
1397 : .type = VTXDEV_MSP,
1398 125 : .band = uint8_t(vtx.get_configured_band() + 1),
1399 250 : .channel = uint8_t(vtx.get_configured_channel() + 1),
1400 125 : .power = msp_vtx_get_power_index(),
1401 250 : .pitmode = vtx.get_configured_pitmode(),
1402 125 : .freq = AP_VideoTX::get_frequency_mhz(vtx.get_configured_band(), vtx.get_configured_channel()),
1403 : // report not-ready until the VTX has uploaded its own config: the HDZero
1404 : // air unit is the MSP master and only runs that handshake against a
1405 : // betaflight FC (FC_VARIANT) that is not yet configured. Once it has
1406 : // pushed its config to us we report ready so it honours our config pushes
1407 125 : .deviceIsReady = _vtx_config_received ? uint8_t(1) : uint8_t(0),
1408 : .table = 0,
1409 : .table_bands = 0,
1410 : .table_channels = 0,
1411 : .table_powerLevels = 0
1412 250 : };
1413 :
1414 125 : sbuf_write_data(dst, &vtx_config, sizeof(vtx_config));
1415 125 : return MSP_RESULT_ACK;
1416 : }
1417 :
1418 6 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_in_vtxtable_powerlevel(sbuf_t *src, sbuf_t *dst)
1419 : {
1420 6 : AP_VideoTX& vtx = AP::vtx();
1421 6 : if (!vtx.get_enabled()) {
1422 0 : return MSP_RESULT_ERROR;
1423 : }
1424 :
1425 : // [u8 powerLevel (one based)][u16 power dBm][u8 label length][label...]; the
1426 : // index is implied by the dBm ordering and the label is display only, so
1427 : // both are ignored and only the supported power level is recorded. The value
1428 : // is dBm (betaflight stores power tables in dBm), not mW; a zero entry is an
1429 : // unused slot.
1430 6 : if (sbuf_bytes_remaining(src) < 3) {
1431 0 : return MSP_RESULT_ERROR;
1432 : }
1433 6 : sbuf_read_u8(src); // power level index, implied by ordering, ignored
1434 6 : const uint16_t power_dbm = sbuf_read_u16(src);
1435 6 : if (power_dbm > 0) {
1436 6 : vtx.update_power_dbm(uint8_t(power_dbm));
1437 : }
1438 :
1439 6 : return MSP_RESULT_ACK;
1440 : }
1441 :
1442 : #endif // AP_MSP_VIDEOTX_ENABLED
1443 :
1444 0 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_board_info(sbuf_t *dst)
1445 : {
1446 0 : const AP_FWVersion &fwver = AP::fwversion();
1447 :
1448 : struct PACKED {
1449 : uint16_t hw_revision;
1450 : uint8_t aio_flags;
1451 : uint8_t capabilities;
1452 : uint8_t fw_string_len;
1453 0 : } fw_info {};
1454 :
1455 : #if HAL_WITH_OSD_BITMAP
1456 : fw_info.aio_flags = 2; // 2 == FC with MAX7456
1457 : #else
1458 0 : fw_info.aio_flags = 0; // 0 == FC without MAX7456
1459 : #endif
1460 0 : fw_info.fw_string_len = strlen(fwver.fw_string);
1461 :
1462 0 : sbuf_write_data(dst, "ARDU", BOARD_IDENTIFIER_LENGTH);
1463 0 : sbuf_write_data(dst, &fw_info, sizeof(fw_info));
1464 0 : sbuf_write_data(dst, fwver.fw_string, strlen(fwver.fw_string));
1465 0 : return MSP_RESULT_ACK;
1466 : }
1467 :
1468 0 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_build_info(sbuf_t *dst)
1469 : {
1470 0 : const AP_FWVersion &fwver = AP::fwversion();
1471 :
1472 : // we don't use real dates here as that would mean we don't get
1473 : // consistent builds. Being able to reproduce the exact build at a
1474 : // later date is a valuable property of the code
1475 0 : sbuf_write_data(dst, "Jan 01 1980", BUILD_DATE_LENGTH);
1476 0 : sbuf_write_data(dst, "00:00:00", BUILD_TIME_LENGTH);
1477 0 : sbuf_write_data(dst, fwver.fw_hash_str, GIT_SHORT_REVISION_LENGTH);
1478 0 : return MSP_RESULT_ACK;
1479 : }
1480 :
1481 0 : MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_uid(sbuf_t *dst)
1482 : {
1483 0 : uint8_t id[12] {};
1484 0 : uint8_t len = sizeof(id);
1485 0 : hal.util->get_system_id_unformatted(id, len);
1486 0 : sbuf_write_data(dst, id, sizeof(id));
1487 0 : return MSP_RESULT_ACK;
1488 : }
1489 :
1490 8610 : void AP_MSP_Telem_Backend::hide_osd_items(void)
1491 : {
1492 : #if OSD_ENABLED
1493 8610 : AP_OSD *osd = AP::osd();
1494 8610 : if (osd == nullptr) {
1495 0 : return;
1496 : }
1497 : #endif
1498 8610 : AP_MSP *msp = AP::msp();
1499 8610 : if (msp == nullptr) {
1500 0 : return;
1501 : }
1502 : #if AP_BATTERY_ENABLED
1503 8610 : const AP_Notify ¬ify = AP::notify();
1504 : #endif
1505 : // clear all and only set the flashing ones
1506 8610 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SATS);
1507 8610 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIR);
1508 8610 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIST);
1509 8610 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SPEED);
1510 8610 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_CRAFT_NAME);
1511 8610 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_AVG_CELL_VOLTAGE);
1512 8610 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE);
1513 8610 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_RTC_DATETIME);
1514 8610 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_RSSI_VALUE);
1515 :
1516 8610 : if (msp->_msp_status.flashing_on) {
1517 : // flash satcount when no 3D Fix
1518 : gps_state_t gps_state;
1519 4239 : update_gps_state(gps_state);
1520 4239 : if (gps_state.fix_type == 0) {
1521 1177 : BIT_SET(osd_hidden_items_bitmask, OSD_GPS_SATS);
1522 : }
1523 : // flash home dir and distance if home is not set
1524 : home_state_t home_state;
1525 4239 : update_home_pos(home_state);
1526 4239 : if (!home_state.home_is_set) {
1527 2236 : BIT_SET(osd_hidden_items_bitmask, OSD_HOME_DIR);
1528 2236 : BIT_SET(osd_hidden_items_bitmask, OSD_HOME_DIST);
1529 : }
1530 : // flash airspeed if there's no estimate
1531 4239 : bool airspeed_en = false;
1532 : #if OSD_ENABLED
1533 4239 : airspeed_en = osd->screen[msp->_msp_status.current_screen].aspeed.enabled;
1534 : #endif
1535 4239 : if (airspeed_en) {
1536 : airspeed_state_t airspeed_state;
1537 0 : update_airspeed(airspeed_state);
1538 0 : if (!airspeed_state.airspeed_have_estimate) {
1539 0 : BIT_SET(osd_hidden_items_bitmask, OSD_GPS_SPEED);
1540 : }
1541 : }
1542 : // flash text flightmode for 3secs after each change
1543 4239 : if (msp->_msp_status.flight_mode_focus) {
1544 286 : BIT_SET(osd_hidden_items_bitmask, OSD_CRAFT_NAME);
1545 : }
1546 : #if AP_BATTERY_ENABLED
1547 : // flash battery on failsafe
1548 4239 : if (notify.flags.failsafe_battery) {
1549 0 : BIT_SET(osd_hidden_items_bitmask, OSD_AVG_CELL_VOLTAGE);
1550 0 : BIT_SET(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE);
1551 : }
1552 : #endif
1553 : // flash rtc if no time available
1554 : #if AP_RTC_ENABLED
1555 : uint64_t time_usec;
1556 4239 : if (!AP::rtc().get_utc_usec(time_usec)) {
1557 1177 : BIT_SET(osd_hidden_items_bitmask, OSD_RTC_DATETIME);
1558 : }
1559 : #else
1560 : BIT_SET(osd_hidden_items_bitmask, OSD_RTC_DATETIME);
1561 : #endif
1562 : // flash rssi if disabled
1563 : float rssi;
1564 4239 : if (!get_rssi(rssi)) {
1565 4239 : BIT_SET(osd_hidden_items_bitmask, OSD_RSSI_VALUE);
1566 : }
1567 : }
1568 : // disable flashing for min/max items
1569 8610 : if (displaying_stats_screen()) {
1570 0 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIST);
1571 0 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SPEED);
1572 0 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_CRAFT_NAME);
1573 0 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE);
1574 0 : BIT_CLEAR(osd_hidden_items_bitmask, OSD_RSSI_VALUE);
1575 : }
1576 : }
1577 :
1578 : #if HAL_WITH_MSP_DISPLAYPORT
1579 : // ported from betaflight/src/main/io/displayport_msp.c
1580 548 : void AP_MSP_Telem_Backend::msp_displayport_heartbeat()
1581 : {
1582 548 : const uint8_t subcmd[] = { msp_displayport_subcmd_e::MSP_DISPLAYPORT_HEARTBEAT };
1583 :
1584 : // heartbeat is used to:
1585 : // a) ensure display is not released by remote OSD software
1586 : // b) prevent OSD Slave boards from displaying a 'disconnected' status.
1587 548 : msp_send_packet(MSP_DISPLAYPORT, MSP::MSP_V1, subcmd, sizeof(subcmd), false);
1588 548 : }
1589 :
1590 548 : void AP_MSP_Telem_Backend::msp_displayport_grab()
1591 : {
1592 548 : msp_displayport_heartbeat();
1593 548 : }
1594 :
1595 0 : void AP_MSP_Telem_Backend::msp_displayport_release()
1596 : {
1597 0 : const uint8_t subcmd[] = { msp_displayport_subcmd_e::MSP_DISPLAYPORT_RELEASE };
1598 :
1599 0 : msp_send_packet(MSP_DISPLAYPORT, MSP::MSP_V1, subcmd, sizeof(subcmd), false);
1600 0 : }
1601 :
1602 548 : void AP_MSP_Telem_Backend::msp_displayport_clear_screen()
1603 : {
1604 548 : const uint8_t subcmd[] = { msp_displayport_subcmd_e::MSP_DISPLAYPORT_CLEAR_SCREEN };
1605 :
1606 548 : msp_send_packet(MSP_DISPLAYPORT, MSP::MSP_V1, subcmd, sizeof(subcmd), false);
1607 548 : }
1608 :
1609 548 : void AP_MSP_Telem_Backend::msp_displayport_draw_screen()
1610 : {
1611 548 : const uint8_t subcmd[] = { msp_displayport_subcmd_e::MSP_DISPLAYPORT_DRAW_SCREEN };
1612 548 : msp_send_packet(MSP_DISPLAYPORT, MSP::MSP_V1, subcmd, sizeof(subcmd), false);
1613 548 : }
1614 :
1615 18843 : void AP_MSP_Telem_Backend::msp_displayport_write_string(uint8_t col, uint8_t row, bool blink, const char *string, const uint8_t font_table)
1616 : {
1617 18843 : const uint8_t len = strnlen(string, OSD_MSP_DISPLAYPORT_MAX_STRING_LENGTH);
1618 :
1619 : struct PACKED {
1620 : uint8_t sub_cmd;
1621 : uint8_t row;
1622 : uint8_t col;
1623 : uint8_t attr;
1624 : uint8_t text[OSD_MSP_DISPLAYPORT_MAX_STRING_LENGTH];
1625 18843 : } packet {};
1626 :
1627 18843 : packet.sub_cmd = msp_displayport_subcmd_e::MSP_DISPLAYPORT_WRITE_STRING;
1628 18843 : packet.row = row;
1629 18843 : packet.col = col;
1630 18843 : packet.attr |= (font_table & 0x03); // first 2 bits are for the table index
1631 18843 : if (blink) {
1632 0 : packet.attr |= DISPLAYPORT_MSP_ATTR_BLINK;
1633 : }
1634 18843 : memcpy(packet.text, string, len);
1635 :
1636 18843 : msp_send_packet(MSP_DISPLAYPORT, MSP::MSP_V1, &packet, 4 + len, false);
1637 18843 : }
1638 :
1639 548 : void AP_MSP_Telem_Backend::msp_displayport_set_options(const uint8_t font_index, const uint8_t screen_resolution)
1640 : {
1641 548 : const uint8_t subcmd[] = { msp_displayport_subcmd_e::MSP_DISPLAYPORT_SET_OPTIONS, font_index, screen_resolution };
1642 548 : msp_send_packet(MSP_DISPLAYPORT, MSP::MSP_V1, subcmd, sizeof(subcmd), false);
1643 548 : }
1644 : #endif //HAL_WITH_MSP_DISPLAYPORT
1645 10909 : bool AP_MSP_Telem_Backend::displaying_stats_screen() const
1646 : {
1647 : #if OSD_ENABLED
1648 10909 : AP_OSD *osd = AP::osd();
1649 10909 : if (osd == nullptr) {
1650 0 : return false;
1651 : }
1652 10909 : AP_MSP *msp = AP::msp();
1653 10909 : if (msp == nullptr) {
1654 0 : return false;
1655 : }
1656 10909 : return osd->screen[msp->_msp_status.current_screen].stat.enabled;
1657 : #else
1658 : return false;
1659 : #endif
1660 : }
1661 :
1662 4323 : bool AP_MSP_Telem_Backend::get_rssi(float &rssi) const
1663 : {
1664 : #if AP_RSSI_ENABLED
1665 4323 : AP_RSSI* ap_rssi = AP::rssi();
1666 4323 : if (ap_rssi == nullptr) {
1667 0 : return false;
1668 : }
1669 4323 : if (!ap_rssi->enabled()) {
1670 4323 : return false;
1671 : }
1672 0 : rssi = ap_rssi->read_receiver_rssi(); // range is [0-1]
1673 0 : return true;
1674 : #else
1675 : return false;
1676 : #endif
1677 : }
1678 :
1679 : #endif //HAL_MSP_ENABLED
|
