Arducopter Yaw
现梳理一遍Poshold模式下的yaw的情况:
- 首先从 Copter::fast_loop() –> update_flight_mode()–> Copter::ModePosHold::run()
// 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
// acro_y_expo 这个值为0-0.5 用来设置当打杆向一边时,设置旋转速度的快慢
// acro_yaw_p 这个值为把遥控yaw输入转化为期望旋转速率,越大意味着旋转速率会更大
if (g2.acro_y_expo <= 0) {
yaw_request = stick_angle * g.acro_yaw_p;
} else {
// expo 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;
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
// 转化pilot 的输入为期望的yaw速率
return yaw_request;
}
- 当无人机在地面ap.land_complete时attitude_control->set_yaw_target_to_current_heading();
—>shift_ef_yaw_target(degrees(_ahrs.yaw - _attitude_target_euler_angle.z)100.0f);
//改变地球坐标系下的目标yaw角度 ,最终算入四元数中
//shift_ef_yaw_target(degrees(_ahrs.yaw - _attitude_target_euler_angle.z)*100.0f)
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
{
float yaw_shift = radians(yaw_shift_cd*0.01f);
Quaternion _attitude_target_update_quat;
_attitude_target_update_quat.from_axis_angle(Vector3f(0.0f, 0.0f, yaw_shift));
_attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat;
}
- 接下来就是姿态更新器attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0);
// 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)
{
// Convert from centidegrees on public interface to radians
// 转换为弧度且缩小100
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);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
// 在四轴上 get_roll_trim_rad() 为0
euler_roll_angle += get_roll_trim_rad();
// 机体坐标系下的前馈 _rate_bf_ff_enabled 开启或者关闭
if (_rate_bf_ff_enabled) {
// translate the roll pitch and yaw acceleration limits to the euler axis
// 将横滚,俯仰和偏航加速度极限转换为欧拉轴 get_accel_yaw_max_radss()为yaw角的最大加速度设置为 27000 cdeg/s/s
// 总的来说,就是为了限制角度变化的加速度
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.
//当启用加速限制和前馈时,使用Sqt控制器计算欧拉。
//角速度将使欧拉角在有限减速下的输入角平稳停止。
//最后由平滑增益指定的指数衰减。
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), _input_tc, 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), _input_tc, 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, _dt);
// 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);
// Limit the angular velocity
ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
// Convert body-frame angular velocity into euler angle derivative of desired attitude
// 转化机体坐标系角度速度到期望姿态欧垃角倒数
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
} 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();
}
// 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, float dt)
{
// Acceleration is limited directly to smooth the beginning of the curve.
if (is_positive(accel_max)) {
float delta_ang_vel = accel_max * dt;
return constrain_float(desired_ang_vel, target_ang_vel-delta_ang_vel, target_ang_vel+delta_ang_vel);
} else {
return desired_ang_vel;
}
}
- 当无人机起飞了,takeoff
// update attitude controller targets
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);