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