void orientationCalcEulers_i(struct OrientationReps* orientation) { if (bit_is_set(orientation->status, ORREP_EULER_I)) return; if (bit_is_set(orientation->status, ORREP_EULER_F)) { EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f); } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) { INT32_EULERS_OF_RMAT(orientation->eulers_i, orientation->rmat_i); } else if (bit_is_set(orientation->status, ORREP_QUAT_I)) { INT32_EULERS_OF_QUAT(orientation->eulers_i, orientation->quat_i); } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) { RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f); SetBit(orientation->status, ORREP_RMAT_I); INT32_EULERS_OF_RMAT(orientation->eulers_i, orientation->rmat_i); } else if (bit_is_set(orientation->status, ORREP_QUAT_F)) { QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f); SetBit(orientation->status, ORREP_QUAT_I); INT32_EULERS_OF_QUAT(orientation->eulers_i, orientation->quat_i); } /* set bit to indicate this representation is computed */ SetBit(orientation->status, ORREP_EULER_I); }
// reset to "hover" setpoint static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initial) { int32_t pitch_rotation_angle; struct Int32Quat pitch_axis_quat; struct Int32Quat pitch_rotated_quat, pitch_rotated_quat2; struct Int32Vect3 y_axis = { 0, 1, 0 }; struct Int32Eulers rotated_eulers; // compose rotation about Y axis (pitch axis) from hover pitch_rotation_angle = ANGLE_BFP_OF_REAL(-QUAT_SETPOINT_HOVER_PITCH); INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle); INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat, *initial, pitch_axis_quat); INT32_EULERS_OF_QUAT(rotated_eulers, pitch_rotated_quat); // reset euler angles rotated_eulers.theta = _theta; rotated_eulers.phi = _psi; INT32_QUAT_OF_EULERS(pitch_rotated_quat, rotated_eulers); // compose rotation about Y axis (pitch axis) to hover pitch_rotation_angle = ANGLE_BFP_OF_REAL(QUAT_SETPOINT_HOVER_PITCH); INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle); INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat2, pitch_rotated_quat, pitch_axis_quat); // store result into setpoint QUAT_COPY(stab_att_sp_quat, pitch_rotated_quat2); }
/* Compute ltp to imu rotation in euler angles and rotation matrice representation from the quaternion representation */ __attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) { /* Compute LTP to IMU euler */ INT32_EULERS_OF_QUAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_quat); /* Compute LTP to IMU rotation matrix */ INT32_RMAT_OF_QUAT(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_quat); }
void stabilization_attitude_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct Int32Quat att_err; struct Int32Quat* att_quat = stateGetNedToBodyQuat_i(); INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ INT32_QUAT_WRAP_SHORTEST(att_err); INT32_QUAT_NORMALIZE(att_err); /* rate error */ struct Int32Rates rate_err; struct Int32Rates* body_rate = stateGetBodyRates_i(); RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate); /* integrated error */ if (enable_integrator) { struct Int32Quat 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; INT32_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); INT32_QUAT_NORMALIZE(new_sum_err); QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat); } else { /* reset accumulator */ INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); INT_EULERS_ZERO( stabilization_att_sum_err ); } /* compute the feed forward command */ attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &stab_att_ref_accel); /* compute the feed back command */ attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat); /* sum feedforward and feedback */ 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]; /* 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_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct Int32Quat att_err; INT32_QUAT_INV_COMP(att_err, ahrs.ltp_to_body_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ INT32_QUAT_WRAP_SHORTEST(att_err); INT32_QUAT_NORMALIZE(att_err); /* rate error */ struct Int32Rates rate_err; RATES_DIFF(rate_err, ahrs.body_rate, stab_att_ref_rate); /* integrated error */ if (enable_integrator) { struct Int32Quat 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; INT32_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); INT32_QUAT_NORMALIZE(new_sum_err); QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat); } else { /* reset accumulator */ INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); INT_EULERS_ZERO( stabilization_att_sum_err ); } attitude_run_ff(stabilization_att_ff_cmd, current_stabilization_gains, &stab_att_ref_accel); attitude_run_fb(stabilization_att_fb_cmd, current_stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat); for (int i = COMMAND_ROLL; i <= COMMAND_YAW; i++) { stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i]; Bound(stabilization_cmd[i], -200, 200); } }
static void test_4_int(void) { printf("euler to quat to euler - int\n"); /* initial euler angles */ struct Int32Eulers _e; EULERS_ASSIGN(_e, ANGLE_BFP_OF_REAL(RadOfDeg(-10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("euler orig ", _e); /* transform to quaternion */ struct Int32Quat _q; INT32_QUAT_OF_EULERS(_q, _e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q); // INT32_QUAT_NORMALIZE(_q); // DISPLAY_INT32_QUAT_2("_q_n", _q); /* back to eulers */ struct Int32Eulers _e2; INT32_EULERS_OF_QUAT(_e2, _q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("back to euler ", _e2); }
void stabilization_attitude_read_rc_absolute(bool_t in_flight) { // FIXME: wtf??? #ifdef AIRPLANE_STICKS pprz_t roll = radio_control.values[RADIO_ROLL]; pprz_t pitch = radio_control.values[RADIO_PITCH]; pprz_t yaw = radio_control.values[RADIO_YAW]; #else // QUAD STICKS pprz_t roll = radio_control.values[RADIO_YAW]; pprz_t pitch = radio_control.values[RADIO_PITCH]; pprz_t yaw = -radio_control.values[RADIO_ROLL]; #endif struct Int32Eulers sticks_eulers; struct Int32Quat sticks_quat, prev_sp_quat; // heading hold? if (in_flight) { // compose setpoint based on previous setpoint + pitch/roll sticks reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), &stab_att_sp_quat); // get commanded yaw rate from sticks sticks_eulers.phi = RATE_BFP_OF_REAL(APPLY_DEADBAND(roll, STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF / RC_UPDATE_FREQ); sticks_eulers.theta = 0; sticks_eulers.psi = 0; // convert yaw rate * dt into quaternion INT32_QUAT_OF_EULERS(sticks_quat, sticks_eulers); QUAT_COPY(prev_sp_quat, stab_att_sp_quat) // update setpoint by rotating by incremental yaw command INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, sticks_quat); } else { /* if not flying, use current body position + pitch/yaw from sticks to compose setpoint */ reset_sp_quat(RATE_BFP_OF_REAL(yaw * YAW_COEF), RATE_BFP_OF_REAL(pitch * PITCH_COEF), stateGetNedToBodyQuat_i()); } // update euler setpoints for telemetry INT32_EULERS_OF_QUAT(stab_att_sp_euler, stab_att_sp_quat); }
float test_eulers_of_quat(struct FloatQuat fq, int display) { struct FloatEulers fe; FLOAT_EULERS_OF_QUAT(fe, fq); struct Int32Quat iq; QUAT_BFP_OF_REAL(iq, fq); struct Int32Eulers ie; INT32_EULERS_OF_QUAT(ie, iq); struct FloatEulers fe2; EULERS_FLOAT_OF_BFP(fe2, ie); EULERS_SUB(fe2, ie); float norm_err = FLOAT_EULERS_NORM(fe2); if (display) { printf("euler of quat\n"); DISPLAY_FLOAT_QUAT("fq", fq); DISPLAY_FLOAT_EULERS_DEG("fe", fe); DISPLAY_INT32_EULERS("ie", ie); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("ieaf", ie); } return norm_err; }
static void test_3(void) { /* Compute BODY to IMU eulers */ struct Int32Eulers b2i_e; EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e); /* Compute BODY to IMU quaternion */ struct Int32Quat b2i_q; INT32_QUAT_OF_EULERS(b2i_q, b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); // INT32_QUAT_NORMALIZE(b2i_q); // DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q); /* Compute BODY to IMU rotation matrix */ struct Int32RMat b2i_r; INT32_RMAT_OF_EULERS(b2i_r, b2i_e); // DISPLAY_INT32_RMAT("b2i_r", b2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r); /* Compute LTP to IMU eulers */ struct Int32Eulers l2i_e; EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e); /* Compute LTP to IMU quaternion */ struct Int32Quat l2i_q; INT32_QUAT_OF_EULERS(l2i_q, l2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q); /* Compute LTP to IMU rotation matrix */ struct Int32RMat l2i_r; INT32_RMAT_OF_EULERS(l2i_r, l2i_e); // DISPLAY_INT32_RMAT("l2i_r", l2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r); /* again but from quaternion */ struct Int32RMat l2i_r2; INT32_RMAT_OF_QUAT(l2i_r2, l2i_q); // DISPLAY_INT32_RMAT("l2i_r2", l2i_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2); /* Compute LTP to BODY quaternion */ struct Int32Quat l2b_q; INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q); /* Compute LTP to BODY rotation matrix */ struct Int32RMat l2b_r; INT32_RMAT_COMP_INV(l2b_r, l2i_r, b2i_r); // DISPLAY_INT32_RMAT("l2b_r", l2b_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r); /* again but from quaternion */ struct Int32RMat l2b_r2; INT32_RMAT_OF_QUAT(l2b_r2, l2b_q); // DISPLAY_INT32_RMAT("l2b_r2", l2b_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2); /* compute LTP to BODY eulers */ struct Int32Eulers l2b_e; INT32_EULERS_OF_RMAT(l2b_e, l2b_r); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e); /* again but from quaternion */ struct Int32Eulers l2b_e2; INT32_EULERS_OF_QUAT(l2b_e2, l2b_q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2); }