姿态控制代码，最开始的地方。
位置：X:\ardupilot\ArduCopter\mode_stabilize.cpp
// stabilize_run - runs the main stabilize controller（运行主稳定控制器）
// should be called at 100hz or more
void Copter::ModeStabilize::run()
{
float target_roll, target_pitch;
float target_yaw_rate;
float pilot_throttle_scaled;
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {
zero_throttle_and_relax_ac();//ac指姿态控制
return;
}
// clear landing flag
set_land_complete(false);
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// apply SIMPLE mode transform to pilot inputs（将简单模式转换应用于试验输入）
update_simple_mode();//根据简单模式，重新设置control_in
AP_Vehicle::MultiCopter &aparm = copter.aparm;
// convert pilot input to lean angles（将飞行员输入转换为倾斜角度）
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());//传入control_in，计算期望的偏航，并返回
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());//油门处理
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// body-frame rate controller is run directly from 100hz loop
// output pilot's throttle
attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
}===========================================================================================
俯仰和横滚的期望值（根据遥控器的输入和最大倾斜角度参数计算得到）
===========================================================================================
位置：X:\ardupilot\ArduCopter\mode_stabilize.cpp
// convert pilot input to lean angles（将飞行员输入转换为倾斜角度，也就是遥控器的输入）
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);其中的：
channel_roll->get_control_in()、channel_pitch->get_control_in()是获取遥控器的输入；
target_roll, target_pitch是限幅处理后，位于正负45度之间的值；
aparm.angle_max是一个角度限制的参数。
位置：X:\ardupilot\ArduCopter\mode.cpp
// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
void Copter::Mode::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
copter.get_pilot_desired_lean_angles(roll_in, pitch_in, roll_out, pitch_out, angle_max);
}这里只是做个一层简单的封装。
X:\ardupilot\ArduCopter\Attitude.cpp
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)
{
// sanity check angle max parameter（完备性检查角最大参数）
aparm.angle_max = constrain_int16(aparm.angle_max,1000,8000);//限制最大角度参数在10到80度之间
// limit max lean angle（限制最大倾斜角度）
angle_max = constrain_float(angle_max, 1000, aparm.angle_max);//最大角度参数有效的情况下，判断参数的角度，而这里的angle_max = aparm.angle_max
// scale roll_in, pitch_in to ANGLE_MAX parameter range
float scaler = aparm.angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;//4500
roll_in *= scaler;//根据最大角度参数的设置，放大和缩小遥控器横滚和俯仰的输入
pitch_in *= scaler;
// do circular limit（做圆形的限制）
float total_in = norm(pitch_in, roll_in);
if (total_in > angle_max) {
float ratio = angle_max / total_in;//处理之后的角度，最终不能大于传参angle_max
roll_in *= ratio;
pitch_in *= ratio;
}
// do lateral tilt to euler roll conversion（横向倾斜到欧拉滚动转换）
roll_in = (18000/M_PI) * atanf(cosf(pitch_in*(M_PI/18000))*tanf(roll_in*(M_PI/18000)));
// return
roll_out = roll_in;//对横滚做了一些处理
pitch_out = pitch_in;//俯仰值不变
}因此，调用函数所得到的target_roll、target_pitch，就是将遥控器的输入做了处理之后，得到的横滚、俯仰的期望值
===========================================================================================
偏航的期望值
===========================================================================================
位置：X:\ardupilot\ArduCopter\mode_stabilize.cpp
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());位置：X:\ardupilot\ArduCopter\mode.cpp
float Copter::Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
return copter.get_pilot_desired_yaw_rate(stick_angle);
}这里只进行了简单的调用。
位置：X:\ardupilot\ArduCopter\Attitude.cpp
// get_pilot_desired_heading - transform pilot's yaw input into a desired yaw rate（将飞行员的偏航输入转换为期望的偏航率）
// returns desired yaw rate in centi-degrees per second（返回所需的偏航率，以每秒钟的速度）
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
float yaw_request;
// calculate yaw rate request
if (g2.acro_y_expo <= 0) {
yaw_request = stick_angle * g.acro_yaw_p;
} else {
// expo（exponent指数） variables
float y_in, y_in3, y_out;
// range check expo
if (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f) {
g2.acro_y_expo = 1.0f;
}
// yaw expo
y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;//4500
y_in3 = y_in*y_in*y_in;
y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in);//应该是泰勒级数
yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * g.acro_yaw_p;
}
// convert pilot input to the desired yaw rate
return yaw_request;
}这里得到的是偏航的期望值。
===========================================================================================
油门的期望值
===========================================================================================
位置：X:\ardupilot\ArduCopter\mode_stabilize.cpp
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());//油门X:\ardupilot\ArduCopter\mode.cpp
float Copter::Mode::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
{
return copter.get_pilot_desired_throttle(throttle_control, thr_mid);
}
X:\ardupilot\ArduCopter\Attitude.cpp
// transform pilot's manual throttle input to make hover throttle mid stick
// used only for manual throttle modes
// thr_mid should be in the range 0 to 1
// returns throttle output 0 to 1
float Copter::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
{
if (thr_mid <= 0.0f) {
thr_mid = motors->get_throttle_hover();//获取悬停油门
}
int16_t mid_stick = get_throttle_mid();//获取油门中值
// protect against unlikely divide by zero
if (mid_stick <= 0) {
mid_stick = 500;
}
// ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000);
// calculate normalised throttle input
float throttle_in;
if (throttle_control < mid_stick) {
// below the deadband（死区之内）
throttle_in = ((float)throttle_control)*0.5f/(float)mid_stick;
}else if(throttle_control > mid_stick) {
// above the deadband（死区之外）
throttle_in = 0.5f + ((float)(throttle_control-mid_stick)) * 0.5f / (float)(1000-mid_stick);
}else{
// must be in the deadband(死区临界值)
throttle_in = 0.5f;
}
float expo = constrain_float(-(thr_mid-0.5)/0.375, -0.5f, 1.0f);
// calculate the output throttle using the given expo function（使用指数函数计算输出节油门）
float throttle_out = throttle_in*(1.0f-expo) + expo*throttle_in*throttle_in*throttle_in;
return throttle_out;
}到这里，横滚、俯仰、偏航、油门的期望值都已计算出来了。
===========================================================================================
姿态
===========================================================================================
位置：X:\ardupilot\ArduCopter\mode_stabilize.cpp
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.cpp
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);//这里是期望的偏航角速率
// calculate the attitude target euler angles（计算姿态目标欧拉角）
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// ensure smoothing gain can not cause overshoot
smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad();//返回0
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// translate the roll pitch and yaw acceleration limits to the euler axis（将滚动距和偏航加速度限制转换为欧拉轴）
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt);
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt);
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
} else {//在没有前馈的情况下
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
// 当前馈没有启用时，目标欧拉角是输入目标，前馈率为零。
_attitude_target_euler_angle.x = euler_roll_angle;//期望的角度值就是目标角度值
_attitude_target_euler_angle.y = euler_pitch_angle;
_attitude_target_euler_angle.z += euler_yaw_rate*_dt;
// Compute quaternion target attitude
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// Set rate feedforward requests to zero
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);//将速度前馈请求设为零
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// Call quaternion attitude controller
attitude_controller_run_quat();//调用四元数姿态控制器
}// translate the roll pitch and yaw acceleration limits to the euler axis
//将滚动距和偏航加速度限制转换为欧拉轴
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.cpp
/ translates body frame acceleration limits to the euler axis（将机体框架加速度限制转换为欧拉轴）
Vector3f AC_AttitudeControl::euler_accel_limit(Vector3f euler_rad, Vector3f euler_accel)
{
float sin_phi = constrain_float(fabsf(sinf(euler_rad.x)), 0.1f, 1.0f);
float cos_phi = constrain_float(fabsf(cosf(euler_rad.x)), 0.1f, 1.0f);
float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f);
Vector3f rot_accel;
if(is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || (euler_accel.x < 0.0f) || (euler_accel.y < 0.0f) || (euler_accel.z < 0.0f)) {
rot_accel.x = euler_accel.x;
rot_accel.y = euler_accel.y;
rot_accel.z = euler_accel.z;
} else {
rot_accel.x = euler_accel.x;//先对齐X轴
rot_accel.y = MIN(euler_accel.y/cos_phi, euler_accel.z/sin_phi);//再对齐Y轴
rot_accel.z = MIN(MIN(euler_accel.x/sin_theta, euler_accel.y/sin_phi), euler_accel.z/cos_phi);//最后对齐Z轴
}
return rot_accel;
}上面是对加速度限制的一种转换。
===========================================================================================
姿态控制---偏航
===========================================================================================
这行代码，用到了，期望的偏航角速率，继续追踪
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z);// limits the acceleration and deceleration of a velocity request（限制速度请求的加速和减速）
float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max)//第二个参数是期望的偏航角速率
{
if (accel_max > 0.0f) {
float delta_ang_vel = accel_max * _dt;//根据最大加速度限制，计算对应最大加、减速度值
//这里计算了偏航角速率的偏差值，如果在加速度的范围类，设置为新的速度的期望值
target_ang_vel += constrain_float(desired_ang_vel - target_ang_vel, -delta_ang_vel, delta_ang_vel);
} else {
target_ang_vel = desired_ang_vel;//重新设置期望的角速度
}
return target_ang_vel;
}===========================================================================================
姿态控制---俯仰、横滚
===========================================================================================
稍后添加------------------------------------------------------------------------------------------------------
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
//将理想姿态的欧拉角导数转换为前馈线角速度矢量
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);这里传入三个参数：目前姿态的欧拉角、目标姿态的欧拉角速度、目标欧拉角速率
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads)
{
float sin_theta = sinf(euler_rad.y);
float cos_theta = cosf(euler_rad.y);
float sin_phi = sinf(euler_rad.x);
float cos_phi = cosf(euler_rad.x);
ang_vel_rads.x = euler_rate_rads.x - sin_theta * euler_rate_rads.z;
ang_vel_rads.y = cos_phi * euler_rate_rads.y + sin_phi * cos_theta * euler_rate_rads.z;
ang_vel_rads.z = -sin_phi * euler_rate_rads.y + cos_theta * cos_phi * euler_rate_rads.z;
}最后，调用四元数姿态控制器
// Call quaternion attitude controller
attitude_controller_run_quat();// Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl::attitude_controller_run_quat()
{
// Retrieve quaternion vehicle attitude
// TODO add _ahrs.get_quaternion()
Quaternion attitude_vehicle_quat;
attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());//获取NED坐标系的四元数
// Compute attitude error（计算姿态偏差）
Vector3f attitude_error_vector;
thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);
// Compute the angular velocity target from the attitude error
_rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);
// Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
_rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro().z;
_rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro().z;
// Add the angular velocity feedforward, rotated into vehicle frame
Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
Quaternion attitude_error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
Quaternion target_ang_vel_quat = attitude_error_quat.inverse()*attitude_target_ang_vel_quat*attitude_error_quat;
// Correct the thrust vector and smoothly add feedforward and yaw input
if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){
_rate_target_ang_vel.z = _ahrs.get_gyro().z;
}else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){
float flip_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE);
_rate_target_ang_vel.x += target_ang_vel_quat.q2*flip_scalar;
_rate_target_ang_vel.y += target_ang_vel_quat.q3*flip_scalar;
_rate_target_ang_vel.z += target_ang_vel_quat.q4;
_rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-flip_scalar) + _rate_target_ang_vel.z*flip_scalar;
} else {
_rate_target_ang_vel.x += target_ang_vel_quat.q2;
_rate_target_ang_vel.y += target_ang_vel_quat.q3;
_rate_target_ang_vel.z += target_ang_vel_quat.q4;
}
if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
// rotate target and normalize
Quaternion attitude_target_update_quat;
attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));
_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
_attitude_target_quat.normalize();
}
}// Compute attitude error
Vector3f attitude_error_vector;
thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);传入四个参数：
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot)
{
Matrix3f att_to_rot_matrix; // rotation from the target body frame to the inertial frame.（从目标身体框架旋转到惯性系）
att_to_quat.rotation_matrix(att_to_rot_matrix);//当前姿态四元数旋转到惯性系，得到一个旋转矩阵
Vector3f att_to_thrust_vec = att_to_rot_matrix*Vector3f(0.0f,0.0f,1.0f);//
Matrix3f att_from_rot_matrix; // rotation from the current body frame to the inertial frame.
att_from_quat.rotation_matrix(att_from_rot_matrix);
Vector3f att_from_thrust_vec = att_from_rot_matrix*Vector3f(0.0f,0.0f,1.0f);//
// the cross product of the desired and target thrust vector defines the rotation vector
//所期望的和目标推力矢量的叉积定义了旋转矢量
Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;
// the dot product is used to calculate the angle between the target and desired thrust vectors
//点积是用来计算目标和想要的推力矢量之间的夹角的
thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec,-1.0f,1.0f));//
// Normalize the thrust rotation vector（使推力旋转矢量正常化）
float thrust_vector_length = thrust_vec_cross.length();//个人理解为推力偏差大小
if(is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)){
thrust_vec_cross = Vector3f(0,0,1);
thrust_vec_dot = 0.0f;
}else{
thrust_vec_cross /= thrust_vector_length;//规范化推力的偏差大小
}
Quaternion thrust_vec_correction_quat;
thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);//推力矢量四元数
thrust_vec_correction_quat = att_from_quat.inverse()*thrust_vec_correction_quat*att_from_quat;
// calculate the remaining rotation required after thrust vector is rotated（计算推力矢量旋转后所需的剩余旋转）
Quaternion heading_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()*att_to_quat;//
Vector3f rotation;
thrust_vec_correction_quat.to_axis_angle(rotation);
att_diff_angle.x = rotation.x;
att_diff_angle.y = rotation.y;
heading_quat.to_axis_angle(rotation);//
att_diff_angle.z = rotation.z;//
if(!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()){
att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP());
heading_quat.from_axis_angle(Vector3f(0.0f,0.0f,att_diff_angle.z));
att_to_quat = att_from_quat*thrust_vec_correction_quat*heading_quat;
}
}