diff --git a/libraries/AP_Baro/AP_Baro_PX4.cpp b/libraries/AP_Baro/AP_Baro_PX4.cpp index 987fed98a4..f62734413e 100644 --- a/libraries/AP_Baro/AP_Baro_PX4.cpp +++ b/libraries/AP_Baro/AP_Baro_PX4.cpp @@ -20,37 +20,43 @@ extern const AP_HAL::HAL& hal; // Public Methods ////////////////////////////////////////////////////////////// bool AP_Baro_PX4::init(void) { - int fd; - - fd = open(BARO_DEVICE_PATH, 0); - if (fd < 0) { + _baro_fd = open(BARO_DEVICE_PATH, O_RDONLY); + if (_baro_fd < 0) { hal.scheduler->panic("Unable to open " BARO_DEVICE_PATH); } /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); - close(fd); + ioctl(_baro_fd, SENSORIOCSPOLLRATE, 150); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); + // average over up to 10 samples + ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 10); return true; } // Read the sensor -uint8_t AP_Baro_PX4::read() +uint8_t AP_Baro_PX4::read(void) { - bool baro_updated; - orb_check(_baro_sub, &baro_updated); + uint16_t count; + float pressure_sum, temperature_sum; + struct baro_report baro_report; - if (baro_updated) { - struct baro_report baro_report; + // read all available samples and average + pressure_sum = 0; + temperature_sum = 0; + count = 0; - orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report); + 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++; + } - _pressure = baro_report.pressure; // Pressure in mbar - _temperature = baro_report.temperature; // Temperature in degrees celcius - _pressure_samples = 1; + if (count != 0) { + _pressure = pressure_sum / count; + _temperature = temperature_sum / count; _last_update = hal.scheduler->millis(); + _pressure_samples = count; healthy = true; return 1; } diff --git a/libraries/AP_Baro/AP_Baro_PX4.h b/libraries/AP_Baro/AP_Baro_PX4.h index 2df70ada2d..430520b64b 100644 --- a/libraries/AP_Baro/AP_Baro_PX4.h +++ b/libraries/AP_Baro/AP_Baro_PX4.h @@ -19,8 +19,8 @@ private: float _temperature; float _pressure; - // ORB subscription handle - int _baro_sub; + // baro driver handle + int _baro_fd; }; #endif // __AP_BARO_PX4_H__