iNavFlight之MSP v2 Sensor报文格式

1. MSP v2传感报文介绍

鉴于飞控近年来发展历程及趋势

  1. 设计成熟度的提升
  2. 大量传感器的应用
  3. 传感器干扰问题分析
  4. 硬件接口多样化问题
  5. 软件设计复杂度的提高
  6. 配置&使用的简洁化要求

在传感器应用领域,期望更多的标准化设计,MSPv2协议在拓展v1时,就考虑了这方面的需求。

  1. 硬件接口:Rx/Tx/VCC/GND (UART)
  2. 软件接口:MSP v2协议格式
  3. 报文解析:根据command来表征和区分不同传感器数据

2. MSP v2协议格式

  +---+---+--------+---------+--------+------+---------+------------------------------+-------------+
  |                            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
  • payload: 传感数据

这里要注意的一个问题是网络字节序,尤其是对通信比较熟悉的朋友。常规的逻辑是这样的:

发送端CPU字节序 — Host2Network转换字节序 —> 网络传输(大端字节序) — Network2Host转换字节序 —> 接收端CPU字节序
上述两个逻辑转换:Host2Network/Network2Host来确保发送和接受CPU能根据本地的存储字节序来解析多字节变量

【但是】飞控代码上看,串口收到报文以后,直接将buffer一个指针强行变换到定义的结构体上了。

【好嘛,这么粗暴处理!!!】STM32可是小端字节序的呀,这么大胆??? 猜测这些传感模块大都是小端字节序或者8位单片机,所以整个系统都是小端的,就没有大系统这么复杂了。

3. MSP v2传感代码流程

iNav应用代码从main开始进入,根据配置信息使能串口;当串口收到传感器MSP v2传感报文时,将信息送到mspProcessSensorCommand进行解析。

taskHandleSerial
 └──> mspSerialProcess
     └──> mspFcProcessCommand
         └──> mspProcessSensorCommand

main
 └──> init
     └──> fcTasksInit   //setTaskEnabled(TASK_SERIAL, true);

4. MSP v2 传感器

截止发稿日,在MSP v2协议上支持的传感器根据cmd(2),有如下几种:

src/main/msp/msp_protocol_v2_sensor.h

#define MSP2_IS_SENSOR_MESSAGE(x)   ((x) >= 0x1F00 && (x) <= 0x1FFF)

#define MSP2_SENSOR_RANGEFINDER     0x1F01
#define MSP2_SENSOR_OPTIC_FLOW      0x1F02
#define MSP2_SENSOR_GPS             0x1F03
#define MSP2_SENSOR_COMPASS         0x1F04
#define MSP2_SENSOR_BAROMETER       0x1F05
#define MSP2_SENSOR_AIRSPEED        0x1F06

4.1 光流传感报文-MSP2_SENSOR_RANGEFINDER

mspSensorOpflowDataMessage_t

typedef struct __attribute__((packed)) {
    uint8_t quality;    // [0;255]
    int32_t motionX;
    int32_t motionY;
} mspSensorOpflowDataMessage_t;

4.2 测距传感报文-MSP2_SENSOR_OPTIC_FLOW

mspSensorRangefinderDataMessage_t

typedef struct __attribute__((packed)) {
    uint8_t quality;    // [0;255]
    int32_t distanceMm; // Negative value for out of range
} mspSensorRangefinderDataMessage_t;

4.3 GPS传感报文-MSP2_SENSOR_GPS

mspSensorGpsDataMessage_t

typedef struct __attribute__((packed)) {
    uint8_t  instance;                  // sensor instance number to support multi-sensor setups
    uint16_t gpsWeek;                   // GPS week, 0xFFFF if not available
    uint32_t msTOW;
    uint8_t  fixType;
    uint8_t  satellitesInView;
    uint16_t horizontalPosAccuracy;     // [cm]
    uint16_t verticalPosAccuracy;       // [cm]
    uint16_t horizontalVelAccuracy;     // [cm/s]
    uint16_t hdop;
    int32_t  longitude;
    int32_t  latitude;
    int32_t  mslAltitude;       // cm
    int32_t  nedVelNorth;       // cm/s
    int32_t  nedVelEast;
    int32_t  nedVelDown;
    uint16_t groundCourse;      // deg * 100, 0..36000
    uint16_t trueYaw;           // deg * 100, values of 0..36000 are valid. 65535 = no data available
    uint16_t year;
    uint8_t  month;
    uint8_t  day;
    uint8_t  hour;
    uint8_t  min;
    uint8_t  sec;
} mspSensorGpsDataMessage_t;

4.4 磁力计传感报文-MSP2_SENSOR_COMPASS

mspSensorCompassDataMessage_t

typedef struct __attribute__((packed)) {
    uint8_t  instance;
    uint32_t timeMs;
    int16_t  magX; // mGauss, front
    int16_t  magY; // mGauss, right
    int16_t  magZ; // mGauss, down
} mspSensorCompassDataMessage_t;

4.5 气压计传感报文-MSP2_SENSOR_BAROMETER

mspSensorBaroDataMessage_t

typedef struct __attribute__((packed)) {
    uint8_t  instance;
    uint32_t timeMs;
    float    pressurePa;
    int16_t  temp; // centi-degrees C
} mspSensorBaroDataMessage_t;

4.6 空速计传感报文-MSP2_SENSOR_AIRSPEED

mspSensorAirspeedDataMessage_t

typedef struct __attribute__((packed)) {
    uint8_t  instance;
    uint32_t timeMs;
    float    diffPressurePa;
    int16_t  temp;              // centi-degrees C
} mspSensorAirspeedDataMessage_t;

5. 参考资料

【1】BetaFlight模块设计之三十二:MSP协议模块分析
【2】Multiwii Serial Protocol Version 2
【3】传感模块:MATEKSYS Optical Flow & LIDAR 3901-L0X

  • 2
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
MSP4305529是德州仪器(Texas Instruments)推出的一款低功耗微控制器,它使用16位的MSP430架构。关于数据帧格式,MSP4305529可以使用不同的通信协议,如UART、SPI和I2C。下面我会介绍一下这些通信协议的数据帧格式。 1. UART(串口):UART是一种异步通信协议,数据以字节的形式通过单个数据线传输。UART的数据帧格式一般包括起始位(Start Bit)、数据位(Data Bits)、校验位(Parity Bit)和停止位(Stop Bit)。数据位通常为8位,校验位可以选择奇偶校验或无校验,停止位通常为1或2位。 2. SPI(串行外设接口):SPI是一种同步的串行通信协议,使用4条线进行通信,包括一个主设备和一个或多个从设备。SPI没有固定的数据帧格式,而是通过时钟信号进行同步。主设备通过时钟信号将数据位一个一个地发送给从设备,并同时接收从设备返回的数据。 3. I2C(两线制串行接口):I2C是一种同步的串行通信协议,使用两条线进行通信,包括一个主设备和一个或多个从设备。I2C的数据帧格式包括起始位、地址字节、数据字节和停止位。主设备发送起始位后,通过地址字节指定通信的从设备,然后发送数据字节进行通信。 需要注意的是,具体的数据帧格式可能会根据具体的应用和配置有所不同,上述介绍仅为通用格式。在使用MSP4305529时,您可以根据具体的通信需求和所选的通信协议,参考相关的技术手册和文档来配置和使用正确的数据帧格式。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值