int main(int argc, char** argv) { read_ascii_flight_log(IN_FILE); imu_init(); ahrs_init(); for (int i=0; i<nb_samples; i++) { feed_imu(i); if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) ahrs_align(); } else { ahrs_propagate((1./AHRS_PROPAGATE_FREQUENCY)); #ifdef ENABLE_MAG_UPDATE if (MAG_AVAILABLE(samples[i].flag)) ahrs_update_mag(); #endif #ifdef ENABLE_ACCEL_UPDATE if (IMU_AVAILABLE(samples[i].flag) && (!MAG_AVAILABLE(samples[i].flag))) ahrs_update_accel(); #endif } store_filter_output(i); } dump_output(OUT_FILE); }
static inline void on_gyro_accel_event( void ) { ImuScaleGyro(imu); ImuScaleAccel(imu); if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) ahrs_align(); } else { ahrs_propagate(); ahrs_update_accel(); ahrs_update_fw_estimator(); } }
void aos_run(void) { aos_compute_state(); aos_compute_sensors(); #ifndef DISABLE_ALIGNEMENT if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { ahrs_align(); } } else { #endif /* DISABLE_ALIGNEMENT */ ahrs_propagate(aos.dt); ahrs_update_accel(); #ifndef DISABLE_MAG_UPDATE ahrs_update_mag(); #endif #if AHRS_USE_GPS_HEADING #if AHRS_TYPE == AHRS_TYPE_ICQ int32_t heading = ANGLE_BFP_OF_REAL(aos.heading_meas); #endif #if AHRS_TYPE == AHRS_TYPE_FCQ float heading = aos.heading_meas; #endif #if AHRS_TYPE == AHRS_TYPE_FCR ahrs_impl.gps_course = aos.heading_meas; ahrs_impl.gps_course_valid = true; #else if (aos.time > 10) { if (!ahrs_impl.heading_aligned) { ahrs_realign_heading(heading); } else { RunOnceEvery(100, ahrs_update_heading(heading)); } } #endif #endif // AHRS_USE_GPS_HEADING #ifndef DISABLE_ALIGNEMENT } #endif }
void aos_run(void) { aos_compute_state(); aos_compute_sensors(); #ifndef DISABLE_ALIGNEMENT if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) ahrs_align(); } else { #endif /* DISABLE_ALIGNEMENT */ ahrs_propagate(); ahrs_update_accel(); ahrs_update_mag(); #ifndef DISABLE_ALIGNEMENT } #endif }
static inline void on_gyro_accel_event( void ) { #ifdef AHRS_CPU_LED LED_ON(AHRS_CPU_LED); #endif // Run aligner on raw data as it also makes averages. if (ahrs.status == AHRS_UNINIT) { ImuScaleGyro(imu); ImuScaleAccel(imu); ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) ahrs_align(); return; } #if PERIODIC_FREQUENCY == 60 ImuScaleGyro(imu); ImuScaleAccel(imu); ahrs_propagate(); ahrs_update_accel(); ahrs_update_fw_estimator(); #else //PERIODIC_FREQUENCY static uint8_t _reduced_propagation_rate = 0; static uint8_t _reduced_correction_rate = 0; static struct Int32Vect3 acc_avg; static struct Int32Rates gyr_avg; RATES_ADD(gyr_avg, imu.gyro_unscaled); VECT3_ADD(acc_avg, imu.accel_unscaled); _reduced_propagation_rate++; if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY)) { } else { _reduced_propagation_rate = 0; RATES_SDIV(imu.gyro_unscaled, gyr_avg, (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY) ); INT_RATES_ZERO(gyr_avg); ImuScaleGyro(imu); ahrs_propagate(); _reduced_correction_rate++; if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY)) { _reduced_correction_rate = 0; VECT3_SDIV(imu.accel_unscaled, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) ); INT_VECT3_ZERO(acc_avg); ImuScaleAccel(imu); ahrs_update_accel(); ahrs_update_fw_estimator(); } } #endif //PERIODIC_FREQUENCY #ifdef AHRS_CPU_LED LED_OFF(AHRS_CPU_LED); #endif #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP new_ins_attitude = 1; #endif }
/* * GX3 can be set up during the startup, or it can be configured to * start sending data automatically after power up. */ void imu_impl_init(void) { // Initialize variables ahrs_impl.gx3_status = GX3Uninit; // Initialize packet ahrs_impl.gx3_packet.status = GX3PacketWaiting; ahrs_impl.gx3_packet.msg_idx = 0; ahrs_impl.gx3_packet.msg_available = FALSE; ahrs_impl.gx3_packet.chksm_error = 0; ahrs_impl.gx3_packet.hdr_error = 0; // It is necessary to wait for GX3 to power up for proper initialization for (uint32_t startup_counter=0; startup_counter<IMU_GX3_LONG_DELAY*2; startup_counter++){ __asm("nop"); } #ifdef GX3_INITIALIZE_DURING_STARTUP #pragma message "GX3 initializing" /* // FOR NON-CONTINUOUS MODE UNCOMMENT THIS //4 byte command for non-Continous Mode so we can set the other settings uart_transmit(&GX3_PORT, 0xc4); uart_transmit(&GX3_PORT, 0xc1); uart_transmit(&GX3_PORT, 0x29); uart_transmit(&GX3_PORT, 0x00); // stop */ //Sampling Settings (0xDB) uart_transmit(&GX3_PORT, 0xdb); //set update speed uart_transmit(&GX3_PORT, 0xa8); uart_transmit(&GX3_PORT, 0xb9); //set rate of IMU link, is 1000/IMU_DIV #define IMU_DIV1 0 #define IMU_DIV2 2 #define ACC_FILT_DIV 2 #define MAG_FILT_DIV 30 #ifdef GX3_SAVE_SETTINGS uart_transmit(&GX3_PORT, 0x02);//set params and save them in non-volatile memory #else uart_transmit(&GX3_PORT, 0x02); //set and don't save #endif uart_transmit(&GX3_PORT, IMU_DIV1); uart_transmit(&GX3_PORT, IMU_DIV2); uart_transmit(&GX3_PORT, 0b00000000); //set options byte 8 - GOOD uart_transmit(&GX3_PORT, 0b00000011); //set options byte 7 - GOOD //0 - calculate orientation, 1 - enable coning & sculling, 2-3 reserved, 4 - no little endian data, // 5 - no NaN supressed, 6 - disable finite size correction, 7 - reserved, // 8 - enable magnetometer, 9 - reserved, 10 - enable magnetic north compensation, 11 - enable gravity compensation // 12 - no quaternion calculation, 13-15 reserved uart_transmit(&GX3_PORT, ACC_FILT_DIV); uart_transmit(&GX3_PORT, MAG_FILT_DIV); //mag window filter size == 33hz uart_transmit(&GX3_PORT, 0x00); uart_transmit(&GX3_PORT, 10); // Up Compensation in secs, def=10s uart_transmit(&GX3_PORT, 0x00); uart_transmit(&GX3_PORT, 10); // North Compensation in secs uart_transmit(&GX3_PORT, 0x00); //power setting = 0, high power/bw uart_transmit(&GX3_PORT, 0x00); //rest of the bytes are 0 uart_transmit(&GX3_PORT, 0x00); uart_transmit(&GX3_PORT, 0x00); uart_transmit(&GX3_PORT, 0x00); uart_transmit(&GX3_PORT, 0x00); // OPTIONAL: realign up and north /* uart_transmit(&GX3_PORT, 0xdd); uart_transmit(&GX3_PORT, 0x54); uart_transmit(&GX3_PORT, 0x4c); uart_transmit(&GX3_PORT, 3); uart_transmit(&GX3_PORT, 10); uart_transmit(&GX3_PORT, 10); uart_transmit(&GX3_PORT, 0x00); uart_transmit(&GX3_PORT, 0x00); uart_transmit(&GX3_PORT, 0x00); uart_transmit(&GX3_PORT, 0x00); */ //Another wait loop for proper GX3 init for (uint32_t startup_counter=0; startup_counter<IMU_GX3_LONG_DELAY; startup_counter++){ __asm("nop"); } #ifdef GX3_SET_WAKEUP_MODE //Mode Preset (0xD5) uart_transmit(&GX3_PORT, 0xD5); uart_transmit(&GX3_PORT, 0xBA); uart_transmit(&GX3_PORT, 0x89); uart_transmit(&GX3_PORT, 0x02); // wake up in continuous mode //Continuous preset (0xD6) uart_transmit(&GX3_PORT, 0xD6); uart_transmit(&GX3_PORT, 0xC6); uart_transmit(&GX3_PORT, 0x6B); uart_transmit(&GX3_PORT, 0xc8); // accel, gyro, R #endif //4 byte command for Continous Mode uart_transmit(&GX3_PORT, 0xc4); uart_transmit(&GX3_PORT, 0xc1); uart_transmit(&GX3_PORT, 0x29); uart_transmit(&GX3_PORT, 0xc8); // accel,gyro,R // Reset gyros to zero ahrs_align(); #endif ahrs.status = AHRS_RUNNING; #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "GX3_INFO", send_gx3); #endif }
void aos_init(int traj_nb) { aos.traj = &traj[traj_nb]; aos.time = 0; aos.dt = 1. / AHRS_PROPAGATE_FREQUENCY; aos.traj->ts = 0; aos.traj->ts = 1.; // default to one second /* default state */ EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler); RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); FLOAT_VECT3_ZERO(aos.ltp_pos); FLOAT_VECT3_ZERO(aos.ltp_vel); FLOAT_VECT3_ZERO(aos.ltp_accel); FLOAT_VECT3_ZERO(aos.ltp_jerk); aos.traj->init_fun(); imu_init(); ahrs_init(); #ifdef PERFECT_SENSORS RATES_ASSIGN(aos.gyro_bias, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.)); VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.); aos.heading_noise = 0.; #else RATES_ASSIGN(aos.gyro_bias, RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.)); RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.)); VECT3_ASSIGN(aos.accel_noise, .5, .5, .5); aos.heading_noise = RadOfDeg(3.); #endif #ifdef FORCE_ALIGNEMENT // DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat); aos_compute_sensors(); // DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias); // DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates); VECT3_COPY(ahrs_aligner.lp_accel, imu.accel); VECT3_COPY(ahrs_aligner.lp_mag, imu.mag); RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro); // DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro); ahrs_align(); // DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias); #endif #ifdef DISABLE_ALIGNEMENT printf("# DISABLE_ALIGNEMENT\n"); #endif #ifdef DISABLE_PROPAGATE printf("# DISABLE_PROPAGATE\n"); #endif #ifdef DISABLE_ACCEL_UPDATE printf("# DISABLE_ACCEL_UPDATE\n"); #endif #ifdef DISABLE_MAG_UPDATE printf("# DISABLE_MAG_UPDATE\n"); #endif printf("# AHRS_TYPE %s\n", ahrs_type_str[AHRS_TYPE]); printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY); #ifdef AHRS_PROPAGATE_LOW_PASS_RATES printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n"); #endif #if AHRS_MAG_UPDATE_YAW_ONLY printf("# AHRS_MAG_UPDATE_YAW_ONLY\n"); #endif #if AHRS_GRAVITY_UPDATE_COORDINATED_TURN printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n"); #endif #if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n"); #endif #ifdef PERFECT_SENSORS printf("# PERFECT_SENSORS\n"); #endif #if AHRS_USE_GPS_HEADING printf("# AHRS_USE_GPS_HEADING\n"); #endif #if USE_AHRS_GPS_ACCELERATIONS printf("# USE_AHRS_GPS_ACCELERATIONS\n"); #endif printf("# tajectory : %s\n", aos.traj->name); };