/* 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; } } } }
/* 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; } } } }