void Ekf::calculateOutputStates()
{
imuSample imu_new = _imu_sample_new;
Vector3f delta_angle;
// Note: We do no not need to consider any bias or scale correction here
// since the base class has already corrected the imu sample
delta_angle(0) = imu_new.delta_ang(0);
delta_angle(1) = imu_new.delta_ang(1);
delta_angle(2) = imu_new.delta_ang(2);
Vector3f delta_vel = imu_new.delta_vel;
delta_angle += _delta_angle_corr;
Quaternion dq;
dq.from_axis_angle(delta_angle);
_output_new.time_us = imu_new.time_us;
_output_new.quat_nominal = dq * _output_new.quat_nominal;
_output_new.quat_nominal.normalize();
matrix::Dcm<float> R_to_earth(_output_new.quat_nominal);
Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr;
delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt;
Vector3f vel_last = _output_new.vel;
_output_new.vel += delta_vel_NED;
_output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt;
if (_imu_updated) {
_output_buffer.push(_output_new);
_imu_updated = false;
}
_output_sample_delayed = _output_buffer.get_oldest();
Quaternion quat_inv = _state.quat_nominal.inversed();
Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv;
q_error.normalize();
Vector3f delta_ang_error;
float scalar;
if (q_error(0) >= 0.0f) {
scalar = -2.0f;
} else {
scalar = 2.0f;
}
delta_ang_error(0) = scalar * q_error(1);
delta_ang_error(1) = scalar * q_error(2);
delta_ang_error(2) = scalar * q_error(3);
_delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt;
_delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt;
_vel_corr = (_state.pos - _output_sample_delayed.pos);
}
/**
* Attitude controller.
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
* Output: '_rates_sp' vector, '_thrust_sp'
*/
void
MulticopterAttitudeControl::control_attitude(float dt)
{
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
/* construct attitude setpoint rotation matrix */
math::Quaternion q_sp(_v_att_sp.q_d[0], _v_att_sp.q_d[1], _v_att_sp.q_d[2], _v_att_sp.q_d[3]);
math::Matrix<3, 3> R_sp = q_sp.to_dcm();
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;
/* calculate weight for yaw control */
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_axis * e_R_z_angle;
/* cross product matrix for e_R_axis */
math::Matrix<3, 3> e_R_cp;
e_R_cp.zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
R_rp = R;
}
/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q_error;
q_error.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q_error(0) >= 0.0f ? q_error.imag() * 2.0f : -q_error.imag() * 2.0f;
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* limit rates */
for (int i = 0; i < 3; i++) {
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
!_v_control_mode.flag_control_manual_enabled) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
} else {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
}
}
/* feed forward yaw setpoint rate */
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
/* weather-vane mode, dampen yaw rate */
if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
_v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) {
float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
// prevent integrator winding up in weathervane mode
_rates_int(2) = 0.0f;
}
}
