PX4IMU等数据处理

1.前置条件

-已搭建PX4开发环境（基于Ubuntu/NuttX）
-熟悉uORB消息系统架构
-使用C++开发模块

2.uORB消息类型
2.1IMU数据

-主消息：sensor_combined（包含加速度计+陀螺仪原始数据）
-独立消息：
-sensor_accel（三轴加速度计原始数据）
-sensor_gyro（三轴陀螺仪原始数据）

-vehicle_attitude EKF2 的 vehicle_attitude 数据，四元数

2.2GPS数据

-主消息：vehicle_gps_position -包含字段：
-经纬度（lat/lon）
-海拔高度（alt）
-定位状态（fix_type）
-卫星数（satellites_used）

2.3消息

-src/lib/drivers/accelerometer/PX4Accelerometer.cpp

-这段代码是 PX4 固件中 PX4Accelerometer 类的实现，负责处理加速度计的数据，包括数据的旋转、缩放、发布以及 FIFO（先进先出）缓存的处理。

-在 PX4 飞控系统中，sensor_accel.h和sensor_gyro.h定义了加速度计和陀螺仪传感器的数据结构和相关话题（topics）。这些数据通过 uORB 消息传递机制在系统中发布和订阅，供其他模块使用。

3.订阅实现步骤

3.1头文件

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include <fcntl.h>
#include <systemlib/mavlink_log.h>

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <poll.h>
#include <math.h>

#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h> // EKF2 处理后的数据
#include <uORB/uORB.h>

3.2初始化订阅

//订阅加速度计和陀螺仪数据
int sensor_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
int sensor_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));      // IMU 完整数据
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));    // EKF2 姿态数据

3.3完整串口发送代码

int rw_uart_thread_main(int argc, char *argv[])
{
    // 初始化串口，使用 PX4 提供的接口
    int uart_read = uart_init("/dev/ttyS2");
    if (false == uart_read)
        return -1;
    if (false == set_uart_baudrate(uart_read, 57600)) {
        printf("[JXF]set_uart_baudrate is failed\n");
        return -1;
    }
    printf("[JXF]uart init is successful\n");

    thread_running = true;

    // 订阅 sensor_accel 和 sensor_gyro 话题
    int sensor_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
    int sensor_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));

    struct sensor_accel_s sensor_accel_data;
    struct sensor_gyro_s sensor_gyro_data;
    char buffer[128];

    while (!thread_should_exit) {
        // 获取 sensor_accel 数据
        if (orb_copy(ORB_ID(sensor_accel), sensor_accel_sub, &sensor_accel_data) == PX4_OK) {
            printf("sensor_accel:\ttimestamp: %" PRIu64 " temperature: %.2f x: %.2f y: %.2f z: %.2f\n",
                   sensor_accel_data.timestamp, (double)sensor_accel_data.temperature,
                   (double)sensor_accel_data.x, (double)sensor_accel_data.y, (double)sensor_accel_data.z);

            snprintf(buffer, sizeof(buffer), "sensor_accel: timestamp: %" PRIu64 " temp: %.2f x: %.2f y: %.2f z: %.2f\n",
                     sensor_accel_data.timestamp, (double)sensor_accel_data.temperature,
                     (double)sensor_accel_data.x, (double)sensor_accel_data.y, (double)sensor_accel_data.z);

            write(uart_read, buffer, strlen(buffer));
        }

        // 获取 sensor_gyro 数据
        if (orb_copy(ORB_ID(sensor_gyro), sensor_gyro_sub, &sensor_gyro_data) == PX4_OK) {
            printf("sensor_gyro:\ttimestamp: %" PRIu64 " temperature: %.2f x: %.2f y: %.2f z: %.2f\n",
                   sensor_gyro_data.timestamp, (double)sensor_gyro_data.temperature,
                   (double)sensor_gyro_data.x, (double)sensor_gyro_data.y, (double)sensor_gyro_data.z);

            snprintf(buffer, sizeof(buffer), "sensor_gyro: timestamp: %" PRIu64 " temp: %.2f x: %.2f y: %.2f z: %.2f\n",
                     sensor_gyro_data.timestamp, (double)sensor_gyro_data.temperature,
                     (double)sensor_gyro_data.x, (double)sensor_gyro_data.y, (double)sensor_gyro_data.z);

            write(uart_read, buffer, strlen(buffer));
        }

        usleep(50000); // 50ms 延迟
    }

    PX4_WARN("[rw_uart] exiting.");
    thread_running = false;

    close(uart_read);
    printf("UART close status: %d\n", uart_read);

    return 0;
}
 main

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include <fcntl.h>
#include <systemlib/mavlink_log.h>

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <poll.h>
#include <math.h>

#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h> // EKF2 处理后的数据
#include <uORB/uORB.h>


static bool thread_should_exit = false;        /**< px4_uorb_subs exit flag */
static bool thread_running = false;            /**< px4_uorb_subs status flag */
static int rw_uart_task;                        /**< Handle of px4_uorb_subs task / thread */

static int uart_init(char * uart_name);
static int set_uart_baudrate(const int fd, unsigned int baud);

/**
 * daemon management function.
 */
__EXPORT int rw_uart_main(int argc, char *argv[]);

/**
 * Mainloop of daemon.
 */
int rw_uart_thread_main(int argc, char *argv[]);

/**
 * Print the correct usage.
 */
static void usage(const char *reason);

static void usage(const char *reason)
{
    if (reason) {
        warnx("%s\n", reason);
    }

    warnx("usage: px4_uorb_adver {start|stop|status} [-p <additional params>]\n\n");
}

int set_uart_baudrate(const int fd, unsigned int baud)
{
    int speed;

    switch (baud) {
        case 9600:   speed = B9600;   break;
        case 19200:  speed = B19200;  break;
        case 38400:  speed = B38400;  break;
        case 57600:  speed = B57600;  break;
        case 115200: speed = B115200; break;
        default:
            warnx("ERR: baudrate: %d\n", baud);
            return -EINVAL;
    }

    struct termios uart_config;
    int termios_state;

    tcgetattr(fd, &uart_config); // 获取终端参数

    // 清除ONLCR标志，防止换行转换
    uart_config.c_oflag &= ~ONLCR;

    // 设置无校验，一个停止位，数据位8
    uart_config.c_cflag &= ~(CSTOPB | PARENB);  // 1停止位，且无校验
    uart_config.c_cflag |= CS8;  // 数据位8

    if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
        warnx("ERR: %d (cfsetispeed)\n", termios_state);
        return false;
    }

    if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
        warnx("ERR: %d (cfsetospeed)\n", termios_state);
        return false;
    }

    // 设置终端参数
    if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
        warnx("ERR: %d (tcsetattr)\n", termios_state);
        return false;
    }

    return true;
}

int uart_init(char * uart_name)
{
    int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);
    if (serial_fd < 0) {
        err(1, "failed to open port: %s", uart_name);
        printf("failed to open port: %s\n", uart_name);
        return false;
    }
    printf("Open the %d\n", serial_fd);
    return serial_fd;
}

/**
消息发布进程，会不断的接收自定义消息
 */
int rw_uart_main(int argc, char *argv[])
{
    if (argc < 2) {
        usage("missing command");
        return 1;
    }

    if (!strcmp(argv[1], "start")) {

        if (thread_running) {
            warnx("px4_uorb_subs already running\n");
            /* this is not an error */
            return 0;
        }

        thread_should_exit = false;//定义一个守护进程
        rw_uart_task = px4_task_spawn_cmd("rw_uart",
            SCHED_DEFAULT,
            SCHED_PRIORITY_DEFAULT,//调度优先级
            2000,//堆栈分配大小
            rw_uart_thread_main,
            (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
        return 0;
    }

    if (!strcmp(argv[1], "stop")) {
        thread_should_exit = true;
        return 0;
    }

    if (!strcmp(argv[1], "status")) {
        if (thread_running) {
            warnx("\trunning\n");

        }
        else {
            warnx("\tnot started\n");
        }

        return 0;
    }

    usage("unrecognized command");
    return 1;
}

int rw_uart_thread_main(int argc, char *argv[])
{
    // 初始化串口，使用 PX4 提供的接口
    int uart_read = uart_init("/dev/ttyS2");
    if (false == uart_read)
        return -1;
    if (false == set_uart_baudrate(uart_read, 57600)) {
        printf("[JXF]set_uart_baudrate is failed\n");
        return -1;
    }
    printf("[JXF]uart init is successful\n");

    thread_running = true;

// 订阅话题
int sensor_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
int sensor_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));      // IMU 完整数据
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));    // EKF2 姿态数据

// 数据结构体
struct sensor_accel_s sensor_accel_data;
struct sensor_gyro_s sensor_gyro_data;
struct sensor_combined_s sensor_combined_data;
struct vehicle_attitude_s vehicle_attitude_data;

char buffer[256]; // 缓冲区

while (!thread_should_exit) {
    // 获取 sensor_accel 数据
    if (orb_copy(ORB_ID(sensor_accel), sensor_accel_sub, &sensor_accel_data) == PX4_OK) {
        printf("sensor_accel: x: %.2f y: %.2f z: %.2f\n",
               (double)sensor_accel_data.x, (double)sensor_accel_data.y, (double)sensor_accel_data.z);

        snprintf(buffer, sizeof(buffer), "sensor_accel: x: %.2f y: %.2f z: %.2f\n",
                 (double)sensor_accel_data.x, (double)sensor_accel_data.y, (double)sensor_accel_data.z);
        write(uart_read, buffer, strlen(buffer));
    }

    // 获取 sensor_gyro 数据
    if (orb_copy(ORB_ID(sensor_gyro), sensor_gyro_sub, &sensor_gyro_data) == PX4_OK) {
        printf("sensor_gyro: x: %.2f y: %.2f z: %.2f\n",
               (double)sensor_gyro_data.x, (double)sensor_gyro_data.y, (double)sensor_gyro_data.z);

        snprintf(buffer, sizeof(buffer), "sensor_gyro: x: %.2f y: %.2f z: %.2f\n",
                 (double)sensor_gyro_data.x, (double)sensor_gyro_data.y, (double)sensor_gyro_data.z);
        write(uart_read, buffer, strlen(buffer));
    }

    // 获取 sensor_combined 数据
    if (orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor_combined_data) == PX4_OK) {
        printf("sensor_combined: accel: %.2f %.2f %.2f gyro: %.2f %.2f %.2f\n",
               (double)sensor_combined_data.accelerometer_m_s2[0],
               (double)sensor_combined_data.accelerometer_m_s2[1],
               (double)sensor_combined_data.accelerometer_m_s2[2],
               (double)sensor_combined_data.gyro_rad[0],
               (double)sensor_combined_data.gyro_rad[1],
               (double)sensor_combined_data.gyro_rad[2]);

        snprintf(buffer, sizeof(buffer), "sensor_combined: accel: %.2f %.2f %.2f gyro: %.2f %.2f %.2f\n",
                 (double)sensor_combined_data.accelerometer_m_s2[0],
                 (double)sensor_combined_data.accelerometer_m_s2[1],
                 (double)sensor_combined_data.accelerometer_m_s2[2],
                 (double)sensor_combined_data.gyro_rad[0],
                 (double)sensor_combined_data.gyro_rad[1],
                 (double)sensor_combined_data.gyro_rad[2]);
        write(uart_read, buffer, strlen(buffer));
    }

    // 获取 vehicle_attitude 数据（移除 timestamp，输出四元数 q）
    if (orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &vehicle_attitude_data) == PX4_OK) {
        printf("vehicle_attitude: q: %.2f %.2f %.2f %.2f\n",
               (double)vehicle_attitude_data.q[0],
               (double)vehicle_attitude_data.q[1],
               (double)vehicle_attitude_data.q[2],
               (double)vehicle_attitude_data.q[3]);

        snprintf(buffer, sizeof(buffer), "vehicle_attitude: q: %.2f %.2f %.2f %.2f\n",
                 (double)vehicle_attitude_data.q[0],
                 (double)vehicle_attitude_data.q[1],
                 (double)vehicle_attitude_data.q[2],
                 (double)vehicle_attitude_data.q[3]);
        write(uart_read, buffer, strlen(buffer));
    }

    usleep(50000); // 50ms 延迟
}

    PX4_WARN("[rw_uart] exiting.");
    thread_running = false;

    close(uart_read);
    printf("UART close status: %d\n", uart_read);

    return 0;
}


 rw-uart.c

3.4结果：

PX4 源码中对二次开发比较重要的代码模块

VehicleAttitude （UORB 消息） |PX4 指南 （main）

---

1. Commander

• 功能

  Commander 模块主要处理 PX4 的常用命令，例如：

  • 传感器校准
  • 模式切换
  • 加锁/解锁
  • 起飞/降落
  • 紧急断电等。

• 输入主题

  vehicle_command：接收外部命令。

• 输出主题

  根据不同的命令 ID，将命令分解为其他主题并发布出去，供其他模块响应。

• 使用方式

  如果需要使用 Commander 模块的功能，可以在自己的程序代码中直接发布主题 vehicle_command。

---

2. EKF2 (Extended Kalman Filter 2)

• 功能

  EKF2 模块负责传感器数据融合，包括 GPS、加速度计、指南针等数据的处理。

• 输入参数

  输入由 PX4 的硬件驱动模块提供，开发者无需关心具体的输入主题。

• 输出主题

  • vehicle_attitude：当前飞控的姿态。
  • vehicle_local_position：当前飞控的本地位置（NED 坐标系，以飞控启动时的初始点为原点）。
  • vehicle_global_position：当前飞控的全球位置。

• 使用方式

  开发者可以直接订阅这些主题，EKF2 会保证数据的实时更新，无需关注具体硬件。

---

3. MAVLink

• 功能

  MAVLink 模块负责 MAVLink 协议通信，解析来自飞控数传串口的数据，并将其分解为多个主题。

• 输入

  来自飞控数传串口的 MAVLink 数据包。

• 输出

  将 MAVLink 数据包解封后生成的主题。

• 特点

  MAVLink 是 PX4 中最上层的模块之一，也是分析飞控的重点模块。

---

4. MC_ATT_CONTROL (Multicopter Attitude Control)

• 功能

  MC_ATT_CONTROL 模块负责多旋翼飞行器的姿态控制。

• 输入主题

  • vehicle_attitude_setpoint：期望姿态信息。

• 输出主题

  • actuator_controls_0：主混控器控制输出。
  • 其他混控器控制组（actuator_controls_1 至 actuator_controls_7），分别对应 8 个混控器控制组。

• 混控器作用

  将期望姿态信息（roll, pitch, yaw）映射到实际的电机转速或舵机角度。

• 注意事项

  不建议直接发布 vehicle_attitude_setpoint 主题，因为 PX4 中对此主题有完整的响应链。如果直接发布，可能导致与其他模块的冲突，进而影响飞行器稳定性。

---

5. MC_POS_CONTROL (Multicopter Position Control)

• 功能

  MC_POS_CONTROL 模块负责多旋翼飞行器的位置控制。

• 输入主题

  • manual_control_setpoint：遥控器的当前输入值（如油门打到 100%，对应的参数可能是 100）。
  • position_setpoint_triplet：期望位置信息（本地坐标系 NED）。

• 输出主题

  • vehicle_attitude_setpoint：作为 MC_ATT_CONTROL 模块的输入。

• 层级关系

  位置控制模块位于姿态控制模块之上，其输出作为姿态控制模块的输入。

• 使用方式

  如果需要控制飞行器飞往某个位置，应发布主题 position_setpoint_triplet。此主题的来源包括：

  • MAVLink 模块（地面站控制）。
  • Navigator 模块（AUTO 模式发起）。

• 注意事项

  不建议直接发布 vehicle_attitude_setpoint，以免与位置控制模块的输出冲突。

---

6. Navigator

• 功能

  Navigator 模块负责导航任务规划，例如 AUTO 模式下的路径规划和任务执行。

• 输入主题

  • 来自 MAVLink 或其他模块的任务指令。

• 输出主题

  • position_setpoint_triplet：期望位置信息。

• 特点

  Navigator 模块是 AUTO 模式的核心模块，负责生成飞行任务的路径点。

---

总结

在 PX4 的二次开发中，了解这些模块的功能、输入输出主题以及它们之间的层级关系非常重要。以下是一些关键点：

1. Commander：用于处理飞控命令。
2. EKF2：提供传感器数据融合后的姿态和位置信息。
3. MAVLink：负责通信协议解析。
4. MC_ATT_CONTROL：实现姿态控制，避免直接发布 vehicle_attitude_setpoint。
5. MC_POS_CONTROL：实现位置控制，推荐通过发布 position_setpoint_triplet 控制飞行器位置。
6. Navigator：负责 AUTO 模式的任务规划。

开发者可以根据需求选择合适的模块进行扩展或修改，同时注意保持与 PX4 系统的整体兼容性。