aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_Baro/AP_Baro_PX4.cpp
View File

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

7
libraries/AP_Baro/AP_Baro_PX4.h
View File

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