Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Baro: Temperature Related Fixups #5930

Merged
merged 2 commits into from
Mar 28, 2017
Merged

Baro: Temperature Related Fixups #5930

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a5462ee
AP_Baro: Limit ground temperature used for the altitude
WickedShell Mar 24, 2017
f3aa555
DataFlash: Log the ground temperature used for baro calculations
WickedShell Mar 24, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
94 changes: 52 additions & 42 deletions libraries/AP_Baro/AP_Baro.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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),
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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) {
Expand All @@ -234,30 +227,28 @@ 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
// given base_pressure in Pascal
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
Expand All @@ -274,23 +265,26 @@ 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;
}

// 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;
}
Expand All @@ -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)
Expand All @@ -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);
}


Expand Down Expand Up @@ -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;
Expand Down
10 changes: 5 additions & 5 deletions libraries/AP_Baro/AP_Baro.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,8 +83,7 @@ class AP_Baro

// 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
Expand All @@ -103,8 +102,8 @@ class AP_Baro
// 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);
Expand Down Expand Up @@ -170,7 +169,6 @@ class AP_Baro
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];

Expand All @@ -184,7 +182,9 @@ class AP_Baro
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;
Expand Down
4 changes: 4 additions & 0 deletions libraries/DataFlash/LogFile.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -829,6 +829,7 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us)
}
float climbrate = baro.get_climb_rate();
float drift_offset = baro.get_baro_drift_offset();
float ground_temp = baro.get_ground_temperature();
struct log_BARO pkt = {
LOG_PACKET_HEADER_INIT(LOG_BARO_MSG),
time_us : time_us,
Expand All @@ -838,6 +839,7 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us)
climbrate : climbrate,
sample_time_ms: baro.get_last_update(0),
drift_offset : drift_offset,
ground_temp : ground_temp,
};
WriteBlock(&pkt, sizeof(pkt));

Expand All @@ -851,6 +853,7 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us)
climbrate : climbrate,
sample_time_ms: baro.get_last_update(1),
drift_offset : drift_offset,
ground_temp : ground_temp,
};
WriteBlock(&pkt2, sizeof(pkt2));
}
Expand All @@ -865,6 +868,7 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us)
climbrate : climbrate,
sample_time_ms: baro.get_last_update(2),
drift_offset : drift_offset,
ground_temp : ground_temp,
};
WriteBlock(&pkt3, sizeof(pkt3));
}
Expand Down
3 changes: 2 additions & 1 deletion libraries/DataFlash/LogStructure.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -191,6 +191,7 @@ struct PACKED log_BARO {
float climbrate;
uint32_t sample_time_ms;
float drift_offset;
float ground_temp;
};

struct PACKED log_AHRS {
Expand Down Expand Up @@ -805,7 +806,7 @@ Format characters in the format string for binary log messages
{ LOG_RSSI_MSG, sizeof(log_RSSI), \
"RSSI", "Qf", "TimeUS,RXRSSI" }, \
{ LOG_BARO_MSG, sizeof(log_BARO), \
"BARO", "QffcfIf", "TimeUS,Alt,Press,Temp,CRt,SMS,Offset" }, \
"BARO", "QffcfIff", "TimeUS,Alt,Press,Temp,CRt,SMS,Offset,GndTemp" }, \
{ LOG_POWR_MSG, sizeof(log_POWR), \
"POWR","QffH","TimeUS,Vcc,VServo,Flags" }, \
{ LOG_CMD_MSG, sizeof(log_Cmd), \
Expand Down