rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08:24:11 +01:00
Code Issues Releases Activity

Make mag updated flag always true when it's updated. Responsibility of INS

Browse Source 
algorithm whether to use data.
This commit is contained in:
James Cotton 2011-08-22 03:05:02 -05:00
parent 4e59e697af
commit 1477ea6e94
2 changed files with 5 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/INS/ins.c
View File

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

12
flight/INS/insgps_helper.c
View File

@ -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)