Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: average over multiple samples in PX4 baro driver

This commit is contained in:
Andrew Tridgell 2013-01-04 20:11:30 +11:00
parent 44837a11f2
commit dbcaa4cf3c
2 changed files with 24 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_Baro/AP_Baro_PX4.cpp
View File

@ -20,37 +20,43 @@ extern const AP_HAL::HAL& hal;
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
bool AP_Baro_PX4::init(void) bool AP_Baro_PX4::init(void)
{ {
int fd; _baro_fd = open(BARO_DEVICE_PATH, O_RDONLY);
if (_baro_fd < 0) {
fd = open(BARO_DEVICE_PATH, 0);
if (fd < 0) {
hal.scheduler->panic("Unable to open " BARO_DEVICE_PATH); hal.scheduler->panic("Unable to open " BARO_DEVICE_PATH);
} }
/* set the driver to poll at 150Hz */ /* set the driver to poll at 150Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 150); ioctl(_baro_fd, SENSORIOCSPOLLRATE, 150);
close(fd);
_baro_sub = orb_subscribe(ORB_ID(sensor_baro)); // average over up to 10 samples
ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 10);
return true; return true;
} }
// Read the sensor // Read the sensor
uint8_t AP_Baro_PX4::read() uint8_t AP_Baro_PX4::read(void)
{ {
bool baro_updated; uint16_t count;
orb_check(_baro_sub, &baro_updated); float pressure_sum, temperature_sum;
struct baro_report baro_report;
if (baro_updated) { // read all available samples and average
struct baro_report baro_report; 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 if (count != 0) {
_temperature = baro_report.temperature; // Temperature in degrees celcius _pressure = pressure_sum / count;
_pressure_samples = 1; _temperature = temperature_sum / count;
_last_update = hal.scheduler->millis(); _last_update = hal.scheduler->millis();
_pressure_samples = count;
healthy = true; healthy = true;
return 1; return 1;
} }

4
libraries/AP_Baro/AP_Baro_PX4.h
View File

@ -19,8 +19,8 @@ private:
float _temperature; float _temperature;
float _pressure; float _pressure;
// ORB subscription handle // baro driver handle
int _baro_sub; int _baro_fd;
}; };
#endif // __AP_BARO_PX4_H__ #endif // __AP_BARO_PX4_H__