void stabilization_attitude_ref_init(void) { FLOAT_EULERS_ZERO(stab_att_sp_euler); FLOAT_QUAT_ZERO( stab_att_sp_quat); FLOAT_EULERS_ZERO(stab_att_ref_euler); FLOAT_QUAT_ZERO( stab_att_ref_quat); FLOAT_RATES_ZERO( stab_att_ref_rate); FLOAT_RATES_ZERO( stab_att_ref_accel); for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]); RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]); } }
void ahrs_init(void) { ahrs_float.status = AHRS_UNINIT; /* * Initialises our IMU alignement variables * This should probably done in the IMU code instead */ struct FloatEulers body_to_imu_euler = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); /* set ltp_to_body to zero */ FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); FLOAT_RATES_ZERO(ahrs_float.body_rate); /* set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler); FLOAT_RATES_ZERO(ahrs_float.imu_rate); /* set inital filter dcm */ set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); }
void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]); VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]); #ifdef HAS_SURFACE_COMMANDS VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]); #endif } FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err ); FLOAT_RATES_ZERO( last_body_rate ); FLOAT_RATES_ZERO( body_rate_d ); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref); #endif }
void stabilization_attitude_enter(void) { stabilization_attitude_ref_enter(); FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); }
static void set_reference_direction(void){ struct NedCoor_d ref_dir_ned; struct EcefCoor_d pos_0_ecef_pprz, ref_dir_ecef; EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned); struct LtpDef_d current_ltp; VECTOR_AS_VECT3(pos_0_ecef_pprz, pos_0_ecef); ltp_def_from_ecef_d(¤t_ltp, &pos_0_ecef_pprz); ecef_of_ned_vect_d(&ref_dir_ecef, ¤t_ltp, &ref_dir_ned); // THIS SOMEWHERE ELSE! DoubleQuat initial_orientation; FLOAT_QUAT_ZERO(initial_orientation); ENU_NED_transformation(current_ltp.ltp_of_ecef); DOUBLE_QUAT_OF_RMAT(initial_orientation, current_ltp.ltp_of_ecef); ins.avg_state.orientation = DOUBLEQUAT_AS_QUATERNIOND(initial_orientation); // THIS SOMEWHERE ELSE! (END) // old transformation: //struct DoubleRMat ned2ecef; //NED_TO_ECEF_MAT(pos_0_lla, ned2ecef.m); //RMAT_VECT3_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned); reference_direction = VECT3_AS_VECTOR3D(ref_dir_ecef).normalized(); //reference_direction = Vector3d(1, 0, 0); std::cout <<"reference direction NED : " << VECT3_AS_VECTOR3D(ref_dir_ned).transpose() << std::endl; std::cout <<"reference direction ECEF: " << reference_direction.transpose() << std::endl; }
void stabilization_attitude_enter(void) { /* reset psi setpoint to current psi angle */ stab_att_sp_euler.psi = stabilization_attitude_get_heading_f(); stabilization_attitude_ref_enter(); FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err ); }
/** * Incorporate errors to reference and zeros state */ static inline void reset_state(void) { ahrs_impl.gibbs_cor.qi = 2.; struct FloatQuat q_tmp; FLOAT_QUAT_COMP(q_tmp, ahrs_impl.ltp_to_imu_quat, ahrs_impl.gibbs_cor); FLOAT_QUAT_NORMALIZE(q_tmp); memcpy(&ahrs_impl.ltp_to_imu_quat, &q_tmp, sizeof(ahrs_impl.ltp_to_imu_quat)); FLOAT_QUAT_ZERO(ahrs_impl.gibbs_cor); }
void stabilization_attitude_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct FloatQuat att_err; FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(att_err); /* rate error */ struct FloatRates rate_err; RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.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, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat); // FIXME: this is very dangerous! only works if this really includes all commands for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) { stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i]; } /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); }
/* init state and measurements */ static inline void init_invariant_state(void) { // init state FLOAT_QUAT_ZERO(ins_impl.state.quat); FLOAT_RATES_ZERO(ins_impl.state.bias); FLOAT_VECT3_ZERO(ins_impl.state.pos); FLOAT_VECT3_ZERO(ins_impl.state.speed); ins_impl.state.as = 1.0f; ins_impl.state.hb = 0.0f; // init measures FLOAT_VECT3_ZERO(ins_impl.meas.pos_gps); FLOAT_VECT3_ZERO(ins_impl.meas.speed_gps); ins_impl.meas.baro_alt = 0.0f; // init baro ins_baro_initialized = FALSE; }
/* the following solver uses the Power Iteration * * It is rather simple: * 1. You choose a starting vektor x_0 (shouldn't be zero) * 2. apply it on the Matrix * x_(k+1) = A * x_k * 3. Repeat this very often. * * The vector converges to the dominat eigenvector, which belongs to the eigenvalue with the biggest absolute value. * * But there is a problem: * With every step, the norm of vector grows. Therefore it's necessary to scale it with every step. * * Important warnings: * I. This function does not converge if the Matrix is singular * II. Pay attention to the loop-condition! It does not end if it's close to the eigenvector! * It ends if the steps are getting too close to each other. * */ DoubleVect4 dominant_Eigenvector(struct DoubleMat44 A, unsigned int maximum_iterations, double precision, struct DoubleMat44 sigma_A, DoubleVect4 *sigma_x) { unsigned int k; DoubleVect4 x_k, x_kp1; double delta = 1, scale; FLOAT_QUAT_ZERO(x_k); //for(k=0; (k<maximum_iterations) && (delta>precision); k++){ for (k = 0; k < maximum_iterations; k++) { // Next step DOUBLE_MAT_VMULT4(x_kp1, A, x_k); // Scale the vector scale = 1 / INFTY_NORM4(x_kp1); QUAT_SMUL(x_kp1, x_kp1, scale); // Calculate the difference between to steps for the loop condition. Store temporarily in x_k QUAT_DIFF(x_k, x_k, x_kp1); delta = INFTY_NORM4(x_k); // Update the next step x_k = x_kp1; if (delta <= precision) { DOUBLE_MAT_VMULT4(*sigma_x, sigma_A, x_k); QUAT_SMUL(*sigma_x, *sigma_x, scale); break; } } #ifdef EKNAV_FROM_LOG_DEBUG printf("Number of iterations: %4i\n", k); #endif if (k == maximum_iterations) { printf("Orientation did not converge. Using maximum uncertainty\n"); //FLOAT_QUAT_ZERO(x_k); QUAT_ASSIGN(*sigma_x, 0, M_PI_2, M_PI_2, M_PI_2); } return x_k; }
void imu_float_init(struct ImuFloat* imuf) { /* 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 EULERS_ASSIGN(imuf->body_to_imu_eulers, IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI); FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers); FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat); FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers); #else EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.); FLOAT_QUAT_ZERO(imuf->body_to_imu_quat); FLOAT_RMAT_ZERO(imuf->body_to_imu_rmat); #endif }
void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_GAIN_NB; i++) { VECT3_ASSIGN(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]); VECT3_ASSIGN(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]); VECT3_ASSIGN(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]); VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]); VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]); VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]); VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]); } FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); }
void ahrs_init(void) { //ahrs_float.status = AHRS_UNINIT; // set to running for now and only use ahrs.status, not ahrs_float.status ahrs.status = AHRS_RUNNING; ahrs_sim_available = FALSE; /* set ltp_to_body to zero */ FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); FLOAT_RATES_ZERO(ahrs_float.body_rate); /* set ltp_to_imu to same as ltp_to_body, currently no difference simulated */ QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_body_quat); EULERS_COPY(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_body_euler); RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_body_rmat); RATES_COPY(ahrs_float.imu_rate, ahrs_float.body_rate); }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; /* * Initialises our IMU alignement variables * This should probably done in the IMU code instead */ struct FloatEulers body_to_imu_euler = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); /* set ltp_to_body to zero */ FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); FLOAT_RATES_ZERO(ahrs_float.body_rate); /* set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler); FLOAT_RATES_ZERO(ahrs_float.imu_rate); /* set inital filter dcm */ set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); #if USE_HIGH_ACCEL_FLAG high_accel_done = FALSE; high_accel_flag = FALSE; #endif ahrs_impl.gps_speed = 0; ahrs_impl.gps_acceleration = 0; ahrs_impl.gps_course = 0; ahrs_impl.gps_course_valid = FALSE; ahrs_impl.gps_age = 100; }
void ahrs_init(void) { ahrs.status = AHRS_UNINIT; /* Initialises IMU alignement */ struct FloatEulers body_to_imu_euler = {IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI}; FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler); FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler); /* Set ltp_to_body to zero */ FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat); FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler); FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat); FLOAT_RATES_ZERO(ahrs_float.body_rate); /* Set ltp_to_imu so that body is zero */ QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat); RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat); EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler); FLOAT_RATES_ZERO(ahrs_float.imu_rate); }
void ahrs_init(void) { int i, j; for (i = 0; i < BAFL_SSIZE; i++) { for (j = 0; j < BAFL_SSIZE; j++) { bafl_T[i][j] = 0.; bafl_P[i][j] = 0.; } /* Insert the diagonal elements of the T-Matrix. These will never change. */ bafl_T[i][i] = 1.0; /* initial covariance values */ if (i<3) { bafl_P[i][i] = 1.0; } else { bafl_P[i][i] = 0.1; } } FLOAT_QUAT_ZERO(bafl_quat); ahrs.status = AHRS_UNINIT; INT_EULERS_ZERO(ahrs.ltp_to_body_euler); INT_EULERS_ZERO(ahrs.ltp_to_imu_euler); INT32_QUAT_ZERO(ahrs.ltp_to_body_quat); INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat); INT_RATES_ZERO(ahrs.body_rate); INT_RATES_ZERO(ahrs.imu_rate); ahrs_float_lkf_SetRaccel(BAFL_SIGMA_ACCEL); ahrs_float_lkf_SetRmag(BAFL_SIGMA_MAG); bafl_Q_att = BAFL_Q_ATT; bafl_Q_gyro = BAFL_Q_GYRO; FLOAT_VECT3_ASSIGN(bafl_h, BAFL_hx,BAFL_hy, BAFL_hz); }
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); }