mirror of https://github.com/ArduPilot/ardupilot
118 lines
3.7 KiB
C++
118 lines
3.7 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
/*
|
|
APM_Baro.cpp - barometer driver
|
|
|
|
This library is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU Lesser General Public License
|
|
as published by the Free Software Foundation; either version 2.1
|
|
of the License, or (at your option) any later version.
|
|
*/
|
|
|
|
#include <FastSerial.h>
|
|
#include <AP_Common.h>
|
|
#include <AP_Baro.h>
|
|
|
|
// table of user settable parameters
|
|
const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = {
|
|
// NOTE: Index numbers 0 and 1 were for the old integer
|
|
// ground temperature and pressure
|
|
|
|
// @Param: ABS_PRESS
|
|
// @DisplayName: Absolute Pressure
|
|
// @Description: calibrated ground pressure
|
|
// @Increment: 1
|
|
AP_GROUPINFO("ABS_PRESS", 2, AP_Baro, _ground_pressure),
|
|
|
|
// @Param: ABS_PRESS
|
|
// @DisplayName: ground temperature
|
|
// @Description: calibrated ground temperature
|
|
// @Increment: 1
|
|
AP_GROUPINFO("TEMP", 3, AP_Baro, _ground_temperature),
|
|
AP_GROUPEND
|
|
};
|
|
|
|
// calibrate the barometer. This must be called at least once before
|
|
// the altitude() or climb_rate() interfaces can be used
|
|
void AP_Baro::calibrate(void (*callback)(unsigned long t))
|
|
{
|
|
float ground_pressure = 0;
|
|
float ground_temperature = 0;
|
|
|
|
while (ground_pressure == 0 || !healthy) {
|
|
read(); // Get initial data from absolute pressure sensor
|
|
ground_pressure = get_pressure();
|
|
ground_temperature = get_temperature();
|
|
callback(20);
|
|
}
|
|
// let the barometer settle for a full second after startup
|
|
// the MS5611 reads quite a long way off for the first second,
|
|
// leading to about 1m of error if we don't wait
|
|
for (uint16_t i = 0; i < 10; i++) {
|
|
do {
|
|
read();
|
|
} while (!healthy);
|
|
ground_pressure = get_pressure();
|
|
ground_temperature = get_temperature();
|
|
callback(100);
|
|
}
|
|
|
|
// now average over 5 values for the ground pressure and
|
|
// temperature settings
|
|
for (uint16_t i = 0; i < 5; i++) {
|
|
do {
|
|
read();
|
|
} while (!healthy);
|
|
ground_pressure = ground_pressure * 0.8 + get_pressure() * 0.2;
|
|
ground_temperature = ground_temperature * 0.8 + get_temperature() * 0.2;
|
|
callback(100);
|
|
}
|
|
|
|
_ground_pressure.set_and_save(ground_pressure);
|
|
_ground_temperature.set_and_save(ground_temperature / 10.0f);
|
|
}
|
|
|
|
// 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)
|
|
{
|
|
float scaling, temp;
|
|
|
|
if (_last_altitude_t == _last_update) {
|
|
// no new information
|
|
return _altitude;
|
|
}
|
|
|
|
// this has no filtering of the pressure values, use a separate
|
|
// filter if you want a smoothed value. The AHRS driver wants
|
|
// unsmoothed values
|
|
scaling = (float)_ground_pressure / (float)get_pressure();
|
|
temp = ((float)_ground_temperature) + 273.15f;
|
|
_altitude = log(scaling) * temp * 29.271267f;
|
|
|
|
_last_altitude_t = _last_update;
|
|
|
|
// ensure the climb rate filter is updated
|
|
_climb_rate_filter.update(_altitude, _last_update);
|
|
|
|
return _altitude;
|
|
}
|
|
|
|
// return current climb_rate estimeate relative to time that calibrate()
|
|
// was called. Returns climb rate in meters/s, positive means up
|
|
// note that this relies on read() being called regularly to get new data
|
|
float AP_Baro::get_climb_rate(void)
|
|
{
|
|
if (_last_climb_rate_t == _last_altitude_t) {
|
|
// no new information
|
|
return _climb_rate;
|
|
}
|
|
_last_climb_rate_t = _last_altitude_t;
|
|
|
|
// we use a 7 point derivative filter on the climb rate. This seems
|
|
// to produce somewhat reasonable results on real hardware
|
|
_climb_rate = _climb_rate_filter.slope() * 1.0e3;
|
|
|
|
return _climb_rate;
|
|
}
|