Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 17:03:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: fix spd3x correct pressure issue

Browse Source
This commit is contained in:
liang.tang 2018-04-24 19:52:44 -07:00 committed by WickedShell
parent 774b091611
commit f4a8006333
1 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp
View File

@ -204,28 +204,28 @@ float AP_Airspeed_SDP3X::_correct_pressure(float press)
return press; return press;
} }
float sign = 1; float sign = 1.0f;
// fix for tube order // fix for tube order
AP_Airspeed::pitot_tube_order tube_order = get_tube_order(); AP_Airspeed::pitot_tube_order tube_order = get_tube_order();
switch (tube_order) { switch (tube_order) {
case AP_Airspeed::PITOT_TUBE_ORDER_NEGATIVE: case AP_Airspeed::PITOT_TUBE_ORDER_NEGATIVE:
press = -press; press = -press;
sign = -1; sign = -1.0f;
//FALLTHROUGH; //FALLTHROUGH;
case AP_Airspeed::PITOT_TUBE_ORDER_POSITIVE: case AP_Airspeed::PITOT_TUBE_ORDER_POSITIVE:
break; break;
case AP_Airspeed::PITOT_TUBE_ORDER_AUTO: case AP_Airspeed::PITOT_TUBE_ORDER_AUTO:
default: default:
if (press < 0) { if (press < 0.0f) {
sign = -1; sign = -1.0f;
press = -press; press = -press;
} }
break; break;
} }
if (press <= 0) { if (press <= 0.0f) {
return 0; return 0.0f;
} }
get_temperature(temperature); get_temperature(temperature);
@ -246,19 +246,19 @@ float AP_Airspeed_SDP3X::_correct_pressure(float press)
*/ */
// flow through sensor // flow through sensor
float flow_SDP3X = (300.805f - 300.878f / (0.00344205f * (float)powf(press, 0.68698f) + 1)) * 1.29f / rho_air; float flow_SDP3X = (300.805f - 300.878f / (0.00344205f * (float)powf(press, 0.68698f) + 1.0f)) * 1.29f / rho_air;
if (flow_SDP3X < 0.0f) { if (flow_SDP3X < 0.0f) {
flow_SDP3X = 0.0f; flow_SDP3X = 0.0f;
} }
// diffential pressure through pitot tube // diffential pressure through pitot tube
float dp_pitot = 28557670.0f - 28557670.0f / (1 + (float)powf((flow_SDP3X / 5027611.0f), 1.227924f)); float dp_pitot = 28557670.0f * (1.0f - 1.0f / (1.0f + (float)powf((flow_SDP3X / 5027611.0f), 1.227924f)));
// uncorrected pressure // uncorrected pressure
float press_uncorrected = (press + dp_pitot) / SSL_AIR_DENSITY; float press_uncorrected = (press + dp_pitot) / SSL_AIR_DENSITY;
// correction for speed at pitot-tube tip due to flow through sensor // correction for speed at pitot-tube tip due to flow through sensor
float dv = 0.0331582 * flow_SDP3X; float dv = 0.0331582f * flow_SDP3X;
// airspeed ratio // airspeed ratio
float ratio = get_airspeed_ratio(); float ratio = get_airspeed_ratio();