?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Blackbox device type 'file' (SITL) considered working when file handler is available
[inav.git] / src / main / io / osd_dji_hd.c
blobb018d8f906811b2a40662edf1acc8715810e9826
1 /*
2 * This file is part of INAV.
3 *
4 * This Source Code Form is subject to the terms of the Mozilla Public
5 * License, v. 2.0. If a copy of the MPL was not distributed with this file,
6 * You can obtain one at http://mozilla.org/MPL/2.0/.
7 *
8 * Alternatively, the contents of this file may be used under the terms
9 * of the GNU General Public License Version 3, as described below:
10 *
11 * This file is free software: you may copy, redistribute and/or modify
12 * it under the terms of the GNU General Public License as published by the
13 * Free Software Foundation, either version 3 of the License, or (at your
14 * option) any later version.
15 *
16 * This file is distributed in the hope that it will be useful, but
17 * WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
19 * Public License for more details.
20 *
21 * You should have received a copy of the GNU General Public License
22 * along with this program. If not, see http://www.gnu.org/licenses/.
23 *
24 * @author Konstantin Sharlaimov (ksharlaimov@inavflight.com)
25 */
26
27 #include <stdbool.h>
28 #include <stdint.h>
29 #include <string.h>
30
31 #include "platform.h"
32
33 #include "build/debug.h"
34 #include "build/version.h"
35
36 #include "common/streambuf.h"
37 #include "common/utils.h"
38 #include "common/maths.h"
39 #include "common/time.h"
40 #include "common/crc.h"
41
42 #include "config/parameter_group.h"
43 #include "config/parameter_group_ids.h"
44
45 #include "fc/fc_core.h"
46 #include "fc/config.h"
47 #include "fc/controlrate_profile.h"
48 #include "fc/fc_msp.h"
49 #include "fc/fc_msp_box.h"
50 #include "fc/runtime_config.h"
51 #include "fc/settings.h"
52 #include "fc/rc_adjustments.h"
53
54 #include "flight/imu.h"
55 #include "flight/pid.h"
56 #include "flight/mixer.h"
57
58 #include "io/serial.h"
59 #include "io/gps.h"
60 #include "io/osd.h"
61 #include "io/osd_dji_hd.h"
62 #include "io/osd_common.h"
63
64 #include "rx/rx.h"
65
66 #include "sensors/sensors.h"
67 #include "sensors/gyro.h"
68 #include "sensors/battery.h"
69 #include "sensors/rangefinder.h"
70 #include "sensors/acceleration.h"
71 #include "sensors/esc_sensor.h"
72 #include "sensors/temperature.h"
73 #include "sensors/pitotmeter.h"
74 #include "sensors/boardalignment.h"
75
76 #include "msp/msp.h"
77 #include "msp/msp_protocol.h"
78 #include "msp/msp_serial.h"
79
80 #include "common/string_light.h"
81 #include "navigation/navigation.h"
82 #include "navigation/navigation_private.h"
83 #include "common/constants.h"
84 #include "scheduler/scheduler.h"
85 #include "common/printf.h"
86 #include <stdlib.h>
87 #include "rx/rx.h"
88 #include "fc/rc_controls.h"
89
90 #if defined(USE_DJI_HD_OSD)
91
92 #define DJI_MSP_BAUDRATE 115200
93
94 #define DJI_ARMING_DISABLE_FLAGS_COUNT 25
95 #define DJI_OSD_WARNING_COUNT 16
96 #define DJI_OSD_TIMER_COUNT 2
97 #define DJI_OSD_FLAGS_OSD_FEATURE (1 << 0)
98 #define EFFICIENCY_UPDATE_INTERVAL (5 * 1000)
99
100 #define RC_RX_LINK_LOST_MSG "!RC RX LINK LOST!"
101
102 // Adjust OSD_MESSAGE's default position when
103 // changing OSD_MESSAGE_LENGTH
104 #define OSD_MESSAGE_LENGTH 28
105
106 #define OSD_ALTERNATING_CHOICES(ms, num_choices) ((millis() / ms) % num_choices)
107 #define _CONST_STR_SIZE(s) ((sizeof(s)/sizeof(s[0]))-1) // -1 to avoid counting final '\0'
108
109 // Wrap all string constants intenteded for display as messages with
110 // this macro to ensure compile time length validation.
111 #define OSD_MESSAGE_STR(x) ({ \
112 STATIC_ASSERT(_CONST_STR_SIZE(x) <= OSD_MESSAGE_LENGTH, message_string_ ## __COUNTER__ ## _too_long); \
113 x; \
114 })
115
116
117 /*
118 * DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0
119 * DJI uses a subset of messages and assume fixed bit positions for flight modes
120 *
121 * To avoid compatibility issues we maintain a separate MSP command processor
122 * but reuse the packet decoder to minimize code duplication
123 */
124
125 PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 3);
126 PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig,
127 .use_name_for_messages = SETTING_DJI_USE_NAME_FOR_MESSAGES_DEFAULT,
128 .esc_temperature_source = SETTING_DJI_ESC_TEMP_SOURCE_DEFAULT,
129 .messageSpeedSource = SETTING_DJI_MESSAGE_SPEED_SOURCE_DEFAULT,
130 .rssi_source = SETTING_DJI_RSSI_SOURCE_DEFAULT,
131 .useAdjustments = SETTING_DJI_USE_ADJUSTMENTS_DEFAULT,
132 .craftNameAlternatingDuration = SETTING_DJI_CN_ALTERNATING_DURATION_DEFAULT
133 );
134
135 #define RSSI_BOUNDARY(PERCENT) (RSSI_MAX_VALUE / 100 * PERCENT)
136
137 typedef enum {
138 DJI_OSD_CN_MESSAGES,
139 DJI_OSD_CN_THROTTLE,
140 DJI_OSD_CN_THROTTLE_AUTO_THR,
141 DJI_OSD_CN_AIR_SPEED,
142 DJI_OSD_CN_EFFICIENCY,
143 DJI_OSD_CN_DISTANCE,
144 DJI_OSD_CN_ADJUSTEMNTS,
145 DJI_OSD_CN_MAX_ELEMENTS
146 } DjiCraftNameElements_t;
147
148 // External dependency on looptime
149 extern timeDelta_t cycleTime;
150
151 // MSP packet decoder state structure
152 static mspPort_t djiMspPort;
153
154 // Mapping table between DJI PID and INAV PID (order is different)
155 const uint8_t djiPidIndexMap[] = {
156 PID_ROLL, // DJI: PID_ROLL
157 PID_PITCH, // DJI: PID_PITCH
158 PID_YAW, // DJI: PID_YAW
159 PID_LEVEL, // DJI: PID_LEVEL
160 PID_HEADING // DJI: PID_MAG
161 };
162
163 typedef struct {
164 int itemIndex; // INAV OSD item
165 features_e depFeature; // INAV feature that item is dependent on
166 } djiOsdMapping_t;
167
168 const djiOsdMapping_t djiOSDItemIndexMap[] = {
169 { OSD_RSSI_VALUE, 0 }, // DJI: OSD_RSSI_VALUE
170 { OSD_MAIN_BATT_VOLTAGE, FEATURE_VBAT }, // DJI: OSD_MAIN_BATT_VOLTAGE
171 { OSD_CROSSHAIRS, 0 }, // DJI: OSD_CROSSHAIRS
172 { OSD_ARTIFICIAL_HORIZON, 0 }, // DJI: OSD_ARTIFICIAL_HORIZON
173 { OSD_HORIZON_SIDEBARS, 0 }, // DJI: OSD_HORIZON_SIDEBARS
174 { OSD_ONTIME, 0 }, // DJI: OSD_ITEM_TIMER_1
175 { OSD_FLYTIME, 0 }, // DJI: OSD_ITEM_TIMER_2
176 { OSD_FLYMODE, 0 }, // DJI: OSD_FLYMODE
177 { OSD_CRAFT_NAME, 0 }, // DJI: OSD_CRAFT_NAME
178 { OSD_THROTTLE_POS, 0 }, // DJI: OSD_THROTTLE_POS
179 { OSD_VTX_CHANNEL, 0 }, // DJI: OSD_VTX_CHANNEL
180 { OSD_CURRENT_DRAW, FEATURE_CURRENT_METER }, // DJI: OSD_CURRENT_DRAW
181 { OSD_MAH_DRAWN, FEATURE_CURRENT_METER }, // DJI: OSD_MAH_DRAWN
182 { OSD_GPS_SPEED, FEATURE_GPS }, // DJI: OSD_GPS_SPEED
183 { OSD_GPS_SATS, FEATURE_GPS }, // DJI: OSD_GPS_SATS
184 { OSD_ALTITUDE, 0 }, // DJI: OSD_ALTITUDE
185 { OSD_ROLL_PIDS, 0 }, // DJI: OSD_ROLL_PIDS
186 { OSD_PITCH_PIDS, 0 }, // DJI: OSD_PITCH_PIDS
187 { OSD_YAW_PIDS, 0 }, // DJI: OSD_YAW_PIDS
188 { OSD_POWER, 0 }, // DJI: OSD_POWER
189 { -1, 0 }, // DJI: OSD_PIDRATE_PROFILE
190 { -1, 0 }, // DJI: OSD_WARNINGS
191 { OSD_MAIN_BATT_CELL_VOLTAGE, 0 }, // DJI: OSD_AVG_CELL_VOLTAGE
192 { OSD_GPS_LON, FEATURE_GPS }, // DJI: OSD_GPS_LON
193 { OSD_GPS_LAT, FEATURE_GPS }, // DJI: OSD_GPS_LAT
194 { OSD_DEBUG, 0 }, // DJI: OSD_DEBUG
195 { OSD_ATTITUDE_PITCH, 0 }, // DJI: OSD_PITCH_ANGLE
196 { OSD_ATTITUDE_ROLL, 0 }, // DJI: OSD_ROLL_ANGLE
197 { -1, 0 }, // DJI: OSD_MAIN_BATT_USAGE
198 { -1, 0 }, // DJI: OSD_DISARMED
199 { OSD_HOME_DIR, FEATURE_GPS }, // DJI: OSD_HOME_DIR
200 { OSD_HOME_DIST, FEATURE_GPS }, // DJI: OSD_HOME_DIST
201 { OSD_HEADING, 0 }, // DJI: OSD_NUMERICAL_HEADING
202 { OSD_VARIO_NUM, 0 }, // DJI: OSD_NUMERICAL_VARIO
203 { -1, 0 }, // DJI: OSD_COMPASS_BAR
204 { OSD_ESC_TEMPERATURE, 0 }, // DJI: OSD_ESC_TEMPERATURE
205 { OSD_ESC_RPM, 0 }, // DJI: OSD_ESC_RPM
206 { OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, FEATURE_CURRENT_METER }, // DJI: OSD_REMAINING_TIME_ESTIMATE
207 { OSD_RTC_TIME, 0 }, // DJI: OSD_RTC_DATETIME
208 { -1, 0 }, // DJI: OSD_ADJUSTMENT_RANGE
209 { -1, 0 }, // DJI: OSD_CORE_TEMPERATURE
210 { -1, 0 }, // DJI: OSD_ANTI_GRAVITY
211 { -1, 0 }, // DJI: OSD_G_FORCE
212 { -1, 0 }, // DJI: OSD_MOTOR_DIAG
213 { -1, 0 }, // DJI: OSD_LOG_STATUS
214 { -1, 0 }, // DJI: OSD_FLIP_ARROW
215 { -1, 0 }, // DJI: OSD_LINK_QUALITY
216 { OSD_TRIP_DIST, FEATURE_GPS }, // DJI: OSD_FLIGHT_DIST
217 { -1, 0 }, // DJI: OSD_STICK_OVERLAY_LEFT
218 { -1, 0 }, // DJI: OSD_STICK_OVERLAY_RIGHT
219 { -1, 0 }, // DJI: OSD_DISPLAY_NAME
220 { -1, 0 }, // DJI: OSD_ESC_RPM_FREQ
221 { -1, 0 }, // DJI: OSD_RATE_PROFILE_NAME
222 { -1, 0 }, // DJI: OSD_PID_PROFILE_NAME
223 { -1, 0 }, // DJI: OSD_PROFILE_NAME
224 { -1, 0 }, // DJI: OSD_RSSI_DBM_VALUE
225 { -1, 0 }, // DJI: OSD_RC_CHANNELS
226 };
227
228 const int djiOSDStatisticsMap[] = {
229 -1, // DJI: OSD_STAT_RTC_DATE_TIME
230 -1, // DJI: OSD_STAT_TIMER_1
231 -1, // DJI: OSD_STAT_TIMER_2
232 -1, // DJI: OSD_STAT_MAX_SPEED
233 -1, // DJI: OSD_STAT_MAX_DISTANCE
234 -1, // DJI: OSD_STAT_MIN_BATTERY
235 -1, // DJI: OSD_STAT_END_BATTERY
236 -1, // DJI: OSD_STAT_BATTERY
237 -1, // DJI: OSD_STAT_MIN_RSSI
238 -1, // DJI: OSD_STAT_MAX_CURRENT
239 -1, // DJI: OSD_STAT_USED_MAH
240 -1, // DJI: OSD_STAT_MAX_ALTITUDE
241 -1, // DJI: OSD_STAT_BLACKBOX
242 -1, // DJI: OSD_STAT_BLACKBOX_NUMBER
243 -1, // DJI: OSD_STAT_MAX_G_FORCE
244 -1, // DJI: OSD_STAT_MAX_ESC_TEMP
245 -1, // DJI: OSD_STAT_MAX_ESC_RPM
246 -1, // DJI: OSD_STAT_MIN_LINK_QUALITY
247 -1, // DJI: OSD_STAT_FLIGHT_DISTANCE
248 -1, // DJI: OSD_STAT_MAX_FFT
249 -1, // DJI: OSD_STAT_TOTAL_FLIGHTS
250 -1, // DJI: OSD_STAT_TOTAL_TIME
251 -1, // DJI: OSD_STAT_TOTAL_DIST
252 -1, // DJI: OSD_STAT_MIN_RSSI_DBM
253 };
254
255 void djiOsdSerialInit(void)
256 {
257 memset(&djiMspPort, 0, sizeof(mspPort_t));
258
259 serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_DJI_HD_OSD);
260
261 if (!portConfig) {
262 return;
263 }
264
265 serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_DJI_HD_OSD, NULL, NULL, DJI_MSP_BAUDRATE, MODE_RXTX, SERIAL_NOT_INVERTED);
266
267 if (serialPort) {
268 resetMspPort(&djiMspPort, serialPort);
269 }
270 }
271
272 static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
273 {
274 memset(flightModeBitmask, 0, sizeof(boxBitmask_t));
275
276 // Map flight modes to DJI-supported bits
277 switch(getFlightModeForTelemetry()) {
278 case FLM_MANUAL:
279 case FLM_ACRO:
280 case FLM_ACRO_AIR:
281 // DJI: No bits set = ACRO
282 break;
283 case FLM_ANGLE:
284 bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE
285 break;
286 case FLM_HORIZON:
287 bitArraySet(flightModeBitmask->bits, 2); // DJI: 1 << 2
288 break;
289 case FLM_RTH:
290 bitArraySet(flightModeBitmask->bits, 5); // DJI: 1 << 5 : GPS_RESQUE
291 break;
292 case FLM_CRUISE:
293 bitArraySet(flightModeBitmask->bits, 3); // DJI: 1 << 3 : technically HEADFREE
294 break;
295 case FLM_FAILSAFE:
296 bitArraySet(flightModeBitmask->bits, 4); // DJI: 1 << 4
297 break;
298 case FLM_LAUNCH:
299 case FLM_ALTITUDE_HOLD:
300 case FLM_POSITION_HOLD:
301 case FLM_MISSION:
302 case FLM_ANGLEHOLD:
303 default:
304 // Unsupported ATM, keep at ANGLE
305 bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE
306 }
307
308 // Set ARMED mode
309 if (ARMING_FLAG(ARMED)) {
310 bitArraySet(flightModeBitmask->bits, 0); // DJI: 1 << 0 : ARMED
311 }
312 }
313
314 static uint32_t djiPackArmingDisabledFlags(void)
315 {
316 // TODO: Map INAV arming disabled flags to DJI/BF ones
317 // https://github.com/betaflight/betaflight/blob/c6e5882dd91fa20d246b8f8af10cf6c92876bc3d/src/main/fc/runtime_config.h#L42
318 // For now hide everything in ARMING_DISABLED_ARM_SWITCH (bit 24)
319
320 return isArmingDisabled() ? (1 << 24) : 0;
321 }
322
323 #if defined(USE_OSD)
324 static uint32_t djiEncodeOSDEnabledWarnings(void)
325 {
326 // TODO:
327 return 0;
328 }
329
330 static void djiSerializeOSDConfigReply(sbuf_t *dst)
331 {
332 // Only send supported flag - always
333 sbufWriteU8(dst, DJI_OSD_FLAGS_OSD_FEATURE);
334
335 // 7456 video system (AUTO/PAL/NTSC)
336 sbufWriteU8(dst, osdConfig()->video_system);
337
338 // Configuration
339 sbufWriteU8(dst, osdConfig()->units);
340
341 // Alarms
342 sbufWriteU8(dst, osdConfig()->rssi_alarm);
343 sbufWriteU16(dst, currentBatteryProfile->capacity.warning);
344
345 // OSD_ITEM_COUNT (previously was timer alarm)
346 sbufWriteU8(dst, 0);
347 sbufWriteU8(dst, ARRAYLEN(djiOSDItemIndexMap));
348
349 // Altitude alarm
350 sbufWriteU16(dst, osdConfig()->alt_alarm);
351
352 // OSD element position and visibility
353 for (unsigned i = 0; i < ARRAYLEN(djiOSDItemIndexMap); i++) {
354 const int inavOSDIdx = djiOSDItemIndexMap[i].itemIndex;
355
356 // We call OSD item supported if it doesn't have dependencies or all feature-dependencies are satistied
357 const bool itemIsSupported = ((djiOSDItemIndexMap[i].depFeature == 0) || feature(djiOSDItemIndexMap[i].depFeature));
358
359 if (inavOSDIdx >= 0 && itemIsSupported) {
360 // Position & visibility are encoded in 16 bits, and is the same between BF/DJI.
361 // However INAV supports co-ords of 0-63 and has 3 layouts, while BF has co-ords 0-31 and visibility profiles.
362 // Re-encode for co-ords of 0-31 and map the layout to all three BF profiles.
363 uint16_t itemPos = osdLayoutsConfig()->item_pos[0][inavOSDIdx];
364 uint16_t itemPosSD = OSD_POS_SD(OSD_X(itemPos), OSD_Y(itemPos));
365
366 // Workarounds for certain OSD element positions
367 // INAV calculates these dynamically, while DJI expects the config to have defined coordinates
368 switch(inavOSDIdx) {
369 case OSD_CROSSHAIRS:
370 itemPosSD = OSD_POS_SD(15, 8);
371 break;
372
373 case OSD_ARTIFICIAL_HORIZON:
374 itemPosSD = OSD_POS_SD(9, 8);
375 break;
376
377 case OSD_HORIZON_SIDEBARS:
378 itemPosSD = OSD_POS_SD(16, 7);
379 break;
380 }
381
382 // Enforce visibility in 3 BF OSD profiles
383 if (OSD_VISIBLE(itemPos)) {
384 itemPosSD |= (0x3000 | OSD_VISIBLE_FLAG_SD);
385 }
386
387 sbufWriteU16(dst, itemPosSD);
388 }
389 else {
390 // Hide OSD elements unsupported by INAV
391 sbufWriteU16(dst, 0);
392 }
393 }
394
395 // Post flight statistics
396 sbufWriteU8(dst, ARRAYLEN(djiOSDStatisticsMap));
397 for (unsigned i = 0; i < ARRAYLEN(djiOSDStatisticsMap); i++ ) {
398 if (djiOSDStatisticsMap[i] >= 0) {
399 // FIXME: Map post-flight statistics from INAV to BF/DJI
400 sbufWriteU8(dst, 0);
401 }
402 else {
403 sbufWriteU8(dst, 0);
404 }
405 }
406
407 // Timers
408 sbufWriteU8(dst, DJI_OSD_TIMER_COUNT);
409 for (int i = 0; i < DJI_OSD_TIMER_COUNT; i++) {
410 // STUB: We don't support BF's OSD timers
411 sbufWriteU16(dst, 0);
412 }
413
414 // Enabled warnings
415 // API < 1.41 stub, kept for compatibility
416 sbufWriteU16(dst, djiEncodeOSDEnabledWarnings() & 0xFFFF);
417
418 // API >= 1.41
419 // Send the warnings count and 32bit enabled warnings flags.
420 sbufWriteU8(dst, DJI_OSD_WARNING_COUNT);
421 sbufWriteU32(dst, djiEncodeOSDEnabledWarnings());
422
423 // DJI OSD expects 1 OSD profile
424 sbufWriteU8(dst, 1);
425 sbufWriteU8(dst, 1);
426
427 // No OSD stick overlay
428 sbufWriteU8(dst, 0);
429
430 // API >= 1.43
431 // Camera frame element width/height - magic numbers taken from Betaflight source
432 //sbufWriteU8(dst, DJI_OSD_SCREEN_WIDTH); // osdConfig()->camera_frame_width
433 //sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height
434 }
435
436 static char * osdArmingDisabledReasonMessage(void)
437 {
438 switch (isArmingDisabledReason()) {
439 case ARMING_DISABLED_FAILSAFE_SYSTEM:
440 // See handling of FAILSAFE_RX_LOSS_MONITORING in failsafe.c
441 if (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) {
442 if (failsafeIsReceivingRxData()) {
443 // If we're not using sticks, it means the ARM switch
444 // hasn't been off since entering FAILSAFE_RX_LOSS_MONITORING
445 // yet
446 return OSD_MESSAGE_STR("DISARM!");
447 }
448 // Not receiving RX data
449 return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
450 }
451 return OSD_MESSAGE_STR("FAILSAFE");
452 case ARMING_DISABLED_NOT_LEVEL:
453 return OSD_MESSAGE_STR("!LEVEL");
454 case ARMING_DISABLED_SENSORS_CALIBRATING:
455 return OSD_MESSAGE_STR("CALIBRATING");
456 case ARMING_DISABLED_SYSTEM_OVERLOADED:
457 return OSD_MESSAGE_STR("OVERLOAD");
458 case ARMING_DISABLED_NAVIGATION_UNSAFE:
459 // Check the exact reason
460 switch (navigationIsBlockingArming(NULL)) {
461 case NAV_ARMING_BLOCKER_NONE:
462 break;
463 case NAV_ARMING_BLOCKER_MISSING_GPS_FIX:
464 return OSD_MESSAGE_STR("NO GPS FIX");
465 case NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE:
466 return OSD_MESSAGE_STR("DISABLE NAV");
467 case NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR:
468 return OSD_MESSAGE_STR("1ST WYP TOO FAR");
469 case NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR:
470 return OSD_MESSAGE_STR("WYP MISCONFIGURED");
471 }
472 break;
473 case ARMING_DISABLED_COMPASS_NOT_CALIBRATED:
474 return OSD_MESSAGE_STR("COMPS CALIB");
475 case ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED:
476 return OSD_MESSAGE_STR("ACC CALIB");
477 case ARMING_DISABLED_ARM_SWITCH:
478 return OSD_MESSAGE_STR("DISARM!");
479 case ARMING_DISABLED_HARDWARE_FAILURE:
480 return OSD_MESSAGE_STR("ERR HW!");
481 // {
482 // if (!HW_SENSOR_IS_HEALTHY(getHwGyroStatus())) {
483 // return OSD_MESSAGE_STR("GYRO FAILURE");
484 // }
485 // if (!HW_SENSOR_IS_HEALTHY(getHwAccelerometerStatus())) {
486 // return OSD_MESSAGE_STR("ACCELEROMETER FAILURE");
487 // }
488 // if (!HW_SENSOR_IS_HEALTHY(getHwCompassStatus())) {
489 // return OSD_MESSAGE_STR("COMPASS FAILURE");
490 // }
491 // if (!HW_SENSOR_IS_HEALTHY(getHwBarometerStatus())) {
492 // return OSD_MESSAGE_STR("BAROMETER FAILURE");
493 // }
494 // if (!HW_SENSOR_IS_HEALTHY(getHwGPSStatus())) {
495 // return OSD_MESSAGE_STR("GPS FAILURE");
496 // }
497 // if (!HW_SENSOR_IS_HEALTHY(getHwRangefinderStatus())) {
498 // return OSD_MESSAGE_STR("RANGE FINDER FAILURE");
499 // }
500 // if (!HW_SENSOR_IS_HEALTHY(getHwPitotmeterStatus())) {
501 // return OSD_MESSAGE_STR("PITOT METER FAILURE");
502 // }
503 // }
504 // return OSD_MESSAGE_STR("HARDWARE FAILURE");
505 case ARMING_DISABLED_BOXFAILSAFE:
506 return OSD_MESSAGE_STR("FAILSAFE ENABLED");
507 case ARMING_DISABLED_RC_LINK:
508 return OSD_MESSAGE_STR("NO RC LINK");
509 case ARMING_DISABLED_THROTTLE:
510 return OSD_MESSAGE_STR("THROTTLE!");
511 case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED:
512 return OSD_MESSAGE_STR("ROLLPITCH!");
513 case ARMING_DISABLED_SERVO_AUTOTRIM:
514 return OSD_MESSAGE_STR("AUTOTRIM!");
515 case ARMING_DISABLED_OOM:
516 return OSD_MESSAGE_STR("MEM LOW");
517 case ARMING_DISABLED_INVALID_SETTING:
518 return OSD_MESSAGE_STR("ERR SETTING");
519 case ARMING_DISABLED_CLI:
520 return OSD_MESSAGE_STR("CLI");
521 case ARMING_DISABLED_PWM_OUTPUT_ERROR:
522 return OSD_MESSAGE_STR("PWM ERR");
523 case ARMING_DISABLED_NO_PREARM:
524 return OSD_MESSAGE_STR("NO PREARM");
525 case ARMING_DISABLED_DSHOT_BEEPER:
526 return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE");
527 // Cases without message
528 case ARMING_DISABLED_LANDING_DETECTED:
529 FALLTHROUGH;
530 case ARMING_DISABLED_CMS_MENU:
531 FALLTHROUGH;
532 case ARMING_DISABLED_OSD_MENU:
533 FALLTHROUGH;
534 case ARMING_DISABLED_ALL_FLAGS:
535 FALLTHROUGH;
536 case ARMED:
537 FALLTHROUGH;
538 case SIMULATOR_MODE_HITL:
539 FALLTHROUGH;
540 case SIMULATOR_MODE_SITL:
541 FALLTHROUGH;
542 case WAS_EVER_ARMED:
543 break;
544 }
545
546 return NULL;
547 }
548
549 static char * osdFailsafePhaseMessage(void)
550 {
551 // See failsafe.h for each phase explanation
552 switch (failsafePhase()) {
553 case FAILSAFE_RETURN_TO_HOME:
554 // XXX: Keep this in sync with OSD_FLYMODE.
555 return OSD_MESSAGE_STR("(RTH)");
556 case FAILSAFE_LANDING:
557 // This should be considered an emergengy landing
558 return OSD_MESSAGE_STR("(EMRGY LANDING)");
559 case FAILSAFE_RX_LOSS_MONITORING:
560 // Only reachable from FAILSAFE_LANDED, which performs
561 // a disarm. Since aircraft has been disarmed, we no
562 // longer show failsafe details.
563 FALLTHROUGH;
564 case FAILSAFE_LANDED:
565 // Very brief, disarms and transitions into
566 // FAILSAFE_RX_LOSS_MONITORING. Note that it prevents
567 // further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM,
568 // so we'll show the user how to re-arm in when
569 // that flag is the reason to prevent arming.
570 FALLTHROUGH;
571 case FAILSAFE_RX_LOSS_IDLE:
572 // This only happens when user has chosen NONE as FS
573 // procedure. The recovery messages should be enough.
574 FALLTHROUGH;
575 case FAILSAFE_IDLE:
576 // Failsafe not active
577 FALLTHROUGH;
578 case FAILSAFE_RX_LOSS_DETECTED:
579 // Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED
580 // or the FS procedure immediately.
581 FALLTHROUGH;
582 case FAILSAFE_RX_LOSS_RECOVERED:
583 // Exiting failsafe
584 break;
585 }
586
587 return NULL;
588 }
589
590 static char * osdFailsafeInfoMessage(void)
591 {
592 if (failsafeIsReceivingRxData()) {
593 // User must move sticks to exit FS mode
594 return OSD_MESSAGE_STR("!MOVE STICKS TO EXIT FS!");
595 }
596
597 return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
598 }
599
600 static char * navigationStateMessage(void)
601 {
602 switch (NAV_Status.state) {
603 case MW_NAV_STATE_NONE:
604 break;
605 case MW_NAV_STATE_RTH_START:
606 return OSD_MESSAGE_STR("STARTING RTH");
607 case MW_NAV_STATE_RTH_CLIMB:
608 return OSD_MESSAGE_STR("ADJUSTING RTH ALTITUDE");
609 case MW_NAV_STATE_RTH_ENROUTE:
610 // TODO: Break this up between climb and head home
611 return OSD_MESSAGE_STR("EN ROUTE TO HOME");
612 case MW_NAV_STATE_HOLD_INFINIT:
613 // Used by HOLD flight modes. No information to add.
614 break;
615 case MW_NAV_STATE_HOLD_TIMED:
616 // TODO: Maybe we can display a count down
617 return OSD_MESSAGE_STR("HOLDING WAYPOINT");
618 break;
619 case MW_NAV_STATE_WP_ENROUTE:
620 // TODO: Show WP number
621 return OSD_MESSAGE_STR("TO WP");
622 case MW_NAV_STATE_PROCESS_NEXT:
623 return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
624 case MW_NAV_STATE_DO_JUMP:
625 // Not used
626 break;
627 case MW_NAV_STATE_LAND_START:
628 // Not used
629 break;
630 case MW_NAV_STATE_EMERGENCY_LANDING:
631 return OSD_MESSAGE_STR("EMRGY LANDING");
632 case MW_NAV_STATE_LAND_IN_PROGRESS:
633 return OSD_MESSAGE_STR("LANDING");
634 case MW_NAV_STATE_HOVER_ABOVE_HOME:
635 if (STATE(FIXED_WING_LEGACY)) {
636 return OSD_MESSAGE_STR("LOITERING AROUND HOME");
637 }
638 return OSD_MESSAGE_STR("HOVERING");
639 case MW_NAV_STATE_LANDED:
640 return OSD_MESSAGE_STR("LANDED");
641 case MW_NAV_STATE_LAND_SETTLE:
642 return OSD_MESSAGE_STR("PREPARING TO LAND");
643 case MW_NAV_STATE_LAND_START_DESCENT:
644 // Not used
645 break;
646 }
647 return NULL;
648 }
649
650
651 /**
652 * Converts velocity based on the current unit system (kmh or mph).
653 * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/second)
654 */
655 static int32_t osdConvertVelocityToUnit(int32_t vel)
656 {
657 switch (osdConfig()->units) {
658 case OSD_UNIT_UK:
659 FALLTHROUGH;
660 case OSD_UNIT_METRIC_MPH:
661 FALLTHROUGH;
662 case OSD_UNIT_IMPERIAL:
663 return CMSEC_TO_CENTIMPH(vel) / 100; // Convert to mph
664 case OSD_UNIT_GA:
665 return CMSEC_TO_CENTIKNOTS(vel) / 100; // Convert to Knots
666 case OSD_UNIT_METRIC:
667 return CMSEC_TO_CENTIKPH(vel) / 100; // Convert to kmh
668 }
669
670 // Unreachable
671 return -1;
672 }
673
674 /**
675 * Converts velocity into a string based on the current unit system.
676 * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
677 */
678 void osdDJIFormatVelocityStr(char* buff)
679 {
680 char sourceBuf[4];
681 int vel = 0;
682 switch (djiOsdConfig()->messageSpeedSource) {
683 case OSD_SPEED_SOURCE_GROUND:
684 strcpy(sourceBuf, "GRD");
685 vel = gpsSol.groundSpeed;
686 break;
687 case OSD_SPEED_SOURCE_3D:
688 strcpy(sourceBuf, "3D");
689 vel = osdGet3DSpeed();
690 break;
691 case OSD_SPEED_SOURCE_AIR:
692 strcpy(sourceBuf, "AIR");
693 #ifdef USE_PITOT
694 vel = getAirspeedEstimate();
695 #endif
696 break;
697 }
698
699 switch (osdConfig()->units) {
700 case OSD_UNIT_UK:
701 FALLTHROUGH;
702 case OSD_UNIT_METRIC_MPH:
703 FALLTHROUGH;
704 case OSD_UNIT_IMPERIAL:
705 tfp_sprintf(buff, "%s %3d MPH", sourceBuf, (int)osdConvertVelocityToUnit(vel));
706 break;
707 case OSD_UNIT_GA:
708 tfp_sprintf(buff, "%s %3d KT", sourceBuf, (int)osdConvertVelocityToUnit(vel));
709 break;
710 case OSD_UNIT_METRIC:
711 tfp_sprintf(buff, "%s %3d KPH", sourceBuf, (int)osdConvertVelocityToUnit(vel));
712 break;
713 }
714 }
715 static void osdDJIFormatThrottlePosition(char *buff, bool autoThr )
716 {
717 int16_t thr = rxGetChannelValue(THROTTLE);
718 if (autoThr && navigationIsControllingThrottle()) {
719 thr = rcCommand[THROTTLE];
720 }
721
722 tfp_sprintf(buff, "%3ld%s", (unsigned long)((constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)), "%THR");
723 }
724
725 /**
726 * Converts distance into a string based on the current unit system.
727 * @param dist Distance in centimeters
728 */
729 static void osdDJIFormatDistanceStr(char *buff, int32_t dist)
730 {
731 int32_t centifeet;
732
733 switch (osdConfig()->units) {
734 case OSD_UNIT_UK:
735 FALLTHROUGH;
736 case OSD_UNIT_IMPERIAL:
737 centifeet = CENTIMETERS_TO_CENTIFEET(dist);
738 if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
739 // Show feet when dist < 0.5mi
740 tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT");
741 }
742 else {
743 // Show miles when dist >= 0.5mi
744 tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)),
745 (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi");
746 }
747 break;
748 case OSD_UNIT_GA:
749 centifeet = CENTIMETERS_TO_CENTIFEET(dist);
750 if (abs(centifeet) < FEET_PER_NAUTICALMILE * 100 / 2) {
751 // Show feet when dist < 0.5mi
752 tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT");
753 }
754 else {
755 // Show miles when dist >= 0.5mi
756 tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100 * FEET_PER_NAUTICALMILE)),
757 (int)((abs(centifeet) % (int)(100 * FEET_PER_NAUTICALMILE)) / FEET_PER_NAUTICALMILE), "NM");
758 }
759 break;
760 case OSD_UNIT_METRIC_MPH:
761 FALLTHROUGH;
762 case OSD_UNIT_METRIC:
763 if (abs(dist) < METERS_PER_KILOMETER * 100) {
764 // Show meters when dist < 1km
765 tfp_sprintf(buff, "%d%s", (int)(dist / 100), "M");
766 }
767 else {
768 // Show kilometers when dist >= 1km
769 tfp_sprintf(buff, "%d.%02d%s", (int)(dist / (100*METERS_PER_KILOMETER)),
770 (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, "KM");
771 }
772 break;
773 }
774 }
775
776 static void osdDJIEfficiencyMahPerKM(char *buff)
777 {
778 // amperage is in centi amps, speed is in cms/s. We want
779 // mah/km. Values over 999 are considered useless and
780 // displayed as "---""
781 static pt1Filter_t eFilterState;
782 static timeUs_t efficiencyUpdated = 0;
783 int32_t value = 0;
784 timeUs_t currentTimeUs = micros();
785 timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
786
787 if ((STATE(GPS_FIX)
788 #ifdef USE_GPS_FIX_ESTIMATION
789 || STATE(GPS_ESTIMATED_FIX)
790 #endif
791 ) && gpsSol.groundSpeed > 0) {
792 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
793 value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
794 1, US2S(efficiencyTimeDelta));
795
796 efficiencyUpdated = currentTimeUs;
797 } else {
798 value = eFilterState.state;
799 }
800 }
801
802 if (value > 0 && value <= 999) {
803 tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM");
804 } else {
805 tfp_sprintf(buff, "%s", "---mAhKM");
806 }
807 }
808
809 static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
810 {
811 switch (adjustmentFunction) {
812 case ADJUSTMENT_RC_EXPO:
813 tfp_sprintf(buff, "RCE %d", currentControlRateProfile->stabilized.rcExpo8);
814 break;
815 case ADJUSTMENT_RC_YAW_EXPO:
816 tfp_sprintf(buff, "RCYE %3d", currentControlRateProfile->stabilized.rcYawExpo8);
817 break;
818 case ADJUSTMENT_MANUAL_RC_EXPO:
819 tfp_sprintf(buff, "MRCE %3d", currentControlRateProfile->manual.rcExpo8);
820 break;
821 case ADJUSTMENT_MANUAL_RC_YAW_EXPO:
822 tfp_sprintf(buff, "MRCYE %3d", currentControlRateProfile->manual.rcYawExpo8);
823 break;
824 case ADJUSTMENT_THROTTLE_EXPO:
825 tfp_sprintf(buff, "TE %3d", currentControlRateProfile->throttle.rcExpo8);
826 break;
827 case ADJUSTMENT_PITCH_ROLL_RATE:
828 tfp_sprintf(buff, "PRR %3d %3d", currentControlRateProfile->stabilized.rates[FD_PITCH], currentControlRateProfile->stabilized.rates[FD_ROLL]);
829 break;
830 case ADJUSTMENT_PITCH_RATE:
831 tfp_sprintf(buff, "PR %3d", currentControlRateProfile->stabilized.rates[FD_PITCH]);
832 break;
833 case ADJUSTMENT_ROLL_RATE:
834 tfp_sprintf(buff, "RR %3d", currentControlRateProfile->stabilized.rates[FD_ROLL]);
835 break;
836 case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
837 tfp_sprintf(buff, "MPRR %3d %3d", currentControlRateProfile->manual.rates[FD_PITCH], currentControlRateProfile->manual.rates[FD_ROLL]);
838 break;
839 case ADJUSTMENT_MANUAL_PITCH_RATE:
840 tfp_sprintf(buff, "MPR %3d", currentControlRateProfile->manual.rates[FD_PITCH]);
841 break;
842 case ADJUSTMENT_MANUAL_ROLL_RATE:
843 tfp_sprintf(buff, "MRR %3d", currentControlRateProfile->manual.rates[FD_ROLL]);
844 break;
845 case ADJUSTMENT_YAW_RATE:
846 tfp_sprintf(buff, "YR %3d", currentControlRateProfile->stabilized.rates[FD_YAW]);
847 break;
848 case ADJUSTMENT_MANUAL_YAW_RATE:
849 tfp_sprintf(buff, "MYR %3d", currentControlRateProfile->manual.rates[FD_YAW]);
850 break;
851 case ADJUSTMENT_PITCH_ROLL_P:
852 tfp_sprintf(buff, "PRP %3d %3d", pidBankMutable()->pid[PID_PITCH].P, pidBankMutable()->pid[PID_ROLL].P);
853 break;
854 case ADJUSTMENT_PITCH_P:
855 tfp_sprintf(buff, "PP %3d", pidBankMutable()->pid[PID_PITCH].P);
856 break;
857 case ADJUSTMENT_ROLL_P:
858 tfp_sprintf(buff, "RP %3d", pidBankMutable()->pid[PID_ROLL].P);
859 break;
860 case ADJUSTMENT_PITCH_ROLL_I:
861 tfp_sprintf(buff, "PRI %3d %3d", pidBankMutable()->pid[PID_PITCH].I, pidBankMutable()->pid[PID_ROLL].I);
862 break;
863 case ADJUSTMENT_PITCH_I:
864 tfp_sprintf(buff, "PI %3d", pidBankMutable()->pid[PID_PITCH].I);
865 break;
866 case ADJUSTMENT_ROLL_I:
867 tfp_sprintf(buff, "RI %3d", pidBankMutable()->pid[PID_ROLL].I);
868 break;
869 case ADJUSTMENT_PITCH_ROLL_D:
870 tfp_sprintf(buff, "PRD %3d %3d", pidBankMutable()->pid[PID_PITCH].D, pidBankMutable()->pid[PID_ROLL].D);
871 break;
872 case ADJUSTMENT_PITCH_ROLL_FF:
873 tfp_sprintf(buff, "PRFF %3d %3d", pidBankMutable()->pid[PID_PITCH].FF, pidBankMutable()->pid[PID_ROLL].FF);
874 break;
875 case ADJUSTMENT_PITCH_D:
876 tfp_sprintf(buff, "PD %3d", pidBankMutable()->pid[PID_PITCH].D);
877 break;
878 case ADJUSTMENT_PITCH_FF:
879 tfp_sprintf(buff, "PFF %3d", pidBankMutable()->pid[PID_PITCH].FF);
880 break;
881 case ADJUSTMENT_ROLL_D:
882 tfp_sprintf(buff, "RD %3d", pidBankMutable()->pid[PID_ROLL].D);
883 break;
884 case ADJUSTMENT_ROLL_FF:
885 tfp_sprintf(buff, "RFF %3d", pidBankMutable()->pid[PID_ROLL].FF);
886 break;
887 case ADJUSTMENT_YAW_P:
888 tfp_sprintf(buff, "YP %3d", pidBankMutable()->pid[PID_YAW].P);
889 break;
890 case ADJUSTMENT_YAW_I:
891 tfp_sprintf(buff, "YI %3d", pidBankMutable()->pid[PID_YAW].I);
892 break;
893 case ADJUSTMENT_YAW_D:
894 tfp_sprintf(buff, "YD %3d", pidBankMutable()->pid[PID_YAW].D);
895 break;
896 case ADJUSTMENT_YAW_FF:
897 tfp_sprintf(buff, "YFF %3d", pidBankMutable()->pid[PID_YAW].FF);
898 break;
899 case ADJUSTMENT_NAV_FW_CRUISE_THR:
900 tfp_sprintf(buff, "CR %4d", currentBatteryProfileMutable->nav.fw.cruise_throttle);
901 break;
902 case ADJUSTMENT_NAV_FW_PITCH2THR:
903 tfp_sprintf(buff, "P2T %3d", currentBatteryProfileMutable->nav.fw.pitch_to_throttle);
904 break;
905 case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
906 tfp_sprintf(buff, "RBA %3d", boardAlignment()->rollDeciDegrees);
907 break;
908 case ADJUSTMENT_PITCH_BOARD_ALIGNMENT:
909 tfp_sprintf(buff, "PBA %3d", boardAlignment()->pitchDeciDegrees);
910 break;
911 case ADJUSTMENT_LEVEL_P:
912 tfp_sprintf(buff, "LP %3d", pidBankMutable()->pid[PID_LEVEL].P);
913 break;
914 case ADJUSTMENT_LEVEL_I:
915 tfp_sprintf(buff, "LI %3d", pidBankMutable()->pid[PID_LEVEL].I);
916 break;
917 case ADJUSTMENT_LEVEL_D:
918 tfp_sprintf(buff, "LD %3d", pidBankMutable()->pid[PID_LEVEL].D);
919 break;
920 case ADJUSTMENT_POS_XY_P:
921 tfp_sprintf(buff, "PXYP %3d", pidBankMutable()->pid[PID_POS_XY].P);
922 break;
923 case ADJUSTMENT_POS_XY_I:
924 tfp_sprintf(buff, "PXYI %3d", pidBankMutable()->pid[PID_POS_XY].I);
925 break;
926 case ADJUSTMENT_POS_XY_D:
927 tfp_sprintf(buff, "PXYD %3d", pidBankMutable()->pid[PID_POS_XY].D);
928 break;
929 case ADJUSTMENT_POS_Z_P:
930 tfp_sprintf(buff, "PZP %3d", pidBankMutable()->pid[PID_POS_Z].P);
931 break;
932 case ADJUSTMENT_POS_Z_I:
933 tfp_sprintf(buff, "PZI %3d", pidBankMutable()->pid[PID_POS_Z].I);
934 break;
935 case ADJUSTMENT_POS_Z_D:
936 tfp_sprintf(buff, "PZD %3d", pidBankMutable()->pid[PID_POS_Z].D);
937 break;
938 case ADJUSTMENT_HEADING_P:
939 tfp_sprintf(buff, "HP %3d", pidBankMutable()->pid[PID_HEADING].P);
940 break;
941 case ADJUSTMENT_VEL_XY_P:
942 tfp_sprintf(buff, "VXYP %3d", pidBankMutable()->pid[PID_VEL_XY].P);
943 break;
944 case ADJUSTMENT_VEL_XY_I:
945 tfp_sprintf(buff, "VXYI %3d", pidBankMutable()->pid[PID_VEL_XY].I);
946 break;
947 case ADJUSTMENT_VEL_XY_D:
948 tfp_sprintf(buff, "VXYD %3d", pidBankMutable()->pid[PID_VEL_XY].D);
949 break;
950 case ADJUSTMENT_VEL_Z_P:
951 tfp_sprintf(buff, "VZP %3d", pidBankMutable()->pid[PID_VEL_Z].P);
952 break;
953 case ADJUSTMENT_VEL_Z_I:
954 tfp_sprintf(buff, "VZI %3d", pidBankMutable()->pid[PID_VEL_Z].I);
955 break;
956 case ADJUSTMENT_VEL_Z_D:
957 tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D);
958 break;
959 case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
960 tfp_sprintf(buff, "MTDPA %4d", navConfigMutable()->fw.minThrottleDownPitchAngle);
961 break;
962 case ADJUSTMENT_TPA:
963 tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID);
964 break;
965 case ADJUSTMENT_TPA_BREAKPOINT:
966 tfp_sprintf(buff, "TPABP %4d", currentControlRateProfile->throttle.pa_breakpoint);
967 break;
968 case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS:
969 tfp_sprintf(buff, "CSM %3d", navConfigMutable()->fw.control_smoothness);
970 break;
971 #ifdef USE_MULTI_MISSION
972 case ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX:
973 tfp_sprintf(buff, "WPI %3d", navConfigMutable()->general.waypoint_multi_mission_index);
974 break;
975 #endif
976 default:
977 tfp_sprintf(buff, "UNSUPPORTED");
978 break;
979 }
980 }
981
982 static bool osdDJIFormatAdjustments(char *buff)
983 {
984 uint8_t adjustmentFunctions[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
985 uint8_t adjustmentCount = getActiveAdjustmentFunctions(adjustmentFunctions);
986
987 if (adjustmentCount > 0 && buff != NULL) {
988 osdDJIAdjustmentMessage(buff, adjustmentFunctions[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, adjustmentCount)]);
989 }
990
991 return adjustmentCount > 0;
992 }
993
994
995 static bool djiFormatMessages(char *buff)
996 {
997 bool haveMessage = false;
998 char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
999 if (ARMING_FLAG(ARMED)) {
1000 // Aircraft is armed. We might have up to 6
1001 // messages to show.
1002 char *messages[6];
1003 unsigned messageCount = 0;
1004
1005 if (FLIGHT_MODE(FAILSAFE_MODE)) {
1006 // In FS mode while being armed too
1007 char *failsafePhaseMessage = osdFailsafePhaseMessage();
1008 char *failsafeInfoMessage = osdFailsafeInfoMessage();
1009 char *navStateFSMessage = navigationStateMessage();
1010
1011 if (failsafePhaseMessage) {
1012 messages[messageCount++] = failsafePhaseMessage;
1013 }
1014
1015 if (failsafeInfoMessage) {
1016 messages[messageCount++] = failsafeInfoMessage;
1017 }
1018
1019 if (navStateFSMessage) {
1020 messages[messageCount++] = navStateFSMessage;
1021 }
1022 } else {
1023 #ifdef USE_SERIALRX_CRSF
1024 if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ && rxLinkStatistics.rfMode == 0) {
1025 messages[messageCount++] = "CRSF LOW RF";
1026 }
1027 #endif
1028 if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
1029 char *navStateMessage = navigationStateMessage();
1030 if (navStateMessage) {
1031 messages[messageCount++] = navStateMessage;
1032 }
1033 } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
1034 messages[messageCount++] = "AUTOLAUNCH";
1035 } else {
1036 if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
1037 // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
1038 // when it doesn't require ANGLE mode (required only in FW
1039 // right now). If if requires ANGLE, its display is handled
1040 // by OSD_FLYMODE.
1041 messages[messageCount++] = "(ALT HOLD)";
1042 }
1043
1044 if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
1045 messages[messageCount++] = "(AUTOTRIM)";
1046 }
1047
1048 if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
1049 messages[messageCount++] = "(AUTOTUNE)";
1050 }
1051
1052 if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)) {
1053 messages[messageCount++] = "(AUTO LEVEL TRIM)";
1054 }
1055
1056 if (FLIGHT_MODE(HEADFREE_MODE)) {
1057 messages[messageCount++] = "(HEADFREE)";
1058 }
1059
1060 if (FLIGHT_MODE(MANUAL_MODE)) {
1061 messages[messageCount++] = "(MANUAL)";
1062 }
1063
1064 if (FLIGHT_MODE(NAV_FW_AUTOLAND)) {
1065 messages[messageCount++] = "(LAND)";
1066 }
1067 }
1068 }
1069 // Pick one of the available messages. Each message lasts
1070 // a second.
1071 if (messageCount > 0) {
1072 strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]);
1073 haveMessage = true;
1074 }
1075 } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
1076 unsigned invalidIndex;
1077 // Check if we're unable to arm for some reason
1078 if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
1079 if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) {
1080 const setting_t *setting = settingGet(invalidIndex);
1081 settingGetName(setting, messageBuf);
1082 for (int ii = 0; messageBuf[ii]; ii++) {
1083 messageBuf[ii] = sl_toupper(messageBuf[ii]);
1084 }
1085 strcpy(buff, messageBuf);
1086 } else {
1087 strcpy(buff, "ERR SETTING");
1088 // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
1089 }
1090 } else {
1091 if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) {
1092 strcpy(buff, "CANT ARM");
1093 // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
1094 } else {
1095 // Show the reason for not arming
1096 strcpy(buff, osdArmingDisabledReasonMessage());
1097 }
1098 }
1099 haveMessage = true;
1100 }
1101 return haveMessage;
1102 }
1103
1104 static void djiSerializeCraftNameOverride(sbuf_t *dst)
1105 {
1106 char djibuf[DJI_CRAFT_NAME_LENGTH] = "\0";
1107 uint16_t *osdLayoutConfig = (uint16_t*)(osdLayoutsConfig()->item_pos[0]);
1108
1109 if (!(OSD_VISIBLE(osdLayoutConfig[OSD_MESSAGES]) && djiFormatMessages(djibuf))
1110 && !(djiOsdConfig()->useAdjustments && osdDJIFormatAdjustments(djibuf))) {
1111
1112 DjiCraftNameElements_t activeElements[DJI_OSD_CN_MAX_ELEMENTS];
1113 uint8_t activeElementsCount = 0;
1114
1115 if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS])) {
1116 activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE;
1117 }
1118
1119 if (OSD_VISIBLE(osdLayoutConfig[OSD_SCALED_THROTTLE_POS])) {
1120 activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE_AUTO_THR;
1121 }
1122
1123 if (OSD_VISIBLE(osdLayoutConfig[OSD_3D_SPEED])) {
1124 activeElements[activeElementsCount++] = DJI_OSD_CN_AIR_SPEED;
1125 }
1126
1127 if (OSD_VISIBLE(osdLayoutConfig[OSD_EFFICIENCY_MAH_PER_KM])) {
1128 activeElements[activeElementsCount++] = DJI_OSD_CN_EFFICIENCY;
1129 }
1130
1131 if (OSD_VISIBLE(osdLayoutConfig[OSD_TRIP_DIST])) {
1132 activeElements[activeElementsCount++] = DJI_OSD_CN_DISTANCE;
1133 }
1134
1135 switch (activeElements[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, activeElementsCount)])
1136 {
1137 case DJI_OSD_CN_THROTTLE:
1138 osdDJIFormatThrottlePosition(djibuf, false);
1139 break;
1140 case DJI_OSD_CN_THROTTLE_AUTO_THR:
1141 osdDJIFormatThrottlePosition(djibuf, true);
1142 break;
1143 case DJI_OSD_CN_AIR_SPEED:
1144 osdDJIFormatVelocityStr(djibuf);
1145 break;
1146 case DJI_OSD_CN_EFFICIENCY:
1147 osdDJIEfficiencyMahPerKM(djibuf);
1148 break;
1149 case DJI_OSD_CN_DISTANCE:
1150 osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance());
1151 break;
1152 default:
1153 break;
1154 }
1155 }
1156
1157 if (djibuf[0] != '\0') {
1158 sbufWriteData(dst, djibuf, strlen(djibuf));
1159 }
1160 }
1161
1162 #endif
1163
1164
1165 static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
1166 {
1167 UNUSED(mspPostProcessFn);
1168
1169 sbuf_t *dst = &reply->buf;
1170 sbuf_t *src = &cmd->buf;
1171
1172 // Start initializing the reply message
1173 reply->cmd = cmd->cmd;
1174 reply->result = MSP_RESULT_ACK;
1175
1176 switch (cmd->cmd) {
1177 case DJI_MSP_API_VERSION:
1178 sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
1179 sbufWriteU8(dst, DJI_API_VERSION_MAJOR);
1180 sbufWriteU8(dst, DJI_API_VERSION_MINOR);
1181 break;
1182
1183 case DJI_MSP_FC_VARIANT:
1184 {
1185 const char * const flightControllerIdentifier = INAV_IDENTIFIER;
1186 sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
1187 }
1188 break;
1189
1190 case DJI_MSP_FC_VERSION:
1191 sbufWriteU8(dst, 4);
1192 sbufWriteU8(dst, 1);
1193 sbufWriteU8(dst, 0);
1194 break;
1195
1196 case DJI_MSP_NAME:
1197 {
1198 #if defined(USE_OSD)
1199 if (djiOsdConfig()->use_name_for_messages) {
1200 djiSerializeCraftNameOverride(dst);
1201 } else {
1202 #endif
1203 sbufWriteData(dst, systemConfig()->craftName, (int)strlen(systemConfig()->craftName));
1204 #if defined(USE_OSD)
1205 }
1206 #endif
1207
1208 break;
1209 }
1210 break;
1211
1212 case DJI_MSP_STATUS:
1213 case DJI_MSP_STATUS_EX:
1214 {
1215 // DJI OSD relies on a statically defined bit order and doesn't use MSP_BOXIDS
1216 // to get actual BOX order. We need a special packBoxModeFlags()
1217 boxBitmask_t flightModeBitmask;
1218 djiPackBoxModeBitmask(&flightModeBitmask);
1219
1220 sbufWriteU16(dst, (uint16_t)cycleTime);
1221 sbufWriteU16(dst, 0);
1222 sbufWriteU16(dst, packSensorStatus());
1223 sbufWriteData(dst, &flightModeBitmask, 4); // unconditional part of flags, first 32 bits
1224 sbufWriteU8(dst, getConfigProfile());
1225
1226 sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
1227 if (cmd->cmd == MSP_STATUS_EX) {
1228 sbufWriteU8(dst, 3); // PID_PROFILE_COUNT
1229 sbufWriteU8(dst, 1); // getCurrentControlRateProfileIndex()
1230 } else {
1231 sbufWriteU16(dst, cycleTime); // gyro cycle time
1232 }
1233
1234 // Cap BoxModeFlags to 32 bits
1235 // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
1236 sbufWriteU8(dst, 0);
1237 // sbufWriteData(dst, ((uint8_t*)&flightModeBitmask) + 4, byteCount);
1238
1239 // Write arming disable flags
1240 sbufWriteU8(dst, DJI_ARMING_DISABLE_FLAGS_COUNT);
1241 sbufWriteU32(dst, djiPackArmingDisabledFlags());
1242
1243 // Extra flags
1244 sbufWriteU8(dst, 0);
1245 }
1246 break;
1247
1248 case DJI_MSP_RC:
1249 // Only send sticks (first 4 channels)
1250 for (int i = 0; i < STICK_CHANNEL_COUNT; i++) {
1251 sbufWriteU16(dst, rxGetChannelValue(i));
1252 }
1253 break;
1254
1255 case DJI_MSP_RAW_GPS:
1256 sbufWriteU8(dst, gpsSol.fixType);
1257 sbufWriteU8(dst, gpsSol.numSat);
1258 sbufWriteU32(dst, gpsSol.llh.lat);
1259 sbufWriteU32(dst, gpsSol.llh.lon);
1260 sbufWriteU16(dst, gpsSol.llh.alt / 100);
1261 sbufWriteU16(dst, osdGetSpeedFromSelectedSource());
1262 sbufWriteU16(dst, gpsSol.groundCourse);
1263 break;
1264
1265 case DJI_MSP_COMP_GPS:
1266 sbufWriteU16(dst, GPS_distanceToHome);
1267 sbufWriteU16(dst, GPS_directionToHome);
1268 sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0);
1269 break;
1270
1271 case DJI_MSP_ATTITUDE:
1272 sbufWriteU16(dst, attitude.values.roll);
1273 sbufWriteU16(dst, attitude.values.pitch);
1274 sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
1275 break;
1276
1277 case DJI_MSP_ALTITUDE:
1278 sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
1279 sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
1280 break;
1281
1282 case DJI_MSP_ANALOG:
1283 sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255));
1284 sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
1285 #ifdef USE_SERIALRX_CRSF
1286 // Range of RSSI field: 0-99: 99 = 150 hz , 0 - 98 50 hz / 4 hz
1287 if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ) {
1288 uint16_t scaledLq = 0;
1289 if (rxLinkStatistics.rfMode >= 2) {
1290 scaledLq = RSSI_MAX_VALUE;
1291 } else {
1292 scaledLq = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 0, RSSI_BOUNDARY(98));
1293 }
1294 sbufWriteU16(dst, scaledLq);
1295 } else {
1296 #endif
1297 sbufWriteU16(dst, getRSSI());
1298 #ifdef USE_SERIALRX_CRSF
1299 }
1300 #endif
1301 sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
1302 sbufWriteU16(dst, getBatteryVoltage());
1303 break;
1304
1305 case DJI_MSP_PID:
1306 for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++) {
1307 sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].P);
1308 sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].I);
1309 sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].D);
1310 }
1311 break;
1312
1313 case DJI_MSP_BATTERY_STATE:
1314 // Battery characteristics
1315 sbufWriteU8(dst, constrain(getBatteryCellCount(), 0, 255));
1316 sbufWriteU16(dst, currentBatteryProfile->capacity.value);
1317
1318 // Battery state
1319 sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); // in 0.1V steps
1320 sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF));
1321 sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF));
1322
1323 // Battery alerts - used values match Betaflight's/DJI's
1324 sbufWriteU8(dst, getBatteryState());
1325
1326 // Additional battery voltage field (in 0.01V steps)
1327 sbufWriteU16(dst, getBatteryVoltage());
1328 break;
1329
1330 case DJI_MSP_RTC:
1331 {
1332 dateTime_t datetime;
1333
1334 // We don't care about validity here - dt will be always set to a sane value
1335 // rtcGetDateTime() will call rtcGetDefaultDateTime() internally
1336 rtcGetDateTime(&datetime);
1337
1338 sbufWriteU16(dst, datetime.year);
1339 sbufWriteU8(dst, datetime.month);
1340 sbufWriteU8(dst, datetime.day);
1341 sbufWriteU8(dst, datetime.hours);
1342 sbufWriteU8(dst, datetime.minutes);
1343 sbufWriteU8(dst, datetime.seconds);
1344 sbufWriteU16(dst, datetime.millis);
1345 }
1346 break;
1347
1348 case DJI_MSP_ESC_SENSOR_DATA:
1349 {
1350 uint16_t protoRpm = 0;
1351 int16_t protoTemp = 0;
1352
1353 #if defined(USE_ESC_SENSOR)
1354 if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
1355 uint32_t motorRpmAcc = 0;
1356 int32_t motorTempAcc = 0;
1357
1358 for (int i = 0; i < getMotorCount(); i++) {
1359 const escSensorData_t * escSensor = getEscTelemetry(i);
1360 motorRpmAcc += escSensor->rpm;
1361 motorTempAcc += escSensor->temperature;
1362 }
1363
1364 protoRpm = motorRpmAcc / getMotorCount();
1365 protoTemp = motorTempAcc / getMotorCount();
1366 }
1367 #endif
1368
1369 switch (djiOsdConfig()->esc_temperature_source) {
1370 // This is ESC temperature (as intended)
1371 case DJI_OSD_TEMP_ESC:
1372 // No-op, temperature is already set to ESC
1373 break;
1374
1375 // Re-purpose the field for core temperature
1376 case DJI_OSD_TEMP_CORE:
1377 getIMUTemperature(&protoTemp);
1378 protoTemp = protoTemp / 10;
1379 break;
1380
1381 // Re-purpose the field for baro temperature
1382 case DJI_OSD_TEMP_BARO:
1383 getBaroTemperature(&protoTemp);
1384 protoTemp = protoTemp / 10;
1385 break;
1386 }
1387
1388 // No motor count, just raw temp and RPM data
1389 sbufWriteU8(dst, protoTemp);
1390 sbufWriteU16(dst, protoRpm);
1391 }
1392 break;
1393
1394 case DJI_MSP_OSD_CONFIG:
1395 #if defined(USE_OSD)
1396 // This involved some serious magic, better contain in a separate function for readability
1397 djiSerializeOSDConfigReply(dst);
1398 #else
1399 sbufWriteU8(dst, 0);
1400 #endif
1401 break;
1402
1403 case DJI_MSP_FILTER_CONFIG:
1404 sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz
1405 sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz
1406 sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz
1407 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_1
1408 sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
1409 sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hz
1410 sbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoff
1411 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2
1412 sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_2
1413 sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_type
1414 sbufWriteU8(dst, GYRO_LPF_256HZ); // BF: gyroConfig()->gyro_hardware_lpf);
1415 sbufWriteU8(dst, 0); // BF: DEPRECATED: gyro_32khz_hardware_lpf
1416 sbufWriteU16(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz);
1417 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_lowpass2_hz);
1418 sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass_type);
1419 sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass2_type);
1420 sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_lowpass2_hz);
1421 sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter2_type);
1422 break;
1423
1424 case DJI_MSP_RC_TUNING:
1425 sbufWriteU8(dst, 100); // INAV doesn't use rcRate
1426 sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
1427 for (int i = 0 ; i < 3; i++) {
1428 // R,P,Y rates see flight_dynamics_index_t
1429 sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]);
1430 }
1431 sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
1432 sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8);
1433 sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8);
1434 sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint);
1435 sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8);
1436 sbufWriteU8(dst, 100); // INAV doesn't use rcRate
1437 sbufWriteU8(dst, 100); // INAV doesn't use rcRate
1438 sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
1439
1440 // added in 1.41
1441 sbufWriteU8(dst, 0);
1442 sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
1443 break;
1444
1445 case DJI_MSP_SET_PID:
1446 // Check if we have enough data for all PID coefficients
1447 if ((unsigned)sbufBytesRemaining(src) >= ARRAYLEN(djiPidIndexMap) * 3) {
1448 for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++) {
1449 pidBankMutable()->pid[djiPidIndexMap[i]].P = sbufReadU8(src);
1450 pidBankMutable()->pid[djiPidIndexMap[i]].I = sbufReadU8(src);
1451 pidBankMutable()->pid[djiPidIndexMap[i]].D = sbufReadU8(src);
1452 }
1453 schedulePidGainsUpdate();
1454 }
1455 else {
1456 reply->result = MSP_RESULT_ERROR;
1457 }
1458 break;
1459
1460 case DJI_MSP_PID_ADVANCED:
1461 // TODO
1462 reply->result = MSP_RESULT_ERROR;
1463 break;
1464
1465 case DJI_MSP_SET_FILTER_CONFIG:
1466 case DJI_MSP_SET_PID_ADVANCED:
1467 case DJI_MSP_SET_RC_TUNING:
1468 // TODO
1469 reply->result = MSP_RESULT_ERROR;
1470 break;
1471
1472 default:
1473 // debug[1]++;
1474 // debug[2] = cmd->cmd;
1475 reply->result = MSP_RESULT_ERROR;
1476 break;
1477 }
1478
1479 // Process DONT_REPLY flag
1480 if (cmd->flags & MSP_FLAG_DONT_REPLY) {
1481 reply->result = MSP_RESULT_NO_REPLY;
1482 }
1483
1484 return reply->result;
1485 }
1486
1487 void djiOsdSerialProcess(void)
1488 {
1489 // Check if DJI OSD is configured
1490 if (!djiMspPort.port) {
1491 return;
1492 }
1493
1494 // Piggyback on existing MSP protocol stack, but pass our special command processing function
1495 mspSerialProcessOnePort(&djiMspPort, MSP_SKIP_NON_MSP_DATA, djiProcessMspCommand);
1496 }
1497
1498 #endif
INAV: Navigation-enabled flight control software