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

ekf2: backend apply code style (generated code still exempt)

Browse Source
This commit is contained in:
Daniel Agar
2021-07-15 21:15:41 -04:00
parent 12ad7b17ce
commit c8e6d93cc0
17 changed files with 387 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/drag_fusion.cpp
+11
View File
@@ -98,8 +98,10 @@ void Ekf::fuseDrag()
// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed
// parallel to the rotor disc and mass flow through the rotor disc.
float pred_acc = 0.0f; // predicted drag acceleration
if (axis_index == 0) {
float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_x) {
// Use a combination of bluff body and propeller momentum drag
const float bcoef_inv = 1.0f / _params.bcoef_x;
@@ -108,10 +110,12 @@ void Ekf::fuseDrag()
const float airspeed = (_params.bcoef_x / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign - rel_wind_body(0) * mcoef_corrrected;
} else if (using_mcoef) {
// Use propeller momentum drag only
Kacc = fmaxf(1e-1f, mcoef_corrrected);
pred_acc = - rel_wind_body(0) * mcoef_corrrected;
} else if (using_bcoef_x) {
// Use bluff body drag only
// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
@@ -120,6 +124,7 @@ void Ekf::fuseDrag()
const float bcoef_inv = 1.0f / _params.bcoef_x;
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(0)) * drag_sign;
} else {
// skip this axis
continue;
@@ -165,6 +170,7 @@ void Ekf::fuseDrag()
if (_drag_innov_var(0) < R_ACC) {
return;
}
const float HK32 = Kacc / _drag_innov_var(0);
// Observation Jacobians
@@ -208,6 +214,7 @@ void Ekf::fuseDrag()
} else if (axis_index == 1) {
float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_y) {
// Use a combination of bluff body and propeller momentum drag
const float bcoef_inv = 1.0f / _params.bcoef_y;
@@ -216,10 +223,12 @@ void Ekf::fuseDrag()
const float airspeed = (_params.bcoef_y / rho) * (- mcoef_corrrected + sqrtf(sq(mcoef_corrrected) + 2.0f * rho * bcoef_inv * fabsf(mea_acc)));
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed + mcoef_corrrected);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign - rel_wind_body(1) * mcoef_corrrected;
} else if (using_mcoef) {
// Use propeller momentum drag only
Kacc = fmaxf(1e-1f, mcoef_corrrected);
pred_acc = - rel_wind_body(1) * mcoef_corrrected;
} else if (using_bcoef_y) {
// Use bluff body drag only
// The airspeed used for linearisation is calculated from the measured acceleration by solving the following quadratic
@@ -228,6 +237,7 @@ void Ekf::fuseDrag()
const float bcoef_inv = 1.0f / _params.bcoef_y;
Kacc = fmaxf(1e-1f, rho * bcoef_inv * airspeed);
pred_acc = 0.5f * bcoef_inv * rho * sq(rel_wind_body(1)) * drag_sign;
} else {
// nothing more to do
return;
@@ -273,6 +283,7 @@ void Ekf::fuseDrag()
// calculation is badly conditioned
return;
}
const float HK32 = Kacc / _drag_innov_var(1);
// Observation Jacobians