一、前言

我所讨论的PX4代码是基于v1.11版本，该版本与之前的版本有不少不同，其中一个比较大的区别在于新版本大部分用到了C++中模板，使得代码没有以前那么容易理解，因此我在后面介绍PX4的姿态估计，控制等重要算法代码时会介绍一部分类与模板的知识，方便大家理解代码。
接下来几章我将讨论我对PX4代码姿态估计的理解，主要的文件是PX4_Firmware/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp，我的理解不一定正确，一家之言，欢迎大家批评指正

二、代码分析

由于PX4代码V1.11版本用了较多类与模板，所以要完全读懂比较麻烦，我只是分享一下我个人理解。首先，PX4在attitude_estimator_q_main.cpp中定义了一个类，后续所有操作都基于这个类进行操作，类的定义如下

class AttitudeEstimatorQ : public ModuleBase<AttitudeEstimatorQ>, public ModuleParams, public px4::WorkItem
{
//这个类继承了三个类：1.ModuleBase，这是一个类模板，含有部分函数调用模板；2.ModuleParams
//3.px4::WorkItem,px4是定义的工作空间，WorkItem是一个类，与工作队列有关，工作队列详细讲解
//见PX4代码解析（4）
public:AttitudeEstimatorQ();//类的构造函数~AttitudeEstimatorQ() override = default;//析构函数static int task_spawn(int argc, char *argv[]);//在PX4中创建进程static int custom_command(int argc, char *argv[]);//提示命令输入错误static int print_usage(const char *reason = nullptr);//输出用法bool init();//初始化，检查能否正常通信private:void Run() override;//姿态估计主程序void update_parameters(bool force = false);//更新传感器参数bool init_attq();//初始化姿态bool update(float dt);//加速度计，磁力计，陀螺仪校正void update_mag_declination(float new_declination);//立即更新磁偏角（以rads为单位）以改变偏航旋转const float _eo_max_std_dev = 100.0f;       /**< Maximum permissible standard deviation for estimated orientation */const float _dt_min = 0.00001f;//dt是指更新数据的时间间隔const float _dt_max = 0.02f;uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
//订阅消息uORB::Subscription        _parameter_update_sub{ORB_ID(parameter_update)};uORB::Subscription      _gps_sub{ORB_ID(vehicle_gps_position)};uORB::Subscription       _local_position_sub{ORB_ID(vehicle_local_position)};uORB::Subscription      _vision_odom_sub{ORB_ID(vehicle_visual_odometry)};uORB::Subscription        _mocap_odom_sub{ORB_ID(vehicle_mocap_odometry)};uORB::Subscription      _magnetometer_sub{ORB_ID(vehicle_magnetometer)};uORB::Publication<vehicle_attitude_s> _att_pub{ORB_ID(vehicle_attitude)};#if defined(ENABLE_LOCKSTEP_SCHEDULER)uORB::Publication<ekf2_timestamps_s> _ekf2_timestamps_pub {ORB_ID(ekf2_timestamps)};
#endiffloat     _mag_decl{0.0f};//float     _bias_max{0.0f};Vector3f    _gyro;//陀螺仪数据Vector3f   _accel;//加速度计数据Vector3f _mag;//磁力计数据Vector3f    _vision_hdg;//通过视觉得到的数据Vector3f _mocap_hdg;//动作捕捉相关数据Quatf      _q;//姿态四元数Vector3f  _rates;//存放修正后的绕三轴角速度Vector3f   _gyro_bias;Vector3f _vel_prev;//前一刻的速度hrt_abstime   _vel_prev_t{0};//用于求加速度Vector3f _pos_acc;//运动加速度，注意：这个加速度不包括垂直方向的hrt_abstime  _last_time{0};bool      _inited{false};//初始化标识bool      _data_good{false};//数据可用标志bool      _ext_hdg_good{false};//外部航向可用标志
//读取相关系统参数DEFINE_PARAMETERS((ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc,(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag,(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg,(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias,(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl,(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a,(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m,(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp,(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas,(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag
#if defined(ENABLE_LOCKSTEP_SCHEDULER), (ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_est_group
#endif)
};

简单介绍了类中主要函数与主要参数的意义，接下来讨论程序怎么运行的。在PX4代码中，各模块都是由**模块名_main()**这个函数完成启动，该函数代码如下：

extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[])
{return AttitudeEstimatorQ::main(argc, argv);
}

这段代码需要注意以下几点
1.extern "C"的主要作用就是为了能够正确实现C++代码调用其他C语言代码。加上extern "C"后，会指示编译器这部分代码按C语言的进行编译，而不是C++的。由于C++支持函数重载，因此编译器编译函数的过程中会将函数的参数类型也加到编译后的代码中，而不仅仅是函数名；而C语言并不支持函数重载，因此编译C语言代码的函数时不会带上函数的参数类型，一般之包括函数名。
2.该函数调用了AttitudeEstimatorQ::main(argc, argv)这个函数，在这里它利用了模板，main（）函数的主要实现在PX4_Firmware/platforms/common/include/px4_platform_common/module.h中，实现代码如下

static int main(int argc, char *argv[]){//判断参数if (argc <= 1 ||strcmp(argv[1], "-h")    == 0 ||strcmp(argv[1], "help")  == 0 ||strcmp(argv[1], "info")  == 0 ||strcmp(argv[1], "usage") == 0) {return T::print_usage();}
//如果输入命令为start，调用start_command_base（）函数启动进程if (strcmp(argv[1], "start") == 0) {// Pass the 'start' argument too, because later on px4_getopt() will ignore the first argument.return start_command_base(argc - 1, argv + 1);}
//输入命令为status，返回进程状态：在运行还是没有运行if (strcmp(argv[1], "status") == 0) {return status_command();}
//输入命令为stop，则停止该进程if (strcmp(argv[1], "stop") == 0) {return stop_command();}
//与信号量操作有关，可以暂时不用理解，后续会分享lock_module(); // Lock here, as the method could access _object.int ret = T::custom_command(argc - 1, argv + 1);unlock_module();return ret;}

可以看到，如果我们输入命令为start，就可以调用start_command_base（）函数启动进程，start_command_base（）函数实现如下：

static int start_command_base(int argc, char *argv[]){int ret = 0;lock_module();
//判断进程是否在运行if (is_running()) {ret = -1;PX4_ERR("Task already running");
//进程没有运行则调用类中task_spawn（）函数启动进程} else {ret = T::task_spawn(argc, argv);
//启动失败if (ret < 0) {PX4_ERR("Task start failed (%i)", ret);}}unlock_module();return ret;}

在前面我们了解到，PX4中各模块都作为一个进程（任务）运行在nuttx系统，因此，在PX4中要完成某项任务（比如姿态解算）需要启动进程，但进程调用的实现一般由主模块的task_spawn（）函数实现，于是返回attitude_estimator_q_main.cpp文件看看task_spawn（）函数的实现（注释在代码中）

int AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
{//创建一个AttitudeEstimatorQ的类，该类的定义在上面已经介绍//注意类的构造函数，在生成这个类时，将该模块加入工作队列AttitudeEstimatorQ *instance = new AttitudeEstimatorQ();//创建成功，if (instance) {_object.store(instance);//原子操作，先不用懂//task_id_is_work_queue定义在moudle.h中，值为-2_task_id = task_id_is_work_queue;//调用init()函数，检查能否正常通信if (instance->init()){//PX_OK == 0,定义在defines.hreturn PX4_OK;}} else {PX4_ERR("alloc failed");}delete instance;_object.store(nullptr);_task_id = -1;//PX4_ERROR == -1,定义在defines.hreturn PX4_ERROR;
}

在前面的代码解析中，我们提到由于飞控平台pixhawk内存较小，而底层运行的Nuttx操作系统需要占用一些空间，这就导致硬件内存十分有限，而每创建进程与线程时都会消耗内存资源，导致pixhawk硬件内存紧张。但是，我们还需要让一些功能可以像多线程一样并行运行，又尽量减少资源消耗，于是操作系统便引入工作队列，详细介绍请见PX4代码解析（4）。我们继续解析代码，在task_spawn()中创建了一个AttitudeEstimatorQ类，创建该类时调用了构造函数AttitudeEstimatorQ(),并在此构造函数中将该任务加入工作队列实现并行运行，构造函数内容如下：

AttitudeEstimatorQ::AttitudeEstimatorQ() :ModuleParams(nullptr),//WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)//将姿态估计任务加入工作队列
{
//参数清零初始化_vel_prev.zero();_pos_acc.zero();_gyro.zero();_accel.zero();_mag.zero();_vision_hdg.zero();_mocap_hdg.zero();_q.zero();//姿态四元数清0_rates.zero();//_gyro_bias.zero();update_parameters(true);
}

在AttitudeEstimatorQ()构造函数中主要对变量进行了初始化，并将任务加入工作队列(程序运行关键步骤)，给大家看看该函数的实现(函数存放在PX4_Firmware/platforms/common/px4_work_queue/WorkIte.cpp）

WorkItem::WorkItem(const char *name, const wq_config_t &config) :_item_name(name)
{
//将配置传给Init函数进行队列初始化if (!Init(config)) {PX4_ERR("init failed");}
}

这个函数需要传入两个参数，第一个就是任务的名称，第二个则是该任务加入哪个队列，队列的工作节点，工作函数等队列相关配置。此外，需要注意的是上述Init函数不是AttitudeEstimatorQ类中的初始函数，而是工作队列初始化函数

WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
//MODULE_NAME在各模块的CmakeList文件，一般都是模块名
//att_pos_ctrl定义在PX4_Firmware/platforms/common/include/px4_platform_common/px4_work_queue/
//WorkQueueManager.hpp文件，static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 7200, -13};

还是在WorkIte.cpp文件，可以找到Init函数的实现，实现如下

bool
WorkItem::Init(const wq_config_t &config)
{// clear any existing firstDeinit();//WorkQueueFindOrCreate（）先在工作队列中找一下该任务是否在队列中，如果不在，将该任务加入队列px4::WorkQueue *wq = WorkQueueFindOrCreate(config);//创建成功，记录该任务队列在队列的有关信息，并返回trueif ((wq != nullptr) && wq->Attach(this)) {_wq = wq;_start_time = hrt_absolute_time();return true;}//创建失败，报错PX4_ERR("%s not available", config.name);return false;
}

当任务添加到队列后，该任务就可以在系统中并发运行，在nuttx调试窗口输入work_queue status命令，可以看到在队列中运行的任务
在任务添加到工作队列后，系统会自动调用module.h中run_trampoline函数

static int run_trampoline(int argc, char *argv[]){int ret = 0;#ifdef __PX4_NUTTX// On NuttX task_create() adds the task name as first argument.argc -= 1;argv += 1;
#endifT *object = T::instantiate(argc, argv);_object.store(object);//调用run函数，实现任务功能if (object) {object->run();} else {PX4_ERR("failed to instantiate object");ret = -1;}exit_and_cleanup();return ret;}

可以看到，PX4新版代码将一些功能做成了类模板，通过调用模板完成任务实现，本文只是讨论了一下代码调用顺序，下一节介绍attitude_estimator_q_main中互补滤波算法实现。

PX4代码解析（5）相关推荐

PX4代码解析(6)
一.前言上一节介绍了PX4姿态估计调用函数的流程,这一节分享一下我对PX4姿态解算的解读.首先,要理解PX4姿态解算的程序,要先从传感器的特性入手,这里主要介绍的传感器有加速度计,磁力计,陀螺仪. ...
px4代码解析(3)
前言在使用PX4飞控时,我们难免要对其进行二次开发,例如修改控制算法,添加新的传感器,这时需要在代码中定义属于自己的消息.本节主要分享一下如何在PX4代码中添加自己的消息一.消息添加与配置从代码 ...
PX4代码解析(1)
前言做pixhawk飞控有一段时间了,但在学习过程中遇到许多困难,目前网上找不到比较完整的PX4学习笔记,我打算结合自己理解,写写自己对PX4源码的理解,不一定对,只是希望与各位大佬交流交流,同时梳 ...
PX4代码解析(4)
一.引言 PX4程序是基于实时操作系统(Real-time operating system, RTOS)的上层应用程序,PX4飞控程序的很多重要模块都是在Nuttx操作系统的调度下运行的.因此,想要 ...
PX4代码解析(2)
前言在大致了解PX4代码架构后,我们需要了解PX4的通信机制.在PX4代码架构中,每通信总线主要分为两个部分,一是内部通信总线uORB,即PX4内部进程通信采用的协议,例如PX4内部姿态控制需要获取 ...
PX4代码解析：振动分析
本篇文章首发于公众号:无人机系统技术.更多无人机技术相关文章请关注此公众号一.前言前面的文章主要都是一些理论知识为主,很多读者朋友看了之后可能会有点枯燥,里面很多公式看起来也比较晦涩,今天起给大家 ...
px4代码jlink调试
之前看了许多介绍px4环境搭建的文章有介绍windows下eclipse和ubuntu下eclipse阅读px4源码的文章,但是没有看到对px4进行代码调试的文章,后来查看px官网发现可以通过visu ...
PX4代码学习系列博客(6)——offboard模式位置控制代码分析（之前转载过，这是第二次转载了）
我刚刚发现这篇文章去年八月份的时候转载过一次了 https://blog.csdn.net/sinat_16643223/article/details/107874349 转载自:https://b ...
matrix_multiply代码解析
matrix_multiply代码解析关于matrix_multiply 程序执行代码里两个矩阵的乘法,并将相乘结果打印在屏幕上. 示例的主要目的是展现怎么实现一个自定义CPU计算任务. 参考:ht ...

PX4代码解析（5）

一、前言

二、代码分析

PX4代码解析（5）相关推荐

最新文章

热门文章