diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index b266c49875..870c41735f 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -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; diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 1e3e5bd3c4..ede2a1458f 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -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;