mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
81a298c9c8
We don't need to expose to other libraries how each backend is implemented. AP_Baro.h is the main header, included by other libraries. Instead of including each backend in the main header, move them to where they are needed. Additionally standardize the order and how we include the headers. The advantages are: - Internals of each backend is not exposed outside of the library - Faster incremental builds since we don't need to recompile whoever includes AP_Baro.h because a backend changed
77 lines
2.4 KiB
C++
77 lines
2.4 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
#include "AP_Baro_PX4.h"
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
|
|
#include <drivers/drv_baro.h>
|
|
#include <drivers/drv_hrt.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
/*
|
|
constructor - opens the PX4 drivers
|
|
*/
|
|
AP_Baro_PX4::AP_Baro_PX4(AP_Baro &baro) :
|
|
AP_Baro_Backend(baro),
|
|
_num_instances(0)
|
|
{
|
|
memset(instances, 0, sizeof(instances));
|
|
instances[0].fd = open(BARO_BASE_DEVICE_PATH"0", O_RDONLY);
|
|
instances[1].fd = open(BARO_BASE_DEVICE_PATH"1", O_RDONLY);
|
|
instances[2].fd = open(BARO_BASE_DEVICE_PATH"2", O_RDONLY);
|
|
|
|
for (uint8_t i=0; i<3; i++) {
|
|
if (instances[i].fd != -1) {
|
|
_num_instances = i+1;
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
|
|
for (uint8_t i=0; i<_num_instances; i++) {
|
|
instances[i].instance = _frontend.register_sensor();
|
|
|
|
/* set the driver to poll at 150Hz */
|
|
ioctl(instances[i].fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX);
|
|
|
|
// average over up to 20 samples
|
|
ioctl(instances[i].fd, SENSORIOCSQUEUEDEPTH, 20);
|
|
}
|
|
}
|
|
|
|
// Read the sensor
|
|
void AP_Baro_PX4::update(void)
|
|
{
|
|
for (uint8_t i=0; i<_num_instances; i++) {
|
|
struct baro_report baro_report;
|
|
struct px4_instance &instance = instances[i];
|
|
while (::read(instance.fd, &baro_report, sizeof(baro_report)) == sizeof(baro_report) &&
|
|
baro_report.timestamp != instance.last_timestamp) {
|
|
instance.pressure_sum += baro_report.pressure; // Pressure in mbar
|
|
instance.temperature_sum += baro_report.temperature; // degrees celcius
|
|
instance.sum_count++;
|
|
instance.last_timestamp = baro_report.timestamp;
|
|
}
|
|
}
|
|
|
|
for (uint8_t i=0; i<_num_instances; i++) {
|
|
struct px4_instance &instance = instances[i];
|
|
if (instance.sum_count > 0) {
|
|
float pressure = (instance.pressure_sum / instance.sum_count) * 100;
|
|
float temperature = instance.temperature_sum / instance.sum_count;
|
|
instance.pressure_sum = 0;
|
|
instance.temperature_sum = 0;
|
|
instance.sum_count = 0;
|
|
_copy_to_frontend(instance.instance, pressure, temperature);
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif // CONFIG_HAL_BOARD
|