Arducopter Yaw
现梳理一遍Poshold模式下的yaw的情况:
- 首先从 Copter::fast_loop() –> update_flight_mode()–> Copter::ModePosHold::run()
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
{
float yaw_request;
if (g2.acro_y_expo <= 0) {
yaw_request = stick_angle * g.acro_yaw_p;
} else {
float y_in, y_in3, y_out;
if (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f) {
g2.acro_y_expo = 1.0f;
}
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;
}
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);
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);
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 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);
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
euler_roll_angle += get_roll_trim_rad();
if (_rate_bf_ff_enabled) {
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()));
_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);
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z, _dt);
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
} else {
_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;
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
attitude_controller_run_quat();
}
float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt)
{
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;
}
}
// update attitude controller targets
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)