forked from Archive/PX4-Autopilot
Merged ETS airspeed driver
This commit is contained in:
Lorenz Meier
commit
614bbb1510
|
|
@ -31,6 +31,7 @@ MODULES += drivers/hott_telemetry
|
|||
MODULES += drivers/blinkm
|
||||
MODULES += drivers/mkblctrl
|
||||
MODULES += drivers/md25
|
||||
MODULES += drivers/ets_airspeed
|
||||
MODULES += modules/sensors
|
||||
|
||||
#
|
||||
|
|
|
|||
|
|
@ -0,0 +1,61 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Airspeed driver interface.
|
||||
* @author Simon Wilks
|
||||
*/
|
||||
|
||||
#ifndef _DRV_AIRSPEED_H
|
||||
#define _DRV_AIRSPEED_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "drv_sensor.h"
|
||||
#include "drv_orb_dev.h"
|
||||
|
||||
#define AIRSPEED_DEVICE_PATH "/dev/airspeed"
|
||||
|
||||
/*
|
||||
* ioctl() definitions
|
||||
*
|
||||
* Airspeed drivers also implement the generic sensor driver
|
||||
* interfaces from drv_sensor.h
|
||||
*/
|
||||
|
||||
#define _AIRSPEEDIOCBASE (0x7700)
|
||||
#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n))
|
||||
|
||||
|
||||
#endif /* _DRV_AIRSPEED_H */
|
||||
|
|
@ -0,0 +1,832 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ets_airspeed.cpp
|
||||
* @author Simon Wilks
|
||||
*
|
||||
* Driver for the Eagle Tree Airspeed V3 connected via I2C.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <semaphore.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/wqueue.h>
|
||||
#include <nuttx/clock.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <systemlib/airspeed.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
#include <drivers/drv_airspeed.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
|
||||
/* Default I2C bus */
|
||||
#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
|
||||
|
||||
/* I2C bus address */
|
||||
#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */
|
||||
|
||||
/* Register address */
|
||||
#define READ_CMD 0x07 /* Read the data */
|
||||
|
||||
/**
|
||||
* The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h.
|
||||
*/
|
||||
#define MIN_ACCURATE_DIFF_PRES_PA 12
|
||||
|
||||
/* Measurement rate is 100Hz */
|
||||
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
|
||||
|
||||
/* Oddly, ERROR is not defined for C++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
#ifndef CONFIG_SCHED_WORKQUEUE
|
||||
# error This requires CONFIG_SCHED_WORKQUEUE.
|
||||
#endif
|
||||
|
||||
class ETSAirspeed : public device::I2C
|
||||
{
|
||||
public:
|
||||
ETSAirspeed(int bus, int address = I2C_ADDRESS);
|
||||
virtual ~ETSAirspeed();
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
void print_info();
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
|
||||
private:
|
||||
work_s _work;
|
||||
unsigned _num_reports;
|
||||
volatile unsigned _next_report;
|
||||
volatile unsigned _oldest_report;
|
||||
differential_pressure_s *_reports;
|
||||
bool _sensor_ok;
|
||||
int _measure_ticks;
|
||||
bool _collect_phase;
|
||||
int _diff_pres_offset;
|
||||
|
||||
orb_advert_t _airspeed_pub;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _buffer_overflows;
|
||||
|
||||
|
||||
/**
|
||||
* Test whether the device supported by the driver is present at a
|
||||
* specific address.
|
||||
*
|
||||
* @param address The I2C bus address to probe.
|
||||
* @return True if the device is present.
|
||||
*/
|
||||
int probe_address(uint8_t address);
|
||||
|
||||
/**
|
||||
* Initialise the automatic measurement state machine and start it.
|
||||
*
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start();
|
||||
|
||||
/**
|
||||
* Stop the automatic measurement state machine.
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* Perform a poll cycle; collect from the previous measurement
|
||||
* and start a new one.
|
||||
*/
|
||||
void cycle();
|
||||
int measure();
|
||||
int collect();
|
||||
|
||||
/**
|
||||
* Static trampoline from the workq context; because we don't have a
|
||||
* generic workq wrapper yet.
|
||||
*
|
||||
* @param arg Instance pointer for the driver that is polling.
|
||||
*/
|
||||
static void cycle_trampoline(void *arg);
|
||||
|
||||
|
||||
};
|
||||
|
||||
/* helper macro for handling report buffer indices */
|
||||
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
|
||||
|
||||
/*
|
||||
* Driver 'main' command.
|
||||
*/
|
||||
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
|
||||
|
||||
ETSAirspeed::ETSAirspeed(int bus, int address) :
|
||||
I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
|
||||
_num_reports(0),
|
||||
_next_report(0),
|
||||
_oldest_report(0),
|
||||
_reports(nullptr),
|
||||
_sensor_ok(false),
|
||||
_measure_ticks(0),
|
||||
_collect_phase(false),
|
||||
_diff_pres_offset(0),
|
||||
_airspeed_pub(-1),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "ets_airspeed_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "ets_airspeed_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "ets_airspeed_buffer_overflows"))
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
|
||||
// work_cancel in the dtor will explode if we don't do this...
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
}
|
||||
|
||||
ETSAirspeed::~ETSAirspeed()
|
||||
{
|
||||
/* make sure we are truly inactive */
|
||||
stop();
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
delete[] _reports;
|
||||
}
|
||||
|
||||
int
|
||||
ETSAirspeed::init()
|
||||
{
|
||||
int ret = ERROR;
|
||||
|
||||
/* do I2C init (and probe) first */
|
||||
if (I2C::init() != OK)
|
||||
goto out;
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_num_reports = 2;
|
||||
_reports = new struct differential_pressure_s[_num_reports];
|
||||
for (unsigned i = 0; i < _num_reports; i++)
|
||||
_reports[i].max_differential_pressure_pa = 0;
|
||||
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* get a publish handle on the airspeed topic */
|
||||
memset(&_reports[0], 0, sizeof(_reports[0]));
|
||||
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
|
||||
|
||||
if (_airspeed_pub < 0)
|
||||
debug("failed to create airspeed sensor object. Did you start uOrb?");
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but we don't really know if it is within range */
|
||||
_sensor_ok = true;
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
ETSAirspeed::probe()
|
||||
{
|
||||
return measure();
|
||||
}
|
||||
|
||||
int
|
||||
ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
|
||||
case SENSORIOCSPOLLRATE: {
|
||||
switch (arg) {
|
||||
|
||||
/* switching to manual polling */
|
||||
case SENSOR_POLLRATE_MANUAL:
|
||||
stop();
|
||||
_measure_ticks = 0;
|
||||
return OK;
|
||||
|
||||
/* external signalling (DRDY) not supported */
|
||||
case SENSOR_POLLRATE_EXTERNAL:
|
||||
|
||||
/* zero would be bad */
|
||||
case 0:
|
||||
return -EINVAL;
|
||||
|
||||
/* set default/max polling rate */
|
||||
case SENSOR_POLLRATE_MAX:
|
||||
case SENSOR_POLLRATE_DEFAULT: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* set interval for next measurement to minimum legal value */
|
||||
_measure_ticks = USEC2TICK(CONVERSION_INTERVAL);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/* adjust to a legal polling interval in Hz */
|
||||
default: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* convert hz to tick interval via microseconds */
|
||||
unsigned ticks = USEC2TICK(1000000 / arg);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(CONVERSION_INTERVAL))
|
||||
return -EINVAL;
|
||||
|
||||
/* update interval for next measurement */
|
||||
_measure_ticks = ticks;
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_measure_ticks == 0)
|
||||
return SENSOR_POLLRATE_MANUAL;
|
||||
|
||||
return (1000 / _measure_ticks);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* add one to account for the sentinel in the ring */
|
||||
arg++;
|
||||
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 2) || (arg > 100))
|
||||
return -EINVAL;
|
||||
|
||||
/* allocate new buffer */
|
||||
struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
|
||||
|
||||
if (nullptr == buf)
|
||||
return -ENOMEM;
|
||||
|
||||
/* reset the measurement state machine with the new buffer, free the old */
|
||||
stop();
|
||||
delete[] _reports;
|
||||
_num_reports = arg;
|
||||
_reports = buf;
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case SENSORIOCGQUEUEDEPTH:
|
||||
return _num_reports - 1;
|
||||
|
||||
case SENSORIOCRESET:
|
||||
/* XXX implement this */
|
||||
return -EINVAL;
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return I2C::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t
|
||||
ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
unsigned count = buflen / sizeof(struct differential_pressure_s);
|
||||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
if (count < 1)
|
||||
return -ENOSPC;
|
||||
|
||||
/* if automatic measurement is enabled */
|
||||
if (_measure_ticks > 0) {
|
||||
|
||||
/*
|
||||
* While there is space in the caller's buffer, and reports, copy them.
|
||||
* Note that we may be pre-empted by the workq thread while we are doing this;
|
||||
* we are careful to avoid racing with them.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_oldest_report != _next_report) {
|
||||
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
|
||||
ret += sizeof(_reports[0]);
|
||||
INCREMENT(_oldest_report, _num_reports);
|
||||
}
|
||||
}
|
||||
|
||||
/* if there was no data, warn the caller */
|
||||
return ret ? ret : -EAGAIN;
|
||||
}
|
||||
|
||||
/* manual measurement - run one conversion */
|
||||
/* XXX really it'd be nice to lock against other readers here */
|
||||
do {
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* trigger a measurement */
|
||||
if (OK != measure()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* wait for it to complete */
|
||||
usleep(CONVERSION_INTERVAL);
|
||||
|
||||
/* run the collection phase */
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
memcpy(buffer, _reports, sizeof(*_reports));
|
||||
ret = sizeof(*_reports);
|
||||
|
||||
} while (0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
ETSAirspeed::measure()
|
||||
{
|
||||
int ret;
|
||||
|
||||
/*
|
||||
* Send the command to begin a measurement.
|
||||
*/
|
||||
uint8_t cmd = READ_CMD;
|
||||
ret = transfer(&cmd, 1, nullptr, 0);
|
||||
|
||||
if (OK != ret)
|
||||
{
|
||||
perf_count(_comms_errors);
|
||||
log("i2c::transfer returned %d", ret);
|
||||
return ret;
|
||||
}
|
||||
ret = OK;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
ETSAirspeed::collect()
|
||||
{
|
||||
int ret = -EIO;
|
||||
|
||||
/* read from the sensor */
|
||||
uint8_t val[2] = {0, 0};
|
||||
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
ret = transfer(nullptr, 0, &val[0], 2);
|
||||
|
||||
if (ret < 0) {
|
||||
log("error reading from sensor: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint16_t diff_pres_pa = val[1] << 8 | val[0];
|
||||
|
||||
param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
|
||||
|
||||
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
|
||||
diff_pres_pa = 0;
|
||||
} else {
|
||||
diff_pres_pa -= _diff_pres_offset;
|
||||
}
|
||||
|
||||
// XXX we may want to smooth out the readings to remove noise.
|
||||
|
||||
_reports[_next_report].timestamp = hrt_absolute_time();
|
||||
_reports[_next_report].differential_pressure_pa = diff_pres_pa;
|
||||
|
||||
// Track maximum differential pressure measured (so we can work out top speed).
|
||||
if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
|
||||
_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
|
||||
}
|
||||
|
||||
/* announce the airspeed if needed, just publish else */
|
||||
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
|
||||
|
||||
/* post a report to the ring - note, not locked */
|
||||
INCREMENT(_next_report, _num_reports);
|
||||
|
||||
/* if we are running up against the oldest report, toss it */
|
||||
if (_next_report == _oldest_report) {
|
||||
perf_count(_buffer_overflows);
|
||||
INCREMENT(_oldest_report, _num_reports);
|
||||
}
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
ret = OK;
|
||||
|
||||
perf_end(_sample_perf);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
ETSAirspeed::start()
|
||||
{
|
||||
/* reset the report ring and state machine */
|
||||
_collect_phase = false;
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1);
|
||||
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
SUBSYSTEM_TYPE_DIFFPRESSURE};
|
||||
static orb_advert_t pub = -1;
|
||||
|
||||
if (pub > 0) {
|
||||
orb_publish(ORB_ID(subsystem_info), pub, &info);
|
||||
} else {
|
||||
pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
ETSAirspeed::stop()
|
||||
{
|
||||
work_cancel(HPWORK, &_work);
|
||||
}
|
||||
|
||||
void
|
||||
ETSAirspeed::cycle_trampoline(void *arg)
|
||||
{
|
||||
ETSAirspeed *dev = (ETSAirspeed *)arg;
|
||||
|
||||
dev->cycle();
|
||||
}
|
||||
|
||||
void
|
||||
ETSAirspeed::cycle()
|
||||
{
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
log("collection error");
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
||||
/* next phase is measurement */
|
||||
_collect_phase = false;
|
||||
|
||||
/*
|
||||
* Is there a collect->measure gap?
|
||||
*/
|
||||
if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
|
||||
|
||||
/* schedule a fresh cycle call when we are ready to measure again */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&ETSAirspeed::cycle_trampoline,
|
||||
this,
|
||||
_measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* measurement phase */
|
||||
if (OK != measure())
|
||||
log("measure error");
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&ETSAirspeed::cycle_trampoline,
|
||||
this,
|
||||
USEC2TICK(CONVERSION_INTERVAL));
|
||||
}
|
||||
|
||||
void
|
||||
ETSAirspeed::print_info()
|
||||
{
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
perf_print_counter(_buffer_overflows);
|
||||
printf("poll interval: %u ticks\n", _measure_ticks);
|
||||
printf("report queue: %u (%u/%u @ %p)\n",
|
||||
_num_reports, _oldest_report, _next_report, _reports);
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace ets_airspeed
|
||||
{
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
const int ERROR = -1;
|
||||
|
||||
ETSAirspeed *g_dev;
|
||||
|
||||
void start(int i2c_bus);
|
||||
void stop();
|
||||
void test();
|
||||
void reset();
|
||||
void info();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*/
|
||||
void
|
||||
start(int i2c_bus)
|
||||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
errx(1, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new ETSAirspeed(i2c_bus);
|
||||
|
||||
if (g_dev == nullptr)
|
||||
goto fail;
|
||||
|
||||
if (OK != g_dev->init())
|
||||
goto fail;
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
goto fail;
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
goto fail;
|
||||
|
||||
exit(0);
|
||||
|
||||
fail:
|
||||
|
||||
if (g_dev != nullptr)
|
||||
{
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
errx(1, "driver start failed");
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the driver
|
||||
*/
|
||||
void
|
||||
stop()
|
||||
{
|
||||
if (g_dev != nullptr)
|
||||
{
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
else
|
||||
{
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform some basic functional tests on the driver;
|
||||
* make sure we can collect data from the sensor in polled
|
||||
* and automatic modes.
|
||||
*/
|
||||
void
|
||||
test()
|
||||
{
|
||||
struct differential_pressure_s report;
|
||||
ssize_t sz;
|
||||
int ret;
|
||||
|
||||
int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH);
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
err(1, "immediate read failed");
|
||||
|
||||
warnx("single read");
|
||||
warnx("diff pressure: %d pa", report.differential_pressure_pa);
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
|
||||
errx(1, "failed to set 2Hz poll rate");
|
||||
|
||||
/* read the sensor 5x and report each value */
|
||||
for (unsigned i = 0; i < 5; i++) {
|
||||
struct pollfd fds;
|
||||
|
||||
/* wait for data to be ready */
|
||||
fds.fd = fd;
|
||||
fds.events = POLLIN;
|
||||
ret = poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1)
|
||||
errx(1, "timed out waiting for sensor data");
|
||||
|
||||
/* now go get it */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
err(1, "periodic read failed");
|
||||
|
||||
warnx("periodic read %u", i);
|
||||
warnx("diff pressure: %d pa", report.differential_pressure_pa);
|
||||
}
|
||||
|
||||
errx(0, "PASS");
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the driver.
|
||||
*/
|
||||
void
|
||||
reset()
|
||||
{
|
||||
int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "failed ");
|
||||
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||
err(1, "driver reset failed");
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
err(1, "driver poll restart failed");
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Print a little info about the driver.
|
||||
*/
|
||||
void
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr)
|
||||
errx(1, "driver not running");
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
|
||||
static void
|
||||
ets_airspeed_usage()
|
||||
{
|
||||
fprintf(stderr, "usage: ets_airspeed [options] command\n");
|
||||
fprintf(stderr, "options:\n");
|
||||
fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT);
|
||||
fprintf(stderr, "command:\n");
|
||||
fprintf(stderr, "\tstart|stop|reset|test|info\n");
|
||||
}
|
||||
|
||||
int
|
||||
ets_airspeed_main(int argc, char *argv[])
|
||||
{
|
||||
int i2c_bus = PX4_I2C_BUS_DEFAULT;
|
||||
|
||||
int i;
|
||||
for (i = 1; i < argc; i++) {
|
||||
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
|
||||
if (argc > i + 1) {
|
||||
i2c_bus = atoi(argv[i + 1]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "start"))
|
||||
ets_airspeed::start(i2c_bus);
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
ets_airspeed::stop();
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[1], "test"))
|
||||
ets_airspeed::test();
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "reset"))
|
||||
ets_airspeed::reset();
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
|
||||
ets_airspeed::info();
|
||||
|
||||
ets_airspeed_usage();
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Makefile to build the Eagle Tree Airspeed V3 driver.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = ets_airspeed
|
||||
MODULE_STACKSIZE = 1024
|
||||
|
||||
SRCS = ets_airspeed.cpp
|
||||
|
|
@ -42,9 +42,11 @@
|
|||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <unistd.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
|
||||
static int airspeed_sub = -1;
|
||||
static int battery_sub = -1;
|
||||
static int sensor_sub = -1;
|
||||
|
||||
|
|
@ -52,6 +54,7 @@ void messages_init(void)
|
|||
{
|
||||
battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
}
|
||||
|
||||
void build_eam_response(uint8_t *buffer, int *size)
|
||||
|
|
@ -81,6 +84,15 @@ void build_eam_response(uint8_t *buffer, int *size)
|
|||
msg.altitude_L = (uint8_t)alt & 0xff;
|
||||
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
|
||||
|
||||
/* get a local copy of the current sensor values */
|
||||
struct airspeed_s airspeed;
|
||||
memset(&airspeed, 0, sizeof(airspeed));
|
||||
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
|
||||
|
||||
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
|
||||
msg.speed_L = (uint8_t)speed & 0xff;
|
||||
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
|
||||
|
||||
msg.stop = STOP_BYTE;
|
||||
|
||||
memcpy(buffer, &msg, *size);
|
||||
|
|
|
|||
|
|
@ -676,22 +676,22 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
|
||||
const int calibration_count = 2500;
|
||||
|
||||
int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
|
||||
struct differential_pressure_s differential_pressure;
|
||||
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
struct differential_pressure_s diff_pres;
|
||||
|
||||
int calibration_counter = 0;
|
||||
float airspeed_offset = 0.0f;
|
||||
float diff_pres_offset = 0.0f;
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } };
|
||||
struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
|
||||
airspeed_offset += differential_pressure.voltage;
|
||||
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
||||
diff_pres_offset += diff_pres.differential_pressure_pa;
|
||||
calibration_counter++;
|
||||
|
||||
} else if (poll_ret == 0) {
|
||||
|
|
@ -701,11 +701,11 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
}
|
||||
}
|
||||
|
||||
airspeed_offset = airspeed_offset / calibration_count;
|
||||
diff_pres_offset = diff_pres_offset / calibration_count;
|
||||
|
||||
if (isfinite(airspeed_offset)) {
|
||||
if (isfinite(diff_pres_offset)) {
|
||||
|
||||
if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
|
||||
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
||||
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
|
||||
}
|
||||
|
||||
|
|
@ -735,7 +735,7 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
|
|||
status->flag_preflight_airspeed_calibration = false;
|
||||
state_machine_publish(status_pub, status, mavlink_fd);
|
||||
|
||||
close(sub_differential_pressure);
|
||||
close(diff_pres_sub);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -1356,10 +1356,10 @@ int commander_thread_main(int argc, char *argv[])
|
|||
struct sensor_combined_s sensors;
|
||||
memset(&sensors, 0, sizeof(sensors));
|
||||
|
||||
int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
struct differential_pressure_s differential_pressure;
|
||||
memset(&differential_pressure, 0, sizeof(differential_pressure));
|
||||
uint64_t last_differential_pressure_time = 0;
|
||||
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
struct differential_pressure_s diff_pres;
|
||||
memset(&diff_pres, 0, sizeof(diff_pres));
|
||||
uint64_t last_diff_pres_time = 0;
|
||||
|
||||
/* Subscribe to command topic */
|
||||
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
|
|
@ -1414,11 +1414,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
|
||||
}
|
||||
|
||||
orb_check(differential_pressure_sub, &new_data);
|
||||
orb_check(diff_pres_sub, &new_data);
|
||||
|
||||
if (new_data) {
|
||||
orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
|
||||
last_differential_pressure_time = differential_pressure.timestamp;
|
||||
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
||||
last_diff_pres_time = diff_pres.timestamp;
|
||||
}
|
||||
|
||||
orb_check(cmd_sub, &new_data);
|
||||
|
|
@ -1633,7 +1633,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/* Check for valid airspeed/differential pressure measurements */
|
||||
if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
|
||||
if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
|
||||
current_status.flag_airspeed_valid = true;
|
||||
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -71,6 +71,7 @@
|
|||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
|
|
@ -443,7 +444,8 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
struct vehicle_vicon_position_s vicon_pos;
|
||||
struct optical_flow_s flow;
|
||||
struct battery_status_s batt;
|
||||
struct differential_pressure_s diff_pressure;
|
||||
struct differential_pressure_s diff_pres;
|
||||
struct airspeed_s airspeed;
|
||||
} buf;
|
||||
memset(&buf, 0, sizeof(buf));
|
||||
|
||||
|
|
@ -461,7 +463,8 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
int vicon_pos_sub;
|
||||
int flow_sub;
|
||||
int batt_sub;
|
||||
int diff_pressure_sub;
|
||||
int diff_pres_sub;
|
||||
int airspeed_sub;
|
||||
} subs;
|
||||
|
||||
/* --- MANAGEMENT - LOGGING COMMAND --- */
|
||||
|
|
@ -558,11 +561,18 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
|
||||
/* --- DIFFERENTIAL PRESSURE --- */
|
||||
/* subscribe to ORB for flow measurements */
|
||||
subs.diff_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
fds[fdsc_count].fd = subs.diff_pressure_sub;
|
||||
subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
fds[fdsc_count].fd = subs.diff_pres_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* --- AIRSPEED --- */
|
||||
/* subscribe to ORB for airspeed */
|
||||
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
fds[fdsc_count].fd = subs.airspeed_sub;
|
||||
fds[fdsc_count].events = POLLIN;
|
||||
fdsc_count++;
|
||||
|
||||
/* WARNING: If you get the error message below,
|
||||
* then the number of registered messages (fdsc)
|
||||
* differs from the number of messages in the above list.
|
||||
|
|
@ -654,7 +664,8 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
|
||||
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
|
||||
orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
|
||||
orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure);
|
||||
orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres);
|
||||
orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
|
||||
orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
|
||||
|
||||
/* if skipping is on or logging is disabled, ignore */
|
||||
|
|
@ -691,9 +702,9 @@ int sdlog_thread_main(int argc, char *argv[])
|
|||
.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
|
||||
.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
|
||||
.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
|
||||
.diff_pressure = buf.diff_pressure.differential_pressure_mbar,
|
||||
.ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s,
|
||||
.true_airspeed = buf.diff_pressure.true_airspeed_m_s
|
||||
.diff_pressure = buf.diff_pres.differential_pressure_pa,
|
||||
.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
|
||||
.true_airspeed = buf.airspeed.true_airspeed_m_s
|
||||
};
|
||||
|
||||
/* put into buffer for later IO */
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
|
|||
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
|
||||
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(SENS_VAIR_OFF, 2.5f);
|
||||
PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
|
||||
|
|
|
|||
|
|
@ -77,6 +77,7 @@
|
|||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
|
||||
#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
|
||||
#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */
|
||||
|
|
@ -98,6 +99,12 @@
|
|||
#define BAT_VOL_LOWPASS_2 0.01f
|
||||
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
|
||||
|
||||
/**
|
||||
* HACK - true temperature is much less than indicated temperature in baro,
|
||||
* subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
|
||||
*/
|
||||
#define PCB_TEMP_ESTIMATE_DEG 5.0f
|
||||
|
||||
#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */
|
||||
|
||||
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
|
||||
|
|
@ -156,6 +163,8 @@ private:
|
|||
int _mag_sub; /**< raw mag data subscription */
|
||||
int _rc_sub; /**< raw rc channels data subscription */
|
||||
int _baro_sub; /**< raw baro data subscription */
|
||||
int _airspeed_sub; /**< airspeed subscription */
|
||||
int _diff_pres_sub; /**< raw differential pressure subscription */
|
||||
int _vstatus_sub; /**< vehicle status subscription */
|
||||
int _params_sub; /**< notification of parameter updates */
|
||||
int _manual_control_sub; /**< notification of manual control updates */
|
||||
|
|
@ -165,13 +174,15 @@ private:
|
|||
orb_advert_t _rc_pub; /**< raw r/c control topic */
|
||||
orb_advert_t _battery_pub; /**< battery status */
|
||||
orb_advert_t _airspeed_pub; /**< airspeed */
|
||||
orb_advert_t _diff_pres_pub; /**< differential_pressure */
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
struct rc_channels_s _rc; /**< r/c channel data */
|
||||
struct battery_status_s _battery_status; /**< battery status */
|
||||
struct baro_report _barometer; /**< barometer data */
|
||||
struct differential_pressure_s _differential_pressure;
|
||||
struct differential_pressure_s _diff_pres;
|
||||
struct airspeed_s _airspeed;
|
||||
|
||||
struct {
|
||||
float min[_rc_max_chan_count];
|
||||
|
|
@ -187,7 +198,7 @@ private:
|
|||
float mag_scale[3];
|
||||
float accel_offset[3];
|
||||
float accel_scale[3];
|
||||
float airspeed_offset;
|
||||
int diff_pres_offset_pa;
|
||||
|
||||
int rc_type;
|
||||
|
||||
|
|
@ -236,7 +247,7 @@ private:
|
|||
param_t accel_scale[3];
|
||||
param_t mag_offset[3];
|
||||
param_t mag_scale[3];
|
||||
param_t airspeed_offset;
|
||||
param_t diff_pres_offset_pa;
|
||||
|
||||
param_t rc_map_roll;
|
||||
param_t rc_map_pitch;
|
||||
|
|
@ -330,6 +341,14 @@ private:
|
|||
*/
|
||||
void baro_poll(struct sensor_combined_s &raw);
|
||||
|
||||
/**
|
||||
* Poll the differential pressure sensor for updated data.
|
||||
*
|
||||
* @param raw Combined sensor data structure into which
|
||||
* data should be returned.
|
||||
*/
|
||||
void diff_pres_poll(struct sensor_combined_s &raw);
|
||||
|
||||
/**
|
||||
* Check for changes in vehicle status.
|
||||
*/
|
||||
|
|
@ -400,6 +419,7 @@ Sensors::Sensors() :
|
|||
_rc_pub(-1),
|
||||
_battery_pub(-1),
|
||||
_airspeed_pub(-1),
|
||||
_diff_pres_pub(-1),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
|
||||
|
|
@ -484,8 +504,8 @@ Sensors::Sensors() :
|
|||
_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
|
||||
_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
|
||||
|
||||
/*Airspeed offset */
|
||||
_parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF");
|
||||
/* Differential pressure offset */
|
||||
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
|
||||
|
||||
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
|
||||
|
||||
|
|
@ -695,7 +715,7 @@ Sensors::parameters_update()
|
|||
param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
|
||||
|
||||
/* Airspeed offset */
|
||||
param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset));
|
||||
param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
|
||||
|
||||
/* scaling of ADC ticks to battery voltage */
|
||||
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
|
||||
|
|
@ -889,6 +909,32 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::diff_pres_poll(struct sensor_combined_s &raw)
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_diff_pres_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
|
||||
|
||||
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
|
||||
raw.differential_pressure_counter++;
|
||||
|
||||
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
|
||||
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f,
|
||||
raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
|
||||
|
||||
/* announce the airspeed if needed, just publish else */
|
||||
if (_airspeed_pub > 0) {
|
||||
orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
|
||||
|
||||
} else {
|
||||
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::vehicle_status_poll()
|
||||
{
|
||||
|
|
@ -1047,31 +1093,18 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
|||
*/
|
||||
if (voltage > 0.4f) {
|
||||
|
||||
float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor
|
||||
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
|
||||
|
||||
float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f,
|
||||
_barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa
|
||||
// XXX HACK - true temperature is much less than indicated temperature in baro,
|
||||
// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
|
||||
|
||||
float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
|
||||
|
||||
//printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true);
|
||||
|
||||
_differential_pressure.timestamp = hrt_absolute_time();
|
||||
_differential_pressure.static_pressure_mbar = _barometer.pressure;
|
||||
_differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f;
|
||||
_differential_pressure.temperature_celcius = _barometer.temperature;
|
||||
_differential_pressure.indicated_airspeed_m_s = airspeed_indicated;
|
||||
_differential_pressure.true_airspeed_m_s = airspeed_true;
|
||||
_differential_pressure.voltage = voltage;
|
||||
_diff_pres.timestamp = hrt_absolute_time();
|
||||
_diff_pres.differential_pressure_pa = diff_pres_pa;
|
||||
_diff_pres.voltage = voltage;
|
||||
|
||||
/* announce the airspeed if needed, just publish else */
|
||||
if (_airspeed_pub > 0) {
|
||||
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_differential_pressure);
|
||||
if (_diff_pres_pub > 0) {
|
||||
orb_publish(ORB_ID(differential_pressure), _diff_pres_pub, &_diff_pres);
|
||||
|
||||
} else {
|
||||
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure);
|
||||
_diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1310,6 +1343,7 @@ Sensors::task_main()
|
|||
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
|
||||
_rc_sub = orb_subscribe(ORB_ID(input_rc));
|
||||
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
|
||||
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
|
|
@ -1336,6 +1370,7 @@ Sensors::task_main()
|
|||
gyro_poll(raw);
|
||||
mag_poll(raw);
|
||||
baro_poll(raw);
|
||||
diff_pres_poll(raw);
|
||||
|
||||
parameter_update_poll(true /* forced */);
|
||||
|
||||
|
|
@ -1384,6 +1419,8 @@ Sensors::task_main()
|
|||
/* check battery voltage */
|
||||
adc_poll(raw);
|
||||
|
||||
diff_pres_poll(raw);
|
||||
|
||||
/* Inform other processes that new data is available to copy */
|
||||
if (_publishing)
|
||||
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
|
||||
|
|
|
|||
|
|
@ -97,7 +97,7 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp
|
|||
float density = get_air_density(static_pressure, temperature_celsius);
|
||||
if (density < 0.0001f || !isfinite(density)) {
|
||||
density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
|
||||
printf("[airspeed] Invalid air density, using density at sea level\n");
|
||||
// printf("[airspeed] Invalid air density, using density at sea level\n");
|
||||
}
|
||||
|
||||
float pressure_difference = total_pressure - static_pressure;
|
||||
|
|
|
|||
|
|
@ -122,6 +122,9 @@ ORB_DEFINE(optical_flow, struct optical_flow_s);
|
|||
#include "topics/omnidirectional_flow.h"
|
||||
ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s);
|
||||
|
||||
#include "topics/airspeed.h"
|
||||
ORB_DEFINE(airspeed, struct airspeed_s);
|
||||
|
||||
#include "topics/differential_pressure.h"
|
||||
ORB_DEFINE(differential_pressure, struct differential_pressure_s);
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,67 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file airspeed.h
|
||||
*
|
||||
* Definition of airspeed topic
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_AIRSPEED_H_
|
||||
#define TOPIC_AIRSPEED_H_
|
||||
|
||||
#include "../uORB.h"
|
||||
#include <stdint.h>
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Airspeed
|
||||
*/
|
||||
struct airspeed_s {
|
||||
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
|
||||
float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */
|
||||
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(airspeed);
|
||||
|
||||
#endif
|
||||
|
|
@ -49,16 +49,14 @@
|
|||
*/
|
||||
|
||||
/**
|
||||
* Differential pressure and airspeed
|
||||
* Differential pressure.
|
||||
*/
|
||||
struct differential_pressure_s {
|
||||
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
|
||||
float static_pressure_mbar; /**< Static / environment pressure */
|
||||
float differential_pressure_mbar; /**< Differential pressure reading */
|
||||
float temperature_celcius; /**< ambient temperature in celcius, -1 if unknown */
|
||||
float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */
|
||||
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
|
||||
float voltage; /**< Voltage from the airspeed sensor (voltage divider already compensated) */
|
||||
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
|
||||
uint16_t differential_pressure_pa; /**< Differential pressure reading */
|
||||
uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */
|
||||
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -103,6 +103,8 @@ struct sensor_combined_s {
|
|||
float mcu_temp_celcius; /**< Internal temperature measurement of MCU */
|
||||
uint32_t baro_counter; /**< Number of raw baro measurements taken */
|
||||
|
||||
float differential_pressure_pa; /**< Airspeed sensor differential pressure */
|
||||
uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue