bool_t ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_int_get_quat_from_accel_mag(&ahrs_icq.ltp_to_imu_quat, lp_accel, lp_mag); ahrs_icq.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ ahrs_int_get_quat_from_accel(&ahrs_icq.ltp_to_imu_quat, lp_accel); ahrs_icq.heading_aligned = FALSE; // supress unused arg warning lp_mag = lp_mag; #endif /* Use low passed gyro value as initial bias */ RATES_COPY(ahrs_icq.gyro_bias, *lp_gyro); RATES_COPY(ahrs_icq.high_rez_bias, *lp_gyro); INT_RATES_LSHIFT(ahrs_icq.high_rez_bias, ahrs_icq.high_rez_bias, 28); ahrs_icq.status = AHRS_ICQ_RUNNING; ahrs_icq.is_aligned = TRUE; return TRUE; }
void ahrs_align(void) { /* Compute an initial orientation using euler angles */ ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); /* Convert initial orientation in quaternion and rotation matrice representations. */ compute_imu_quat_and_rmat_from_euler(); compute_body_orientation(); /* Use low passed gyro value as initial bias */ RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro); INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28); ahrs.status = AHRS_RUNNING; }
//Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired void stabilization_rate_read_rc_switched_sticks( void ) { if(ROLL_RATE_DEADBAND_EXCEEDED()) stabilization_rate_sp.r = (int32_t) -radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ; else stabilization_rate_sp.r = 0; if(PITCH_RATE_DEADBAND_EXCEEDED()) stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ; else stabilization_rate_sp.q = 0; if(YAW_RATE_DEADBAND_EXCEEDED()) stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ; else stabilization_rate_sp.p = 0; // Setpoint at ref resolution INT_RATES_LSHIFT(stabilization_rate_sp, stabilization_rate_sp, REF_FRAC - INT32_RATE_FRAC); }
void ahrs_align(void) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_int_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); ahrs_impl.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ ahrs_int_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel); ahrs_impl.heading_aligned = FALSE; #endif set_body_state_from_quat(); /* Use low passed gyro value as initial bias */ RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro); RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro); INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28); ahrs.status = AHRS_RUNNING; }