iNavFlight之MSP DJI协议分析
- 1. iNav串行口通信
- 1.1 iNav 串口任务
- 1.2 调用逻辑
- 2. iNav串行抽象
- 2.1 框架代码
- 2.2 MSP(DJI)协议处理
- 3. DJI协议相关实现
- 3.1 DJI串口初始化
- 3.2 DJI命令集
- 3.3. DJI相关函数
- 4. 协议格式
- 5. 参考资料
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 modemspSerialProcess(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 taskdjiOsdSerialProcess();
#endif#ifdef USE_MSP_OSD// Capture MSP Displayport messages to determine if VTX is connectedmspOsdSerialProcess(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 configuredif (!djiMspPort.port) {return;}// Piggyback on existing MSP protocol stack, but pass our special command processing functionmspSerialProcessOnePort(&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 connectedmspOsdSerialProcess(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 requestmspPort->lastActivityMs = millis();mspPort->pendingRequest = MSP_PENDING_NONE;// Process incoming byteswhile (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 messagereply->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 {
#endifsbufWriteData(dst, systemConfig()->name, (int)strlen(systemConfig()->name));
#if defined(USE_OSD)}
#endifbreak;}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 bitssbufWriteU8(dst, getConfigProfile());sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));if (cmd->cmd == MSP_STATUS_EX) {sbufWriteU8(dst, 3); // PID_PROFILE_COUNTsbufWriteU8(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 followsbufWriteU8(dst, 0);// sbufWriteData(dst, ((uint8_t*)&flightModeBitmask) + 4, byteCount);// Write arming disable flagssbufWriteU8(dst, DJI_ARMING_DISABLE_FLAGS_COUNT);sbufWriteU32(dst, djiPackArmingDisabledFlags());// Extra flagssbufWriteU8(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 hzif (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 {
#endifsbufWriteU16(dst, getRSSI());
#ifdef USE_SERIALRX_CRSF}
#endifsbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF)); // send amperage in 0.01 A steps, range is -320A to 320AsbufWriteU16(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 characteristicssbufWriteU8(dst, constrain(getBatteryCellCount(), 0, 255));sbufWriteU16(dst, currentBatteryProfile->capacity.value);// Battery statesbufWriteU8(dst, constrain(getBatteryVoltage() / 10, 0, 255)); // in 0.1V stepssbufWriteU16(dst, constrain(getMAhDrawn(), 0, 0xFFFF));sbufWriteU16(dst, constrain(getAmperage(), -0x8000, 0x7FFF));// Battery alerts - used values match Betaflight's/DJI'ssbufWriteU8(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() internallyrtcGetDateTime(&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_DATAuint16_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();}
#endifswitch (djiOsdConfig()->esc_temperature_source) {// This is ESC temperature (as intended)case DJI_OSD_TEMP_ESC:// No-op, temperature is already set to ESCbreak;// Re-purpose the field for core temperaturecase DJI_OSD_TEMP_CORE:getIMUTemperature(&protoTemp);protoTemp = protoTemp / 10;break;// Re-purpose the field for baro temperaturecase DJI_OSD_TEMP_BARO:getBaroTemperature(&protoTemp);protoTemp = protoTemp / 10;break;}// No motor count, just raw temp and RPM datasbufWriteU8(dst, protoTemp);sbufWriteU16(dst, protoRpm);}else {// Use standard MSP_ESC_SENSOR_DATA messagesbufWriteU8(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 ESCbreak;// Re-purpose the field for core temperaturecase DJI_OSD_TEMP_CORE:getIMUTemperature(&motorTemp);motorTemp = motorTemp / 10;break;// Re-purpose the field for baro temperaturecase DJI_OSD_TEMP_BARO:getBaroTemperature(&motorTemp);motorTemp = motorTemp / 10;break;}// Add data for this motor to the packetsbufWriteU8(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 readabilitydjiSerializeOSDConfigReply(dst);
#elsesbufWriteU8(dst, 0);
#endifbreak;case DJI_MSP_FILTER_CONFIG:sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hzsbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hzsbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hzsbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_1sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_1sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_notch_hzsbufWriteU16(dst, 1); // BF: currentPidProfile->dterm_notch_cutoffsbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_soft_notch_hz_2sbufWriteU16(dst, 1); // BF: gyroConfig()->gyro_soft_notch_cutoff_2sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_typesbufWriteU8(dst, gyroConfig()->gyro_lpf); // BF: gyroConfig()->gyro_hardware_lpf);sbufWriteU8(dst, 0); // BF: DEPRECATED: gyro_32khz_hardware_lpfsbufWriteU16(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 rcRatesbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);for (int i = 0 ; i < 3; i++) {// R,P,Y rates see flight_dynamics_index_tsbufWriteU8(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 rcRatesbufWriteU8(dst, 100); // INAV doesn't use rcRatesbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8);// added in 1.41sbufWriteU8(dst, 0);sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID);break;case DJI_MSP_SET_PID:// Check if we have enough data for all PID coefficientsif ((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:// TODOreply->result = MSP_RESULT_ERROR;break;case DJI_MSP_SET_FILTER_CONFIG:case DJI_MSP_SET_PID_ADVANCED:case DJI_MSP_SET_RC_TUNING:// TODOreply->result = MSP_RESULT_ERROR;break;default:// debug[1]++;// debug[2] = cmd->cmd;reply->result = MSP_RESULT_ERROR;break;}// Process DONT_REPLY flagif (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. 协议格式
+---+---+--------+---------+--------+------+---------+------------------------------+-------------+| Multiwii Serial Protocol V2 length = 9 + payload size |+---+---+--------+---------+--------+------+---------+------------------------------+-------------+| $ | X | < ! > | flag(1) | cmd(2) | size(2) | payload(16bit len) | checksum_v2 |+---+---+--------+---------+--------+------+---------+------------------------------+-------------+
- ‘$’：表示SOF(Start Of a frame)
- ‘X’：表示V2
- ‘<’: 表示request
- ‘>’：表示response
- ‘!’：表示error
5. 参考资料
【1】iNav 2.4.0 Release Notes – Support for DJI HD FPV in Feb 6, 2020
【2】Multiwii Serial Protocol
【3】Multiwii Serial Protocol (MSP)
【4】BetaFlight模块设计之三十二：MSP协议模块分析
【5】iNavFlight之MSP DJI协议天空端请求报文
【6】iNavFlight之MSP DJI协议飞控端请求应答
