void orientationCalcQuat_i(struct OrientationReps* orientation) { if (bit_is_set(orientation->status, ORREP_QUAT_I)) return; if (bit_is_set(orientation->status, ORREP_QUAT_F)) { QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f); } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) { INT32_QUAT_OF_RMAT(orientation->quat_i, orientation->rmat_i); } else if (bit_is_set(orientation->status, ORREP_EULER_I)) { INT32_QUAT_OF_EULERS(orientation->quat_i, orientation->eulers_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_QUAT_OF_RMAT(orientation->quat_i, orientation->rmat_i); } else if (bit_is_set(orientation->status, ORREP_EULER_F)) { EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f); SetBit(orientation->status, ORREP_EULER_I); INT32_QUAT_OF_EULERS(orientation->quat_i, orientation->eulers_i); } /* set bit to indicate this representation is computed */ SetBit(orientation->status, ORREP_QUAT_I); }
void imu_init(void) { /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); /* Compute quaternion and rotation matrix for conversions between body and imu frame */ #if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALISE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); #else INT32_QUAT_ZERO(imu.body_to_imu_quat); INT32_RMAT_ZERO(imu.body_to_imu_rmat); #endif imu_impl_init(); }
static void test_10(void) { struct FloatEulers euler; EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.)); DISPLAY_FLOAT_EULERS_DEG("euler", euler); struct FloatQuat quat; FLOAT_QUAT_OF_EULERS(quat, euler); DISPLAY_FLOAT_QUAT("####quat", quat); struct Int32Eulers euleri; EULERS_BFP_OF_REAL(euleri, euler); DISPLAY_INT32_EULERS("euleri", euleri); struct Int32Quat quati; INT32_QUAT_OF_EULERS(quati, euleri); DISPLAY_INT32_QUAT("####quat", quati); struct Int32RMat rmati; INT32_RMAT_OF_EULERS(rmati, euleri); DISPLAY_INT32_RMAT("####rmat", rmati); struct Int32Quat quat_ltp_to_body; struct Int32Quat body_to_imu_quat; INT32_QUAT_ZERO( body_to_imu_quat); INT32_QUAT_COMP_INV(quat_ltp_to_body, body_to_imu_quat, quati); DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body); }
void imu_init(void) { /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); #if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); #else #if USE_MAGNETOMETER #pragma message "Info: Magnetomter neutrals are set to zero!" #endif INT_VECT3_ZERO(imu.mag_neutral); #endif /* Compute quaternion and rotation matrix for conversions between body and imu frame */ struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALIZE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); imu_impl_init(); }
// 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 quaternion and rotation matrice representation from the euler angle representation */ __attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) { /* Compute LTP to IMU quaternion */ INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler); /* Compute LTP to IMU rotation matrix */ INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler); }
void imu_init(void) { #ifdef IMU_POWER_GPIO gpio_setup_output(IMU_POWER_GPIO); IMU_POWER_GPIO_ON(IMU_POWER_GPIO); #endif /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); #if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); #else #if USE_MAGNETOMETER INFO("Magnetometer neutrals are set to zero, you should calibrate!") #endif INT_VECT3_ZERO(imu.mag_neutral); #endif /* Compute quaternion and rotation matrix for conversions between body and imu frame */ struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALIZE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); #if USE_IMU_FLOAT #else // !USE_IMU_FLOAT register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag); #endif // !USE_IMU_FLOAT #endif // DOWNLINK imu_impl_init(); }
static void test_2(void) { struct Int32Vect3 v1 = { 5000, 5000, 5000 }; DISPLAY_INT32_VECT3("v1", v1); struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)}; DISPLAY_FLOAT_EULERS("euler_f", euler_f); struct Int32Eulers euler_i; EULERS_BFP_OF_REAL(euler_i, euler_f); DISPLAY_INT32_EULERS("euler_i", euler_i); struct Int32Quat quat_i; INT32_QUAT_OF_EULERS(quat_i, euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); INT32_QUAT_NORMALIZE(quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; INT32_QUAT_VMULT(v2, quat_i, v1); DISPLAY_INT32_VECT3("v2", v2); struct Int32RMat rmat_i; INT32_RMAT_OF_QUAT(rmat_i, quat_i); DISPLAY_INT32_RMAT("rmat_i", rmat_i); struct Int32Vect3 v3; INT32_RMAT_VMULT(v3, rmat_i, v1); DISPLAY_INT32_VECT3("v3", v3); struct Int32RMat rmat_i2; INT32_RMAT_OF_EULERS(rmat_i2, euler_i); DISPLAY_INT32_RMAT("rmat_i2", rmat_i2); struct Int32Vect3 v4; INT32_RMAT_VMULT(v4, rmat_i2, v1); DISPLAY_INT32_VECT3("v4", v4); struct FloatQuat quat_f; FLOAT_QUAT_OF_EULERS(quat_f, euler_f); DISPLAY_FLOAT_QUAT("quat_f", quat_f); struct FloatVect3 v5; VECT3_COPY(v5, v1); DISPLAY_FLOAT_VECT3("v5", v5); struct FloatVect3 v6; FLOAT_QUAT_VMULT(v6, quat_f, v5); DISPLAY_FLOAT_VECT3("v6", v6); }
static void test_1(void) { struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)}; DISPLAY_FLOAT_EULERS("euler_f", euler_f); struct Int32Eulers euler_i; EULERS_BFP_OF_REAL(euler_i, euler_f); DISPLAY_INT32_EULERS("euler_i", euler_i); struct FloatQuat quat_f; FLOAT_QUAT_OF_EULERS(quat_f, euler_f); DISPLAY_FLOAT_QUAT("quat_f", quat_f); struct Int32Quat quat_i; INT32_QUAT_OF_EULERS(quat_i, euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); struct Int32RMat rmat_i; INT32_RMAT_OF_QUAT(rmat_i, quat_i); DISPLAY_INT32_RMAT("rmat_i", rmat_i); }
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); }
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); }
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) { // copy euler setpoint for debugging memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers)); INT32_QUAT_OF_EULERS(stab_att_sp_quat, *sp_euler); INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat); }