fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 00:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

PX4 style fixes (excluding EKF generated code)

Browse Source
This commit is contained in:
Daniel Agar
2018-05-03 22:25:36 -04:00
parent e5952fadaf
commit c5b2f59f57
22 changed files with 384 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/mag_fusion.cpp
+23 -6
View File
@@ -146,11 +146,14 @@ void Ekf::fuseMag()
// Perform an innovation consistency check and report the result
bool healthy = true;
for (uint8_t index = 0; index <= 2; index++) {
_mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]);
if (_mag_test_ratio[index] > 1.0f) {
healthy = false;
_innov_check_fail_status.value |= (1 << (index + 3));
} else {
_innov_check_fail_status.value &= ~(1 << (index + 3));
}
@@ -213,6 +216,7 @@ void Ekf::fuseMag()
for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 0.0f;
}
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
@@ -267,6 +271,7 @@ void Ekf::fuseMag()
for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 0.0f;
}
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
@@ -321,6 +326,7 @@ void Ekf::fuseMag()
for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 0.0f;
}
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
@@ -341,6 +347,7 @@ void Ekf::fuseMag()
// then calculate P - KHP
float KHP[_k_num_states][_k_num_states];
float KH[10];
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_MAG[0];
@@ -374,11 +381,12 @@ void Ekf::fuseMag()
_fault_status.flags.bad_mag_x = false;
_fault_status.flags.bad_mag_y = false;
_fault_status.flags.bad_mag_z = false;
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);
//flag as unhealthy
healthy = false;
@@ -386,8 +394,10 @@ void Ekf::fuseMag()
// update individual measurement health status
if (index == 0) {
_fault_status.flags.bad_mag_x = true;
} else if (index == 1) {
_fault_status.flags.bad_mag_y = true;
} else if (index == 2) {
_fault_status.flags.bad_mag_z = true;
}
@@ -473,6 +483,7 @@ void Ekf::fuseHeading()
if (_control_status.flags.mag_3D) {
// don't apply bias corrections if we are learning them
mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
} else {
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
}
@@ -512,6 +523,7 @@ void Ekf::fuseHeading()
float t12 = t8*t11*4.0f;
float t13 = t12+1.0f;
float t14;
if (fabsf(t13) > 1e-6f) {
t14 = 1.0f/t13;
} else {
@@ -590,9 +602,11 @@ void Ekf::fuseHeading()
if (_control_status.flags.mag_hdg) {
// using magnetic heading tuning parameter
R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
} else if (_control_status.flags.ev_yaw) {
// using error estimate from external vision data
R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
} else {
// there is no yaw observation
return;
@@ -602,6 +616,7 @@ void Ekf::fuseHeading()
// calculate the innovaton variance
float PH[4];
_heading_innov_var = R_YAW;
for (unsigned row = 0; row <= 3; row++) {
PH[row] = 0.0f;
@@ -696,6 +711,7 @@ void Ekf::fuseHeading()
// then calculate P - KHP
float KHP[_k_num_states][_k_num_states];
float KH[4];
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_YAW[0];
@@ -716,11 +732,12 @@ void Ekf::fuseHeading()
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
_fault_status.flags.bad_mag_hdg = false;
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);
//flag as unhealthy
healthy = false;
@@ -853,8 +870,8 @@ void Ekf::fuseDeclination()
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);
//flag as unhealthy
healthy = false;