Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: Use SSL variables 

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
This commit is contained in:
Patrick José Pereira 2018-05-10 23:27:48 -03:00 committed by Francisco Ferreira
parent 237101a50e
commit b014b6c857
4 changed files with 11 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Baro/AP_Baro.cpp
View File

@ -297,7 +297,7 @@ float AP_Baro::get_EAS2TAS(void)
// provides a more consistent reading then trying to estimate a complete
// ISA model atmosphere
float tempK = get_ground_temperature() + C_TO_KELVIN - ISA_LAPSE_RATE * altitude;
_EAS2TAS = safe_sqrt(AIR_DENSITY_SEA_LEVEL / ((float)get_pressure() / (ISA_GAS_CONSTANT * tempK)));
_EAS2TAS = safe_sqrt(SSL_AIR_DENSITY / ((float)get_pressure() / (ISA_GAS_CONSTANT * tempK)));
_last_altitude_EAS2TAS = altitude;
return _EAS2TAS;
}

14
libraries/AP_Baro/AP_Baro_HIL.cpp
View File

@ -29,12 +29,12 @@ void AP_Baro::SimpleAtmosphere(
if (h < 11.0f) {
// Troposphere
theta=(288.15f-6.5f*h)/288.15f;
delta=powf(theta, GMR/6.5f);
theta = (SSL_AIR_TEMPERATURE - 6.5f * h) / SSL_AIR_TEMPERATURE;
delta = powf(theta, GMR / 6.5f);
} else {
// Stratosphere
theta=216.65f/288.15f;
delta=0.2233611f*expf(-GMR*(h-11.0f)/216.65f);
theta = 216.65f / SSL_AIR_TEMPERATURE;
delta = 0.2233611f * expf(-GMR * (h - 11.0f) / 216.65f);
}
sigma = delta/theta;
@ -66,8 +66,7 @@ void AP_Baro::SimpleUnderWaterAtmosphere(
// \f$P = \rho (kg) \cdot gravity (m/s2) \cdot depth (m)\f$
// \f$P_{atmosphere} = 101.325 kPa\f$
// \f$P_{total} = P_{atmosphere} + P_{fluid}\f$
const float pAtm = 101325; // Pa
delta = (pAtm + (seaDensity * 1e3) * GRAVITY_MSS * (alt * 1e3)) / pAtm;
delta = (SSL_AIR_PRESSURE + (seaDensity * 1e3) * GRAVITY_MSS * (alt * 1e3)) / SSL_AIR_PRESSURE;
// From: http://residualanalysis.blogspot.com.br/2010/02/temperature-of-ocean-water-at-given.html
// \f$T(D)\f$ Temperature underwater at given temperature
@ -84,10 +83,9 @@ void AP_Baro::SimpleUnderWaterAtmosphere(
void AP_Baro::setHIL(float altitude_msl)
{
float sigma, delta, theta;
const float p0 = 101325;
SimpleAtmosphere(altitude_msl*0.001f, sigma, delta, theta);
float p = p0 * delta;
float p = SSL_AIR_PRESSURE * delta;
float T = 303.16f * theta - C_TO_KELVIN; // Assume 30 degrees at sea level - converted to degrees Kelvin
_hil.pressure = p;

6
libraries/AP_Baro/AP_Baro_SITL.cpp
View File

@ -98,20 +98,18 @@ void AP_Baro_SITL::_timer()
sim_alt = _buffer[best_index].data;
}
const float p0 = 101325.0f;
#if !APM_BUILD_TYPE(APM_BUILD_ArduSub)
float sigma, delta, theta;
AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta);
float p = p0 * delta;
float p = SSL_AIR_PRESSURE * delta;
float T = 303.16f * theta - C_TO_KELVIN; // Assume 30 degrees at sea level - converted to degrees Kelvin
temperature_adjustment(p, T);
#else
float rho, delta, theta;
AP_Baro::SimpleUnderWaterAtmosphere(-sim_alt * 0.001f, rho, delta, theta);
float p = p0 * delta;
float p = SSL_AIR_PRESSURE * delta;
float T = 303.16f * theta - C_TO_KELVIN; // Assume 30 degrees at sea level - converted to degrees Kelvin
#endif

4
libraries/AP_HAL_SITL/sitl_airspeed.cpp
View File

@ -41,13 +41,13 @@ void SITL_State::_update_airspeed(float airspeed)
// compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude
// algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure
float tube_pressure = abs(_sitl->arspd_fail_pressure - _barometer->get_pressure() + _sitl->arspd_fail_pitot_pressure);
airspeed = 340.29409348 * sqrt(5 * (pow((tube_pressure / 101325.01576 + 1), 2.0/7.0) - 1.0));
airspeed = 340.29409348 * sqrt(5 * (pow((tube_pressure / SSL_AIR_PRESSURE + 1), 2.0/7.0) - 1.0));
}
if (!is_zero(_sitl->arspd2_fail_pressure)) {
// compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude
// algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure
float tube_pressure = abs(_sitl->arspd2_fail_pressure - _barometer->get_pressure() + _sitl->arspd2_fail_pitot_pressure);
airspeed2 = 340.29409348 * sqrt(5 * (pow((tube_pressure / 101325.01576 + 1), 2.0/7.0) - 1.0));
airspeed2 = 340.29409348 * sqrt(5 * (pow((tube_pressure / SSL_AIR_PRESSURE + 1), 2.0/7.0) - 1.0));
}
float airspeed_pressure = (airspeed * airspeed) / airspeed_ratio;