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); }