#include "systick.h"
#include "sensor.h"
#include "quaternion.h"
#include "filter.h"

AHRS 	 uav_ahrs;
SINS uav_ins;

void Get_Status_Feedback_lite(void)
{
  float _rpy[3];
  //从姿态四元数中提取三个方向姿态角度
  quaternion_to_euler(uav_ahrs.quaternion, _rpy);
  uav_ahrs.roll  = _rpy[0];
  uav_ahrs.pitch = _rpy[1];
  uav_ahrs.yaw   = _rpy[2];
  //计算出姿态角的正、余弦值备用
  uav_ahrs.sin_rpy[_PIT] = FastSin(uav_ahrs.pitch*DEG2RAD);
  uav_ahrs.cos_rpy[_PIT] = FastCos(uav_ahrs.pitch*DEG2RAD);
  uav_ahrs.sin_rpy[_ROL] = FastSin(uav_ahrs.roll*DEG2RAD);
  uav_ahrs.cos_rpy[_ROL] = FastCos(uav_ahrs.roll*DEG2RAD);
  uav_ahrs.sin_rpy[_YAW] = FastSin(uav_ahrs.yaw *DEG2RAD);
  uav_ahrs.cos_rpy[_YAW] = FastCos(uav_ahrs.yaw *DEG2RAD);
  //将等效EN方向的位置速度转化成相对机头的前后、左右方向来
  from_vio_to_body_frame(uav_ins.position[_EAST],
                         uav_ins.position[_NORTH],
                         &uav_ins._position[_EAST],
                         &uav_ins._position[_NORTH],
                         uav_ahrs.yaw);

  from_vio_to_body_frame(uav_ins.speed[_EAST],
                         uav_ins.speed[_NORTH],
                         &uav_ins._speed[_EAST],
                         &uav_ins._speed[_NORTH],
                         uav_ahrs.yaw);
}