Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: switched to in-tree I2C driver for all boards

Browse Source
This commit is contained in:
Andrew Tridgell 2016-10-31 20:48:22 +11:00
parent af947dbd38
commit 497b87fa65
6 changed files with 44 additions and 185 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -32,44 +32,12 @@ extern const AP_HAL::HAL &hal;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define ARSPD_DEFAULT_PIN 1
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <sys/stat.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <systemlib/airspeed.h>
#include <drivers/drv_airspeed.h>
#include <uORB/topics/differential_pressure.h>
#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45)
#define ARSPD_DEFAULT_PIN 0
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51)
#define ARSPD_DEFAULT_PIN 0
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52)
#define ARSPD_DEFAULT_PIN 0
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
#define ARSPD_DEFAULT_PIN 0
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
#define ARSPD_DEFAULT_PIN 0
#elif defined(CONFIG_ARCH_BOARD_VRCORE_V10)
#define ARSPD_DEFAULT_PIN 0
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54)
#define ARSPD_DEFAULT_PIN 0
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
#define ARSPD_DEFAULT_PIN 11
#else
#define ARSPD_DEFAULT_PIN 15
#define ARSPD_DEFAULT_PIN AP_AIRSPEED_I2C_PIN
#endif
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
#define ARSPD_DEFAULT_PIN 5
#else
#define ARSPD_DEFAULT_PIN AP_AIRSPEED_I2C_PIN
#endif
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
#define PSI_RANGE_DEFAULT 0.05
#endif
#else
#define ARSPD_DEFAULT_PIN 0
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
#define PSI_RANGE_DEFAULT 0.05
#endif
#ifndef PSI_RANGE_DEFAULT

5
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -8,7 +8,6 @@
#include "AP_Airspeed_Backend.h"
#include "AP_Airspeed_I2C.h"
#include "AP_Airspeed_PX4.h"
#include "AP_Airspeed_analog.h"
class Airspeed_Calibration {
@ -180,9 +179,5 @@ private:
void update_calibration(float raw_pressure);
AP_Airspeed_Analog analog{_pin, _psi_range};
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
AP_Airspeed_PX4 digital{_psi_range};
#else
AP_Airspeed_I2C digital{_psi_range};
#endif
};

47
libraries/AP_Airspeed/AP_Airspeed_I2C.cpp
View File

@ -50,13 +50,20 @@ bool AP_Airspeed_I2C::init()
return false;
}
// lots of retries during probe
_dev->set_retries(5);
_measure();
hal.scheduler->delay(10);
_collect();
_dev->get_semaphore()->give();
// drop to 2 retries for runtime
_dev->set_retries(2);
if (_last_sample_time_ms != 0) {
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Airspeed_I2C::_timer, void));
_dev->register_periodic_callback(20000,
FUNCTOR_BIND_MEMBER(&AP_Airspeed_I2C::_timer, bool));
return true;
}
return false;
@ -110,27 +117,47 @@ void AP_Airspeed_I2C::_collect()
_pressure = diff_press_PSI * PSI_to_Pa;
_temperature = ((200.0f * dT_raw) / 2047) - 50;
_voltage_correction(_pressure, _temperature);
_last_sample_time_ms = AP_HAL::millis();
}
// 1kHz timer
void AP_Airspeed_I2C::_timer()
{
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
}
/**
correct for 5V rail voltage if the system_power ORB topic is
available
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
offset versus voltage for 3 sensors
*/
void AP_Airspeed_I2C::_voltage_correction(float &diff_press_pa, float &temperature)
{
const float slope = 65.0f;
const float temp_slope = 0.887f;
/*
apply a piecewise linear correction within range given by above graph
*/
float voltage_diff = hal.analogin->board_voltage() - 5.0f;
voltage_diff = constrain_float(voltage_diff, -0.7f, 0.5f);
diff_press_pa -= voltage_diff * slope;
temperature -= voltage_diff * temp_slope;
}
// 50Hz timer
bool AP_Airspeed_I2C::_timer()
{
if (_measurement_started_ms == 0) {
_measure();
_dev->get_semaphore()->give();
return;
return true;
}
if ((AP_HAL::millis() - _measurement_started_ms) > 10) {
_collect();
// start a new measurement
_measure();
}
_dev->get_semaphore()->give();
return true;
}
// return the current differential_pressure in Pascal

4
libraries/AP_Airspeed/AP_Airspeed_I2C.h
View File

@ -45,7 +45,9 @@ public:
private:
void _measure();
void _collect();
void _timer();
bool _timer();
void _voltage_correction(float &diff_press_pa, float &temperature);
float _temperature;
float _pressure;
uint32_t _last_sample_time_ms;

88
libraries/AP_Airspeed/AP_Airspeed_PX4.cpp
View File

@ -1,88 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* PX4 airspeed backend
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_Airspeed_PX4.h"
#include <drivers/drv_airspeed.h>
#include <uORB/topics/differential_pressure.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
extern const AP_HAL::HAL &hal;
bool AP_Airspeed_PX4::init()
{
_fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY);
if (_fd == -1) {
return false;
}
if (OK != ioctl(_fd, SENSORIOCSPOLLRATE, 100) ||
OK != ioctl(_fd, SENSORIOCSQUEUEDEPTH, 15)) {
hal.console->println("Failed to setup airspeed driver rate and queue");
}
return true;
}
// read the airspeed sensor
bool AP_Airspeed_PX4::get_differential_pressure(float &pressure)
{
if (_fd == -1) {
return false;
}
// read from the PX4 airspeed sensor
float psum = 0;
float tsum = 0;
uint16_t count = 0;
struct differential_pressure_s report;
while (::read(_fd, &report, sizeof(report)) == sizeof(report) &&
report.timestamp != _last_timestamp) {
psum += report.differential_pressure_raw_pa / _psi_range.get();
tsum += report.temperature;
count++;
_last_timestamp = report.timestamp;
}
if (count == 0) {
return false;
}
pressure = psum / count;
_temperature = tsum / count;
return true;
}
// read the temperature
bool AP_Airspeed_PX4::get_temperature(float &temperature)
{
if (_temperature < -80) {
// almost certainly a bad reading. The ETS driver on PX4
// returns -1000
return false;
}
temperature = _temperature;
return true;
}
#endif // CONFIG_HAL_BOARD

45
libraries/AP_Airspeed/AP_Airspeed_PX4.h
View File

@ -1,45 +0,0 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
/*
backend driver for airspeed from PX4Firmware
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include "AP_Airspeed_Backend.h"
class AP_Airspeed_PX4 : public AP_Airspeed_Backend {
public:
// constructor
AP_Airspeed_PX4(const AP_Float &psi_range) :
_psi_range(psi_range) {}
// probe and initialise the sensor
bool init(void);
// return the current differential_pressure in Pascal
bool get_differential_pressure(float &pressure);
// return the current temperature in degrees C, if available
bool get_temperature(float &temperature);
private:
int _fd = -1;
uint64_t _last_timestamp;
float _temperature;
const AP_Float &_psi_range;
};