From 1477ea6e94bca019f021eb72b8151b45a9344653 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 22 Aug 2011 03:05:02 -0500 Subject: [PATCH] Make mag updated flag always true when it's updated. Responsibility of INS algorithm whether to use data. --- flight/INS/ins.c | 8 +------- flight/INS/insgps_helper.c | 12 ++++-------- 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/flight/INS/ins.c b/flight/INS/ins.c index 4180f1b46..b083b14d5 100644 --- a/flight/INS/ins.c +++ b/flight/INS/ins.c @@ -444,13 +444,7 @@ void get_mag_data() mag_data.scaled.axis[1] = -(mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1]; mag_data.scaled.axis[2] = -(mag_data.raw.axis[1] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2]; - // Only use if magnetic length reasonable - float Blen = sqrt(pow(mag_data.scaled.axis[0],2) + pow(mag_data.scaled.axis[1],2) + pow(mag_data.scaled.axis[2],2)); - - mag_data.updated = (home.Set == HOMELOCATION_SET_TRUE) && - ((home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0)) && - ((mag_data.raw.axis[0] != 0) || (mag_data.raw.axis[1] != 0) || (mag_data.raw.axis[2] != 0)) && - ((Blen < mag_len * (1 + INSGPS_MAGTOL)) && (Blen > mag_len * (1 - INSGPS_MAGTOL))); + mag_data.updated = true; } } diff --git a/flight/INS/insgps_helper.c b/flight/INS/insgps_helper.c index 4960fc36a..fe19c9233 100644 --- a/flight/INS/insgps_helper.c +++ b/flight/INS/insgps_helper.c @@ -42,6 +42,7 @@ extern void send_velocity(void); extern void send_position(void); extern volatile int8_t ahrs_algorithm; extern void get_accel_gyro_data(); +extern void get_mag_data(); static uint32_t ins_last_time; @@ -257,19 +258,14 @@ void ins_init_algorithm() /* Ensure we get mag data in a timely manner */ uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec while(using_mags && !mag_data.updated && fail_count--) { + get_mag_data(); get_accel_gyro_data(); AhrsPoll(); + PIOS_DELAY_WaituS(2000); } using_mags &= mag_data.updated; - if (using_mags) { - /* Spin waiting for mag data */ - while(!mag_data.updated) { - get_accel_gyro_data(); - AhrsPoll(); - } - mag_data.updated = false; - + if (using_mags) { RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe); R2Quaternion(Rbe,q); if (using_gps)