PX4代码解析（5）

一、前言

我所讨论的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)};
#endif

	float		_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.h
			return PX4_OK;
		 }

	} 
	else 
	{
		PX4_ERR("alloc failed");
	}

	delete instance;
	_object.store(nullptr);
	_task_id = -1;
	 //PX4_ERROR == -1,定义在defines.h
	return 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 first
	Deinit();
  //WorkQueueFindOrCreate（）先在工作队列中找一下该任务是否在队列中，如果不在，将该任务加入队列
	px4::WorkQueue *wq = WorkQueueFindOrCreate(config);
  //创建成功，记录该任务队列在队列的有关信息，并返回true
	if ((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;
#endif

		T *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中互补滤波算法实现。