iNavFlight之MSP DJI协议飞控端请求应答
- 1. 报文格式
- 2. 报文标志(flag)
- 3. 报文命令(cmd)
- 4. 请求应答 & 反馈报文
- 4.1 DJI_MSP_API_VERSION
- 4.2 DJI_MSP_FC_VARIANT
- 4.3 DJI_MSP_FC_VERSION
- 4.4 DJI_MSP_NAME
- 4.5 DJI_MSP_STATUS & DJI_MSP_STATUS_EX
- 4.6 DJI_MSP_RC
- 4.7 DJI_MSP_RAW_GPS
- 4.8 DJI_MSP_COMP_GPS
- 4.9 DJI_MSP_ATTITUDE
- 4.10 DJI_MSP_ALTITUDE
- 4.11 DJI_MSP_ANALOG
- 4.12 DJI_MSP_PID
- 4.13 DJI_MSP_BATTERY_STATE
- 4.14 DJI_MSP_RTC
- 4.15 DJI_MSP_ESC_SENSOR_DATA
- 4.16 DJI_MSP_OSD_CONFIG
- 4.17 DJI_MSP_FILTER_CONFIG
- 4.18 DJI_MSP_RC_TUNING
- 4.19 DJI_MSP_SET_PID
- 4.20 Unsupported
- 5. 参考资料
MSP DJI协议是用于DJI天空端与飞控端之间的通信协议，其工作模式符合C/S经典设计。
这里我们重点介绍下天空端请求报文格式和命令。
1. 报文格式
+---+---+--------+---------+--------+------+---------+------------------------------+-------------+
| 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
- ‘>’：表示response
- ‘!’：表示error
2. 报文标志(flag)
请求报文：MSP_RESULT_ACK or MSP_RESULT_ERROR //截止发稿日
// return positive for ACK, negative on error, zero for no reply
typedef enum {
MSP_RESULT_ACK = 1,
MSP_RESULT_ERROR = -1,
MSP_RESULT_NO_REPLY = 0
} mspResult_e;
3. 报文命令(cmd)
#define DJI_MSP_API_VERSION 1 //
#define DJI_MSP_FC_VARIANT 2 //
#define DJI_MSP_FC_VERSION 3 //
#define DJI_MSP_NAME 10 // For OSD 'Craft Name'
#define DJI_MSP_OSD_CONFIG 84 // OSD item count + positions
#define DJI_MSP_FILTER_CONFIG 92 //
#define DJI_MSP_PID_ADVANCED 94 //
#define DJI_MSP_STATUS 101 // For OSD ‘armingTime’, Flight controller arming status
#define DJI_MSP_RC 105 //
#define DJI_MSP_RAW_GPS 106 // For OSD ‘GPS Sats’ + coordinates
#define DJI_MSP_COMP_GPS 107 // GPS direction to home & distance to home
#define DJI_MSP_ATTITUDE 108 // For OSD ‘Angle: roll & pitch’
#define DJI_MSP_ALTITUDE 109 // For OSD ‘Numerical Vario’
#define DJI_MSP_ANALOG 110 // For OSD ‘RSSI Value’, For OSD ‘Battery voltage’ etc
#define DJI_MSP_RC_TUNING 111 //
#define DJI_MSP_PID 112 // For OSD ‘PID roll, yaw, pitch'
#define DJI_MSP_BATTERY_STATE 130 // For OSD ‘Battery current mAh drawn’ etc
#define DJI_MSP_ESC_SENSOR_DATA 134 // For OSD ‘ESC temperature’
#define DJI_MSP_STATUS_EX 150 // For OSD ‘Fly mode', For OSD ‘Disarmed’
#define DJI_MSP_RTC 247 // For OSD ‘RTC date time’
4. 请求应答 & 反馈报文
这里主要将payload内容整理出来。
4.1 DJI_MSP_API_VERSION
反馈内容：API版本，三个字节
示例显示：“1.42”
case DJI_MSP_API_VERSION:
sbufWriteU8(dst, MSP_PROTOCOL_VERSION);
sbufWriteU8(dst, DJI_API_VERSION_MAJOR);
sbufWriteU8(dst, DJI_API_VERSION_MINOR);
break;
4.2 DJI_MSP_FC_VARIANT
反馈内容：飞控版本，字符串
示例显示：“INAV”
case DJI_MSP_FC_VARIANT:
{
const char * const flightControllerIdentifier = INAV_IDENTIFIER;
sbufWriteData(dst, flightControllerIdentifier, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
}
break;
4.3 DJI_MSP_FC_VERSION
反馈内容：飞控版本，三字节
示例显示：“4.1.0”
case DJI_MSP_FC_VERSION:
sbufWriteU8(dst, 4);
sbufWriteU8(dst, 1);
sbufWriteU8(dst, 0);
break;
4.4 DJI_MSP_NAME
反馈内容：模型名称，字符串
示例显示：“AocodaRC F7DUAL”
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;
4.5 DJI_MSP_STATUS & DJI_MSP_STATUS_EX
反馈内容：
【1】cycleTime(4B)
注：PID loop time in micro second
示例显示：TBD
【2】Reserved(4B)
示例显示：TBD
【3】SensorStatus(4B)
bit00: SENSOR_ACC
bit01: SENSOR_BARO
bit02: SENSOR_MAG
bit03: SENSOR_GPS
bit04: SENSOR_RANGEFINDER
bit05: SENSOR_OPFLOW
bit06: SENSOR_PITOT
bit07: SENSOR_TEMP
bit15: Hardware failure
示例显示：TBD
【4】FlightMode(4B)
bit00: ARMED
bit01: FLM_ANGLE
bit02: FLM_HORIZON
bit03: FLM_CRUISE //HeadFree
bit04: FLM_FAILSAFE
bit05: FLM_RTH
示例显示：TBD
【5】ConfigProfile(1B)
示例显示：1 //当前使用的Profile ID
【6】SystemLoadPercent(4B)
示例显示：65 //65% CPU使用率
【7】DJI_MSP_STATUS:GyroCycleTime(4B) or DJI_MSP_STATUS_EX:PID_PROFILE_COUNT(3B) & RateProfileIndex(1B)
注：DJI_MSP_STATUS和DJI_MSP_STATUS_EX主要差异点
- 示例显示(DJI_MSP_STATUS)：PID loop time in micro second --> 125
- 示例显示(DJI_MSP_STATUS_EX)：PID_PROFILE_COUNT(3B) & RateProfileIndex(1B) --> 3,1
【8】Reserved(1B)
示例显示：TBD
【9】DJI_ARMING_DISABLE_FLAGS_COUNT(1B)
示例显示：25
【10】djiPackArmingDisabledFlags(4B)
bit24: ARMING_DISABLED_ARM_SWITCH
示例显示：0x1000000
【11】Reserved(1B)
示例显示：TBD
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;
uint16_t packSensorStatus(void)
{
// Sensor bits
uint16_t sensorStatus =
IS_ENABLED(sensors(SENSOR_ACC)) << 0 |
IS_ENABLED(sensors(SENSOR_BARO)) << 1 |
IS_ENABLED(sensors(SENSOR_MAG)) << 2 |
IS_ENABLED(sensors(SENSOR_GPS)) << 3 |
IS_ENABLED(sensors(SENSOR_RANGEFINDER)) << 4 |
IS_ENABLED(sensors(SENSOR_OPFLOW)) << 5 |
IS_ENABLED(sensors(SENSOR_PITOT)) << 6 |
IS_ENABLED(sensors(SENSOR_TEMP)) << 7;
// Hardware failure indication bit
if (!isHardwareHealthy()) {
sensorStatus |= 1 << 15; // Bit 15 of sensor bit field indicates hardware failure
}
return sensorStatus;
}
static void djiPackBoxModeBitmask(boxBitmask_t * flightModeBitmask)
{
memset(flightModeBitmask, 0, sizeof(boxBitmask_t));
// Map flight modes to DJI-supported bits
switch(getFlightModeForTelemetry()) {
case FLM_MANUAL:
case FLM_ACRO:
case FLM_ACRO_AIR:
// DJI: No bits set = ACRO
break;
case FLM_ANGLE:
bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE
break;
case FLM_HORIZON:
bitArraySet(flightModeBitmask->bits, 2); // DJI: 1 << 2
break;
case FLM_RTH:
bitArraySet(flightModeBitmask->bits, 5); // DJI: 1 << 5 : GPS_RESQUE
break;
case FLM_CRUISE:
bitArraySet(flightModeBitmask->bits, 3); // DJI: 1 << 3 : technically HEADFREE
break;
case FLM_FAILSAFE:
bitArraySet(flightModeBitmask->bits, 4); // DJI: 1 << 4
break;
case FLM_LAUNCH:
case FLM_ALTITUDE_HOLD:
case FLM_POSITION_HOLD:
case FLM_MISSION:
default:
// Unsupported ATM, keep at ANGLE
bitArraySet(flightModeBitmask->bits, 1); // DJI: 1 << 1 : ANGLE
}
// Set ARMED mode
if (ARMING_FLAG(ARMED)) {
bitArraySet(flightModeBitmask->bits, 0); // DJI: 1 << 0 : ARMED
}
}
static uint32_t djiPackArmingDisabledFlags(void)
{
// TODO: Map INAV arming disabled flags to DJI/BF ones
// https://github.com/betaflight/betaflight/blob/c6e5882dd91fa20d246b8f8af10cf6c92876bc3d/src/main/fc/runtime_config.h#L42
// For now hide everything in ARMING_DISABLED_ARM_SWITCH (bit 24)
return isArmingDisabled() ? (1 << 24) : 0;
}
4.6 DJI_MSP_RC
反馈内容：channel_1(1B) + channel_2(1B) + channel_3(1B) + channel_4(1B)
示例显示：横滚(1500), 俯仰(1500), 方向(1500), 油门(1200)
case DJI_MSP_RC:
// Only send sticks (first 4 channels)
for (int i = 0; i < STICK_CHANNEL_COUNT; i++) {
sbufWriteU16(dst, rxGetChannelValue(i));
}
break;
4.7 DJI_MSP_RAW_GPS
反馈内容：
fixType(1B) + numSat(1B) + lat(4B) + lon(4B) + alt(2B)/100 + speed(2B) + groundCourse(2B)
示例显示：TBD
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;
4.8 DJI_MSP_COMP_GPS
反馈内容：
distanceToHome(2B) + directionToHome(2B) + gpsHeartbeat(1B)
示例显示：TBD
case DJI_MSP_COMP_GPS:
sbufWriteU16(dst, GPS_distanceToHome);
sbufWriteU16(dst, GPS_directionToHome);
sbufWriteU8(dst, gpsSol.flags.gpsHeartbeat ? 1 : 0);
break;
4.9 DJI_MSP_ATTITUDE
反馈内容：
roll(2B, DECIDEGREES) + pitch(2B, DECIDEGREES) + yaw(2B, DEGREES)
示例显示：TBD
case DJI_MSP_ATTITUDE:
sbufWriteU16(dst, attitude.values.roll);
sbufWriteU16(dst, attitude.values.pitch);
sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
break;
4.10 DJI_MSP_ALTITUDE
反馈内容：
EstimatedActualPosition(4B) + EstimatedActualVelocity(2B)
示例显示：TBD
case DJI_MSP_ALTITUDE:
sbufWriteU32(dst, lrintf(getEstimatedActualPosition(Z)));
sbufWriteU16(dst, lrintf(getEstimatedActualVelocity(Z)));
break;
4.11 DJI_MSP_ANALOG
反馈内容：
BatteryVoltage/10(1B, 0.1V steps) + MAhDrawn(2B, milliamp hours) + RSSI(2B) + Amperage(2B, 0.01A steps) + BatteryVoltage(2B)
示例显示：TBD
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;
4.12 DJI_MSP_PID
反馈内容：
P(1B) + I(1B) + D(1B)
示例显示：TBD
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;
4.13 DJI_MSP_BATTERY_STATE
反馈内容：
BatteryCellCount(1B) + capacity(2B) + BatteryVoltage/10(1B, 0.1V steps) + MAhDrawn(2B) + Amperage(2B) + BatteryState(1B) + BatteryVoltage(2B)
示例显示：TBD
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;
4.14 DJI_MSP_RTC
反馈内容：
year(2B) + month(1B) + day(1B) + hours(1B) + minutes(1B) + seconds(1B) + millis(1B)
示例显示：TBD
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;
4.15 DJI_MSP_ESC_SENSOR_DATA
反馈内容：
DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA: Temp(1B) + Rpm(2B)
STANDARD: N x (Temp(1B) + Rpm(2B))
示例显示：TBD
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;
4.16 DJI_MSP_OSD_CONFIG
反馈内容：
1B: 1 //supported flag, DJI_OSD_FLAGS_OSD_FEATURE
1B: video_system //7456 video system, AUTO/PAL/NTSC
1B: units //Configuration
3B: rssi_alarm(1B) + capacity.warning(2B) //Alarms
2B: 0(1B) + ARRAYLEN(djiOSDItemIndexMap)(1B) //OSD_ITEM_COUNT
2B: alt_alarm //Altitude alarm
2nB: (nx2B) //OSD element position and visibility, n=ARRAYLEN(djiOSDItemIndexMap)
1 + nB: n(1B) + 0(nB) //Post flight statistics待修正代码, n=ARRAYLEN(djiOSDStatisticsMap)
5B: 2(1B) + 0, 0 (2x2=4B) //Timers
2B: 0(2B) //djiEncodeOSDEnabledWarnings, 尚未实现
5B: 16(1B) + 0(4B) //djiEncodeOSDEnabledWarnings, 尚未实现
2B: 1(1B) + 1(1B) //DJI OSD expects 1 OSD profile
1B: 0(1B) //No OSD stick overlay
示例显示：TBD
#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;
static void djiSerializeOSDConfigReply(sbuf_t *dst)
{
// Only send supported flag - always
sbufWriteU8(dst, DJI_OSD_FLAGS_OSD_FEATURE);
// 7456 video system (AUTO/PAL/NTSC)
sbufWriteU8(dst, osdConfig()->video_system);
// Configuration
sbufWriteU8(dst, osdConfig()->units);
// Alarms
sbufWriteU8(dst, osdConfig()->rssi_alarm);
sbufWriteU16(dst, currentBatteryProfile->capacity.warning);
// OSD_ITEM_COUNT (previously was timer alarm)
sbufWriteU8(dst, 0);
sbufWriteU8(dst, ARRAYLEN(djiOSDItemIndexMap));
// Altitude alarm
sbufWriteU16(dst, osdConfig()->alt_alarm);
// OSD element position and visibility
for (unsigned i = 0; i < ARRAYLEN(djiOSDItemIndexMap); i++) {
const int inavOSDIdx = djiOSDItemIndexMap[i].itemIndex;
// We call OSD item supported if it doesn't have dependencies or all feature-dependencies are satistied
const bool itemIsSupported = ((djiOSDItemIndexMap[i].depFeature == 0) || feature(djiOSDItemIndexMap[i].depFeature));
if (inavOSDIdx >= 0 && itemIsSupported) {
// Position & visibility are encoded in 16 bits, and is the same between BF/DJI.
// However INAV supports co-ords of 0-63 and has 3 layouts, while BF has co-ords 0-31 and visibility profiles.
// Re-encode for co-ords of 0-31 and map the layout to all three BF profiles.
uint16_t itemPos = osdLayoutsConfig()->item_pos[0][inavOSDIdx];
uint16_t itemPosSD = OSD_POS_SD(OSD_X(itemPos), OSD_Y(itemPos));
// Workarounds for certain OSD element positions
// INAV calculates these dynamically, while DJI expects the config to have defined coordinates
switch(inavOSDIdx) {
case OSD_CROSSHAIRS:
itemPosSD = OSD_POS_SD(15, 8);
break;
case OSD_ARTIFICIAL_HORIZON:
itemPosSD = OSD_POS_SD(9, 8);
break;
case OSD_HORIZON_SIDEBARS:
itemPosSD = OSD_POS_SD(16, 7);
break;
}
// Enforce visibility in 3 BF OSD profiles
if (OSD_VISIBLE(itemPos)) {
itemPosSD |= (0x3000 | OSD_VISIBLE_FLAG_SD);
}
sbufWriteU16(dst, itemPosSD);
}
else {
// Hide OSD elements unsupported by INAV
sbufWriteU16(dst, 0);
}
}
// Post flight statistics
sbufWriteU8(dst, ARRAYLEN(djiOSDStatisticsMap));
for (unsigned i = 0; i < ARRAYLEN(djiOSDStatisticsMap); i++ ) {
if (djiOSDStatisticsMap[i] >= 0) {
// FIXME: Map post-flight statistics from INAV to BF/DJI
sbufWriteU8(dst, 0);
}
else {
sbufWriteU8(dst, 0);
}
}
// Timers
sbufWriteU8(dst, DJI_OSD_TIMER_COUNT);
for (int i = 0; i < DJI_OSD_TIMER_COUNT; i++) {
// STUB: We don't support BF's OSD timers
sbufWriteU16(dst, 0);
}
// Enabled warnings
// API < 1.41 stub, kept for compatibility
sbufWriteU16(dst, djiEncodeOSDEnabledWarnings() & 0xFFFF);
// API >= 1.41
// Send the warnings count and 32bit enabled warnings flags.
sbufWriteU8(dst, DJI_OSD_WARNING_COUNT);
sbufWriteU32(dst, djiEncodeOSDEnabledWarnings());
// DJI OSD expects 1 OSD profile
sbufWriteU8(dst, 1);
sbufWriteU8(dst, 1);
// No OSD stick overlay
sbufWriteU8(dst, 0);
// API >= 1.43
// Camera frame element width/height - magic numbers taken from Betaflight source
//sbufWriteU8(dst, DJI_OSD_SCREEN_WIDTH); // osdConfig()->camera_frame_width
//sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height
}
4.17 DJI_MSP_FILTER_CONFIG
反馈内容：略
示例显示：TBD
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;
4.18 DJI_MSP_RC_TUNING
反馈内容：
1B: 100 // INAV doesn’t use rcRate
1B: stabilized.rcExpo8
3B: R,P,Y rates
1B: throttle.dynPID
1B: throttle.rcMid8
1B: throttle.rcExpo8
2B: throttle.pa_breakpoint
1B: stabilized.rcYawExpo8
1B: 100 // INAV doesn’t use rcRate
1B: 100 // INAV doesn’t use rcRate
1B: stabilized.rcExpo8
1B: 0
1B: throttle.dynPID
示例显示：TBD
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;
4.19 DJI_MSP_SET_PID
反馈内容：成功或者失败，payload为空
根据DJI协议设置PID，接受报文内容：P(1B) + I(1B) + D(1B)
示例显示：TBD
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;
4.20 Unsupported
暂不支持命令，截止发稿日。
- DJI_MSP_PID_ADVANCED
- DJI_MSP_SET_FILTER_CONFIG
- DJI_MSP_SET_PID_ADVANCED
- DJI_MSP_SET_RC_TUNING
5. 参考资料
【1】iNavFlight之MSP DJI协议分析
【2】BetaFlight模块设计之三十二：MSP协议模块分析
【3】iNavFlight之MSP DJI协议天空端请求报文
1106
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
