iNavFlight之MSP DJI协议分析
Posted lida2003
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了iNavFlight之MSP DJI协议分析相关的知识，希望对你有一定的参考价值。
iNavFlight之MSP DJI协议分析
MSP DJI协议主要是为了解决如何将飞控内部信息传送到DJI数字图传，进而在DJI的数字系统的视频上叠加飞控OSD信息。
注：目前模拟图传的做法是通过MAX7456芯片将模拟VI和飞控
1. iNav串行口通信
1.1 iNav 串口任务
taskHandleSerial是STM32任务之一。
[TASK_SERIAL] =
.taskName = "SERIAL",
.taskFunc = taskHandleSerial,
.desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
.staticPriority = TASK_PRIORITY_LOW,
,
1.2 调用逻辑
taskHandleSerial是专门集中处理串口的任务。其中djiosdSerialProcess是与DJI天空端通信的实现代码入口。
void taskHandleSerial(timeUs_t currentTimeUs)
UNUSED(currentTimeUs);
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode)
cliProcess();
// Allow MSP processing even if in CLI mode
mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
#if defined(USE_DJI_HD_OSD)
// DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task
djiOsdSerialProcess();
#endif
#ifdef USE_MSP_OSD
// Capture MSP Displayport messages to determine if VTX is connected
mspOsdSerialProcess(mspFcProcessCommand);
#endif
mspSerialProcess是正常的MSP控制协议函数实现。
void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++)
mspPort_t * const mspPort = &mspPorts[portIndex];
if (mspPort->port)
mspSerialProcessOnePort(mspPort, evaluateNonMspData, mspProcessCommandFn);
djiOsdSerialProcess是专门用于处理与DJI天空端通信的函数实现。
void djiOsdSerialProcess(void)
// Check if DJI OSD is configured
if (!djiMspPort.port)
return;
// Piggyback on existing MSP protocol stack, but pass our special command processing function
mspSerialProcessOnePort(&djiMspPort, MSP_SKIP_NON_MSP_DATA, djiProcessMspCommand);
这段代码据说没有使能，详见:how to open MSP_DISPLAYPORT #6415。但是从代码上看是有编译到的，详见：#define USE_MSP_OSD，因此应该是特殊配置的FUNCTION_MSP_OSD端口(且不是DJI的)。
#ifdef USE_MSP_OSD
// Capture MSP Displayport messages to determine if VTX is connected
mspOsdSerialProcess(mspFcProcessCommand);
#endif
2. iNav串行抽象
从该代码实现角度看，是典型的C/S模型。
通用框架mspSerialProcessOnePort主要是提供收包，异常处理，解析，调用命令处理流程和报文反馈，其主要差异在于命令处理流程部分。
2.1 框架代码
mspSerialProcessOnePort框架代码是MSP协议的通用实现，其主要差异在于命令码处理的mspProcessCommandFn函数。
- 当MSP_SKIP_NON_MSP_DATA，用mspProcessCommandFn处理；
a) djiProcessMspCommand: MSP(DJI)协议处理
b) mspFcProcessCommand: MSP(Control)协议处理
- 当MSP_EVALUATE_NON_MSP_DATA，mspEvaluateNonMspData；
命令行模式
void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
mspPostProcessFnPtr mspPostProcessFn = NULL;
if (serialRxBytesWaiting(mspPort->port))
// There are bytes incoming - abort pending request
mspPort->lastActivityMs = millis();
mspPort->pendingRequest = MSP_PENDING_NONE;
// Process incoming bytes
while (serialRxBytesWaiting(mspPort->port))
const uint8_t c = serialRead(mspPort->port);
const bool consumed = mspSerialProcessReceivedData(mspPort, c);
if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA)
mspEvaluateNonMspData(mspPort, c);
if (mspPort->c_state == MSP_COMMAND_RECEIVED)
mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
break; // process one command at a time so as not to block.
if (mspPostProcessFn)
waitForSerialPortToFinishTransmitting(mspPort->port);
mspPostProcessFn(mspPort->port);
else
mspProcessPendingRequest(mspPort);
2.2 MSP(DJI)协议处理
DJI天空端发出请求，FC接收到请求后反馈请求结果，详见：djiProcessMspCommand。
static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
UNUSED(mspPostProcessFn);
sbuf_t *dst = &reply->buf;
sbuf_t *src = &cmd->buf;
// Start initializing the reply message
reply->cmd = cmd->cmd;
reply->result = MSP_RESULT_ACK;
switch (cmd->cmd)
case DJI_MSP_API_VERSION:
sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
sbufWriteU8(dst, DJI_API_VERSION_MAJOR);
sbufWriteU8(dst, DJI_API_VERSION_MINOR);
break;
case DJI_MSP_FC_VARIANT:
const char * const flightControllerIdentifier = INAV_IDENTIFIER;
sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
break;
case DJI_MSP_FC_VERSION:
sbufWriteU8(dst, 4);
sbufWriteU8(dst, 1);
sbufWriteU8(dst, 0);
break;
case DJI_MSP_NAME:
#if defined(USE_OSD)
if (djiOsdConfig()->use_name_for_messages)
djiSerializeCraftNameOverride(dst);
else
#endif
sbufWriteData(dst, systemConfig()->name, (int)strlen(systemConfig()->name));
#if defined(USE_OSD)
#endif
break;
break;
case DJI_MSP_STATUS:
case DJI_MSP_STATUS_EX:
// DJI OSD relies on a statically defined bit order and doesn't use MSP_BOXIDS
// to get actual BOX order. We need a special packBoxModeFlags()
boxBitmask_t flightModeBitmask;
djiPackBoxModeBitmask(&flightModeBitmask);
sbufWriteU16(dst, (uint16_t)cycleTime);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, packSensorStatus());
sbufWriteData(dst, &flightModeBitmask, 4); // unconditional part of flags, first 32 bits
sbufWriteU8(dst, getConfigProfile());
sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
if (cmd->cmd == MSP_STATUS_EX)
sbufWriteU8(dst, 3); // PID_PROFILE_COUNT
sbufWriteU8(dst, 1); // getCurrentControlRateProfileIndex()
else
sbufWriteU16(dst, cycleTime); // gyro cycle time
// Cap BoxModeFlags to 32 bits
// write flightModeFlags header. Lowest 4 bits contain number of bytes that follow
sbufWriteU8(dst, 0);
// sbufWriteData(dst, ((uint8_t*)&flightModeBitmask) + 4, byteCount);
// Write arming disable flags
sbufWriteU8(dst, DJI_ARMING_DISABLE_FLAGS_COUNT);
sbufWriteU32(dst, djiPackArmingDisabledFlags());
// Extra flags
sbufWriteU8(dst, 0);
break;
case DJI_MSP_RC:
// Only send sticks (first 4 channels)
for (int i = 0; i < STICK_CHANNEL_COUNT; i++)
sbufWriteU16(dst, rxGetChannelValue(i));
break;
case DJI_MSP_RAW_GPS:
sbufWriteU8(dst, gpsSol.fixType);
sbufWriteU8(dst, gpsSol.numSat);
sbufWriteU32(dst, gpsSol.llh.lat);
sbufWriteU32(dst, gpsSol.llh.lon);
sbufWriteU16(dst, gpsSol.llh.alt / 100);
sbufWriteU16(dst, osdGetSpeedFromSelectedSource());
sbufWriteU16(dst, gpsSol.groundCourse);
break;
case DJI_MSP_COMP_GPS:
sbufWriteU16(dst, GPS_distanceToHome);
sbufWriteU16(dst, GPS_directionToHome);
sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0);
break;
case DJI_MSP_ATTITUDE:
sbufWriteU16(dst, attitude.values.roll);
sbufWriteU16(dst, attitude.values.pitch);
sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
break;
case DJI_MSP_ALTITUDE:
sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
break;
case DJI_MSP_ANALOG:
sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255));
sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF)); // milliamp hours drawn from battery
#ifdef USE_SERIALRX_CRSF
// Range of RSSI field: 0-99: 99 = 150 hz , 0 - 98 50 hz / 4 hz
if (djiOsdConfig()->rssi_source == DJI_CRSF_LQ)
uint16_t scaledLq = 0;
if (rxLinkStatistics.rfMode >= 2)
scaledLq = RSSI_MAX_VALUE;
else
scaledLq = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 0, RSSI_BOUNDARY(98));
sbufWriteU16(dst, scaledLq);
else
#endif
sbufWriteU16(dst, getRSSI());
#ifdef USE_SERIALRX_CRSF
#endif
sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320A
sbufWriteU16(dst, getBatteryVoltage());
break;
case DJI_MSP_PID:
for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++)
sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].P);
sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].I);
sbufWriteU8(dst, pidBank()->pid[djiPidIndexMap[i]].D);
break;
case DJI_MSP_BATTERY_STATE:
// Battery characteristics
sbufWriteU8(dst, constrain(getBatteryCellCount(), 0, 255));
sbufWriteU16(dst, currentBatteryProfile->capacity.value);
// Battery state
sbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); // in 0.1V steps
sbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF));
sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF));
// Battery alerts - used values match Betaflight's/DJI's
sbufWriteU8(dst, getBatteryState());
// Additional battery voltage field (in 0.01V steps)
sbufWriteU16(dst, getBatteryVoltage());
break;
case DJI_MSP_RTC:
dateTime_t datetime;
// We don't care about validity here - dt will be always set to a sane value
// rtcGetDateTime() will call rtcGetDefaultDateTime() internally
rtcGetDateTime(&datetime);
sbufWriteU16(dst, datetime.year);
sbufWriteU8(dst, datetime.month);
sbufWriteU8(dst, datetime.day);
sbufWriteU8(dst, datetime.hours);
sbufWriteU8(dst, datetime.minutes);
sbufWriteU8(dst, datetime.seconds);
sbufWriteU16(dst, datetime.millis);
break;
case DJI_MSP_ESC_SENSOR_DATA:
if (djiOsdConfig()->proto_workarounds & DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA)
// Version 1.00.06 of DJI firmware is not using the standard MSP_ESC_SENSOR_DATA
uint16_t protoRpm = 0;
int16_t protoTemp = 0;
#if defined(USE_ESC_SENSOR)
if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0)
uint32_t motorRpmAcc = 0;
int32_t motorTempAcc = 0;
for (int i = 0; i < getMotorCount(); i++)
const escSensorData_t * escSensor = getEscTelemetry(i);
motorRpmAcc += escSensor->rpm;
motorTempAcc += escSensor->temperature;
protoRpm = motorRpmAcc / getMotorCount();
protoTemp = motorTempAcc / getMotorCount();
#endif
switch (djiOsdConfig()->esc_temperature_source)
// This is ESC temperature (as intended)
case DJI_OSD_TEMP_ESC:
// No-op, temperature is already set to ESC
break;
// Re-purpose the field for core temperature
case DJI_OSD_TEMP_CORE:
getIMUTemperature(&protoTemp);
protoTemp = protoTemp / 10;
break;
// Re-purpose the field for baro temperature
case DJI_OSD_TEMP_BARO:
getBaroTemperature(&protoTemp);
protoTemp = protoTemp / 10;
break;
// No motor count, just raw temp and RPM data
sbufWriteU8(dst, protoTemp);
sbufWriteU16(dst, protoRpm);
else
// Use standard MSP_ESC_SENSOR_DATA message
sbufWriteU8(dst, getMotorCount());
for (int i = 0; i < getMotorCount(); i++)
uint16_t motorRpm = 0;
int16_t motorTemp = 0;
// If ESC_SENSOR is enabled, pull the telemetry data and get motor RPM
#if defined(USE_ESC_SENSOR)
if (STATE(ESC_SENSOR_ENABLED))
const escSensorData_t * escSensor = getEscTelemetry(i);
motorRpm = escSensor->rpm;
motorTemp = escSensor->temperature;
#endif
// Now populate temperature field (which we may override for different purposes)
switch (djiOsdConfig()->esc_temperature_source)
// This is ESC temperature (as intended)
case DJI_OSD_TEMP_ESC:
// No-op, temperature is already set to ESC
break;
// Re-purpose the field for core temperature
case DJI_OSD_TEMP_CORE:
getIMUTemperature(&motorTemp);
motorTemp = motorTemp / 10;
break;
// Re-purpose the field for baro temperature
case DJI_OSD_TEMP_BARO:
getBaroTemperature(&motorTemp);
motorTemp = motorTemp / 10;
break;
// Add data for this motor to the packet
sbufWriteU8(dst, motorTemp);
sbufWriteU16(dst, motorRpm);
break;
case DJI_MSP_OSD_CONFIG:
#if defined(USE_OSD)
// This involved some serious magic, better contain in a separate function for readability
djiSerializeOSDConfigReply(dst);
#else
sbufWriteU8(dst, 0);
#endif
break;
case DJI_MSP_FILTER_CONFIG:
sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_1
sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1
sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hz
sbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoff
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2
sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_2
sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_type
sbufWriteU8(dst, gyroConfig()->gyro_lpf); // BF: gyroConfig()->gyro_hardware_lpf);
sbufWriteU8(dst, 0); // BF: DEPRECATED: gyro_32khz_hardware_lpf
sbufWriteU16(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz);
sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_lowpass2_hz);
sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass_type);
sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass2_type);
sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_lowpass2_hz);
sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter2_type);
break;
case DJI_MSP_RC_TUNING:
sbufWriteU8(dst, 100); // INAV doesn't use rcRate
sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
for (int i = 0 ; i < 3; i++)
// R,P,Y rates see flight_dynamics_index_t
sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]);
sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8);
sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8);
sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint);
sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8);
sbufWriteU8(dst, 100); // INAV doesn't use rcRate
sbufWriteU8(dst, 100); // INAV doesn't use rcRate
sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);
// added in 1.41
sbufWriteU8(dst, 0);
sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);
break;
case DJI_MSP_SET_PID:
// Check if we have enough data for all PID coefficients
if ((unsigned)sbufBytesRemaining(src) >= ARRAYLEN(djiPidIndexMap) * 3)
for (unsigned i = 0; i < ARRAYLEN(djiPidIndexMap); i++)
pidBankMutable()->pid[djiPidIndexMap[i]].P = sbufReadU8(src);
pidBankMutable()->pid[djiPidIndexMap[i]].I = sbufReadU8(src);
pidBankMutable()->pid[djiPidIndexMap[i]].D = sbufReadU8(src);
schedulePidGainsUpdate();
else
reply->result = MSP_RESULT_ERROR;
break;
case DJI_MSP_PID_ADVANCED:
// TODO
reply->result = MSP_RESULT_ERROR;
break;
case DJI_MSP_SET_FILTER_CONFIG:
case DJI_MSP_SET_PID_ADVANCED:
case DJI_MSP_SET_RC_TUNING:
// TODO
reply->result = MSP_RESULT_ERROR;
break;
default:
// debug[1]++;
// debug[2] = cmd->cmd;
reply->result = MSP_RESULT_ERROR;
break;
// Process DONT_REPLY flag
if (cmd->flags & MSP_FLAG_DONT_REPLY)
reply->result = MSP_RESULT_NO_REPLY;
return reply->result;
3. DJI协议相关实现
3.1 DJI串口初始化
main
└──> init
└──> djiOsdSerialInit
3.2 DJI命令集
最新DJI命令集请查阅这里。
#define DJI_MSP_API_VERSION 1 // INAV: Implemented | DSI: ??? |
#define DJI_MSP_FC_VARIANT 2 // INAV: Implemented | DSI: ??? |
#define DJI_MSP_FC_VERSION 3 // INAV: Implemented | DSI: ??? |
#define DJI_MSP_NAME 10 // INAV: Implemented | DSI: Implemented | For OSD 'Craft Name'
#define DJI_MSP_OSD_CONFIG 84 // INAV: Implemented | DSI: Implemented | OSD item count + positions
#define DJI_MSP_FILTER_CONFIG 92 // INAV: Not implemented | DSI: Implemented |
#define DJI_MSP_PID_ADVANCED 94 // INAV: Not implemented | DSI: Implemented |
#define DJI_MSP_STATUS 101 // INAV: Implemented | DSI: Implemented | For OSD ‘armingTime’, Flight controller arming status
#define DJI_MSP_RC 105 // INAV: Implemented | DSI: Implemented |
#define DJI_MSP_RAW_GPS 106 // INAV: Implemented | DSI: Implemented | For OSD ‘GPS Sats’ + coordinates
#define DJI_MSP_COMP_GPS 107 // INAV: Implemented | DSI: Not implemented | GPS direction to home & distance to home
#define DJI_MSP_ATTITUDE 108 // INAV: Implemented | DSI: Implemented | For OSD ‘Angle: roll & pitch’
#define DJI_MSP_ALTITUDE 109 // INAV: Implemented | DSI: Implemented | For OSD ‘Numerical Vario’
#define DJI_MSP_ANALOG 110 // INAV: Implemented | DSI: Implemented | For OSD ‘RSSI Value’, For OSD ‘Battery voltage’ etc
#define DJI_MSP_RC_TUNING 111 // INAV: Not implemented | DSI: Implemented |
#define DJI_MSP_PID 112 // INAV: Implemented | DSI: Implemented | For OSD ‘PID roll, yaw, pitch'
#define DJI_MSP_BATTERY_STATE 130 // INAV: Implemented | DSI: Implemented | For OSD ‘Battery current mAh drawn’ etc
#define DJI_MSP_ESC_SENSOR_DATA 134 // INAV: Implemented | DSI: Implemented | For OSD ‘ESC temperature’
#define DJI_MSP_STATUS_EX 150 // INAV: Implemented | DSI: Implemented | For OSD ‘Fly mode', For OSD ‘Disarmed’
#define DJI_MSP_RTC 247 // INAV: Implemented | DSI: Implemented | For OSD ‘RTC date time’
3.3. DJI相关函数
static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
static uint32_t djiPackArmingDisabledFlags(void)
static uint32_t djiEncodeOSDEnabledWarnings(void) //TODO
static void djiSerializeOSDConfigReply(sbuf_t *dst)
static char * osdArmingDisabledReasonMessage(void)
static char * osdFailsafePhaseMessage(void)
static char * osdFailsafeInfoMessage(void)
static char * navigationStateMessage(void)
static int32_t osdConvertVelocityToUnit(int32_t vel)
void osdDJIFormatVelocityStr(char* buff)
static void osdDJIFormatThrottlePosition(char *buff, bool autoThr )
static void osdDJIFormatDistanceStr(char *buff, int32_t dist)
static void osdDJIEfficiencyMahPerKM(char *buff)
static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction)
static bool osdDJIFormatAdjustments(char *buff)
static bool djiFormatMessages(char *buff)
static void djiSerializeCraftNameOverride(sbuf_t *dst)
4. 参考资料
【1】iNav 2.4.0 Release Notes – Support for DJI HD FPV in Feb 6, 2020
【2】Multiwii Serial Protocol
【3】BetaFlight模块设计之三十二：MSP协议模块分析
【4】Multiwii Serial Protocol (MSP)
以上是关于iNavFlight之MSP DJI协议分析的主要内容，如果未能解决你的问题，请参考以下文章