?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Remove unnecessary parameters from profiles
[inav.git] / src / main / io / osd_dji_hd.c
blob70b14a69cb1796171cdee4614d59aad368cbbed5
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, 2);
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 .proto_workarounds = SETTING_DJI_WORKAROUNDS_DEFAULT,
130 .messageSpeedSource = SETTING_DJI_MESSAGE_SPEED_SOURCE_DEFAULT,
131 .rssi_source = SETTING_DJI_RSSI_SOURCE_DEFAULT,
132 .useAdjustments = SETTING_DJI_USE_ADJUSTMENTS_DEFAULT,
133 .craftNameAlternatingDuration = SETTING_DJI_CN_ALTERNATING_DURATION_DEFAULT
134 );
135
136 #define RSSI_BOUNDARY(PERCENT) (RSSI_MAX_VALUE / 100 * PERCENT)
137
138 typedef enum {
139 DJI_OSD_CN_MESSAGES,
140 DJI_OSD_CN_THROTTLE,
141 DJI_OSD_CN_THROTTLE_AUTO_THR,
142 DJI_OSD_CN_AIR_SPEED,
143 DJI_OSD_CN_EFFICIENCY,
144 DJI_OSD_CN_DISTANCE,
145 DJI_OSD_CN_ADJUSTEMNTS,
146 DJI_OSD_CN_MAX_ELEMENTS
147 } DjiCraftNameElements_t;
148
149 // External dependency on looptime
150 extern timeDelta_t cycleTime;
151
152 // MSP packet decoder state structure
153 static mspPort_t djiMspPort;
154
155 // Mapping table between DJI PID and INAV PID (order is different)
156 const uint8_t djiPidIndexMap[] = {
157 PID_ROLL, // DJI: PID_ROLL
158 PID_PITCH, // DJI: PID_PITCH
159 PID_YAW, // DJI: PID_YAW
160 PID_LEVEL, // DJI: PID_LEVEL
161 PID_HEADING // DJI: PID_MAG
162 };
163
164 typedef struct {
165 int itemIndex; // INAV OSD item
166 features_e depFeature; // INAV feature that item is dependent on
167 } djiOsdMapping_t;
168
169 const djiOsdMapping_t djiOSDItemIndexMap[] = {
170 { OSD_RSSI_VALUE, 0 }, // DJI: OSD_RSSI_VALUE
171 { OSD_MAIN_BATT_VOLTAGE, FEATURE_VBAT }, // DJI: OSD_MAIN_BATT_VOLTAGE
172 { OSD_CROSSHAIRS, 0 }, // DJI: OSD_CROSSHAIRS
173 { OSD_ARTIFICIAL_HORIZON, 0 }, // DJI: OSD_ARTIFICIAL_HORIZON
174 { OSD_HORIZON_SIDEBARS, 0 }, // DJI: OSD_HORIZON_SIDEBARS
175 { OSD_ONTIME, 0 }, // DJI: OSD_ITEM_TIMER_1
176 { OSD_FLYTIME, 0 }, // DJI: OSD_ITEM_TIMER_2
177 { OSD_FLYMODE, 0 }, // DJI: OSD_FLYMODE
178 { OSD_CRAFT_NAME, 0 }, // DJI: OSD_CRAFT_NAME
179 { OSD_THROTTLE_POS, 0 }, // DJI: OSD_THROTTLE_POS
180 { OSD_VTX_CHANNEL, 0 }, // DJI: OSD_VTX_CHANNEL
181 { OSD_CURRENT_DRAW, FEATURE_CURRENT_METER }, // DJI: OSD_CURRENT_DRAW
182 { OSD_MAH_DRAWN, FEATURE_CURRENT_METER }, // DJI: OSD_MAH_DRAWN
183 { OSD_GPS_SPEED, FEATURE_GPS }, // DJI: OSD_GPS_SPEED
184 { OSD_GPS_SATS, FEATURE_GPS }, // DJI: OSD_GPS_SATS
185 { OSD_ALTITUDE, 0 }, // DJI: OSD_ALTITUDE
186 { OSD_ROLL_PIDS, 0 }, // DJI: OSD_ROLL_PIDS
187 { OSD_PITCH_PIDS, 0 }, // DJI: OSD_PITCH_PIDS
188 { OSD_YAW_PIDS, 0 }, // DJI: OSD_YAW_PIDS
189 { OSD_POWER, 0 }, // DJI: OSD_POWER
190 { -1, 0 }, // DJI: OSD_PIDRATE_PROFILE
191 { -1, 0 }, // DJI: OSD_WARNINGS
192 { OSD_MAIN_BATT_CELL_VOLTAGE, 0 }, // DJI: OSD_AVG_CELL_VOLTAGE
193 { OSD_GPS_LON, FEATURE_GPS }, // DJI: OSD_GPS_LON
194 { OSD_GPS_LAT, FEATURE_GPS }, // DJI: OSD_GPS_LAT
195 { OSD_DEBUG, 0 }, // DJI: OSD_DEBUG
196 { OSD_ATTITUDE_PITCH, 0 }, // DJI: OSD_PITCH_ANGLE
197 { OSD_ATTITUDE_ROLL, 0 }, // DJI: OSD_ROLL_ANGLE
198 { -1, 0 }, // DJI: OSD_MAIN_BATT_USAGE
199 { -1, 0 }, // DJI: OSD_DISARMED
200 { OSD_HOME_DIR, FEATURE_GPS }, // DJI: OSD_HOME_DIR
201 { OSD_HOME_DIST, FEATURE_GPS }, // DJI: OSD_HOME_DIST
202 { OSD_HEADING, 0 }, // DJI: OSD_NUMERICAL_HEADING
203 { OSD_VARIO_NUM, 0 }, // DJI: OSD_NUMERICAL_VARIO
204 { -1, 0 }, // DJI: OSD_COMPASS_BAR
205 { OSD_ESC_TEMPERATURE, 0 }, // DJI: OSD_ESC_TEMPERATURE
206 { OSD_ESC_RPM, 0 }, // DJI: OSD_ESC_RPM
207 { OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, FEATURE_CURRENT_METER }, // DJI: OSD_REMAINING_TIME_ESTIMATE
208 { OSD_RTC_TIME, 0 }, // DJI: OSD_RTC_DATETIME
209 { -1, 0 }, // DJI: OSD_ADJUSTMENT_RANGE
210 { -1, 0 }, // DJI: OSD_CORE_TEMPERATURE
211 { -1, 0 }, // DJI: OSD_ANTI_GRAVITY
212 { -1, 0 }, // DJI: OSD_G_FORCE
213 { -1, 0 }, // DJI: OSD_MOTOR_DIAG
214 { -1, 0 }, // DJI: OSD_LOG_STATUS
215 { -1, 0 }, // DJI: OSD_FLIP_ARROW
216 { -1, 0 }, // DJI: OSD_LINK_QUALITY
217 { OSD_TRIP_DIST, FEATURE_GPS }, // DJI: OSD_FLIGHT_DIST
218 { -1, 0 }, // DJI: OSD_STICK_OVERLAY_LEFT
219 { -1, 0 }, // DJI: OSD_STICK_OVERLAY_RIGHT
220 { -1, 0 }, // DJI: OSD_DISPLAY_NAME
221 { -1, 0 }, // DJI: OSD_ESC_RPM_FREQ
222 { -1, 0 }, // DJI: OSD_RATE_PROFILE_NAME
223 { -1, 0 }, // DJI: OSD_PID_PROFILE_NAME
224 { -1, 0 }, // DJI: OSD_PROFILE_NAME
225 { -1, 0 }, // DJI: OSD_RSSI_DBM_VALUE
226 { -1, 0 }, // DJI: OSD_RC_CHANNELS
227 };
228
229 const int djiOSDStatisticsMap[] = {
230 -1, // DJI: OSD_STAT_RTC_DATE_TIME
231 -1, // DJI: OSD_STAT_TIMER_1
232 -1, // DJI: OSD_STAT_TIMER_2
233 -1, // DJI: OSD_STAT_MAX_SPEED
234 -1, // DJI: OSD_STAT_MAX_DISTANCE
235 -1, // DJI: OSD_STAT_MIN_BATTERY
236 -1, // DJI: OSD_STAT_END_BATTERY
237 -1, // DJI: OSD_STAT_BATTERY
238 -1, // DJI: OSD_STAT_MIN_RSSI
239 -1, // DJI: OSD_STAT_MAX_CURRENT
240 -1, // DJI: OSD_STAT_USED_MAH
241 -1, // DJI: OSD_STAT_MAX_ALTITUDE
242 -1, // DJI: OSD_STAT_BLACKBOX
243 -1, // DJI: OSD_STAT_BLACKBOX_NUMBER
244 -1, // DJI: OSD_STAT_MAX_G_FORCE
245 -1, // DJI: OSD_STAT_MAX_ESC_TEMP
246 -1, // DJI: OSD_STAT_MAX_ESC_RPM
247 -1, // DJI: OSD_STAT_MIN_LINK_QUALITY
248 -1, // DJI: OSD_STAT_FLIGHT_DISTANCE
249 -1, // DJI: OSD_STAT_MAX_FFT
250 -1, // DJI: OSD_STAT_TOTAL_FLIGHTS
251 -1, // DJI: OSD_STAT_TOTAL_TIME
252 -1, // DJI: OSD_STAT_TOTAL_DIST
253 -1, // DJI: OSD_STAT_MIN_RSSI_DBM
254 };
255
256 void djiOsdSerialInit(void)
257 {
258 memset(&djiMspPort, 0, sizeof(mspPort_t));
259
260 serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_DJI_HD_OSD);
261
262 if (!portConfig) {
263 return;
264 }
265
266 serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_DJI_HD_OSD, NULL, NULL, DJI_MSP_BAUDRATE, MODE_RXTX, SERIAL_NOT_INVERTED);
267
268 if (serialPort) {
269 resetMspPort(&djiMspPort, serialPort);
270 }
271 }
272
273 static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
274 {
275 memset(flightModeBitmask, 0, sizeof(boxBitmask_t));
276
277 // Map flight modes to DJI-supported bits
278 switch(getFlightModeForTelemetry()) {
279 case FLM_MANUAL:
280 case FLM_ACRO:
281 case FLM_ACRO_AIR:
282 // DJI: No bits set = ACRO
283 break;
284 case FLM_ANGLE:
285 bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE
286 break;
287 case FLM_HORIZON:
288 bitArraySet(flightModeBitmask->bits, 2); // DJI: 1 << 2
289 break;
290 case FLM_RTH:
291 bitArraySet(flightModeBitmask->bits, 5); // DJI: 1 << 5 : GPS_RESQUE
292 break;
293 case FLM_CRUISE:
294 bitArraySet(flightModeBitmask->bits, 3); // DJI: 1 << 3 : technically HEADFREE
295 break;
296 case FLM_FAILSAFE:
297 bitArraySet(flightModeBitmask->bits, 4); // DJI: 1 << 4
298 break;
299 case FLM_LAUNCH:
300 case FLM_ALTITUDE_HOLD:
301 case FLM_POSITION_HOLD:
302 case FLM_MISSION:
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_BOXKILLSWITCH:
508 return OSD_MESSAGE_STR("KILLSWITCH ENABLED");
509 case ARMING_DISABLED_RC_LINK:
510 return OSD_MESSAGE_STR("NO RC LINK");
511 case ARMING_DISABLED_THROTTLE:
512 return OSD_MESSAGE_STR("THROTTLE!");
513 case ARMING_DISABLED_ROLLPITCH_NOT_CENTERED:
514 return OSD_MESSAGE_STR("ROLLPITCH!");
515 case ARMING_DISABLED_SERVO_AUTOTRIM:
516 return OSD_MESSAGE_STR("AUTOTRIM!");
517 case ARMING_DISABLED_OOM:
518 return OSD_MESSAGE_STR("MEM LOW");
519 case ARMING_DISABLED_INVALID_SETTING:
520 return OSD_MESSAGE_STR("ERR SETTING");
521 case ARMING_DISABLED_CLI:
522 return OSD_MESSAGE_STR("CLI");
523 case ARMING_DISABLED_PWM_OUTPUT_ERROR:
524 return OSD_MESSAGE_STR("PWM ERR");
525 case ARMING_DISABLED_NO_PREARM:
526 return OSD_MESSAGE_STR("NO PREARM");
527 case ARMING_DISABLED_DSHOT_BEEPER:
528 return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE");
529 // Cases without message
530 case ARMING_DISABLED_LANDING_DETECTED:
531 FALLTHROUGH;
532 case ARMING_DISABLED_CMS_MENU:
533 FALLTHROUGH;
534 case ARMING_DISABLED_OSD_MENU:
535 FALLTHROUGH;
536 case ARMING_DISABLED_ALL_FLAGS:
537 FALLTHROUGH;
538 case ARMED:
539 FALLTHROUGH;
540 case SIMULATOR_MODE:
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", (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) && gpsSol.groundSpeed > 0) {
788 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
789 value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
790 1, US2S(efficiencyTimeDelta));
791
792 efficiencyUpdated = currentTimeUs;
793 } else {
794 value = eFilterState.state;
795 }
796 }
797
798 if (value > 0 && value <= 999) {
799 tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM");
800 } else {
801 tfp_sprintf(buff, "%s", "---mAhKM");
802 }
803 }
804
805 static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
806 {
807 switch (adjustmentFunction) {
808 case ADJUSTMENT_RC_EXPO:
809 tfp_sprintf(buff, "RCE %d", currentControlRateProfile->stabilized.rcExpo8);
810 break;
811 case ADJUSTMENT_RC_YAW_EXPO:
812 tfp_sprintf(buff, "RCYE %3d", currentControlRateProfile->stabilized.rcYawExpo8);
813 break;
814 case ADJUSTMENT_MANUAL_RC_EXPO:
815 tfp_sprintf(buff, "MRCE %3d", currentControlRateProfile->manual.rcExpo8);
816 break;
817 case ADJUSTMENT_MANUAL_RC_YAW_EXPO:
818 tfp_sprintf(buff, "MRCYE %3d", currentControlRateProfile->manual.rcYawExpo8);
819 break;
820 case ADJUSTMENT_THROTTLE_EXPO:
821 tfp_sprintf(buff, "TE %3d", currentControlRateProfile->throttle.rcExpo8);
822 break;
823 case ADJUSTMENT_PITCH_ROLL_RATE:
824 tfp_sprintf(buff, "PRR %3d %3d", currentControlRateProfile->stabilized.rates[FD_PITCH], currentControlRateProfile->stabilized.rates[FD_ROLL]);
825 break;
826 case ADJUSTMENT_PITCH_RATE:
827 tfp_sprintf(buff, "PR %3d", currentControlRateProfile->stabilized.rates[FD_PITCH]);
828 break;
829 case ADJUSTMENT_ROLL_RATE:
830 tfp_sprintf(buff, "RR %3d", currentControlRateProfile->stabilized.rates[FD_ROLL]);
831 break;
832 case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
833 tfp_sprintf(buff, "MPRR %3d %3d", currentControlRateProfile->manual.rates[FD_PITCH], currentControlRateProfile->manual.rates[FD_ROLL]);
834 break;
835 case ADJUSTMENT_MANUAL_PITCH_RATE:
836 tfp_sprintf(buff, "MPR %3d", currentControlRateProfile->manual.rates[FD_PITCH]);
837 break;
838 case ADJUSTMENT_MANUAL_ROLL_RATE:
839 tfp_sprintf(buff, "MRR %3d", currentControlRateProfile->manual.rates[FD_ROLL]);
840 break;
841 case ADJUSTMENT_YAW_RATE:
842 tfp_sprintf(buff, "YR %3d", currentControlRateProfile->stabilized.rates[FD_YAW]);
843 break;
844 case ADJUSTMENT_MANUAL_YAW_RATE:
845 tfp_sprintf(buff, "MYR %3d", currentControlRateProfile->manual.rates[FD_YAW]);
846 break;
847 case ADJUSTMENT_PITCH_ROLL_P:
848 tfp_sprintf(buff, "PRP %3d %3d", pidBankMutable()->pid[PID_PITCH].P, pidBankMutable()->pid[PID_ROLL].P);
849 break;
850 case ADJUSTMENT_PITCH_P:
851 tfp_sprintf(buff, "PP %3d", pidBankMutable()->pid[PID_PITCH].P);
852 break;
853 case ADJUSTMENT_ROLL_P:
854 tfp_sprintf(buff, "RP %3d", pidBankMutable()->pid[PID_ROLL].P);
855 break;
856 case ADJUSTMENT_PITCH_ROLL_I:
857 tfp_sprintf(buff, "PRI %3d %3d", pidBankMutable()->pid[PID_PITCH].I, pidBankMutable()->pid[PID_ROLL].I);
858 break;
859 case ADJUSTMENT_PITCH_I:
860 tfp_sprintf(buff, "PI %3d", pidBankMutable()->pid[PID_PITCH].I);
861 break;
862 case ADJUSTMENT_ROLL_I:
863 tfp_sprintf(buff, "RI %3d", pidBankMutable()->pid[PID_ROLL].I);
864 break;
865 case ADJUSTMENT_PITCH_ROLL_D:
866 tfp_sprintf(buff, "PRD %3d %3d", pidBankMutable()->pid[PID_PITCH].D, pidBankMutable()->pid[PID_ROLL].D);
867 break;
868 case ADJUSTMENT_PITCH_ROLL_FF:
869 tfp_sprintf(buff, "PRFF %3d %3d", pidBankMutable()->pid[PID_PITCH].FF, pidBankMutable()->pid[PID_ROLL].FF);
870 break;
871 case ADJUSTMENT_PITCH_D:
872 tfp_sprintf(buff, "PD %3d", pidBankMutable()->pid[PID_PITCH].D);
873 break;
874 case ADJUSTMENT_PITCH_FF:
875 tfp_sprintf(buff, "PFF %3d", pidBankMutable()->pid[PID_PITCH].FF);
876 break;
877 case ADJUSTMENT_ROLL_D:
878 tfp_sprintf(buff, "RD %3d", pidBankMutable()->pid[PID_ROLL].D);
879 break;
880 case ADJUSTMENT_ROLL_FF:
881 tfp_sprintf(buff, "RFF %3d", pidBankMutable()->pid[PID_ROLL].FF);
882 break;
883 case ADJUSTMENT_YAW_P:
884 tfp_sprintf(buff, "YP %3d", pidBankMutable()->pid[PID_YAW].P);
885 break;
886 case ADJUSTMENT_YAW_I:
887 tfp_sprintf(buff, "YI %3d", pidBankMutable()->pid[PID_YAW].I);
888 break;
889 case ADJUSTMENT_YAW_D:
890 tfp_sprintf(buff, "YD %3d", pidBankMutable()->pid[PID_YAW].D);
891 break;
892 case ADJUSTMENT_YAW_FF:
893 tfp_sprintf(buff, "YFF %3d", pidBankMutable()->pid[PID_YAW].FF);
894 break;
895 case ADJUSTMENT_NAV_FW_CRUISE_THR:
896 tfp_sprintf(buff, "CR %4d", currentBatteryProfileMutable->nav.fw.cruise_throttle);
897 break;
898 case ADJUSTMENT_NAV_FW_PITCH2THR:
899 tfp_sprintf(buff, "P2T %3d", currentBatteryProfileMutable->nav.fw.pitch_to_throttle);
900 break;
901 case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
902 tfp_sprintf(buff, "RBA %3d", boardAlignment()->rollDeciDegrees);
903 break;
904 case ADJUSTMENT_PITCH_BOARD_ALIGNMENT:
905 tfp_sprintf(buff, "PBA %3d", boardAlignment()->pitchDeciDegrees);
906 break;
907 case ADJUSTMENT_LEVEL_P:
908 tfp_sprintf(buff, "LP %3d", pidBankMutable()->pid[PID_LEVEL].P);
909 break;
910 case ADJUSTMENT_LEVEL_I:
911 tfp_sprintf(buff, "LI %3d", pidBankMutable()->pid[PID_LEVEL].I);
912 break;
913 case ADJUSTMENT_LEVEL_D:
914 tfp_sprintf(buff, "LD %3d", pidBankMutable()->pid[PID_LEVEL].D);
915 break;
916 case ADJUSTMENT_POS_XY_P:
917 tfp_sprintf(buff, "PXYP %3d", pidBankMutable()->pid[PID_POS_XY].P);
918 break;
919 case ADJUSTMENT_POS_XY_I:
920 tfp_sprintf(buff, "PXYI %3d", pidBankMutable()->pid[PID_POS_XY].I);
921 break;
922 case ADJUSTMENT_POS_XY_D:
923 tfp_sprintf(buff, "PXYD %3d", pidBankMutable()->pid[PID_POS_XY].D);
924 break;
925 case ADJUSTMENT_POS_Z_P:
926 tfp_sprintf(buff, "PZP %3d", pidBankMutable()->pid[PID_POS_Z].P);
927 break;
928 case ADJUSTMENT_POS_Z_I:
929 tfp_sprintf(buff, "PZI %3d", pidBankMutable()->pid[PID_POS_Z].I);
930 break;
931 case ADJUSTMENT_POS_Z_D:
932 tfp_sprintf(buff, "PZD %3d", pidBankMutable()->pid[PID_POS_Z].D);
933 break;
934 case ADJUSTMENT_HEADING_P:
935 tfp_sprintf(buff, "HP %3d", pidBankMutable()->pid[PID_HEADING].P);
936 break;
937 case ADJUSTMENT_VEL_XY_P:
938 tfp_sprintf(buff, "VXYP %3d", pidBankMutable()->pid[PID_VEL_XY].P);
939 break;
940 case ADJUSTMENT_VEL_XY_I:
941 tfp_sprintf(buff, "VXYI %3d", pidBankMutable()->pid[PID_VEL_XY].I);
942 break;
943 case ADJUSTMENT_VEL_XY_D:
944 tfp_sprintf(buff, "VXYD %3d", pidBankMutable()->pid[PID_VEL_XY].D);
945 break;
946 case ADJUSTMENT_VEL_Z_P:
947 tfp_sprintf(buff, "VZP %3d", pidBankMutable()->pid[PID_VEL_Z].P);
948 break;
949 case ADJUSTMENT_VEL_Z_I:
950 tfp_sprintf(buff, "VZI %3d", pidBankMutable()->pid[PID_VEL_Z].I);
951 break;
952 case ADJUSTMENT_VEL_Z_D:
953 tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D);
954 break;
955 case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
956 tfp_sprintf(buff, "MTDPA %4d", navConfigMutable()->fw.minThrottleDownPitchAngle);
957 break;
958 case ADJUSTMENT_TPA:
959 tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID);
960 break;
961 case ADJUSTMENT_TPA_BREAKPOINT:
962 tfp_sprintf(buff, "TPABP %4d", currentControlRateProfile->throttle.pa_breakpoint);
963 break;
964 case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS:
965 tfp_sprintf(buff, "CSM %3d", navConfigMutable()->fw.control_smoothness);
966 break;
967 #ifdef USE_MULTI_MISSION
968 case ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX:
969 tfp_sprintf(buff, "WPI %3d", navConfigMutable()->general.waypoint_multi_mission_index);
970 break;
971 #endif
972 default:
973 tfp_sprintf(buff, "UNSUPPORTED");
974 break;
975 }
976 }
977
978 static bool osdDJIFormatAdjustments(char *buff)
979 {
980 uint8_t adjustmentFunctions[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
981 uint8_t adjustmentCount = getActiveAdjustmentFunctions(adjustmentFunctions);
982
983 if (adjustmentCount > 0 && buff != NULL) {
984 osdDJIAdjustmentMessage(buff, adjustmentFunctions[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, adjustmentCount)]);
985 }
986
987 return adjustmentCount > 0;
988 }
989
990
991 static bool djiFormatMessages(char *buff)
992 {
993 bool haveMessage = false;
994 char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
995 if (ARMING_FLAG(ARMED)) {
996 // Aircraft is armed. We might have up to 6
997 // messages to show.
998 char *messages[6];
999 unsigned messageCount = 0;
1000
1001 if (FLIGHT_MODE(FAILSAFE_MODE)) {
1002 // In FS mode while being armed too
1003 char *failsafePhaseMessage = osdFailsafePhaseMessage();
1004 char *failsafeInfoMessage = osdFailsafeInfoMessage();
1005 char *navStateFSMessage = navigationStateMessage();
1006
1007 if (failsafePhaseMessage) {
1008 messages[messageCount++] = failsafePhaseMessage;
1009 }
1010
1011 if (failsafeInfoMessage) {
1012 messages[messageCount++] = failsafeInfoMessage;
1013 }
1014
1015 if (navStateFSMessage) {
1016 messages[messageCount++] = navStateFSMessage;
1017 }
1018 } else {
1019 #ifdef USE_SERIALRX_CRSF
1020 if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ && rxLinkStatistics.rfMode == 0) {
1021 messages[messageCount++] = "CRSF LOW RF";
1022 }
1023 #endif
1024 if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
1025 char *navStateMessage = navigationStateMessage();
1026 if (navStateMessage) {
1027 messages[messageCount++] = navStateMessage;
1028 }
1029 } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
1030 messages[messageCount++] = "AUTOLAUNCH";
1031 } else {
1032 if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
1033 // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
1034 // when it doesn't require ANGLE mode (required only in FW
1035 // right now). If if requires ANGLE, its display is handled
1036 // by OSD_FLYMODE.
1037 messages[messageCount++] = "(ALT HOLD)";
1038 }
1039
1040 if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
1041 messages[messageCount++] = "(AUTOTRIM)";
1042 }
1043
1044 if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
1045 messages[messageCount++] = "(AUTOTUNE)";
1046 }
1047
1048 if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)) {
1049 messages[messageCount++] = "(AUTOLEVEL)";
1050 }
1051
1052 if (FLIGHT_MODE(HEADFREE_MODE)) {
1053 messages[messageCount++] = "(HEADFREE)";
1054 }
1055
1056 if (FLIGHT_MODE(MANUAL_MODE)) {
1057 messages[messageCount++] = "(MANUAL)";
1058 }
1059 }
1060 }
1061 // Pick one of the available messages. Each message lasts
1062 // a second.
1063 if (messageCount > 0) {
1064 strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]);
1065 haveMessage = true;
1066 }
1067 } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
1068 unsigned invalidIndex;
1069 // Check if we're unable to arm for some reason
1070 if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
1071 if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) {
1072 const setting_t *setting = settingGet(invalidIndex);
1073 settingGetName(setting, messageBuf);
1074 for (int ii = 0; messageBuf[ii]; ii++) {
1075 messageBuf[ii] = sl_toupper(messageBuf[ii]);
1076 }
1077 strcpy(buff, messageBuf);
1078 } else {
1079 strcpy(buff, "ERR SETTING");
1080 // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
1081 }
1082 } else {
1083 if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) {
1084 strcpy(buff, "CANT ARM");
1085 // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
1086 } else {
1087 // Show the reason for not arming
1088 strcpy(buff, osdArmingDisabledReasonMessage());
1089 }
1090 }
1091 haveMessage = true;
1092 }
1093 return haveMessage;
1094 }
1095
1096 static void djiSerializeCraftNameOverride(sbuf_t *dst)
1097 {
1098 char djibuf[DJI_CRAFT_NAME_LENGTH] = "\0";
1099 uint16_t *osdLayoutConfig = (uint16_t*)(osdLayoutsConfig()->item_pos[0]);
1100
1101 if (!(OSD_VISIBLE(osdLayoutConfig[OSD_MESSAGES]) && djiFormatMessages(djibuf))
1102 && !(djiOsdConfig()->useAdjustments && osdDJIFormatAdjustments(djibuf))) {
1103
1104 DjiCraftNameElements_t activeElements[DJI_OSD_CN_MAX_ELEMENTS];
1105 uint8_t activeElementsCount = 0;
1106
1107 if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS])) {
1108 activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE;
1109 }
1110
1111 if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS_AUTO_THR])) {
1112 activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE_AUTO_THR;
1113 }
1114
1115 if (OSD_VISIBLE(osdLayoutConfig[OSD_3D_SPEED])) {
1116 activeElements[activeElementsCount++] = DJI_OSD_CN_AIR_SPEED;
1117 }
1118
1119 if (OSD_VISIBLE(osdLayoutConfig[OSD_EFFICIENCY_MAH_PER_KM])) {
1120 activeElements[activeElementsCount++] = DJI_OSD_CN_EFFICIENCY;
1121 }
1122
1123 if (OSD_VISIBLE(osdLayoutConfig[OSD_TRIP_DIST])) {
1124 activeElements[activeElementsCount++] = DJI_OSD_CN_DISTANCE;
1125 }
1126
1127 switch (activeElements[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, activeElementsCount)])
1128 {
1129 case DJI_OSD_CN_THROTTLE:
1130 osdDJIFormatThrottlePosition(djibuf, false);
1131 break;
1132 case DJI_OSD_CN_THROTTLE_AUTO_THR:
1133 osdDJIFormatThrottlePosition(djibuf, true);
1134 break;
1135 case DJI_OSD_CN_AIR_SPEED:
1136 osdDJIFormatVelocityStr(djibuf);
1137 break;
1138 case DJI_OSD_CN_EFFICIENCY:
1139 osdDJIEfficiencyMahPerKM(djibuf);
1140 break;
1141 case DJI_OSD_CN_DISTANCE:
1142 osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance());
1143 break;
1144 default:
1145 break;
1146 }
1147 }
1148
1149 if (djibuf[0] != '\0') {
1150 sbufWriteData(dst, djibuf, strlen(djibuf));
1151 }
1152 }
1153
1154 #endif
1155
1156
1157 static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
1158 {
1159 UNUSED(mspPostProcessFn);
1160
1161 sbuf_t *dst = &reply->buf;
1162 sbuf_t *src = &cmd->buf;
1163
1164 // Start initializing the reply message
1165 reply->cmd = cmd->cmd;
1166 reply->result = MSP_RESULT_ACK;
1167
1168 switch (cmd->cmd) {
1169 case DJI_MSP_API_VERSION:
1170 sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
1171 sbufWriteU8(dst, DJI_API_VERSION_MAJOR);
1172 sbufWriteU8(dst, DJI_API_VERSION_MINOR);
1173 break;
1174
1175 case DJI_MSP_FC_VARIANT:
1176 {
1177 const char * const flightControllerIdentifier = INAV_IDENTIFIER;
1178 sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
1179 }
1180 break;
1181
1182 case DJI_MSP_FC_VERSION:
1183 sbufWriteU8(dst, 4);
1184 sbufWriteU8(dst, 1);
1185 sbufWriteU8(dst, 0);
1186 break;
1187
1188 case DJI_MSP_NAME:
1189 {
1190 #if defined(USE_OSD)
1191 if (djiOsdConfig()->use_name_for_messages) {
1192 djiSerializeCraftNameOverride(dst);
1193 } else {
1194 #endif
1195 sbufWriteData(dst, systemConfig()->name, (int)strlen(systemConfig()->name));
1196 #if defined(USE_OSD)
1197 }
1198 #endif
1199
1200 break;
1201 }
1202 break;
1203
1204 case DJI_MSP_STATUS:
1205 case DJI_MSP_STATUS_EX:
1206 {
1207 // DJI OSD relies on a statically defined bit order and doesn't use MSP_BOXIDS
1208 // to get actual BOX order. We need a special packBoxModeFlags()
1209 boxBitmask_t flightModeBitmask;
1210 djiPackBoxModeBitmask(&flightModeBitmask);
1211
1212 sbufWriteU16(dst, (uint16_t)cycleTime);
1213 sbufWriteU16(dst, 0);
1214 sbufWriteU16(dst, packSensorStatus());
1215 sbufWriteData(dst, &flightModeBitmask, 4); // unconditional part of flags, first 32 bits
1216 sbufWriteU8(dst, getConfigProfile());
1217
1218 sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
1219 if (cmd->cmd == MSP_STATUS_EX) {
1220 sbufWriteU8(dst, 3); // PID_PROFILE_COUNT
1221 sbufWriteU8(dst, 1); // getCurrentControlRateProfileIndex()
1222 } else {
1223 sbufWriteU16(dst, cycleTime); // gyro cycle time
1224 }
1225
1226 // Cap BoxModeFlags to 32 bits
1227 // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
1228 sbufWriteU8(dst, 0);
1229 // sbufWriteData(dst, ((uint8_t*)&flightModeBitmask) + 4, byteCount);
1230
1231 // Write arming disable flags
1232 sbufWriteU8(dst, DJI_ARMING_DISABLE_FLAGS_COUNT);
1233 sbufWriteU32(dst, djiPackArmingDisabledFlags());
1234
1235 // Extra flags
1236 sbufWriteU8(dst, 0);
1237 }
1238 break;
1239
1240 case DJI_MSP_RC:
1241 // Only send sticks (first 4 channels)
1242 for (int i = 0; i < STICK_CHANNEL_COUNT; i++) {
1243 sbufWriteU16(dst, rxGetChannelValue(i));
1244 }
1245 break;
1246
1247 case DJI_MSP_RAW_GPS:
1248 sbufWriteU8(dst, gpsSol.fixType);
1249 sbufWriteU8(dst, gpsSol.numSat);
1250 sbufWriteU32(dst, gpsSol.llh.lat);
1251 sbufWriteU32(dst, gpsSol.llh.lon);
1252 sbufWriteU16(dst, gpsSol.llh.alt / 100);
1253 sbufWriteU16(dst, osdGetSpeedFromSelectedSource());
1254 sbufWriteU16(dst, gpsSol.groundCourse);
1255 break;
1256
1257 case DJI_MSP_COMP_GPS:
1258 sbufWriteU16(dst, GPS_distanceToHome);
1259 sbufWriteU16(dst, GPS_directionToHome);
1260 sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0);
1261 break;
1262
1263 case DJI_MSP_ATTITUDE:
1264 sbufWriteU16(dst, attitude.values.roll);
1265 sbufWriteU16(dst, attitude.values.pitch);
1266 sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
1267 break;
1268
1269 case DJI_MSP_ALTITUDE:
1270 sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
1271 sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
1272 break;
1273
1274 case DJI_MSP_ANALOG:
1275 sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255));
1276 sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
1277 #ifdef USE_SERIALRX_CRSF
1278 // Range of RSSI field: 0-99: 99 = 150 hz , 0 - 98 50 hz / 4 hz
1279 if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ) {
1280 uint16_t scaledLq = 0;
1281 if (rxLinkStatistics.rfMode >= 2) {
1282 scaledLq = RSSI_MAX_VALUE;
1283 } else {
1284 scaledLq = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 0, RSSI_BOUNDARY(98));
1285 }
1286 sbufWriteU16(dst, scaledLq);
1287 } else {
1288 #endif
1289 sbufWriteU16(dst, getRSSI());
1290 #ifdef USE_SERIALRX_CRSF
1291 }
1292 #endif
1293 sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
1294 sbufWriteU16(dst, getBatteryVoltage());
1295 break;
1296
1297 case DJI_MSP_PID:
1298 for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++) {
1299 sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].P);
1300 sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].I);
1301 sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].D);
1302 }
1303 break;
1304
1305 case DJI_MSP_BATTERY_STATE:
1306 // Battery characteristics
1307 sbufWriteU8(dst, constrain(getBatteryCellCount(), 0, 255));
1308 sbufWriteU16(dst, currentBatteryProfile->capacity.value);
1309
1310 // Battery state
1311 sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); // in 0.1V steps
1312 sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF));
1313 sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF));
1314
1315 // Battery alerts - used values match Betaflight's/DJI's
1316 sbufWriteU8(dst, getBatteryState());
1317
1318 // Additional battery voltage field (in 0.01V steps)
1319 sbufWriteU16(dst, getBatteryVoltage());
1320 break;
1321
1322 case DJI_MSP_RTC:
1323 {
1324 dateTime_t datetime;
1325
1326 // We don't care about validity here - dt will be always set to a sane value
1327 // rtcGetDateTime() will call rtcGetDefaultDateTime() internally
1328 rtcGetDateTime(&datetime);
1329
1330 sbufWriteU16(dst, datetime.year);
1331 sbufWriteU8(dst, datetime.month);
1332 sbufWriteU8(dst, datetime.day);
1333 sbufWriteU8(dst, datetime.hours);
1334 sbufWriteU8(dst, datetime.minutes);
1335 sbufWriteU8(dst, datetime.seconds);
1336 sbufWriteU16(dst, datetime.millis);
1337 }
1338 break;
1339
1340 case DJI_MSP_ESC_SENSOR_DATA:
1341 if (djiOsdConfig()->proto_workarounds & DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA) {
1342 // Version 1.00.06 of DJI firmware is not using the standard MSP_ESC_SENSOR_DATA
1343 uint16_t protoRpm = 0;
1344 int16_t protoTemp = 0;
1345
1346 #if defined(USE_ESC_SENSOR)
1347 if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
1348 uint32_t motorRpmAcc = 0;
1349 int32_t motorTempAcc = 0;
1350
1351 for (int i = 0; i < getMotorCount(); i++) {
1352 const escSensorData_t * escSensor = getEscTelemetry(i);
1353 motorRpmAcc += escSensor->rpm;
1354 motorTempAcc += escSensor->temperature;
1355 }
1356
1357 protoRpm = motorRpmAcc / getMotorCount();
1358 protoTemp = motorTempAcc / getMotorCount();
1359 }
1360 #endif
1361
1362 switch (djiOsdConfig()->esc_temperature_source) {
1363 // This is ESC temperature (as intended)
1364 case DJI_OSD_TEMP_ESC:
1365 // No-op, temperature is already set to ESC
1366 break;
1367
1368 // Re-purpose the field for core temperature
1369 case DJI_OSD_TEMP_CORE:
1370 getIMUTemperature(&protoTemp);
1371 protoTemp = protoTemp / 10;
1372 break;
1373
1374 // Re-purpose the field for baro temperature
1375 case DJI_OSD_TEMP_BARO:
1376 getBaroTemperature(&protoTemp);
1377 protoTemp = protoTemp / 10;
1378 break;
1379 }
1380
1381 // No motor count, just raw temp and RPM data
1382 sbufWriteU8(dst, protoTemp);
1383 sbufWriteU16(dst, protoRpm);
1384 }
1385 else {
1386 // Use standard MSP_ESC_SENSOR_DATA message
1387 sbufWriteU8(dst, getMotorCount());
1388 for (int i = 0; i < getMotorCount(); i++) {
1389 uint16_t motorRpm = 0;
1390 int16_t motorTemp = 0;
1391
1392 // If ESC_SENSOR is enabled, pull the telemetry data and get motor RPM
1393 #if defined(USE_ESC_SENSOR)
1394 if (STATE(ESC_SENSOR_ENABLED)) {
1395 const escSensorData_t * escSensor = getEscTelemetry(i);
1396 motorRpm = escSensor->rpm;
1397 motorTemp = escSensor->temperature;
1398 }
1399 #endif
1400
1401 // Now populate temperature field (which we may override for different purposes)
1402 switch (djiOsdConfig()->esc_temperature_source) {
1403 // This is ESC temperature (as intended)
1404 case DJI_OSD_TEMP_ESC:
1405 // No-op, temperature is already set to ESC
1406 break;
1407
1408 // Re-purpose the field for core temperature
1409 case DJI_OSD_TEMP_CORE:
1410 getIMUTemperature(&motorTemp);
1411 motorTemp = motorTemp / 10;
1412 break;
1413
1414 // Re-purpose the field for baro temperature
1415 case DJI_OSD_TEMP_BARO:
1416 getBaroTemperature(&motorTemp);
1417 motorTemp = motorTemp / 10;
1418 break;
1419 }
1420
1421 // Add data for this motor to the packet
1422 sbufWriteU8(dst, motorTemp);
1423 sbufWriteU16(dst, motorRpm);
1424 }
1425 }
1426 break;
1427
1428 case DJI_MSP_OSD_CONFIG:
1429 #if defined(USE_OSD)
1430 // This involved some serious magic, better contain in a separate function for readability
1431 djiSerializeOSDConfigReply(dst);
1432 #else
1433 sbufWriteU8(dst, 0);
1434 #endif
1435 break;
1436
1437 case DJI_MSP_FILTER_CONFIG:
1438 sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz
1439 sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz
1440 sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz
1441 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_1
1442 sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
1443 sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hz
1444 sbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoff
1445 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2
1446 sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_2
1447 sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_type
1448 sbufWriteU8(dst, gyroConfig()->gyro_lpf); // BF: gyroConfig()->gyro_hardware_lpf);
1449 sbufWriteU8(dst, 0); // BF: DEPRECATED: gyro_32khz_hardware_lpf
1450 sbufWriteU16(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz);
1451 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_lowpass2_hz);
1452 sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass_type);
1453 sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass2_type);
1454 sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_lowpass2_hz);
1455 sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter2_type);
1456 break;
1457
1458 case DJI_MSP_RC_TUNING:
1459 sbufWriteU8(dst, 100); // INAV doesn't use rcRate
1460 sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
1461 for (int i = 0 ; i < 3; i++) {
1462 // R,P,Y rates see flight_dynamics_index_t
1463 sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]);
1464 }
1465 sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
1466 sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8);
1467 sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8);
1468 sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint);
1469 sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8);
1470 sbufWriteU8(dst, 100); // INAV doesn't use rcRate
1471 sbufWriteU8(dst, 100); // INAV doesn't use rcRate
1472 sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
1473
1474 // added in 1.41
1475 sbufWriteU8(dst, 0);
1476 sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
1477 break;
1478
1479 case DJI_MSP_SET_PID:
1480 // Check if we have enough data for all PID coefficients
1481 if ((unsigned)sbufBytesRemaining(src) >= ARRAYLEN(djiPidIndexMap) * 3) {
1482 for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++) {
1483 pidBankMutable()->pid[djiPidIndexMap[i]].P = sbufReadU8(src);
1484 pidBankMutable()->pid[djiPidIndexMap[i]].I = sbufReadU8(src);
1485 pidBankMutable()->pid[djiPidIndexMap[i]].D = sbufReadU8(src);
1486 }
1487 schedulePidGainsUpdate();
1488 }
1489 else {
1490 reply->result = MSP_RESULT_ERROR;
1491 }
1492 break;
1493
1494 case DJI_MSP_PID_ADVANCED:
1495 // TODO
1496 reply->result = MSP_RESULT_ERROR;
1497 break;
1498
1499 case DJI_MSP_SET_FILTER_CONFIG:
1500 case DJI_MSP_SET_PID_ADVANCED:
1501 case DJI_MSP_SET_RC_TUNING:
1502 // TODO
1503 reply->result = MSP_RESULT_ERROR;
1504 break;
1505
1506 default:
1507 // debug[1]++;
1508 // debug[2] = cmd->cmd;
1509 reply->result = MSP_RESULT_ERROR;
1510 break;
1511 }
1512
1513 // Process DONT_REPLY flag
1514 if (cmd->flags & MSP_FLAG_DONT_REPLY) {
1515 reply->result = MSP_RESULT_NO_REPLY;
1516 }
1517
1518 return reply->result;
1519 }
1520
1521 void djiOsdSerialProcess(void)
1522 {
1523 // Check if DJI OSD is configured
1524 if (!djiMspPort.port) {
1525 return;
1526 }
1527
1528 // Piggyback on existing MSP protocol stack, but pass our special command processing function
1529 mspSerialProcessOnePort(&djiMspPort, MSP_SKIP_NON_MSP_DATA, djiProcessMspCommand);
1530 }
1531
1532 #endif
INAV: Navigation-enabled flight control software