px4平台之我见

Px4平台之我见1Px4平台之我见Author：jingwenyiPx4平台之我见..................................................................................................................................1第一章各传感器数据的更新流程...........................................................................................2第一节imu数据更新代码流程.......................................................................................2第二节gps数据更新代码流程.........................................................................................4第三节rc数据处理流程..................................................................................................5第二章代码分析基础知识.......................................................................................................7第一节3X3矩阵操作.......................................................................................................7第二节飞行器的三种模式介绍.....................................................................................14第三节22状态EKF滤波..............................................................................................18第四节Pxio......................................................................................................................21第五节kalman滤波.......................................................................................................24第三章px4各种模式的分析..................................................................................................29第一节悬停模式.............................................................................................................29第四章ekf和dcm对数据的处理..........................................................................................41第一节ekf和dcm代码总体流程介绍.........................................................................41第二节Ekf状态方程分析..............................................................................................42第三节空速传感器ekf代码分析.................................................................................61第三节磁力计ekf算法分析.........................................................................................66第四节速度和位置的kalmanfilter算法.....................................................................76第五章mavlink与地面站的通信.........................................................................................86第一节mavlink协议.......................................................................................................86第六章Pid控制......................................................................................................................124第七章相关实验测试...........................................................................................................125实验一IMU减振实验报告分析....................................................................................125Px4平台之我见2第一章各传感器数据的更新流程第一节imu数据更新代码流程voidCopter::loop(){fast_loop();}---voidCopter::fast_loop(){read_AHRS();}---voidCopter::read_AHRS(void){//这个地方调用了AP_AHRS_NavEKF中的updata,而不是AP_AHRS_DCM的updataahrs.update();}---voidAP_AHRS_NavEKF::update(void){AP_AHRS_DCM::update();//这里直接调用了AP_AHRS_DCM的updata}---AP_AHRS_DCM::update(void){//跟新imu的值_ins.update();}---voidAP_InertialSensor::update(void){for(uint8_ti=0;i_backend_count;i++){_backends[i]-update();//imu更新}}--boolAP_InertialSensor_PX4::update(void){_get_sample();//读取imu的数据//将加速度计的值付给AP_InertialSensor结构体_accel成员_publish_accel(_accel_instance[k],accel,false);//将加速度计对时间积分的速度付给AP_InertialSensor结构体_delta_velocity_publish_delta_velocity(_accel_instance[k],_delta_velocity_accumulator[k],_delta_velocity_dt[k]);Px4平台之我见3//将陀螺仪的值付给AP_InertialSensor结构体_gyro成员_publish_gyro(_gyro_instance[k],gyro,false);//将ekf需要使用的值保持到AP_InertialSensor结构体_delta_angle_publish_delta_angle(_gyro_instance[k],_delta_angle_accumulator[k]);}_get_sample简单分析voidAP_InertialSensor_PX4::_get_sample(){//获取陀螺仪数据boolgyro_valid=_get_gyro_sample(i,gyro_report);//获取加速度计数据boolaccel_valid=_get_accel_sample(i,accel_report);.....//处理采样的数据，并保存到AP_InertialSensor_PX4_new_gyro_sample(i,gyro_report);_new_gyro_sample(i,gyro_report);.......}--AP_InertialSensor_PX4::_new_accel_sample(......){//将加速度的数据加入到_accel_in_accel_in[i]=_accel_filter[i].apply(accel);//将加速度对时间积分算出速度，供ekf使用Vector3fdelVel=Vector3f(accel.x,accel.y,accel.z)*dt;_delta_velocity_accumulator[i]+=delVel;_delta_velocity_dt[i]+=dt;}--AP_InertialSensor_PX4::_new_gyro_sample(.....){//将陀螺仪的数据记录到_gyro_in中_gyro_in[i]=_gyro_filter[i].apply(gyro);//计算角度的累积值供ekf使用//=((_delta_angle_accumulator[i]+_last_delAng[i]*(1.0f/6.0f))%delAng)*0.5f;_delta_angle_accumulator[i]+=delAng+delConing;}Px4平台之我见4第二节gps数据更新代码流程//启动一个跟新gps的任务{SCHED_TASK(update_GPS),8,200}---voidCopter::update_GPS(void){gps.update();//更新gps数据//将gps数据写入flashDataFlash.Log_Write_GPS(gps,i,current_loc.alt);}---VoidAP_GPS::update(void){update_instance(i);}--AP_GPS::update_instance(uint8_tinstance){boolresult=drivers[instance]-read();}--AP_GPS_PX4::read(void)//jwygpsread{//从串口中读if(OK==orb_copy(ORB_ID(vehicle_gps_position),_gps_sub,&_gps_pos)){if(_gps_pos.fix_type=2){//保存位置数据state.location.lat