Pixhawk之获取传感器数据并更新姿态

博主：UAV

声明：尊重版权，转载请注明出处。

原文地址：

联系方式：595493514@qq.com

技术交流QQ：595493514

read_AHRS();是负责更新姿态函数，更新姿态要获取传感器数据进行姿态解算，我们先来分析下传感器是怎么读取的。read_AHRS();调用的是ahrs.update()；ahrs是类AP_AHRS_NavEKF的一个实例， ahrs.update()调用了下面三个函数，update_DCM(); update_EKF1(); update_EKF2()；我们先分析update_DCM();  调用了AP_AHRS_DCM::update()，继续调用_ins.update()；_ins.update()就是更新加速度和陀螺仪的数据，因为class AP_AHRS_DCM : public AP_AHRS，所以 AP_AHRS_DCM是AP_AHRS的一个子类。在AP_ARHS.h的文件中找到_ins 的定义：AP_InertialSensor   &_ins；找到函数void AP_InertialSensor::update(void)中更新数据的函数是_backends[i]->update()；到这来就要分析下_backends是什么。在/src/APM/ardupilot/libraries/AP_InertialSensor/AP_InertialSensor.h 的头文件中有下面的定义AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS]；所以_backends是一个指针数组。在/src/APM/ardupilot/libraries/AP_InertialSensor/AP_InertialSensor.cpp有对其进行赋值，_add_backend(AP_InertialSensor_PX4::detect(*this))。到这来我们分析出来了，传感器的数据获取是通过AP_InertialSensor_PX4实现：

bool AP_InertialSensor_PX4::update(void)
{
    // get the latest sample from the sensor drivers
    _get_sample();//读取底层传感器数据

    for (uint8_t k=0; k<_num_accel_instances; k++) {
        update_accel(_accel_instance[k]);//更新加速度
    }

    for (uint8_t k=0; k<_num_gyro_instances; k++) {
        update_gyro(_gyro_instance[k]);//更新陀螺仪
    }
   
    return true;
}

再分析下采样函数：

void AP_InertialSensor_PX4::_get_sample()
{
    for (uint8_t i=0; i<MAX(_num_accel_instances,_num_gyro_instances);i++) {
        struct accel_report accel_report;
        struct gyro_report gyro_report;

        bool gyro_valid = _get_gyro_sample(i,gyro_report);//获取陀螺数据
        bool accel_valid = _get_accel_sample(i,accel_report);//获取加速度数据

        while(gyro_valid || accel_valid) {

            if(gyro_valid && accel_valid && gyro_report.timestamp <= accel_report.timestamp) {
                _new_gyro_sample(i,gyro_report);
                gyro_valid = _get_gyro_sample(i,gyro_report);
                continue;
            }
 
            if(accel_valid) {
                _new_accel_sample(i,accel_report);
                accel_valid = _get_accel_sample(i,accel_report);
                continue;
            }
           
            if(gyro_valid) {
                _new_gyro_sample(i,gyro_report);
                gyro_valid = _get_gyro_sample(i,gyro_report);
            }
        }
    }
}

直接与最底层的read 函数打交道啦

这里的read其实就是对应驱动代码中的read 啦

我这里用测试打印验证的哦：

L3GD20::read

_get_gyro_sample 1

L3GD20::read

_get_gyro_sample 1

L3GD20::read

_get_gyro_sample 1

L3GD20::read

_get_gyro_sample 1

L3GD20::read

_get_gyro_sample 1

L3GD20::read

函数中_get_accel_sample(uint8_t i, struct accel_report &accel_report)的参数i其实就是选择文件路径啦

其实也是对应于传感器的数据项目

例如陀螺的路径

#define GYRO_BASE_DEVICE_PATH "/dev/gyro"
#define GYRO0_DEVICE_PATH "/dev/gyro0"
#define GYRO1_DEVICE_PATH "/dev/gyro1"
#define GYRO2_DEVICE_PATH "/dev/gyro2"

对于多传感器，肯定是要分多次读取，MPU6000和L3GD20都是在这里读取，数据格式为结构体accel_report和gyro_report的格式

读取数据的格式，例如陀螺的：

struct sensor_gyro_s {
 uint64_t timestamp;
 uint64_t integral_dt;
 uint64_t error_count;
 float x;
 float y;
 float z;
 float x_integral;
 float y_integral;
 float z_integral;
 float temperature;
 float range_rad_s;
 float scaling;
 int16_t x_raw;
 int16_t y_raw;
 int16_t z_raw;
 int16_t temperature_raw;
};

加速度：

struct sensor_accel_s {
 uint64_t timestamp;
 uint64_t integral_dt;
 uint64_t error_count;
 float x;
 float y;
 float z;
 float x_integral;
 float y_integral;
 float z_integral;
 float temperature;
 float range_m_s2;
 float scaling;
 int16_t x_raw;
 int16_t y_raw;
 int16_t z_raw;
 int16_t temperature_raw;
};

bool AP_InertialSensor_PX4::_get_accel_sample(uint8_t i, struct accel_report &accel_report)
{
    if (i<_num_accel_instances &&
        _accel_fd[i] != -1 &&
        ::read(_accel_fd[i], &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
        accel_report.timestamp != _last_accel_timestamp[i]) {
        return true;
    }
    return false;
}

bool AP_InertialSensor_PX4::_get_gyro_sample(uint8_t i, struct gyro_report &gyro_report)
{
    if (i<_num_gyro_instances &&
        _gyro_fd[i] != -1 &&
        ::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
        gyro_report.timestamp != _last_gyro_timestamp[i]) {
        return true;
    }
    return false;
}

读取l3gd20的read函数：

/src/APM/ardupilot/modules/PX4Firmware/src/drivers/l3gd20/l3gd20.cpp

ssize_t L3GD20::read(struct file *filp, char *buffer, size_t buflen)