Browse Source

AP_NavEKF3: Fix CI build errors

c415-sdk
Paul Riseborough 4 years ago committed by Andrew Tridgell
parent
commit
5da62aeaa2
  1. 30
      libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp

30
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp

@ -501,31 +501,27 @@ void NavEKF3_core::FuseDragForces() @@ -501,31 +501,27 @@ void NavEKF3_core::FuseDragForces()
// predicted sign of drag force
const float dragForceSign = is_positive(rel_wind_body[axis_index]) ? -1.0f : 1.0f;
float airSpd; // Airspeed estimated using drag model and used for linearisation point
float Kacc; // Derivative of specific force wrt airspeed
if (axis_index == 0) {
if (!using_bcoef_x && !using_mcoef) {
// skip this axis
continue;
}
// drag can be modelled as an arbitrary combination of bluff body drag that proportional to
// speed squared, and rotor momentum drag that is proportional to speed.
float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_x) {
// mixed bluff body and propeller momentum drag
airSpd = (bcoef.x / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef.x) * fabsf(mea_acc)));
const float airSpd = (bcoef.x / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef.x) * fabsf(mea_acc)));
Kacc = fmaxf(1e-1f, (rho / bcoef.x) * airSpd + mcoef * density_ratio);
predAccel = (0.5f / bcoef[0]) * rho * sq(rel_wind_body[0]) * dragForceSign - rel_wind_body[0] * mcoef * density_ratio;
} else if (using_mcoef) {
// propeller momentum drag only
airSpd = fabsf(mea_acc) / mcoef;
Kacc = fmaxf(1e-1f, mcoef * density_ratio);
predAccel = - rel_wind_body[0] * mcoef * density_ratio;
} else if (using_bcoef_x) {
// bluff body drag only
airSpd = sqrtf((2.0f * bcoef.x * fabsf(mea_acc)) / rho);
const float airSpd = sqrtf((2.0f * bcoef.x * fabsf(mea_acc)) / rho);
Kacc = fmaxf(1e-1f, (rho / bcoef.x) * airSpd);
predAccel = (0.5f / bcoef[0]) * rho * sq(rel_wind_body[0]) * dragForceSign;
} else {
// skip this axis
continue;
}
// intermediate variables
@ -610,28 +606,26 @@ void NavEKF3_core::FuseDragForces() @@ -610,28 +606,26 @@ void NavEKF3_core::FuseDragForces()
} else if (axis_index == 1) {
if (!using_bcoef_y && !using_mcoef) {
// skip this axis
continue;
}
// drag can be modelled as an arbitrary combination of bluff body drag that proportional to
// speed squared, and rotor momentum drag that is proportional to speed.
float Kacc; // Derivative of specific force wrt airspeed
if (using_mcoef && using_bcoef_y) {
// mixed bluff body and propeller momentum drag
airSpd = (bcoef.y / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef.y) * fabsf(mea_acc)));
const float airSpd = (bcoef.y / rho) * (- mcoef + sqrtf(sq(mcoef) + 2.0f * (rho / bcoef.y) * fabsf(mea_acc)));
Kacc = fmaxf(1e-1f, (rho / bcoef.y) * airSpd + mcoef * density_ratio);
predAccel = (0.5f / bcoef[1]) * rho * sq(rel_wind_body[1]) * dragForceSign - rel_wind_body[1] * mcoef * density_ratio;
} else if (using_mcoef) {
// propeller momentum drag only
airSpd = fabsf(mea_acc) / mcoef;
Kacc = fmaxf(1e-1f, mcoef * density_ratio);
predAccel = - rel_wind_body[1] * mcoef * density_ratio;
} else if (using_bcoef_y) {
// bluff body drag only
airSpd = sqrtf((2.0f * bcoef.y * fabsf(mea_acc)) / rho);
const float airSpd = sqrtf((2.0f * bcoef.y * fabsf(mea_acc)) / rho);
Kacc = fmaxf(1e-1f, (rho / bcoef.y) * airSpd);
predAccel = (0.5f / bcoef[1]) * rho * sq(rel_wind_body[1]) * dragForceSign;
} else {
// nothing more to do
return;
}
// intermediate variables

Loading…
Cancel
Save