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
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)