
	/**Firmware/src/lib/mathlib/math/Quaternion.hpp
	 * create rotation matrix for the quaternion
	 */由四元数得到方向余弦旋转矩阵
	Matrix<3, 3> to_dcm(void) const {
		Matrix<3, 3> R;
		float aSq = data[0] * data[0];
		float bSq = data[1] * data[1];
		float cSq = data[2] * data[2];
		float dSq = data[3] * data[3];
		R.data[0][0] = aSq + bSq - cSq - dSq;
		R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
		R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
		R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
		R.data[1][1] = aSq - bSq + cSq - dSq;
		R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
		R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
		R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
		R.data[2][2] = aSq - bSq - cSq + dSq;
		return R;
	}

	/**Firmware/src/lib/mathlib/math/Quaternion.hpp
	 * set quaternion to rotation by DCM
	 * Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
	 */由方向余弦旋转矩阵得到四元数
	void from_dcm(const Matrix<3, 3> &dcm) {
		float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2];
		if (tr > 0.0f) {
			float s = sqrtf(tr + 1.0f);
			data[0] = s * 0.5f;
			s = 0.5f / s;
			data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s;
			data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s;
			data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s;
		} else {
			/* Find maximum diagonal element in dcm
			* store index in dcm_i */
			int dcm_i = 0;
			for (int i = 1; i < 3; i++) {
				if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) {
					dcm_i = i;
				}
			}
			int dcm_j = (dcm_i + 1) % 3;
			int dcm_k = (dcm_i + 2) % 3;
			float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] -
			dcm.data[dcm_k][dcm_k]) + 1.0f);
			data[dcm_i + 1] = s * 0.5f;
			s = 0.5f / s;
			data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s;
			data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s;
			data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s;
		}
	}

	/**Firmware/src/lib/mathlib/math/Matrix.hpp
	 * get euler angles from rotation matrix
	 */由旋转矩阵得到欧拉角
	Vector<3> to_euler(void) const {
		Vector<3> euler;
		euler.data[1] = asinf(-data[2][0]);

		if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
			euler.data[0] = 0.0f;
			euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];

		} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
			euler.data[0] = 0.0f;
			euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];

		} else {
			euler.data[0] = atan2f(data[2][1], data[2][2]);
			euler.data[2] = atan2f(data[1][0], data[0][0]);
		}

		return euler;
	}
	
	/**Firmware/src/lib/mathlib/math/Matrix.hpp
	 * set zero matrix  零矩阵
	 */
	void zero(void) {
		memset(data, 0, sizeof(data));
	}
	
	/**Firmware/src/lib/mathlib/math/Matrix.hpp
	 * set identity matrix单位矩阵
	 */
	void identity(void) {
		memset(data, 0, sizeof(data));
		unsigned int n = (M < N) ? M : N;

		for (unsigned int i = 0; i < n; i++)
			data[i][i] = 1;
	}

typedef struct pollfd px4_pollfd_struct_t;
	
/* In the standard poll() definition, the size of the event set is 'short'.
 * Here we pick the smallest storage element that will contain all of the
 * poll events.
 */
/* 在标准轮询（）的定义，设置事件的大小是“short”。 在这里，最小存储元件将包含所有的轮训事件*/
typedef uint8_t pollevent_t;

/* This is the Nuttx variant of the standard pollfd structure. */
/*这是标准的pollfd结构的Nuttx变量*/
struct pollfd
{
  int         fd;       /* The descriptor being polled */
  sem_t      *sem;      /* Pointer to semaphore used to post output event */
  pollevent_t events;   /* The input event flags */
  pollevent_t revents;  /* The output event flags */
  FAR void   *priv;     /* For use by drivers */
};

	/**Firmware/src/lib/mathlib/math/Vector.hpp
	 * element by element multiplication
	 */
    Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
    {
        Matrix<Type, M, N> res;
        const Matrix<Type, M, N> &self = *this;

        for (size_t i = 0; i < M; i++) {
            for (size_t j = 0; j < N; j++) {
                res(i , j) = self(i, j)*other(i, j);
            }
        }

        return res;
    }
	
	/**Firmware/src/lib/mathlib/math/Vector.hpp
	 * element by element division
	 */
	const Vector<N> edivide(const Vector<N> &v) const {
		Vector<N> res;

		for (unsigned int i = 0; i < N; i++)
			res.data[i] = data[i] / v.data[i];

		return res;
	}
		
	/**Firmware/src/lib/mathlib/math/Matrix.hpp
	 * create a rotation matrix from given euler angles
	 * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
	 */由欧拉角得到旋转矩阵
	void from_euler(float roll, float pitch, float yaw) {
		float cp = cosf(pitch);
		float sp = sinf(pitch);
		float sr = sinf(roll);
		float cr = cosf(roll);
		float sy = sinf(yaw);
		float cy = cosf(yaw);

		data[0][0] = cp * cy;
		data[0][1] = (sr * sp * cy) - (cr * sy);
		data[0][2] = (cr * sp * cy) + (sr * sy);
		data[1][0] = cp * sy;
		data[1][1] = (sr * sp * sy) + (cr * cy);
		data[1][2] = (cr * sp * sy) - (sr * cy);
		data[2][0] = -sp;
		data[2][1] = sr * cp;
		data[2][2] = cr * cp;
	}
