// return current altitude estimate relative to time that calibrate()
// was called. Returns altitude in meters
// note that this relies on read() being called regularly to get new data
float AP_Baro::get_altitude(void)
{
if (_ground_pressure == 0) {
// called before initialisation
return 0;
}
if (_last_altitude_t == _last_update) {
// no new information
return _altitude + _alt_offset;
}
float pressure = get_pressure();
float alt = get_altitude_difference(_ground_pressure, pressure);
// record that we have consumed latest data
_last_altitude_t = _last_update;
// sanity check altitude
if (isnan(alt) || isinf(alt)) {
_flags.alt_ok = false;
} else {
_altitude = alt;
_flags.alt_ok = true;
}
// ensure the climb rate filter is updated
_climb_rate_filter.update(_altitude, _last_update);
return _altitude + _alt_offset;
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (fabsf(_alt_offset - _alt_offset_active) > 0.1f) {
// if there's more than 10cm difference then slowly slew to it via LPF.
// The EKF does not like step inputs so this keeps it happy
_alt_offset_active = (0.9f*_alt_offset_active) + (0.1f*_alt_offset);
} else {
_alt_offset_active = _alt_offset;
}
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->update();
}
}
// consider a sensor as healthy if it has had an update in the
// last 0.5 seconds
uint32_t now = AP_HAL::millis();
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && !is_zero(sensors[i].pressure);
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
if (is_zero(sensors[i].ground_pressure)) {
sensors[i].ground_pressure = sensors[i].pressure;
}
float altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
// sanity check altitude
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
if (sensors[i].alt_ok) {
sensors[i].altitude = altitude + _alt_offset_active;
}
}
}
// ensure the climb rate filter is updated
if (healthy()) {
_climb_rate_filter.update(get_altitude(), get_last_update());
}
// choose primary sensor
if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
_primary = _primary_baro;
} else {
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
}
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->update();
}
}
// consider a sensor as healthy if it has had an update in the
// last 0.5 seconds
uint32_t now = AP_HAL::millis();
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && !is_zero(sensors[i].pressure);
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
if (is_zero(sensors[i].ground_pressure)) {
sensors[i].ground_pressure = sensors[i].pressure;
}
float altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
// sanity check altitude
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
if (sensors[i].alt_ok) {
sensors[i].altitude = altitude + _alt_offset;
}
}
}
// ensure the climb rate filter is updated
if (healthy()) {
_climb_rate_filter.update(get_altitude(), get_last_update());
}
// choose primary sensor
if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
_primary = _primary_baro;
} else {
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
}
}
// return current altitude estimate relative to time that calibrate()
// was called. Returns altitude in meters
// note that this relies on read() being called regularly to get new data
float AP_Baro::get_altitude(void)
{
if (_ground_pressure == 0) {
// called before initialisation
return 0;
}
if (_last_altitude_t == _last_update) {
// no new information
return _altitude + _alt_offset;
}
_altitude = get_altitude_difference(_ground_pressure, get_pressure());
_last_altitude_t = _last_update;
// ensure the climb rate filter is updated
_climb_rate_filter.update(_altitude, _last_update);
return _altitude + _alt_offset;
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->update();
}
}
// consider a sensor as healthy if it has had an update in the
// last 0.5 seconds
uint32_t now = hal.scheduler->millis();
for (uint8_t i=0; i<_num_sensors; i++) {
sensors[i].healthy = (now - sensors[i].last_update_ms < 500) && sensors[i].pressure != 0;
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
if (sensors[i].ground_pressure == 0) {
sensors[i].ground_pressure = sensors[i].pressure;
}
sensors[i].altitude = get_altitude_difference(sensors[i].ground_pressure, sensors[i].pressure);
// sanity check altitude
sensors[i].alt_ok = !(isnan(sensors[i].altitude) || isinf(sensors[i].altitude));
}
}
// choose primary sensor
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
// ensure the climb rate filter is updated
_climb_rate_filter.update(get_altitude(), get_last_update());
}
/*
call update on all drivers
*/
void AP_Baro::update(void)
{
if (fabsf(_alt_offset - _alt_offset_active) > 0.01f) {
// If there's more than 1cm difference then slowly slew to it via LPF.
// The EKF does not like step inputs so this keeps it happy.
_alt_offset_active = (0.95f*_alt_offset_active) + (0.05f*_alt_offset);
} else {
_alt_offset_active = _alt_offset;
}
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->backend_update(i);
}
}
for (uint8_t i=0; i<_num_sensors; i++) {
if (sensors[i].healthy) {
// update altitude calculation
float ground_pressure = sensors[i].ground_pressure;
if (!is_positive(ground_pressure) || isnan(ground_pressure) || isinf(ground_pressure)) {
sensors[i].ground_pressure = sensors[i].pressure;
}
float altitude = sensors[i].altitude;
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);
} 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;
}
// sanity check altitude
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
if (sensors[i].alt_ok) {
sensors[i].altitude = altitude + _alt_offset_active;
}
}
if (_hil.have_alt) {
sensors[0].altitude = _hil.altitude;
}
if (_hil.have_last_update) {
sensors[0].last_update_ms = _hil.last_update_ms;
}
}
// ensure the climb rate filter is updated
if (healthy()) {
_climb_rate_filter.update(get_altitude(), get_last_update());
}
// choose primary sensor
if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
_primary = _primary_baro;
} else {
_primary = 0;
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
_primary = i;
break;
}
}
}
}
