AP_Baro: added accumulate method to PX4 driver, and fixed scaling

This commit is contained in:
Andrew Tridgell 2013-01-20 22:11:58 +11:00
parent 96b87e3b44
commit 74f7b0f218
2 changed files with 29 additions and 24 deletions

View File

@ -20,7 +20,7 @@ extern const AP_HAL::HAL& hal;
// Public Methods //////////////////////////////////////////////////////////////
bool AP_Baro_PX4::init(void)
{
if (_baro_fd == -1) {
if (_baro_fd <= 0) {
_baro_fd = open(BARO_DEVICE_PATH, O_RDONLY);
if (_baro_fd < 0) {
hal.scheduler->panic("Unable to open " BARO_DEVICE_PATH);
@ -39,31 +39,35 @@ bool AP_Baro_PX4::init(void)
// Read the sensor
uint8_t AP_Baro_PX4::read(void)
{
uint16_t count;
float pressure_sum, temperature_sum;
accumulate();
if (_sum_count == 0) {
// no data available
return 0;
}
_pressure = (_pressure_sum / _sum_count) * 100.0f;
_temperature = (_temperature_sum / _sum_count) * 10.0f;
_pressure_samples = _sum_count;
_pressure_sum = 0;
_temperature_sum = 0;
_sum_count = 0;
healthy = true;
return 1;
}
// accumulate sensor values
void AP_Baro_PX4::accumulate(void)
{
struct baro_report baro_report;
// read all available samples and average
pressure_sum = 0;
temperature_sum = 0;
count = 0;
while (::read(_baro_fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report)) {
pressure_sum += baro_report.pressure; // Pressure in mbar
temperature_sum += baro_report.temperature; // degrees celcius
count++;
}
if (count != 0) {
_pressure = pressure_sum / count;
_temperature = temperature_sum / count;
_pressure_sum += baro_report.pressure; // Pressure in mbar
_temperature_sum += baro_report.temperature; // degrees celcius
_sum_count++;
_last_update = hal.scheduler->millis();
_pressure_samples = count;
healthy = true;
return 1;
}
return 0;
}
float AP_Baro_PX4::get_pressure() {

View File

@ -8,19 +8,20 @@
class AP_Baro_PX4 : public AP_Baro
{
public:
AP_Baro_PX4() : AP_Baro() {
_baro_fd = -1;
}
bool init();
uint8_t read();
float get_pressure();
float get_temperature();
int32_t get_raw_pressure();
int32_t get_raw_temp();
void accumulate(void);
private:
float _temperature;
float _pressure;
float _pressure_sum;
float _temperature_sum;
uint32_t _sum_count;
// baro driver handle
int _baro_fd;