void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); VECT3_ASSIGN(stabilization_gains.p, STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN); VECT3_ASSIGN(stabilization_gains.d, STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN); VECT3_ASSIGN(stabilization_gains.i, STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN); VECT3_ASSIGN(stabilization_gains.dd, STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN); FLOAT_EULERS_ZERO( stabilization_att_sum_err ); #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_init(void) { stabilization_attitude_ref_init(); INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); INT_EULERS_ZERO( stabilization_att_sum_err ); }
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_init(void) { stabilization_attitude_ref_init(); VECT3_ASSIGN(stabilization_gains.p, STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN); VECT3_ASSIGN(stabilization_gains.d, STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN); VECT3_ASSIGN(stabilization_gains.i, STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN); VECT3_ASSIGN(stabilization_gains.dd, STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN); INT_EULERS_ZERO( stabilization_att_sum_err ); }
void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_INDI", send_att_indi); #endif }
void stabilization_attitude_init(void) { stabilization_attitude_ref_init(); INT32_QUAT_ZERO( stabilization_att_sum_err_quat ); INT_EULERS_ZERO( stabilization_att_sum_err ); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE_REF", send_att_ref); register_periodic_telemetry(DefaultPeriodic, "AHRS_REF_QUAT", send_ahrs_ref_quat); #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 ); }