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);
};
