mirror of https://github.com/ArduPilot/ardupilot
AP_Baro: Use SSL variables
Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
This commit is contained in:
parent
237101a50e
commit
b014b6c857
|
@ -297,7 +297,7 @@ float AP_Baro::get_EAS2TAS(void)
|
||||||
// provides a more consistent reading then trying to estimate a complete
|
// provides a more consistent reading then trying to estimate a complete
|
||||||
// ISA model atmosphere
|
// ISA model atmosphere
|
||||||
float tempK = get_ground_temperature() + C_TO_KELVIN - ISA_LAPSE_RATE * altitude;
|
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;
|
_last_altitude_EAS2TAS = altitude;
|
||||||
return _EAS2TAS;
|
return _EAS2TAS;
|
||||||
}
|
}
|
||||||
|
|
|
@ -29,12 +29,12 @@ void AP_Baro::SimpleAtmosphere(
|
||||||
|
|
||||||
if (h < 11.0f) {
|
if (h < 11.0f) {
|
||||||
// Troposphere
|
// Troposphere
|
||||||
theta=(288.15f-6.5f*h)/288.15f;
|
theta = (SSL_AIR_TEMPERATURE - 6.5f * h) / SSL_AIR_TEMPERATURE;
|
||||||
delta=powf(theta, GMR/6.5f);
|
delta = powf(theta, GMR / 6.5f);
|
||||||
} else {
|
} else {
|
||||||
// Stratosphere
|
// Stratosphere
|
||||||
theta=216.65f/288.15f;
|
theta = 216.65f / SSL_AIR_TEMPERATURE;
|
||||||
delta=0.2233611f*expf(-GMR*(h-11.0f)/216.65f);
|
delta = 0.2233611f * expf(-GMR * (h - 11.0f) / 216.65f);
|
||||||
}
|
}
|
||||||
|
|
||||||
sigma = delta/theta;
|
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 = \rho (kg) \cdot gravity (m/s2) \cdot depth (m)\f$
|
||||||
// \f$P_{atmosphere} = 101.325 kPa\f$
|
// \f$P_{atmosphere} = 101.325 kPa\f$
|
||||||
// \f$P_{total} = P_{atmosphere} + P_{fluid}\f$
|
// \f$P_{total} = P_{atmosphere} + P_{fluid}\f$
|
||||||
const float pAtm = 101325; // Pa
|
delta = (SSL_AIR_PRESSURE + (seaDensity * 1e3) * GRAVITY_MSS * (alt * 1e3)) / SSL_AIR_PRESSURE;
|
||||||
delta = (pAtm + (seaDensity * 1e3) * GRAVITY_MSS * (alt * 1e3)) / pAtm;
|
|
||||||
|
|
||||||
// From: http://residualanalysis.blogspot.com.br/2010/02/temperature-of-ocean-water-at-given.html
|
// From: http://residualanalysis.blogspot.com.br/2010/02/temperature-of-ocean-water-at-given.html
|
||||||
// \f$T(D)\f$ Temperature underwater at given temperature
|
// \f$T(D)\f$ Temperature underwater at given temperature
|
||||||
|
@ -84,10 +83,9 @@ void AP_Baro::SimpleUnderWaterAtmosphere(
|
||||||
void AP_Baro::setHIL(float altitude_msl)
|
void AP_Baro::setHIL(float altitude_msl)
|
||||||
{
|
{
|
||||||
float sigma, delta, theta;
|
float sigma, delta, theta;
|
||||||
const float p0 = 101325;
|
|
||||||
|
|
||||||
SimpleAtmosphere(altitude_msl*0.001f, sigma, delta, theta);
|
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
|
float T = 303.16f * theta - C_TO_KELVIN; // Assume 30 degrees at sea level - converted to degrees Kelvin
|
||||||
|
|
||||||
_hil.pressure = p;
|
_hil.pressure = p;
|
||||||
|
|
|
@ -98,20 +98,18 @@ void AP_Baro_SITL::_timer()
|
||||||
sim_alt = _buffer[best_index].data;
|
sim_alt = _buffer[best_index].data;
|
||||||
}
|
}
|
||||||
|
|
||||||
const float p0 = 101325.0f;
|
|
||||||
|
|
||||||
#if !APM_BUILD_TYPE(APM_BUILD_ArduSub)
|
#if !APM_BUILD_TYPE(APM_BUILD_ArduSub)
|
||||||
float sigma, delta, theta;
|
float sigma, delta, theta;
|
||||||
|
|
||||||
AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, 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
|
float T = 303.16f * theta - C_TO_KELVIN; // Assume 30 degrees at sea level - converted to degrees Kelvin
|
||||||
|
|
||||||
temperature_adjustment(p, T);
|
temperature_adjustment(p, T);
|
||||||
#else
|
#else
|
||||||
float rho, delta, theta;
|
float rho, delta, theta;
|
||||||
AP_Baro::SimpleUnderWaterAtmosphere(-sim_alt * 0.001f, 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
|
float T = 303.16f * theta - C_TO_KELVIN; // Assume 30 degrees at sea level - converted to degrees Kelvin
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -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
|
// 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
|
// 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);
|
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)) {
|
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
|
// 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
|
// 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);
|
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;
|
float airspeed_pressure = (airspeed * airspeed) / airspeed_ratio;
|
||||||
|
|
Loading…
Reference in New Issue