PX4笔记杂记(一) thrustToAttitude函数

PX4 thrustToAttitude函数

该函数建立在ControlMath类下，作用是将目标油门矢量以及目标偏航角转化为所需的姿态，返回的是目标姿态结构体（包括三轴欧拉和油门大小方向）

需要注意的点有：
1、目标体轴系坐标轴的推导问题
  此处很难理解，为什么用在地面系上的y轴投影叉乘油门方向即可得到最终的体轴系x轴的坐标。
  首先需要明确一点，最终需要求得的三轴方向均定义在NED地面轴系，目标油门方向也定义在地面系下。
  因而观察我们已知信息：
  ①目标油门方向在地面系下的向量z_b坐标，且在四轴模式下该方向就是体轴系的z轴。
  ②需要偏转的偏航角
  要求得最终的体轴系坐标，就需要知道除z_b以外的另一坐标轴坐标，最后通过叉乘的方式得到第三轴方向
   由偏航角的定义可知，偏航角是地轴系和体轴系间夹角，是体轴系x轴或y轴在地轴系上的投影与地轴系的x轴和y轴之间的夹角
而用目标偏航角可以计算体轴系y轴在地面系上的投影y_c坐标:

  表面上y_c作为坐标旋转中间轴系，很难看出其与x_C的关系，但实际上并非如此。整个旋转过程依照ZYX顺序进行，首先由NED轴系开始，绕NEDz轴旋转yaw角，得到y_c和x_c（在NED系XY平面中），z轴不变，此时再绕y_c旋转theta角得到x_b和z_m(中间轴)，最终绕x_b旋转得到z_b,y_b。
  可以看出，x_b作为最终轴是绕y_c旋转所得的，因而其必然与y_c垂直，最后一步的旋转为绕x_b自身旋转因而不改变x_b方向，所以y_c与x_b垂直关系必然不改变。
   由向量叉乘性质可知，只需要知道两个垂直于该向量且不重合的向量即可通过两者的叉乘得到该向量。因而用y_c和z_b（油门方向）叉乘就得到了最终体轴系x轴在地面系中的坐标。再反用x_b与z_b叉乘即可得到真正的体轴系y轴y_b方位。
   后面旋转矩阵的计算不再赘述
2、为什么旋转矩阵就是由体轴系三轴在地面系的坐标组成：
    这是一个简单矩阵运算问题。

    这是从地面轴系转化到体轴系坐标矩阵即旋转矩阵设为S。
    对其取逆（此处和转置相同）得S’,此时为从体轴系转化到地面轴系矩阵，在其右侧乘以体轴系坐标[1，0，0]（即体轴系下x轴方向），计算得到的就是体轴系x轴在地面轴系的坐标。而这个值就是前面算的向量body_x，根据矩阵运算可得，这个向量正好等于S‘的第一行，也就是S的第一列，同理可得body_y就是S的第二列，body_z就是S的第三列。
3、att_sp的值
    att_sp中yaw_body是直接通过外部赋予的，油门大小和额方向也是由外部赋予，只有roll_body和pitch_body是通过此处计算得到