static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev) { mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, get_sys_time_msec(), stateGetNedToBodyQuat_f()->qi, stateGetNedToBodyQuat_f()->qx, stateGetNedToBodyQuat_f()->qy, stateGetNedToBodyQuat_f()->qz, stateGetBodyRates_f()->p, stateGetBodyRates_f()->q, stateGetBodyRates_f()->r); MAVLinkSendMessage(); }
static inline void mavlink_send_attitude_quaternion(void) { mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, get_sys_time_msec(), stateGetNedToBodyQuat_f()->qi, stateGetNedToBodyQuat_f()->qx, stateGetNedToBodyQuat_f()->qy, stateGetNedToBodyQuat_f()->qz, stateGetBodyRates_f()->p, stateGetBodyRates_f()->q, stateGetBodyRates_f()->r); MAVLinkSendMessage(); }
void ahrs_realign_heading(float heading) { FLOAT_ANGLE_NORMALIZE(heading); /* quaternion representing only the heading rotation from ltp to body */ struct FloatQuat q_h_new; q_h_new.qx = 0.0; q_h_new.qy = 0.0; q_h_new.qz = sinf(heading/2.0); q_h_new.qi = cosf(heading/2.0); struct FloatQuat* ltp_to_body_quat = stateGetNedToBodyQuat_f(); /* quaternion representing current heading only */ struct FloatQuat q_h; QUAT_COPY(q_h, *ltp_to_body_quat); q_h.qx = 0.; q_h.qy = 0.; FLOAT_QUAT_NORMALIZE(q_h); /* quaternion representing rotation from current to new heading */ struct FloatQuat q_c; FLOAT_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new); /* correct current heading in body frame */ struct FloatQuat q; FLOAT_QUAT_COMP_NORM_SHORTEST(q, q_c, *ltp_to_body_quat); /* compute ltp to imu rotation quaternion and matrix */ FLOAT_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, q, ahrs_impl.body_to_imu_quat); FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); /* set state */ stateSetNedToBodyQuat_f(&q); ahrs_impl.heading_aligned = TRUE; }
void ahrs_realign_heading(float heading) { FLOAT_ANGLE_NORMALIZE(heading); /* quaternion representing only the heading rotation from ltp to body */ struct FloatQuat q_h_new; q_h_new.qx = 0.0; q_h_new.qy = 0.0; q_h_new.qz = sinf(heading/2.0); q_h_new.qi = cosf(heading/2.0); struct FloatQuat* ltp_to_body_quat = stateGetNedToBodyQuat_f(); /* quaternion representing current heading only */ struct FloatQuat q_h; QUAT_COPY(q_h, *ltp_to_body_quat); q_h.qx = 0.; q_h.qy = 0.; float_quat_normalize(&q_h); /* quaternion representing rotation from current to new heading */ struct FloatQuat q_c; float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new); /* correct current heading in body frame */ struct FloatQuat q; float_quat_comp_norm_shortest(&q, &q_c, ltp_to_body_quat); /* compute ltp to imu rotation quaternion and matrix */ struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); float_quat_comp(&ahrs_impl.ltp_to_imu_quat, &q, body_to_imu_quat); float_rmat_of_quat(&ahrs_impl.ltp_to_imu_rmat, &ahrs_impl.ltp_to_imu_quat); /* set state */ stateSetNedToBodyQuat_f(&q); ahrs_impl.heading_aligned = TRUE; }
void stabilization_attitude_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct FloatQuat att_err; struct FloatQuat* att_quat = stateGetNedToBodyQuat_f(); FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_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, stab_att_ref_rate, *body_rate); /* rate_d error */ struct FloatRates body_rate_d; RATES_DIFF(body_rate_d, *body_rate, last_body_rate); RATES_COPY(last_body_rate, *body_rate); /* integrated error */ if (enable_integrator) { struct FloatQuat new_sum_err, scaled_att_err; /* update accumulator */ scaled_att_err.qi = att_err.qi; scaled_att_err.qx = att_err.qx / IERROR_SCALE; scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); } else { /* reset accumulator */ FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); } attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_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); }
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); }