本节主要记录ardupilot 计算遥控器水平滚动，将俯仰控制量转换为目标预期加速度和预测加速度，然后将其转换为预期加速度，并结合天空飞行控制代码获取目标速度。写这篇文章的目的，ardupilot的loiter刹车不是很好，希望能找到更好的loiter制动控制效果，欢迎批评指正，一起讨论！在查看代码之前，请注意两个坐标系、身体坐标系和地理坐标系，以及哪个坐标系控制量。

     ///定义目标水平滚动角度         float target_roll, target_pitch;         //---- apply SIMPLE mode transform to pilot inputs         update_simple_mode();          ///将飞行员转换到倾斜角度--- convert pilot input to lean angles         get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(),           attitude_control->get_althold_lean_angle_max()); 

关键函数是get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(),
attitude_control->get_althold_lean_angle_max());

• 该函数的形参：target_roll, target_pitch, loiter_nav->get_angle_max_cd(),
  attitude_control->get_althold_lean_angle_max()

void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const { 
             //油门故障检查------ throttle failsafe check     if (copter.failsafe.radio || !copter.ap.rc_receiver_present)     { 
                 roll_out = 0;         pitch_out = 0;         return;     }     ///获得横滚和俯仰输入----- fetch roll and pitch inputs     roll_out = channel_roll->get_control_in() pitch_out = channel_pitch->get_control_in(); //限制最大倾斜角度----- limit max lean angle angle_limit = constrain_float(angle_limit, 1000.0f, angle_max); // scale roll and pitch inputs to ANGLE_MAX parameter range //将横滚和俯仰输入缩放到角度_最大参数范围 float scaler = angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX; //最大不超过45度 roll_out *= scaler; pitch_out *= scaler; //做循环限制------ do circular limit float total_in = norm(pitch_out, roll_out); if (total_in > angle_limit) { 
          float ratio = angle_limit / total_in; //2/2gen2 roll_out *= ratio; pitch_out *= ratio; } // do lateral tilt to euler roll conversion //进行横向倾斜到欧拉俯仰角转换,范围不超过【-17 +17】度 //这里进行的处理应该是：横滚角度=artan(rzy,rzz)=arctan(sin(r)cos(p)/cos(r)/cos(p)) //roll=arctan(sin(r)cos(p)/cos(r)/cos(p))=arctan(cosp*tanr/1)也就是把cosp设置最大值，roll有最小值 roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000))); // roll_out and pitch_out are returned } 

到这里得到了遥杆横滚+俯仰输入量到目标横滚角+俯仰角。

void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd, float dt)
{ 
        
    // Convert from centidegrees on public interface to radians
	//从公共界面上的cm度转换为弧度
    const float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);    //转换成了弧度
    const float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);  //转换成了弧度

    //convert our desired attitude to an acceleration vector assuming we are hovering
    //假设我们在悬停,将我们想要的姿态转换成加速度矢量
    const float pilot_cos_pitch_target = constrain_float(cosf(euler_pitch_angle), 0.5f, 1.0f);  //限制在0-60度
    //右侧的加速度,这个直接根据https://www.sohu.com/a/243549866_466960实现
    const float pilot_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(euler_roll_angle)/pilot_cos_pitch_target;
    //前进的加速度
    const float pilot_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(euler_pitch_angle);

    //旋转加速度矢量到地理坐标系lat/lon坐标系------ rotate acceleration vectors input to lat/lon frame
    _desired_accel.x = (pilot_accel_fwd_cms*_ahrs.cos_yaw() - pilot_accel_rgt_cms*_ahrs.sin_yaw());
    _desired_accel.y = (pilot_accel_fwd_cms*_ahrs.sin_yaw() + pilot_accel_rgt_cms*_ahrs.cos_yaw());

    // difference between where we think we should be and where we want to be
    //我们认为我们应该去的地方和我们想要去的地方之间的差异
    Vector2f angle_error(wrap_PI(euler_roll_angle - _predicted_euler_angle.x), wrap_PI(euler_pitch_angle - _predicted_euler_angle.y));
    // calculate the angular velocity that we would expect given our desired and predicted attitude
    //根据我们期望和预测的姿态，计算出我们期望的角速度
    _attitude_control.input_shaping_rate_predictor(angle_error, _predicted_euler_rate, dt);
    // update our predicted attitude based on our predicted angular velocity
    //根据我们的预测角速度更新我们的预测姿态
    _predicted_euler_angle += _predicted_euler_rate * dt;

    // convert our predicted attitude to an acceleration vector assuming we are hovering
    //假设我们在悬停，将预测的姿态转换为加速度矢量
    const float pilot_predicted_cos_pitch_target = cosf(_predicted_euler_angle.y);
    const float pilot_predicted_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(_predicted_euler_angle.x)/pilot_predicted_cos_pitch_target;
    const float pilot_predicted_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(_predicted_euler_angle.y);

    // rotate acceleration vectors input to lat/lon frame
    //旋转输入到lat/lon框架的加速度矢量
    _predicted_accel.x = (pilot_predicted_accel_fwd_cms*_ahrs.cos_yaw() - pilot_predicted_accel_rgt_cms*_ahrs.sin_yaw());
    _predicted_accel.y = (pilot_predicted_accel_fwd_cms*_ahrs.sin_yaw() + pilot_predicted_accel_rgt_cms*_ahrs.cos_yaw());

}

这个函数有几个关键点需要注意：
1.目标横滚角，俯仰角说的是机体坐标系下的，需要把这个目标角度转换成机体坐标系下的加速度量。重点是下面公式

    //convert our desired attitude to an acceleration vector assuming we are hovering
    //假设我们在悬停,将我们想要的姿态转换成加速度矢量
    const float pilot_cos_pitch_target = constrain_float(cosf(euler_pitch_angle), 0.5f, 1.0f);  //限制在0-60度
    //右侧的加速度,这个直接根据https://www.sohu.com/a/243549866_466960实现
    const float pilot_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(euler_roll_angle)/pilot_cos_pitch_target;
    //前进的加速度
    const float pilot_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(euler_pitch_angle);

这个是如何实现的？
上面代码核心实现：把飞行员操作输入的目标横滚角及目标俯仰角度转换成飞行器NED坐标系下的的目标加速度值，这里假设轴受力平衡，即在轴上保持静止，目标高度不变。则其受力如下图所示。这里把无人机看成质点，若想让无人机在NED坐标系轴保持静止，需要的垂直拉力刚好抵消重力，外力的分力产生维持轴上产生向前的加速度及轴上产生向右的加速度。






对于上面的正负号，不一致，原因在于ardupilot规定 横滚左负，右正；俯仰，上正，下负。

2.得到机体坐标系下的加速度后，我们还需要得到无人机地理坐标系下的速度信息。

3.加速度预测加速度

这里需要注意的是
_predicted_euler_angle.x表示横滚角，_predicted_euler_angle.y表示俯仰角，这里的转换公式和上面一样，这里不在进行讲述。

总结：
这里我们需要问这里是做什么？我们得到了两个加速度信息，一个是期望加速度，一个是预测加速度，这两个对后续的期望速度都有影响，同时根据期望加速度进行刹车相关处理，预测加速度对期望速度进行更新。

calc_desired_velocity(dt)

desired_vel_to_pos(dt);

void AC_PosControl::desired_vel_to_pos(float nav_dt)
{ 
        
    //nav_dt范围检查---- range check nav_dt
    if (nav_dt < 0)
    { 
        
        return;
    }

    // 更新目标位置-----update target position
    if (_flags.reset_desired_vel_to_pos)
    { 
        
        _flags.reset_desired_vel_to_pos = false;
    } else
    { 
        
        _pos_target.x += _vel_desired.x * nav_dt; //当前位置=上次位置+速度×时间
        _pos_target.y += _vel_desired.y * nav_dt;
    }
}

    //增加速度前馈------ add velocity feed-forward
    _vel_target.x += _vel_desired.x;
    _vel_target.y += _vel_desired.y;

这个代码在void AC_PosControl::run_xy_controller(float dt)中