第一个: Sub::get_pilot_desired_lean_angles函数里
// 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)));
这里注释为将横向倾斜转换为欧拉滚转,可不可以理解为对横滚进行了限制,还是在欧拉转换公式里有一个数学关系?
第二个:attitude_controller_run_quat()函数里
_rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);//roll、pitch、yaw
// Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
// todo: this should probably be a matrix that couples yaw as well.
_rate_target_ang_vel.x += constrain_float(attitude_error_vector.y, -M_PI / 4, M_PI / 4) * _ahrs.get_gyro().z;//angle_delta_v_x+angle_delta_y*wbz
_rate_target_ang_vel.y += -constrain_float(attitude_error_vector.x, -M_PI / 4, M_PI / 4) * _ahrs.get_gyro().z;
ang_vel_limit(_rate_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
新版的源码里是没这一部分代码,这里的数学意义又是什么?注释为添加前馈项,试图确保滚转和俯仰误差与机身框架而非参考框架一起旋转,这可能是一个耦合yaw的矩阵。
x轴角速率误差'= x轴角速率误差 + y轴的姿态误差 * 陀螺仪获取的z轴角速度
y轴的速率误差'= y轴的速率误差 - x轴的姿态误差 * 陀螺仪获取的z轴角速度
这是怎么和Z轴角速度扯上关系的?难道这里应用了欧拉微分方程或者小角度转换关系?
记录一下,有时间再从数学上找到依据,欢迎大家讨论。