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 //////////////////////////////////////////////////////////////
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;
}

4
libraries/AP_Baro/AP_Baro_PX4.h
View File

@ -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__