rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-22 14:19:42 +01:00
Code Issues Releases Activity

LP-443 bugfixes in insgps14state - prevent NaN propagation from bug in magnetometer handling, fix missing dimension in array

Browse Source
This commit is contained in:
Eric Price 2017-03-02 09:51:27 +01:00
parent 74bd1589e8
commit 3b65559904
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/libraries/insgps14state.c
View File

@ -76,7 +76,7 @@ static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
// 7.....oXoXXXXX. XXX....... ......XXXX....
// 8.....oXXoXXXX. XXX....... ......XXXX....
// 9.....oXXXoXXX. XXX....... ..X...........
// a.............. .......... ..............
// a.............. ..........
// b.............. ..........
// c.............. ..........
// d.............. ..........
@ -84,8 +84,8 @@ static void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
static int8_t FrowMin[NUMX] = { 3, 4, 5, 6, 6, 6, 5, 5, 5, 5, 14, 14, 14, 14 };
static int8_t FrowMax[NUMX] = { 3, 4, 5, 13, 13, 13, 12, 12, 12, 12, -1, -1, -1, -1 };
static int8_t GrowMin[NUMX] = { 10, 10, 10, 3, 3, 3, 0, 0, 0, 0, 10, 10, 10 };
static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, -1, -1, -1 };
static int8_t GrowMin[NUMX] = { 10, 10, 10, 3, 3, 3, 0, 0, 0, 0, 10, 10, 10, 10 };
static int8_t GrowMax[NUMX] = { -1, -1, -1, 5, 5, 5, 2, 2, 2, 2, -1, -1, -1, -1 };
static int8_t HrowMin[NUMV] = { 0, 1, 2, 3, 4, 5, 6, 6, 6, 2 };
static int8_t HrowMax[NUMV] = { 0, 1, 2, 3, 4, 5, 9, 9, 9, 2 };
@ -461,6 +461,9 @@ void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[
Z[6] = Rbe_a[0][0] * mag_data[0] + Rbe_a[1][0] * mag_data[1] + Rbe_a[2][0] * mag_data[2];
Z[7] = Rbe_a[0][1] * mag_data[0] + Rbe_a[1][1] * mag_data[1] + Rbe_a[2][1] * mag_data[2];
Z[8] = Rbe_a[0][2] * mag_data[0] + Rbe_a[1][2] * mag_data[1] + Rbe_a[2][2] * mag_data[2];
if (!(IS_REAL(Z[6]) && IS_REAL(Z[7]) && IS_REAL(Z[8]))) {
SensorsUsed = SensorsUsed & ~MAG_SENSORS;
}
}
// barometric altimeter in meters and in local NED frame