ardupilot 加速度转换成欧拉角

目录

摘要

本节主要记录自己分析ardupilot中加速度如何转换成欧拉角的过程，这里主要说的是横滚角和俯仰角度的转换。

0序言

在进行主题讲解之前，我们先要知道ardupilot的导航坐标系是NED（北-东-地），载体坐标系是前右下（Forward- Right -Down）。
ardupilot中用到这部分的代码主要在下面：

程序位置：libraries\AC_AttitudeControl\AC_PosControl.cpp
程序用途：将NED坐标系下的期望加速度转换成机体坐标系下的欧拉角度(pitch,roll)
源程序：

```cpp
void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
{
   
    float accel_right, accel_forward;

    // rotate accelerations into body forward-right frame
    // todo: this should probably be based on the desired heading not the current heading
    //旋转加速度到机体前右坐标系
    //todo:这应该可能依据这个目标机头不是当前机头
    accel_forward = accel_x_cmss * _ahrs.cos_yaw() + accel_y_cmss * _ahrs.sin_yaw();
    accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw();

    //update angle targets that will be passed to stabilize controller
    //更新目标角度将传递到姿态控制器
    pitch_target = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
    float cos_pitch_target = cosf(pitch_target * M_PI / 18000.0f);
    roll_target = atanf(accel_right * cos_pitch_target / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
}

1.推导过程