PX4源码分析以及思路随笔1

，函数为：

//DCM

：

DirectionCosine Matrix

，方向余弦矩阵

AP_ AHRS_DCM::update

(

void

)

{

float

delta_t;

if

(

_last_startup_ms

== 0) {

_last_startup_ms

= AP_HAL::millis();

}

(1)

更新加速度计和陀螺仪

// tell the IMU to grab some data

_ins

.update();

// ask the IMU how much time this sensor reading represents

delta_t =

_ins

.get_delta_time();

// if the update call took more than 0.2 seconds then discard it,

// otherwise we may move too far. This happens when arming motors

// in ArduCopter

if

(delta_t > 0.2f) {

memset(&

_ra_sum

[0], 0,

(

_ra_sum

< br>));

_ra_deltat

= 0;

return

;

}

(2)

使用陀螺仪更新四元素矩阵（做余弦矩阵的归一化）

// Integrate the DCM matrix using gyro inputs

matrix_update(delta_t);

(3)

标准化

// Normalize the DCM matrix

normalize();

(4)

执行漂移修正

使用加速度计和罗盘与该次计算的四元素矩阵做差，

修正出下一

次陀螺仪的输出

// Perform drift correction

drift_correction(delta_t);

(5)

检查错误值

// paranoid check for bad values in the DCM matrix

check_matrix();

(6)

计算俯仰角，偏航角，翻转角

// Calculate pitch, roll, yaw for stabilization and navigation

euler_angles();

(7)

更新三角函数的值，包括俯仰角的余弦和翻滚角的余弦

// update trig values including _cos_roll, cos_pitch

update_trig();

}

返回目录

3.1.1.1 _ins

.update()

更新加速度计和陀螺仪

// tell the IMU to grab some data

_ins

.update();

函数 定义：

void

AP_InertialSensor::up date

(

void

)

：

在函数中：

[i]->update();

为主要操作步骤。

_backend s

[i]

的定义为：

(

是一个指针数组

)

AP_InertialSensor_Backend

*< /p>

_backends

[INS_MAX_BACKENDS];

查找

_backends

所指向的数据 ，有两个函数值得注意：

void

A P_InertialSensor::_add_backend

(AP_Inert ialSensor_Backend

*backend)

Void

AP_InertialSensor::detect_backends< /p>

(

void

)

考虑在

detect_backends()

中进行了调用：< /p>

#elif

HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT ==

HAL_INS_VRBRAIN

_add_ backend(AP_InertialSensor_PX4::detect(*

this

));

因此得出

_backends

指向的是类

AP_Ine rtialSensor_PX4

。找到函数如下：

bool

AP_InertialSensor_PX4::

(

void

)

{

// get the latest sample from the sensor drivers

_get_sample();

// uint8_t _num_accel_instances;

// uint8_t _num_gyro_instances;

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]); //

陀螺仪

}

returntrue

;

}

其中两个重要的函数分别为：

< /p>

update_accel(_accel_instance[k])

< br>和

update_gyro(_gyro_instance[k]).

首先看

update_accel( _accel_instance[k])

：

输入参数为

_accel_instance[k]

和

_gyro_instance[k]

，定义为：

#define

INS_MAX_INSTANCES 3

uint8_t _accel_instance[INS_MAX_INSTANCES];

uint8_t _gyro_instance[INS_MAX_INSTANCES];

随后看

update_accel()

和

update_gyro()

的定义：

void

AP_InertialS ensor_Backend::update_accel

(

uint8_t

instance)

{

hal.

scheduler

->suspend_timer_procs();

//

任务 调度，推迟进程

//_imu._new_acce l_data[instance]

为布尔型变量，推测为观察是否可以更新

//

其中

_p ublish_accel()

应该就是

if

(

_imu

.

_new_accel_data

[instance] ) {

//

两个参数，其中一个是

< br>instance

，另一个参考为过滤后的加速度计数据？？

_publish_accel(instance,

_imu

.

_accel_filtered

[instance]);

_imu

.

_new_accel_data

[instance] =

false

;

}

// possibly update filter frequency

设定频率？

if

(

_last_accel_f ilter_hz

[instance] != _accel_filter_cutoff()) {

_imu

.

_accel_filter

[instance].s et_cutoff_frequency(_accel_raw_sample_r

ate(instance), _accel_filter_cutoff());

_last_accel_filter_hz

[instance] = _accel_filter_cutoff();

}

//

任务调度，推测为经过一个进程后重新设置

hal.

scheduler

->resume_timer_procs();

}

下面对于陀螺仪的控制基本没有区别：

void

AP_InertialSensor_Backend::update_ gyro

(

uint8_t

instance)

{

hal.