Пример #1
0
// 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;
}
Пример #2
0
/*
  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;
            }
        }
    }
}
Пример #3
0
/*
  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;
            }
        }
    }
}
Пример #4
0
// 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;
}
Пример #5
0
/*
  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());
}
Пример #6
0
/*
  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;
            }
        }
    }
}