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

AP_Baro_PX4: fixed baro driver to run at full rate 

we were skipping samples due to a timer handling bug. It now checks
for new samples at 100Hz
This commit is contained in:
Andrew Tridgell 2013-05-01 08:53:30 +10:00
parent ad01aeee44
commit d7ec985029
2 changed files with 18 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_Baro/AP_Baro_PX4.cpp
View File

@ -12,8 +12,7 @@
#include <unistd.h>
#include <drivers/drv_baro.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_hrt.h>
extern const AP_HAL::HAL& hal;
@ -22,7 +21,6 @@ float AP_Baro_PX4::_temperature_sum;
uint32_t AP_Baro_PX4::_sum_count;
uint32_t AP_Baro_PX4::_last_timer;
uint64_t AP_Baro_PX4::_last_timestamp;
volatile bool AP_Baro_PX4::_in_accumulate;
int AP_Baro_PX4::_baro_fd;
// Public Methods //////////////////////////////////////////////////////////////
@ -41,6 +39,9 @@ bool AP_Baro_PX4::init(void)
ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 10);
hal.scheduler->register_timer_process(_baro_timer);
// give the timer a chance to run and gather one sample
hal.scheduler->delay(40);
}
return true;
@ -49,12 +50,19 @@ bool AP_Baro_PX4::init(void)
// Read the sensor
uint8_t AP_Baro_PX4::read(void)
{
accumulate();
if (_sum_count == 0) {
// no data available
return 0;
hal.scheduler->suspend_timer_procs();
// try to accumulate one more sample, so we have the latest data
_accumulate();
// consider the baro healthy if we got a reading in the last 0.2s
healthy = (hrt_absolute_time() - _last_timestamp < 200000);
if (!healthy || _sum_count == 0) {
hal.scheduler->resume_timer_procs();
return healthy;
}
_pressure = (_pressure_sum / _sum_count) * 100.0f;
_temperature = (_temperature_sum / _sum_count) * 10.0f;
_pressure_samples = _sum_count;
@ -63,7 +71,8 @@ uint8_t AP_Baro_PX4::read(void)
_temperature_sum = 0;
_sum_count = 0;
healthy = true;
hal.scheduler->resume_timer_procs();
return 1;
}
@ -71,11 +80,6 @@ uint8_t AP_Baro_PX4::read(void)
void AP_Baro_PX4::_accumulate(void)
{
struct baro_report baro_report;
if (_in_accumulate) {
return;
}
_in_accumulate = true;
while (::read(_baro_fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report) &&
baro_report.timestamp != _last_timestamp) {
_pressure_sum += baro_report.pressure; // Pressure in mbar
@ -83,19 +87,12 @@ void AP_Baro_PX4::_accumulate(void)
_sum_count++;
_last_timestamp = baro_report.timestamp;
}
_in_accumulate = false;
}
// accumulate sensor values
void AP_Baro_PX4::accumulate(void)
{
_accumulate();
}
void AP_Baro_PX4::_baro_timer(uint32_t now)
{
// accumulate samples at 100Hz
if (now - _last_timer > 10000) {
if (now - _last_timer < 10000) {
return;
}
_last_timer = hal.scheduler->micros();

2
libraries/AP_Baro/AP_Baro_PX4.h
View File

@ -14,7 +14,6 @@ public:
float get_temperature();
int32_t get_raw_pressure();
int32_t get_raw_temp();
void accumulate(void);
private:
float _temperature;
@ -22,7 +21,6 @@ private:
static float _pressure_sum;
static float _temperature_sum;
static uint32_t _sum_count;
static volatile bool _in_accumulate;
static void _accumulate(void);
static void _baro_timer(uint32_t now);
static uint64_t _last_timestamp;