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

AP_Baro: Limit ground temperature used for the altitude 

Fix an incorrect EAS2TAS that was double counting altitude
This commit is contained in:
Michael du Breuil 2017-03-23 21:58:53 -07:00 committed by Andrew Tridgell
parent 495358edbb
commit c37209a8d5
2 changed files with 57 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
libraries/AP_Baro/AP_Baro.cpp
View File

@ -35,6 +35,14 @@
#include "AP_Baro_qflight.h"
#include "AP_Baro_QURT.h"
#define C_TO_KELVIN 273.15f
// Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers
#define ISA_GAS_CONSTANT 287.26f
#define ISA_LAPSE_RATE 0.0065f
#define INTERNAL_TEMPERATURE_CLAMP 35.0f
extern const AP_HAL::HAL& hal;
// table of user settable parameters
@ -60,7 +68,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
// @ReadOnly: True
// @Volatile: True
// @User: Advanced
AP_GROUPINFO("TEMP", 3, AP_Baro, sensors[0].ground_temperature, 0),
AP_GROUPINFO("TEMP", 3, AP_Baro, _user_ground_temperature, 0),
// index 4 reserved for old AP_Int8 version in legacy FRAM
//AP_GROUPINFO("ALT_OFFSET", 4, AP_Baro, _alt_offset, 0),
@ -104,15 +112,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
// @User: Advanced
AP_GROUPINFO("ABS_PRESS2", 9, AP_Baro, sensors[1].ground_pressure, 0),
// @Param: TEMP2
// @DisplayName: ground temperature
// @Description: calibrated ground temperature in degrees Celsius
// @Units: degrees celsius
// @Increment: 1
// @ReadOnly: True
// @Volatile: True
// @User: Advanced
AP_GROUPINFO("TEMP2", 10, AP_Baro, sensors[1].ground_temperature, 0),
// Slot 10 used to be TEMP2
#endif
#if BARO_MAX_INSTANCES > 2
@ -126,15 +126,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
// @User: Advanced
AP_GROUPINFO("ABS_PRESS3", 11, AP_Baro, sensors[2].ground_pressure, 0),
// @Param: TEMP3
// @DisplayName: ground temperature
// @Description: calibrated ground temperature in degrees Celsius
// @Units: degrees celsius
// @Increment: 1
// @ReadOnly: True
// @Volatile: True
// @User: Advanced
AP_GROUPINFO("TEMP3", 12, AP_Baro, sensors[2].ground_temperature, 0),
// Slot 12 used to be TEMP3
#endif
AP_GROUPEND
@ -209,11 +201,12 @@ void AP_Baro::calibrate(bool save)
} else {
if (save) {
sensors[i].ground_pressure.set_and_save(sum_pressure[i] / count[i]);
sensors[i].ground_temperature.set_and_save(sum_temperature[i] / count[i]);
}
}
}
_guessed_ground_temperature = get_external_temperature();
// panic if all sensors are not calibrated
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].calibrated) {
@ -234,22 +227,20 @@ void AP_Baro::update_calibration()
if (healthy(i)) {
sensors[i].ground_pressure.set(get_pressure(i));
}
float last_temperature = sensors[i].ground_temperature;
sensors[i].ground_temperature.set(get_calibration_temperature(i));
// don't notify the GCS too rapidly or we flood the link
uint32_t now = AP_HAL::millis();
if (now - _last_notify_ms > 10000) {
sensors[i].ground_pressure.notify();
sensors[i].ground_temperature.notify();
_last_notify_ms = now;
}
if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) {
// reset _EAS2TAS to force it to recalculate. This happens
// when a digital airspeed sensor comes online
_EAS2TAS = 0;
}
}
// always update the guessed ground temp
_guessed_ground_temperature = get_external_temperature();
// force EAS2TAS to recalculate
_EAS2TAS = 0;
}
// return altitude difference in meters between current pressure and a
@ -257,7 +248,7 @@ void AP_Baro::update_calibration()
float AP_Baro::get_altitude_difference(float base_pressure, float pressure) const
{
float ret;
float temp = get_ground_temperature() + 273.15f;
float temp = get_ground_temperature() + C_TO_KELVIN;
float scaling = pressure / base_pressure;
// This is an exact calculation that is within +-2.5m of the standard
@ -274,13 +265,16 @@ float AP_Baro::get_altitude_difference(float base_pressure, float pressure) cons
float AP_Baro::get_EAS2TAS(void)
{
float altitude = get_altitude();
if ((fabsf(altitude - _last_altitude_EAS2TAS) < 100.0f) && !is_zero(_EAS2TAS)) {
if ((fabsf(altitude - _last_altitude_EAS2TAS) < 25.0f) && !is_zero(_EAS2TAS)) {
// not enough change to require re-calculating
return _EAS2TAS;
}
float tempK = get_calibration_temperature() + 273.15f - 0.0065f * altitude;
_EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (287.26f * tempK)));
// only estimate lapse rate for the difference from the ground location
// 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(1.225f / ((float)get_pressure() / (ISA_GAS_CONSTANT * tempK)));
_last_altitude_EAS2TAS = altitude;
return _EAS2TAS;
}
@ -288,9 +282,9 @@ float AP_Baro::get_EAS2TAS(void)
// return air density / sea level density - decreases as altitude climbs
float AP_Baro::get_air_density_ratio(void)
{
float eas2tas = get_EAS2TAS();
const float eas2tas = get_EAS2TAS();
if (eas2tas > 0.0f) {
return 1.0f/(sq(get_EAS2TAS()));
return 1.0f/(sq(eas2tas));
} else {
return 1.0f;
}
@ -309,6 +303,17 @@ float AP_Baro::get_climb_rate(void)
return _climb_rate_filter.slope() * 1.0e3f;
}
// returns the ground temperature in degrees C, selecting either a user
// provided one, or the internal estimate
float AP_Baro::get_ground_temperature(void) const
{
if (is_zero(_user_ground_temperature)) {
return _guessed_ground_temperature;
} else {
return _user_ground_temperature;
}
}
/*
set external temperature to be used for calibration (degrees C)
@ -322,22 +327,21 @@ void AP_Baro::set_external_temperature(float temperature)
/*
get the temperature in degrees C to be used for calibration purposes
*/
float AP_Baro::get_calibration_temperature(uint8_t instance) const
float AP_Baro::get_external_temperature(const uint8_t instance) const
{
// if we have a recent external temperature then use it
if (_last_external_temperature_ms != 0 && AP_HAL::millis() - _last_external_temperature_ms < 10000) {
return _external_temperature;
}
// if we don't have an external temperature then use the minimum
// of the barometer temperature and 25 degrees C. The reason for
// of the barometer temperature and 35 degrees C. The reason for
// not just using the baro temperature is it tends to read high,
// often 30 degrees above the actual temperature. That means the
// EAS2TAS tends to be off by quite a large margin
float ret = get_temperature(instance);
if (ret > 25) {
ret = 25;
}
return ret;
// EAS2TAS tends to be off by quite a large margin, as well as
// the calculation of altitude difference betweeen two pressures
// reporting a high temperature will cause the aircraft to
// estimate itself as flying higher then it actually is.
return MIN(get_temperature(instance), INTERNAL_TEMPERATURE_CLAMP);
}
@ -370,6 +374,12 @@ bool AP_Baro::_add_backend(AP_Baro_Backend *backend)
*/
void AP_Baro::init(void)
{
// ensure that there isn't a previous ground temperature saved
if (!is_zero(_user_ground_temperature)) {
_user_ground_temperature.set_and_save(0.0f);
_user_ground_temperature.notify();
}
if (_hil_mode) {
drivers[0] = new AP_Baro_HIL(*this);
_num_drivers = 1;

10
libraries/AP_Baro/AP_Baro.h
View File

@ -83,8 +83,7 @@ public:
// ground temperature in degrees C
// the ground values are only valid after calibration
float get_ground_temperature(void) const { return get_ground_temperature(_primary); }
float get_ground_temperature(uint8_t i) const { return sensors[i].ground_temperature.get(); }
float get_ground_temperature(void) const;
// ground pressure in Pascal
// the ground values are only valid after calibration
@ -103,8 +102,8 @@ public:
// settable parameters
static const struct AP_Param::GroupInfo var_info[];
float get_calibration_temperature(void) const { return get_calibration_temperature(_primary); }
float get_calibration_temperature(uint8_t instance) const;
float get_external_temperature(void) const { return get_external_temperature(_primary); };
float get_external_temperature(const uint8_t instance) const;
// HIL (and SITL) interface, setting altitude
void setHIL(float altitude_msl);
@ -170,7 +169,6 @@ private:
float pressure; // pressure in Pascal
float temperature; // temperature in degrees C
float altitude; // calculated altitude
AP_Float ground_temperature;
AP_Float ground_pressure;
} sensors[BARO_MAX_INSTANCES];
@ -184,7 +182,9 @@ private:
uint32_t _last_external_temperature_ms;
DerivativeFilterFloat_Size7 _climb_rate_filter;
AP_Float _specific_gravity; // the specific gravity of fluid for an ROV 1.00 for freshwater, 1.024 for salt water
AP_Float _user_ground_temperature; // user override of the ground temperature used for EAS2TAS
bool _hil_mode:1;
float _guessed_ground_temperature; // currently ground temperature estimate using our best abailable source
// when did we last notify the GCS of new pressure reference?
uint32_t _last_notify_ms;