diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 4454a0fe00..7da41ab9a3 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -235,7 +235,8 @@ void AP_Baro::update_calibration() { for (uint8_t i=0; i<_num_sensors; i++) { if (healthy(i)) { - sensors[i].ground_pressure.set(get_pressure(i)); + float corrected_pressure = get_pressure(i) + sensors[i].p_correction; + sensors[i].ground_pressure.set(corrected_pressure); } // don't notify the GCS too rapidly or we flood the link @@ -568,13 +569,13 @@ void AP_Baro::update(void) sensors[i].ground_pressure = sensors[i].pressure; } float altitude = sensors[i].altitude; + float corrected_pressure = sensors[i].pressure + sensors[i].p_correction; if (sensors[i].type == BARO_TYPE_AIR) { - float pressure = sensors[i].pressure + sensors[i].p_correction; - altitude = get_altitude_difference(sensors[i].ground_pressure, pressure); + altitude = get_altitude_difference(sensors[i].ground_pressure, corrected_pressure); } else if (sensors[i].type == BARO_TYPE_WATER) { //101325Pa is sea level air pressure, 9800 Pascal/ m depth in water. //No temperature or depth compensation for density of water. - altitude = (sensors[i].ground_pressure - sensors[i].pressure) / 9800.0f / _specific_gravity; + altitude = (sensors[i].ground_pressure - corrected_pressure) / 9800.0f / _specific_gravity; } // sanity check altitude sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude)); diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 71e92e79d2..7e3c14c836 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -63,6 +63,10 @@ public: float get_temperature(void) const { return get_temperature(_primary); } float get_temperature(uint8_t instance) const { return sensors[instance].temperature; } + // get pressure correction in Pascal. Divide by 100 for millibars or hectopascals + float get_pressure_correction(void) const { return get_pressure_correction(_primary); } + float get_pressure_correction(uint8_t instance) const { return sensors[instance].p_correction; } + // accumulate a reading on sensors. Some backends without their // own thread or a timer may need this. void accumulate(void);