PX4 固定翼姿态控制代码Run函数

Run函数

1. 何时运行

	// only run controller if attitude changed
	vehicle_attitude_s att;

	if (_att_sub.update(&att)) {
   
   

		// only update parameters if they changed
		bool params_updated = _parameter_update_sub.updated();

运行的逻辑是当姿态att消息改变之后 就运行
姿态时刻在改变，姿态数据的采集、发布到uorb上
fw_att_control是一个单独的线程，监控着姿态数据并运行
具体运行的频率这还真不知道
但这个频率应该是相对稳定的 我只需要一个相对稳定的控制频率来做一件事
就是记录0.5s左右的一段时间的俯仰角的值
我会把这段内容保存为一个fifo的队列
不断刷新

2. 数据来源

		const float dt = math::constrain((att.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f);
		_last_run = att.timestamp;

		/* get current rotation matrix and euler angles from control state quaternions */
		matrix::Dcmf R = matrix::Quatf(att.q);

		vehicle_angular_velocity_s angular_velocity{
   
   };
		_vehicle_rates_sub.copy(&angular_velocity);
		float rollspeed = angular_velocity.xyz[0];
		float pitchspeed = angular_velocity.xyz[