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

AP_Baro: simplify PX4 driver

Browse Source 
we can now rely on queueing in NuttX driver
This commit is contained in:
Andrew Tridgell 2013-08-28 19:18:32 +10:00
parent 1ff669eb97
commit 395739dded
2 changed files with 10 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_Baro/AP_Baro_PX4.cpp
View File

@ -16,13 +16,6 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
float AP_Baro_PX4::_pressure_sum;
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;
int AP_Baro_PX4::_baro_fd;
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
bool AP_Baro_PX4::init(void) bool AP_Baro_PX4::init(void)
{ {
@ -35,13 +28,12 @@ bool AP_Baro_PX4::init(void)
/* set the driver to poll at 150Hz */ /* set the driver to poll at 150Hz */
ioctl(_baro_fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX); ioctl(_baro_fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX);
// average over up to 10 samples // average over up to 20 samples
ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 10); ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 20);
hal.scheduler->register_timer_process(_baro_timer);
// give the timer a chance to run and gather one sample // give the timer a chance to run and gather one sample
hal.scheduler->delay(40); hal.scheduler->delay(40);
_accumulate();
} }
return true; return true;
@ -50,19 +42,15 @@ 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)
{ {
hal.scheduler->suspend_timer_procs();
// try to accumulate one more sample, so we have the latest data // try to accumulate one more sample, so we have the latest data
_accumulate(); _accumulate();
// consider the baro healthy if we got a reading in the last 0.2s // consider the baro healthy if we got a reading in the last 0.2s
healthy = (hrt_absolute_time() - _last_timestamp < 200000); healthy = (hrt_absolute_time() - _last_timestamp < 200000);
if (!healthy || _sum_count == 0) { if (!healthy || _sum_count == 0) {
hal.scheduler->resume_timer_procs();
return healthy; return healthy;
} }
_pressure = (_pressure_sum / _sum_count) * 100.0f; _pressure = (_pressure_sum / _sum_count) * 100.0f;
_temperature = (_temperature_sum / _sum_count) * 10.0f; _temperature = (_temperature_sum / _sum_count) * 10.0f;
_pressure_samples = _sum_count; _pressure_samples = _sum_count;
@ -71,8 +59,6 @@ uint8_t AP_Baro_PX4::read(void)
_temperature_sum = 0; _temperature_sum = 0;
_sum_count = 0; _sum_count = 0;
hal.scheduler->resume_timer_procs();
return 1; return 1;
} }
@ -89,16 +75,6 @@ void AP_Baro_PX4::_accumulate(void)
} }
} }
void AP_Baro_PX4::_baro_timer(uint32_t now)
{
// accumulate samples at 100Hz
if (now - _last_timer < 10000) {
return;
}
_last_timer = hal.scheduler->micros();
_accumulate();
}
float AP_Baro_PX4::get_pressure() { float AP_Baro_PX4::get_pressure() {
return _pressure; return _pressure;
} }

15
libraries/AP_Baro/AP_Baro_PX4.h
View File

@ -18,15 +18,14 @@ public:
private: private:
float _temperature; float _temperature;
float _pressure; float _pressure;
static float _pressure_sum; float _pressure_sum;
static float _temperature_sum; float _temperature_sum;
static uint32_t _sum_count; uint32_t _sum_count;
static void _accumulate(void); void _accumulate(void);
static void _baro_timer(uint32_t now); void _baro_timer(uint32_t now);
static uint64_t _last_timestamp; uint64_t _last_timestamp;
// baro driver handle // baro driver handle
static int _baro_fd; int _baro_fd;
static uint32_t _last_timer;
}; };
#endif // __AP_BARO_PX4_H__ #endif // __AP_BARO_PX4_H__