void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
{
float_quat_inv_comp(b2c, a2b, a2c);
float_quat_wrap_shortest(b2c);
float_quat_normalize(b2c);
}
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) {
/* cmd_x is positive to north = negative pitch
* cmd_y is positive to east = positive roll
*
* orientation vector describing simultaneous rotation of roll/pitch
*/
const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0};
/* quaternion from that orientation vector */
struct FloatQuat q_rp;
float_quat_of_orientation_vect(&q_rp, &ov);
/* as rotation matrix */
struct FloatRMat R_rp;
float_rmat_of_quat(&R_rp, &q_rp);
/* body x-axis (before heading command) is first column */
struct FloatVect3 b_x;
VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]);
/* body z-axis (thrust vect) is last column */
struct FloatVect3 thrust_vect;
VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]);
/// @todo optimize yaw angle calculation
/*
* Instead of using the psi setpoint angle to rotate around the body z-axis,
* calculate the real angle needed to align the projection of the body x-axis
* onto the horizontal plane with the psi setpoint.
*
* angle between two vectors a and b:
* angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n))
* where the normal n is the thrust vector (i.e. both a and b lie in that plane)
*/
// desired heading vect in earth x-y plane
const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0};
/* projection of desired heading onto body x-y plane
* b = v - dot(v,n)*n
*/
float dot = VECT3_DOT_PRODUCT(psi_vect, thrust_vect);
struct FloatVect3 dotn;
VECT3_SMUL(dotn, thrust_vect, dot);
// b = v - dot(v,n)*n
struct FloatVect3 b;
VECT3_DIFF(b, psi_vect, dotn);
dot = VECT3_DOT_PRODUCT(b_x, b);
struct FloatVect3 cross;
VECT3_CROSS_PRODUCT(cross, b_x, b);
// norm of the cross product
float nc = FLOAT_VECT3_NORM(cross);
// angle = atan2(norm(cross(a,b)), dot(a,b))
float yaw2 = atan2(nc, dot) / 2.0;
// negative angle if needed
// sign(dot(cross(a,b), n)
float dot_cross_ab = VECT3_DOT_PRODUCT(cross, thrust_vect);
if (dot_cross_ab < 0) {
yaw2 = -yaw2;
}
/* quaternion with yaw command */
struct FloatQuat q_yaw;
QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
/* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
float_quat_comp(quat, &q_rp, &q_yaw);
float_quat_normalize(quat);
float_quat_wrap_shortest(quat);
}
void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
{
float_quat_comp_inv(a2b, a2c, b2c);
float_quat_wrap_shortest(a2b);
float_quat_normalize(a2b);
}
void stabilization_attitude_run(bool enable_integrator)
{
/*
* Update reference
*/
static const float dt = (1./PERIODIC_FREQUENCY);
attitude_ref_quat_float_update(&att_ref_quat_f, &stab_att_sp_quat, dt);
/*
* Compute errors for feedback
*/
/* attitude error */
struct FloatQuat att_err;
struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
float_quat_inv_comp(&att_err, att_quat, &att_ref_quat_f.quat);
/* wrap it in the shortest direction */
float_quat_wrap_shortest(&att_err);
/* rate error */
struct FloatRates rate_err;
struct FloatRates *body_rate = stateGetBodyRates_f();
RATES_DIFF(rate_err, att_ref_quat_f.rate, *body_rate);
/* rate_d error */
RATES_DIFF(body_rate_d, *body_rate, last_body_rate);
RATES_COPY(last_body_rate, *body_rate);
#define INTEGRATOR_BOUND 1.0
/* integrated error */
if (enable_integrator) {
stabilization_att_sum_err_quat.qx += att_err.qx / IERROR_SCALE;
stabilization_att_sum_err_quat.qy += att_err.qy / IERROR_SCALE;
stabilization_att_sum_err_quat.qz += att_err.qz / IERROR_SCALE;
Bound(stabilization_att_sum_err_quat.qx, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qy, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
Bound(stabilization_att_sum_err_quat.qz, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);
} else {
/* reset accumulator */
float_quat_identity(&stabilization_att_sum_err_quat);
}
attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &att_ref_quat_f.accel);
attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d,
&stabilization_att_sum_err_quat);
stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];
stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];
stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
#ifdef HAS_SURFACE_COMMANDS
stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] +
stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] +
stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] +
stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
#endif
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}