?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Merge pull request #8806 from iNavFlight/MrD_OSD-Throttle-Options
[inav.git] / src / main / io / osd_dji_hd.c
blob957f998c5e5e3c8cd4eac15682c45ce8af0173fc
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_HITL:
541 FALLTHROUGH;
542 case SIMULATOR_MODE_SITL:
543 FALLTHROUGH;
544 case WAS_EVER_ARMED:
545 break;
546 }
547
548 return NULL;
549 }
550
551 static char * osdFailsafePhaseMessage(void)
552 {
553 // See failsafe.h for each phase explanation
554 switch (failsafePhase()) {
555 case FAILSAFE_RETURN_TO_HOME:
556 // XXX: Keep this in sync with OSD_FLYMODE.
557 return OSD_MESSAGE_STR("(RTH)");
558 case FAILSAFE_LANDING:
559 // This should be considered an emergengy landing
560 return OSD_MESSAGE_STR("(EMRGY LANDING)");
561 case FAILSAFE_RX_LOSS_MONITORING:
562 // Only reachable from FAILSAFE_LANDED, which performs
563 // a disarm. Since aircraft has been disarmed, we no
564 // longer show failsafe details.
565 FALLTHROUGH;
566 case FAILSAFE_LANDED:
567 // Very brief, disarms and transitions into
568 // FAILSAFE_RX_LOSS_MONITORING. Note that it prevents
569 // further rearming via ARMING_DISABLED_FAILSAFE_SYSTEM,
570 // so we'll show the user how to re-arm in when
571 // that flag is the reason to prevent arming.
572 FALLTHROUGH;
573 case FAILSAFE_RX_LOSS_IDLE:
574 // This only happens when user has chosen NONE as FS
575 // procedure. The recovery messages should be enough.
576 FALLTHROUGH;
577 case FAILSAFE_IDLE:
578 // Failsafe not active
579 FALLTHROUGH;
580 case FAILSAFE_RX_LOSS_DETECTED:
581 // Very brief, changes to FAILSAFE_RX_LOSS_RECOVERED
582 // or the FS procedure immediately.
583 FALLTHROUGH;
584 case FAILSAFE_RX_LOSS_RECOVERED:
585 // Exiting failsafe
586 break;
587 }
588
589 return NULL;
590 }
591
592 static char * osdFailsafeInfoMessage(void)
593 {
594 if (failsafeIsReceivingRxData()) {
595 // User must move sticks to exit FS mode
596 return OSD_MESSAGE_STR("!MOVE STICKS TO EXIT FS!");
597 }
598
599 return OSD_MESSAGE_STR(RC_RX_LINK_LOST_MSG);
600 }
601
602 static char * navigationStateMessage(void)
603 {
604 switch (NAV_Status.state) {
605 case MW_NAV_STATE_NONE:
606 break;
607 case MW_NAV_STATE_RTH_START:
608 return OSD_MESSAGE_STR("STARTING RTH");
609 case MW_NAV_STATE_RTH_CLIMB:
610 return OSD_MESSAGE_STR("ADJUSTING RTH ALTITUDE");
611 case MW_NAV_STATE_RTH_ENROUTE:
612 // TODO: Break this up between climb and head home
613 return OSD_MESSAGE_STR("EN ROUTE TO HOME");
614 case MW_NAV_STATE_HOLD_INFINIT:
615 // Used by HOLD flight modes. No information to add.
616 break;
617 case MW_NAV_STATE_HOLD_TIMED:
618 // TODO: Maybe we can display a count down
619 return OSD_MESSAGE_STR("HOLDING WAYPOINT");
620 break;
621 case MW_NAV_STATE_WP_ENROUTE:
622 // TODO: Show WP number
623 return OSD_MESSAGE_STR("TO WP");
624 case MW_NAV_STATE_PROCESS_NEXT:
625 return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
626 case MW_NAV_STATE_DO_JUMP:
627 // Not used
628 break;
629 case MW_NAV_STATE_LAND_START:
630 // Not used
631 break;
632 case MW_NAV_STATE_EMERGENCY_LANDING:
633 return OSD_MESSAGE_STR("EMRGY LANDING");
634 case MW_NAV_STATE_LAND_IN_PROGRESS:
635 return OSD_MESSAGE_STR("LANDING");
636 case MW_NAV_STATE_HOVER_ABOVE_HOME:
637 if (STATE(FIXED_WING_LEGACY)) {
638 return OSD_MESSAGE_STR("LOITERING AROUND HOME");
639 }
640 return OSD_MESSAGE_STR("HOVERING");
641 case MW_NAV_STATE_LANDED:
642 return OSD_MESSAGE_STR("LANDED");
643 case MW_NAV_STATE_LAND_SETTLE:
644 return OSD_MESSAGE_STR("PREPARING TO LAND");
645 case MW_NAV_STATE_LAND_START_DESCENT:
646 // Not used
647 break;
648 }
649 return NULL;
650 }
651
652
653 /**
654 * Converts velocity based on the current unit system (kmh or mph).
655 * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/second)
656 */
657 static int32_t osdConvertVelocityToUnit(int32_t vel)
658 {
659 switch (osdConfig()->units) {
660 case OSD_UNIT_UK:
661 FALLTHROUGH;
662 case OSD_UNIT_METRIC_MPH:
663 FALLTHROUGH;
664 case OSD_UNIT_IMPERIAL:
665 return CMSEC_TO_CENTIMPH(vel) / 100; // Convert to mph
666 case OSD_UNIT_GA:
667 return CMSEC_TO_CENTIKNOTS(vel) / 100; // Convert to Knots
668 case OSD_UNIT_METRIC:
669 return CMSEC_TO_CENTIKPH(vel) / 100; // Convert to kmh
670 }
671
672 // Unreachable
673 return -1;
674 }
675
676 /**
677 * Converts velocity into a string based on the current unit system.
678 * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
679 */
680 void osdDJIFormatVelocityStr(char* buff)
681 {
682 char sourceBuf[4];
683 int vel = 0;
684 switch (djiOsdConfig()->messageSpeedSource) {
685 case OSD_SPEED_SOURCE_GROUND:
686 strcpy(sourceBuf, "GRD");
687 vel = gpsSol.groundSpeed;
688 break;
689 case OSD_SPEED_SOURCE_3D:
690 strcpy(sourceBuf, "3D");
691 vel = osdGet3DSpeed();
692 break;
693 case OSD_SPEED_SOURCE_AIR:
694 strcpy(sourceBuf, "AIR");
695 #ifdef USE_PITOT
696 vel = getAirspeedEstimate();
697 #endif
698 break;
699 }
700
701 switch (osdConfig()->units) {
702 case OSD_UNIT_UK:
703 FALLTHROUGH;
704 case OSD_UNIT_METRIC_MPH:
705 FALLTHROUGH;
706 case OSD_UNIT_IMPERIAL:
707 tfp_sprintf(buff, "%s %3d MPH", sourceBuf, (int)osdConvertVelocityToUnit(vel));
708 break;
709 case OSD_UNIT_GA:
710 tfp_sprintf(buff, "%s %3d KT", sourceBuf, (int)osdConvertVelocityToUnit(vel));
711 break;
712 case OSD_UNIT_METRIC:
713 tfp_sprintf(buff, "%s %3d KPH", sourceBuf, (int)osdConvertVelocityToUnit(vel));
714 break;
715 }
716 }
717 static void osdDJIFormatThrottlePosition(char *buff, bool autoThr )
718 {
719 int16_t thr = rxGetChannelValue(THROTTLE);
720 if (autoThr && navigationIsControllingThrottle()) {
721 thr = rcCommand[THROTTLE];
722 }
723
724 tfp_sprintf(buff, "%3ld%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR");
725 }
726
727 /**
728 * Converts distance into a string based on the current unit system.
729 * @param dist Distance in centimeters
730 */
731 static void osdDJIFormatDistanceStr(char *buff, int32_t dist)
732 {
733 int32_t centifeet;
734
735 switch (osdConfig()->units) {
736 case OSD_UNIT_UK:
737 FALLTHROUGH;
738 case OSD_UNIT_IMPERIAL:
739 centifeet = CENTIMETERS_TO_CENTIFEET(dist);
740 if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
741 // Show feet when dist < 0.5mi
742 tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT");
743 }
744 else {
745 // Show miles when dist >= 0.5mi
746 tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)),
747 (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, "Mi");
748 }
749 break;
750 case OSD_UNIT_GA:
751 centifeet = CENTIMETERS_TO_CENTIFEET(dist);
752 if (abs(centifeet) < FEET_PER_NAUTICALMILE * 100 / 2) {
753 // Show feet when dist < 0.5mi
754 tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT");
755 }
756 else {
757 // Show miles when dist >= 0.5mi
758 tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100 * FEET_PER_NAUTICALMILE)),
759 (int)((abs(centifeet) % (int)(100 * FEET_PER_NAUTICALMILE)) / FEET_PER_NAUTICALMILE), "NM");
760 }
761 break;
762 case OSD_UNIT_METRIC_MPH:
763 FALLTHROUGH;
764 case OSD_UNIT_METRIC:
765 if (abs(dist) < METERS_PER_KILOMETER * 100) {
766 // Show meters when dist < 1km
767 tfp_sprintf(buff, "%d%s", (int)(dist / 100), "M");
768 }
769 else {
770 // Show kilometers when dist >= 1km
771 tfp_sprintf(buff, "%d.%02d%s", (int)(dist / (100*METERS_PER_KILOMETER)),
772 (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, "KM");
773 }
774 break;
775 }
776 }
777
778 static void osdDJIEfficiencyMahPerKM(char *buff)
779 {
780 // amperage is in centi amps, speed is in cms/s. We want
781 // mah/km. Values over 999 are considered useless and
782 // displayed as "---""
783 static pt1Filter_t eFilterState;
784 static timeUs_t efficiencyUpdated = 0;
785 int32_t value = 0;
786 timeUs_t currentTimeUs = micros();
787 timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
788
789 if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
790 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
791 value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
792 1, US2S(efficiencyTimeDelta));
793
794 efficiencyUpdated = currentTimeUs;
795 } else {
796 value = eFilterState.state;
797 }
798 }
799
800 if (value > 0 && value <= 999) {
801 tfp_sprintf(buff, "%3d%s", (int)value, "mAhKM");
802 } else {
803 tfp_sprintf(buff, "%s", "---mAhKM");
804 }
805 }
806
807 static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
808 {
809 switch (adjustmentFunction) {
810 case ADJUSTMENT_RC_EXPO:
811 tfp_sprintf(buff, "RCE %d", currentControlRateProfile->stabilized.rcExpo8);
812 break;
813 case ADJUSTMENT_RC_YAW_EXPO:
814 tfp_sprintf(buff, "RCYE %3d", currentControlRateProfile->stabilized.rcYawExpo8);
815 break;
816 case ADJUSTMENT_MANUAL_RC_EXPO:
817 tfp_sprintf(buff, "MRCE %3d", currentControlRateProfile->manual.rcExpo8);
818 break;
819 case ADJUSTMENT_MANUAL_RC_YAW_EXPO:
820 tfp_sprintf(buff, "MRCYE %3d", currentControlRateProfile->manual.rcYawExpo8);
821 break;
822 case ADJUSTMENT_THROTTLE_EXPO:
823 tfp_sprintf(buff, "TE %3d", currentControlRateProfile->throttle.rcExpo8);
824 break;
825 case ADJUSTMENT_PITCH_ROLL_RATE:
826 tfp_sprintf(buff, "PRR %3d %3d", currentControlRateProfile->stabilized.rates[FD_PITCH], currentControlRateProfile->stabilized.rates[FD_ROLL]);
827 break;
828 case ADJUSTMENT_PITCH_RATE:
829 tfp_sprintf(buff, "PR %3d", currentControlRateProfile->stabilized.rates[FD_PITCH]);
830 break;
831 case ADJUSTMENT_ROLL_RATE:
832 tfp_sprintf(buff, "RR %3d", currentControlRateProfile->stabilized.rates[FD_ROLL]);
833 break;
834 case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE:
835 tfp_sprintf(buff, "MPRR %3d %3d", currentControlRateProfile->manual.rates[FD_PITCH], currentControlRateProfile->manual.rates[FD_ROLL]);
836 break;
837 case ADJUSTMENT_MANUAL_PITCH_RATE:
838 tfp_sprintf(buff, "MPR %3d", currentControlRateProfile->manual.rates[FD_PITCH]);
839 break;
840 case ADJUSTMENT_MANUAL_ROLL_RATE:
841 tfp_sprintf(buff, "MRR %3d", currentControlRateProfile->manual.rates[FD_ROLL]);
842 break;
843 case ADJUSTMENT_YAW_RATE:
844 tfp_sprintf(buff, "YR %3d", currentControlRateProfile->stabilized.rates[FD_YAW]);
845 break;
846 case ADJUSTMENT_MANUAL_YAW_RATE:
847 tfp_sprintf(buff, "MYR %3d", currentControlRateProfile->manual.rates[FD_YAW]);
848 break;
849 case ADJUSTMENT_PITCH_ROLL_P:
850 tfp_sprintf(buff, "PRP %3d %3d", pidBankMutable()->pid[PID_PITCH].P, pidBankMutable()->pid[PID_ROLL].P);
851 break;
852 case ADJUSTMENT_PITCH_P:
853 tfp_sprintf(buff, "PP %3d", pidBankMutable()->pid[PID_PITCH].P);
854 break;
855 case ADJUSTMENT_ROLL_P:
856 tfp_sprintf(buff, "RP %3d", pidBankMutable()->pid[PID_ROLL].P);
857 break;
858 case ADJUSTMENT_PITCH_ROLL_I:
859 tfp_sprintf(buff, "PRI %3d %3d", pidBankMutable()->pid[PID_PITCH].I, pidBankMutable()->pid[PID_ROLL].I);
860 break;
861 case ADJUSTMENT_PITCH_I:
862 tfp_sprintf(buff, "PI %3d", pidBankMutable()->pid[PID_PITCH].I);
863 break;
864 case ADJUSTMENT_ROLL_I:
865 tfp_sprintf(buff, "RI %3d", pidBankMutable()->pid[PID_ROLL].I);
866 break;
867 case ADJUSTMENT_PITCH_ROLL_D:
868 tfp_sprintf(buff, "PRD %3d %3d", pidBankMutable()->pid[PID_PITCH].D, pidBankMutable()->pid[PID_ROLL].D);
869 break;
870 case ADJUSTMENT_PITCH_ROLL_FF:
871 tfp_sprintf(buff, "PRFF %3d %3d", pidBankMutable()->pid[PID_PITCH].FF, pidBankMutable()->pid[PID_ROLL].FF);
872 break;
873 case ADJUSTMENT_PITCH_D:
874 tfp_sprintf(buff, "PD %3d", pidBankMutable()->pid[PID_PITCH].D);
875 break;
876 case ADJUSTMENT_PITCH_FF:
877 tfp_sprintf(buff, "PFF %3d", pidBankMutable()->pid[PID_PITCH].FF);
878 break;
879 case ADJUSTMENT_ROLL_D:
880 tfp_sprintf(buff, "RD %3d", pidBankMutable()->pid[PID_ROLL].D);
881 break;
882 case ADJUSTMENT_ROLL_FF:
883 tfp_sprintf(buff, "RFF %3d", pidBankMutable()->pid[PID_ROLL].FF);
884 break;
885 case ADJUSTMENT_YAW_P:
886 tfp_sprintf(buff, "YP %3d", pidBankMutable()->pid[PID_YAW].P);
887 break;
888 case ADJUSTMENT_YAW_I:
889 tfp_sprintf(buff, "YI %3d", pidBankMutable()->pid[PID_YAW].I);
890 break;
891 case ADJUSTMENT_YAW_D:
892 tfp_sprintf(buff, "YD %3d", pidBankMutable()->pid[PID_YAW].D);
893 break;
894 case ADJUSTMENT_YAW_FF:
895 tfp_sprintf(buff, "YFF %3d", pidBankMutable()->pid[PID_YAW].FF);
896 break;
897 case ADJUSTMENT_NAV_FW_CRUISE_THR:
898 tfp_sprintf(buff, "CR %4d", currentBatteryProfileMutable->nav.fw.cruise_throttle);
899 break;
900 case ADJUSTMENT_NAV_FW_PITCH2THR:
901 tfp_sprintf(buff, "P2T %3d", currentBatteryProfileMutable->nav.fw.pitch_to_throttle);
902 break;
903 case ADJUSTMENT_ROLL_BOARD_ALIGNMENT:
904 tfp_sprintf(buff, "RBA %3d", boardAlignment()->rollDeciDegrees);
905 break;
906 case ADJUSTMENT_PITCH_BOARD_ALIGNMENT:
907 tfp_sprintf(buff, "PBA %3d", boardAlignment()->pitchDeciDegrees);
908 break;
909 case ADJUSTMENT_LEVEL_P:
910 tfp_sprintf(buff, "LP %3d", pidBankMutable()->pid[PID_LEVEL].P);
911 break;
912 case ADJUSTMENT_LEVEL_I:
913 tfp_sprintf(buff, "LI %3d", pidBankMutable()->pid[PID_LEVEL].I);
914 break;
915 case ADJUSTMENT_LEVEL_D:
916 tfp_sprintf(buff, "LD %3d", pidBankMutable()->pid[PID_LEVEL].D);
917 break;
918 case ADJUSTMENT_POS_XY_P:
919 tfp_sprintf(buff, "PXYP %3d", pidBankMutable()->pid[PID_POS_XY].P);
920 break;
921 case ADJUSTMENT_POS_XY_I:
922 tfp_sprintf(buff, "PXYI %3d", pidBankMutable()->pid[PID_POS_XY].I);
923 break;
924 case ADJUSTMENT_POS_XY_D:
925 tfp_sprintf(buff, "PXYD %3d", pidBankMutable()->pid[PID_POS_XY].D);
926 break;
927 case ADJUSTMENT_POS_Z_P:
928 tfp_sprintf(buff, "PZP %3d", pidBankMutable()->pid[PID_POS_Z].P);
929 break;
930 case ADJUSTMENT_POS_Z_I:
931 tfp_sprintf(buff, "PZI %3d", pidBankMutable()->pid[PID_POS_Z].I);
932 break;
933 case ADJUSTMENT_POS_Z_D:
934 tfp_sprintf(buff, "PZD %3d", pidBankMutable()->pid[PID_POS_Z].D);
935 break;
936 case ADJUSTMENT_HEADING_P:
937 tfp_sprintf(buff, "HP %3d", pidBankMutable()->pid[PID_HEADING].P);
938 break;
939 case ADJUSTMENT_VEL_XY_P:
940 tfp_sprintf(buff, "VXYP %3d", pidBankMutable()->pid[PID_VEL_XY].P);
941 break;
942 case ADJUSTMENT_VEL_XY_I:
943 tfp_sprintf(buff, "VXYI %3d", pidBankMutable()->pid[PID_VEL_XY].I);
944 break;
945 case ADJUSTMENT_VEL_XY_D:
946 tfp_sprintf(buff, "VXYD %3d", pidBankMutable()->pid[PID_VEL_XY].D);
947 break;
948 case ADJUSTMENT_VEL_Z_P:
949 tfp_sprintf(buff, "VZP %3d", pidBankMutable()->pid[PID_VEL_Z].P);
950 break;
951 case ADJUSTMENT_VEL_Z_I:
952 tfp_sprintf(buff, "VZI %3d", pidBankMutable()->pid[PID_VEL_Z].I);
953 break;
954 case ADJUSTMENT_VEL_Z_D:
955 tfp_sprintf(buff, "VZD %3d", pidBankMutable()->pid[PID_VEL_Z].D);
956 break;
957 case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
958 tfp_sprintf(buff, "MTDPA %4d", navConfigMutable()->fw.minThrottleDownPitchAngle);
959 break;
960 case ADJUSTMENT_TPA:
961 tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID);
962 break;
963 case ADJUSTMENT_TPA_BREAKPOINT:
964 tfp_sprintf(buff, "TPABP %4d", currentControlRateProfile->throttle.pa_breakpoint);
965 break;
966 case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS:
967 tfp_sprintf(buff, "CSM %3d", navConfigMutable()->fw.control_smoothness);
968 break;
969 #ifdef USE_MULTI_MISSION
970 case ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX:
971 tfp_sprintf(buff, "WPI %3d", navConfigMutable()->general.waypoint_multi_mission_index);
972 break;
973 #endif
974 default:
975 tfp_sprintf(buff, "UNSUPPORTED");
976 break;
977 }
978 }
979
980 static bool osdDJIFormatAdjustments(char *buff)
981 {
982 uint8_t adjustmentFunctions[MAX_SIMULTANEOUS_ADJUSTMENT_COUNT];
983 uint8_t adjustmentCount = getActiveAdjustmentFunctions(adjustmentFunctions);
984
985 if (adjustmentCount > 0 && buff != NULL) {
986 osdDJIAdjustmentMessage(buff, adjustmentFunctions[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, adjustmentCount)]);
987 }
988
989 return adjustmentCount > 0;
990 }
991
992
993 static bool djiFormatMessages(char *buff)
994 {
995 bool haveMessage = false;
996 char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)];
997 if (ARMING_FLAG(ARMED)) {
998 // Aircraft is armed. We might have up to 6
999 // messages to show.
1000 char *messages[6];
1001 unsigned messageCount = 0;
1002
1003 if (FLIGHT_MODE(FAILSAFE_MODE)) {
1004 // In FS mode while being armed too
1005 char *failsafePhaseMessage = osdFailsafePhaseMessage();
1006 char *failsafeInfoMessage = osdFailsafeInfoMessage();
1007 char *navStateFSMessage = navigationStateMessage();
1008
1009 if (failsafePhaseMessage) {
1010 messages[messageCount++] = failsafePhaseMessage;
1011 }
1012
1013 if (failsafeInfoMessage) {
1014 messages[messageCount++] = failsafeInfoMessage;
1015 }
1016
1017 if (navStateFSMessage) {
1018 messages[messageCount++] = navStateFSMessage;
1019 }
1020 } else {
1021 #ifdef USE_SERIALRX_CRSF
1022 if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ && rxLinkStatistics.rfMode == 0) {
1023 messages[messageCount++] = "CRSF LOW RF";
1024 }
1025 #endif
1026 if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
1027 char *navStateMessage = navigationStateMessage();
1028 if (navStateMessage) {
1029 messages[messageCount++] = navStateMessage;
1030 }
1031 } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
1032 messages[messageCount++] = "AUTOLAUNCH";
1033 } else {
1034 if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
1035 // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO
1036 // when it doesn't require ANGLE mode (required only in FW
1037 // right now). If if requires ANGLE, its display is handled
1038 // by OSD_FLYMODE.
1039 messages[messageCount++] = "(ALT HOLD)";
1040 }
1041
1042 if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
1043 messages[messageCount++] = "(AUTOTRIM)";
1044 }
1045
1046 if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
1047 messages[messageCount++] = "(AUTOTUNE)";
1048 }
1049
1050 if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)) {
1051 messages[messageCount++] = "(AUTOLEVEL)";
1052 }
1053
1054 if (FLIGHT_MODE(HEADFREE_MODE)) {
1055 messages[messageCount++] = "(HEADFREE)";
1056 }
1057
1058 if (FLIGHT_MODE(MANUAL_MODE)) {
1059 messages[messageCount++] = "(MANUAL)";
1060 }
1061 }
1062 }
1063 // Pick one of the available messages. Each message lasts
1064 // a second.
1065 if (messageCount > 0) {
1066 strcpy(buff, messages[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, messageCount)]);
1067 haveMessage = true;
1068 }
1069 } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
1070 unsigned invalidIndex;
1071 // Check if we're unable to arm for some reason
1072 if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
1073 if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) {
1074 const setting_t *setting = settingGet(invalidIndex);
1075 settingGetName(setting, messageBuf);
1076 for (int ii = 0; messageBuf[ii]; ii++) {
1077 messageBuf[ii] = sl_toupper(messageBuf[ii]);
1078 }
1079 strcpy(buff, messageBuf);
1080 } else {
1081 strcpy(buff, "ERR SETTING");
1082 // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
1083 }
1084 } else {
1085 if (OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_SHORT, 2) == 0) {
1086 strcpy(buff, "CANT ARM");
1087 // TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
1088 } else {
1089 // Show the reason for not arming
1090 strcpy(buff, osdArmingDisabledReasonMessage());
1091 }
1092 }
1093 haveMessage = true;
1094 }
1095 return haveMessage;
1096 }
1097
1098 static void djiSerializeCraftNameOverride(sbuf_t *dst)
1099 {
1100 char djibuf[DJI_CRAFT_NAME_LENGTH] = "\0";
1101 uint16_t *osdLayoutConfig = (uint16_t*)(osdLayoutsConfig()->item_pos[0]);
1102
1103 if (!(OSD_VISIBLE(osdLayoutConfig[OSD_MESSAGES]) && djiFormatMessages(djibuf))
1104 && !(djiOsdConfig()->useAdjustments && osdDJIFormatAdjustments(djibuf))) {
1105
1106 DjiCraftNameElements_t activeElements[DJI_OSD_CN_MAX_ELEMENTS];
1107 uint8_t activeElementsCount = 0;
1108
1109 if (OSD_VISIBLE(osdLayoutConfig[OSD_THROTTLE_POS])) {
1110 activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE;
1111 }
1112
1113 if (OSD_VISIBLE(osdLayoutConfig[OSD_SCALED_THROTTLE_POS])) {
1114 activeElements[activeElementsCount++] = DJI_OSD_CN_THROTTLE_AUTO_THR;
1115 }
1116
1117 if (OSD_VISIBLE(osdLayoutConfig[OSD_3D_SPEED])) {
1118 activeElements[activeElementsCount++] = DJI_OSD_CN_AIR_SPEED;
1119 }
1120
1121 if (OSD_VISIBLE(osdLayoutConfig[OSD_EFFICIENCY_MAH_PER_KM])) {
1122 activeElements[activeElementsCount++] = DJI_OSD_CN_EFFICIENCY;
1123 }
1124
1125 if (OSD_VISIBLE(osdLayoutConfig[OSD_TRIP_DIST])) {
1126 activeElements[activeElementsCount++] = DJI_OSD_CN_DISTANCE;
1127 }
1128
1129 switch (activeElements[OSD_ALTERNATING_CHOICES(DJI_ALTERNATING_DURATION_LONG, activeElementsCount)])
1130 {
1131 case DJI_OSD_CN_THROTTLE:
1132 osdDJIFormatThrottlePosition(djibuf, false);
1133 break;
1134 case DJI_OSD_CN_THROTTLE_AUTO_THR:
1135 osdDJIFormatThrottlePosition(djibuf, true);
1136 break;
1137 case DJI_OSD_CN_AIR_SPEED:
1138 osdDJIFormatVelocityStr(djibuf);
1139 break;
1140 case DJI_OSD_CN_EFFICIENCY:
1141 osdDJIEfficiencyMahPerKM(djibuf);
1142 break;
1143 case DJI_OSD_CN_DISTANCE:
1144 osdDJIFormatDistanceStr(djibuf, getTotalTravelDistance());
1145 break;
1146 default:
1147 break;
1148 }
1149 }
1150
1151 if (djibuf[0] != '\0') {
1152 sbufWriteData(dst, djibuf, strlen(djibuf));
1153 }
1154 }
1155
1156 #endif
1157
1158
1159 static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
1160 {
1161 UNUSED(mspPostProcessFn);
1162
1163 sbuf_t *dst = &reply->buf;
1164 sbuf_t *src = &cmd->buf;
1165
1166 // Start initializing the reply message
1167 reply->cmd = cmd->cmd;
1168 reply->result = MSP_RESULT_ACK;
1169
1170 switch (cmd->cmd) {
1171 case DJI_MSP_API_VERSION:
1172 sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
1173 sbufWriteU8(dst, DJI_API_VERSION_MAJOR);
1174 sbufWriteU8(dst, DJI_API_VERSION_MINOR);
1175 break;
1176
1177 case DJI_MSP_FC_VARIANT:
1178 {
1179 const char * const flightControllerIdentifier = INAV_IDENTIFIER;
1180 sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
1181 }
1182 break;
1183
1184 case DJI_MSP_FC_VERSION:
1185 sbufWriteU8(dst, 4);
1186 sbufWriteU8(dst, 1);
1187 sbufWriteU8(dst, 0);
1188 break;
1189
1190 case DJI_MSP_NAME:
1191 {
1192 #if defined(USE_OSD)
1193 if (djiOsdConfig()->use_name_for_messages) {
1194 djiSerializeCraftNameOverride(dst);
1195 } else {
1196 #endif
1197 sbufWriteData(dst, systemConfig()->craftName, (int)strlen(systemConfig()->craftName));
1198 #if defined(USE_OSD)
1199 }
1200 #endif
1201
1202 break;
1203 }
1204 break;
1205
1206 case DJI_MSP_STATUS:
1207 case DJI_MSP_STATUS_EX:
1208 {
1209 // DJI OSD relies on a statically defined bit order and doesn't use MSP_BOXIDS
1210 // to get actual BOX order. We need a special packBoxModeFlags()
1211 boxBitmask_t flightModeBitmask;
1212 djiPackBoxModeBitmask(&flightModeBitmask);
1213
1214 sbufWriteU16(dst, (uint16_t)cycleTime);
1215 sbufWriteU16(dst, 0);
1216 sbufWriteU16(dst, packSensorStatus());
1217 sbufWriteData(dst, &flightModeBitmask, 4); // unconditional part of flags, first 32 bits
1218 sbufWriteU8(dst, getConfigProfile());
1219
1220 sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
1221 if (cmd->cmd == MSP_STATUS_EX) {
1222 sbufWriteU8(dst, 3); // PID_PROFILE_COUNT
1223 sbufWriteU8(dst, 1); // getCurrentControlRateProfileIndex()
1224 } else {
1225 sbufWriteU16(dst, cycleTime); // gyro cycle time
1226 }
1227
1228 // Cap BoxModeFlags to 32 bits
1229 // write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
1230 sbufWriteU8(dst, 0);
1231 // sbufWriteData(dst, ((uint8_t*)&flightModeBitmask) + 4, byteCount);
1232
1233 // Write arming disable flags
1234 sbufWriteU8(dst, DJI_ARMING_DISABLE_FLAGS_COUNT);
1235 sbufWriteU32(dst, djiPackArmingDisabledFlags());
1236
1237 // Extra flags
1238 sbufWriteU8(dst, 0);
1239 }
1240 break;
1241
1242 case DJI_MSP_RC:
1243 // Only send sticks (first 4 channels)
1244 for (int i = 0; i < STICK_CHANNEL_COUNT; i++) {
1245 sbufWriteU16(dst, rxGetChannelValue(i));
1246 }
1247 break;
1248
1249 case DJI_MSP_RAW_GPS:
1250 sbufWriteU8(dst, gpsSol.fixType);
1251 sbufWriteU8(dst, gpsSol.numSat);
1252 sbufWriteU32(dst, gpsSol.llh.lat);
1253 sbufWriteU32(dst, gpsSol.llh.lon);
1254 sbufWriteU16(dst, gpsSol.llh.alt / 100);
1255 sbufWriteU16(dst, osdGetSpeedFromSelectedSource());
1256 sbufWriteU16(dst, gpsSol.groundCourse);
1257 break;
1258
1259 case DJI_MSP_COMP_GPS:
1260 sbufWriteU16(dst, GPS_distanceToHome);
1261 sbufWriteU16(dst, GPS_directionToHome);
1262 sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0);
1263 break;
1264
1265 case DJI_MSP_ATTITUDE:
1266 sbufWriteU16(dst, attitude.values.roll);
1267 sbufWriteU16(dst, attitude.values.pitch);
1268 sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
1269 break;
1270
1271 case DJI_MSP_ALTITUDE:
1272 sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
1273 sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
1274 break;
1275
1276 case DJI_MSP_ANALOG:
1277 sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255));
1278 sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
1279 #ifdef USE_SERIALRX_CRSF
1280 // Range of RSSI field: 0-99: 99 = 150 hz , 0 - 98 50 hz / 4 hz
1281 if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ) {
1282 uint16_t scaledLq = 0;
1283 if (rxLinkStatistics.rfMode >= 2) {
1284 scaledLq = RSSI_MAX_VALUE;
1285 } else {
1286 scaledLq = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 0, RSSI_BOUNDARY(98));
1287 }
1288 sbufWriteU16(dst, scaledLq);
1289 } else {
1290 #endif
1291 sbufWriteU16(dst, getRSSI());
1292 #ifdef USE_SERIALRX_CRSF
1293 }
1294 #endif
1295 sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
1296 sbufWriteU16(dst, getBatteryVoltage());
1297 break;
1298
1299 case DJI_MSP_PID:
1300 for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++) {
1301 sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].P);
1302 sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].I);
1303 sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].D);
1304 }
1305 break;
1306
1307 case DJI_MSP_BATTERY_STATE:
1308 // Battery characteristics
1309 sbufWriteU8(dst, constrain(getBatteryCellCount(), 0, 255));
1310 sbufWriteU16(dst, currentBatteryProfile->capacity.value);
1311
1312 // Battery state
1313 sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); // in 0.1V steps
1314 sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF));
1315 sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF));
1316
1317 // Battery alerts - used values match Betaflight's/DJI's
1318 sbufWriteU8(dst, getBatteryState());
1319
1320 // Additional battery voltage field (in 0.01V steps)
1321 sbufWriteU16(dst, getBatteryVoltage());
1322 break;
1323
1324 case DJI_MSP_RTC:
1325 {
1326 dateTime_t datetime;
1327
1328 // We don't care about validity here - dt will be always set to a sane value
1329 // rtcGetDateTime() will call rtcGetDefaultDateTime() internally
1330 rtcGetDateTime(&datetime);
1331
1332 sbufWriteU16(dst, datetime.year);
1333 sbufWriteU8(dst, datetime.month);
1334 sbufWriteU8(dst, datetime.day);
1335 sbufWriteU8(dst, datetime.hours);
1336 sbufWriteU8(dst, datetime.minutes);
1337 sbufWriteU8(dst, datetime.seconds);
1338 sbufWriteU16(dst, datetime.millis);
1339 }
1340 break;
1341
1342 case DJI_MSP_ESC_SENSOR_DATA:
1343 if (djiOsdConfig()->proto_workarounds & DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA) {
1344 // Version 1.00.06 of DJI firmware is not using the standard MSP_ESC_SENSOR_DATA
1345 uint16_t protoRpm = 0;
1346 int16_t protoTemp = 0;
1347
1348 #if defined(USE_ESC_SENSOR)
1349 if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
1350 uint32_t motorRpmAcc = 0;
1351 int32_t motorTempAcc = 0;
1352
1353 for (int i = 0; i < getMotorCount(); i++) {
1354 const escSensorData_t * escSensor = getEscTelemetry(i);
1355 motorRpmAcc += escSensor->rpm;
1356 motorTempAcc += escSensor->temperature;
1357 }
1358
1359 protoRpm = motorRpmAcc / getMotorCount();
1360 protoTemp = motorTempAcc / getMotorCount();
1361 }
1362 #endif
1363
1364 switch (djiOsdConfig()->esc_temperature_source) {
1365 // This is ESC temperature (as intended)
1366 case DJI_OSD_TEMP_ESC:
1367 // No-op, temperature is already set to ESC
1368 break;
1369
1370 // Re-purpose the field for core temperature
1371 case DJI_OSD_TEMP_CORE:
1372 getIMUTemperature(&protoTemp);
1373 protoTemp = protoTemp / 10;
1374 break;
1375
1376 // Re-purpose the field for baro temperature
1377 case DJI_OSD_TEMP_BARO:
1378 getBaroTemperature(&protoTemp);
1379 protoTemp = protoTemp / 10;
1380 break;
1381 }
1382
1383 // No motor count, just raw temp and RPM data
1384 sbufWriteU8(dst, protoTemp);
1385 sbufWriteU16(dst, protoRpm);
1386 }
1387 else {
1388 // Use standard MSP_ESC_SENSOR_DATA message
1389 sbufWriteU8(dst, getMotorCount());
1390 for (int i = 0; i < getMotorCount(); i++) {
1391 uint16_t motorRpm = 0;
1392 int16_t motorTemp = 0;
1393
1394 // If ESC_SENSOR is enabled, pull the telemetry data and get motor RPM
1395 #if defined(USE_ESC_SENSOR)
1396 if (STATE(ESC_SENSOR_ENABLED)) {
1397 const escSensorData_t * escSensor = getEscTelemetry(i);
1398 motorRpm = escSensor->rpm;
1399 motorTemp = escSensor->temperature;
1400 }
1401 #endif
1402
1403 // Now populate temperature field (which we may override for different purposes)
1404 switch (djiOsdConfig()->esc_temperature_source) {
1405 // This is ESC temperature (as intended)
1406 case DJI_OSD_TEMP_ESC:
1407 // No-op, temperature is already set to ESC
1408 break;
1409
1410 // Re-purpose the field for core temperature
1411 case DJI_OSD_TEMP_CORE:
1412 getIMUTemperature(&motorTemp);
1413 motorTemp = motorTemp / 10;
1414 break;
1415
1416 // Re-purpose the field for baro temperature
1417 case DJI_OSD_TEMP_BARO:
1418 getBaroTemperature(&motorTemp);
1419 motorTemp = motorTemp / 10;
1420 break;
1421 }
1422
1423 // Add data for this motor to the packet
1424 sbufWriteU8(dst, motorTemp);
1425 sbufWriteU16(dst, motorRpm);
1426 }
1427 }
1428 break;
1429
1430 case DJI_MSP_OSD_CONFIG:
1431 #if defined(USE_OSD)
1432 // This involved some serious magic, better contain in a separate function for readability
1433 djiSerializeOSDConfigReply(dst);
1434 #else
1435 sbufWriteU8(dst, 0);
1436 #endif
1437 break;
1438
1439 case DJI_MSP_FILTER_CONFIG:
1440 sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz
1441 sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz
1442 sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz
1443 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_1
1444 sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
1445 sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hz
1446 sbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoff
1447 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2
1448 sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_2
1449 sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_type
1450 sbufWriteU8(dst, gyroConfig()->gyro_lpf); // BF: gyroConfig()->gyro_hardware_lpf);
1451 sbufWriteU8(dst, 0); // BF: DEPRECATED: gyro_32khz_hardware_lpf
1452 sbufWriteU16(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz);
1453 sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_lowpass2_hz);
1454 sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass_type);
1455 sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass2_type);
1456 sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_lowpass2_hz);
1457 sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter2_type);
1458 break;
1459
1460 case DJI_MSP_RC_TUNING:
1461 sbufWriteU8(dst, 100); // INAV doesn't use rcRate
1462 sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
1463 for (int i = 0 ; i < 3; i++) {
1464 // R,P,Y rates see flight_dynamics_index_t
1465 sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]);
1466 }
1467 sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
1468 sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8);
1469 sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8);
1470 sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint);
1471 sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8);
1472 sbufWriteU8(dst, 100); // INAV doesn't use rcRate
1473 sbufWriteU8(dst, 100); // INAV doesn't use rcRate
1474 sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
1475
1476 // added in 1.41
1477 sbufWriteU8(dst, 0);
1478 sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
1479 break;
1480
1481 case DJI_MSP_SET_PID:
1482 // Check if we have enough data for all PID coefficients
1483 if ((unsigned)sbufBytesRemaining(src) >= ARRAYLEN(djiPidIndexMap) * 3) {
1484 for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++) {
1485 pidBankMutable()->pid[djiPidIndexMap[i]].P = sbufReadU8(src);
1486 pidBankMutable()->pid[djiPidIndexMap[i]].I = sbufReadU8(src);
1487 pidBankMutable()->pid[djiPidIndexMap[i]].D = sbufReadU8(src);
1488 }
1489 schedulePidGainsUpdate();
1490 }
1491 else {
1492 reply->result = MSP_RESULT_ERROR;
1493 }
1494 break;
1495
1496 case DJI_MSP_PID_ADVANCED:
1497 // TODO
1498 reply->result = MSP_RESULT_ERROR;
1499 break;
1500
1501 case DJI_MSP_SET_FILTER_CONFIG:
1502 case DJI_MSP_SET_PID_ADVANCED:
1503 case DJI_MSP_SET_RC_TUNING:
1504 // TODO
1505 reply->result = MSP_RESULT_ERROR;
1506 break;
1507
1508 default:
1509 // debug[1]++;
1510 // debug[2] = cmd->cmd;
1511 reply->result = MSP_RESULT_ERROR;
1512 break;
1513 }
1514
1515 // Process DONT_REPLY flag
1516 if (cmd->flags & MSP_FLAG_DONT_REPLY) {
1517 reply->result = MSP_RESULT_NO_REPLY;
1518 }
1519
1520 return reply->result;
1521 }
1522
1523 void djiOsdSerialProcess(void)
1524 {
1525 // Check if DJI OSD is configured
1526 if (!djiMspPort.port) {
1527 return;
1528 }
1529
1530 // Piggyback on existing MSP protocol stack, but pass our special command processing function
1531 mspSerialProcessOnePort(&djiMspPort, MSP_SKIP_NON_MSP_DATA, djiProcessMspCommand);
1532 }
1533
1534 #endif
INAV: Navigation-enabled flight control software