博主:UAV
声明:尊重版权,转载请注明出处。
原文地址:
技术交流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实现: