fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 17:04:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

SDP3x pitot compensation: fixed the compensation and protect agains negative compensation values

Browse Source
This commit is contained in:
Andreas Antener 2017-08-02 20:19:28 +02:00 committed by Lorenz Meier
parent b59aefc989
commit 4ea49242d3
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/systemlib/airspeed.c
View File

@ -78,12 +78,17 @@ float calc_indicated_airspeed_corrected(enum AIRSPEED_PITOT_MODEL pmodel, enum A
break;
case AIRSPEED_SENSOR_MODEL_SDP3X: {
//
double flow_SDP33 = (300.805 - 300.878 / (0.00344205 * dp * 0.68698 * 0.68698 + 1)) * 1.29 / rho_air;
// flow through sensor
double flow_SDP33 = (300.805 - 300.878 / (0.00344205 * pow(dp, 0.68698) + 1)) * 1.29 / rho_air;
// for too small readings the compensation might result in a negative flow which causes numerical issues
if (flow_SDP33 < 0.0) {
flow_SDP33 = 0.0;
}
switch (pmodel) {
case AIRSPEED_PITOT_MODEL_HB:
dp_pitot = 28557670.0 - 28557670.0 / (1 + (flow_SDP33 / 5027611.0) * 1.227924 * 1.227924);
dp_pitot = 28557670.0 - 28557670.0 / (1 + pow((flow_SDP33 / 5027611.0), 1.227924));
break;
default: