dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #343 from PX4/meas_split 

ETS airspeed and MEAS airspeed combined driver
This commit is contained in:
Lorenz Meier 2013-07-28 06:41:42 -07:00
parent 30405331bd 382c637fab
commit 57db5d912b
11 changed files with 1187 additions and 412 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
makefiles/config_px4fmu-v1_default.mk
View File

@ -32,7 +32,9 @@ MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/mkblctrl
MODULES += drivers/md25
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
MODULES += modules/sensors
#

378
src/drivers/airspeed/airspeed.cpp Normal file
View File

@ -0,0 +1,378 @@
/****************************************************************************
*
* 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>
#include <drivers/airspeed/airspeed.h>
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
I2C("Airspeed", 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.0f),
_airspeed_pub(-1),
_conversion_interval(conversion_interval),
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "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));
}
Airspeed::~Airspeed()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
}
int
Airspeed::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)
warnx("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
Airspeed::probe()
{
return measure();
}
int
Airspeed::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;
case AIRSPEEDIOCSSCALE: {
struct airspeed_scale *s = (struct airspeed_scale*)arg;
_diff_pres_offset = s->offset_pa;
return OK;
}
case AIRSPEEDIOCGSCALE: {
struct airspeed_scale *s = (struct airspeed_scale*)arg;
s->offset_pa = _diff_pres_offset;
s->scale = 1.0f;
return OK;
}
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
ssize_t
Airspeed::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;
}
void
Airspeed::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)&Airspeed::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
Airspeed::stop()
{
work_cancel(HPWORK, &_work);
}
void
Airspeed::cycle_trampoline(void *arg)
{
Airspeed *dev = (Airspeed *)arg;
dev->cycle();
}
void
Airspeed::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
warnx("poll interval: %u ticks", _measure_ticks);
warnx("report queue: %u (%u/%u @ %p)",
_num_reports, _oldest_report, _next_report, _reports);
}

169
src/drivers/airspeed/airspeed.h Normal file
View File

@ -0,0 +1,169 @@
/****************************************************************************
*
* 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.h
* @author Simon Wilks
*
* Generic driver for airspeed sensors 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
/* 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 __EXPORT Airspeed : public device::I2C
{
public:
Airspeed(int bus, int address, unsigned conversion_interval);
virtual ~Airspeed();
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.
*/
virtual void print_info();
protected:
virtual int probe();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
virtual void cycle() = 0;
virtual int measure() = 0;
virtual int collect() = 0;
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;
float _diff_pres_offset;
orb_advert_t _airspeed_pub;
unsigned _conversion_interval;
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();
/**
* 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)

38
src/drivers/airspeed/module.mk Normal file
View File

@ -0,0 +1,38 @@
############################################################################
#
# 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 generic airspeed driver.
#
SRCS = airspeed.cpp

9
src/drivers/drv_airspeed.h
View File

@ -57,5 +57,14 @@
#define _AIRSPEEDIOCBASE (0x7700)
#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n))
#define AIRSPEEDIOCSSCALE __AIRSPEEDIOC(0)
#define AIRSPEEDIOCGSCALE __AIRSPEEDIOC(1)
/** airspeed scaling factors; out = (in * Vscale) + offset */
struct airspeed_scale {
float offset_pa;
float scale;
};
#endif /* _DRV_AIRSPEED_H */

390
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -72,9 +72,7 @@
#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
#include <drivers/airspeed/airspeed.h>
/* I2C bus address */
#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */
@ -91,336 +89,32 @@
/* 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
class ETSAirspeed : public Airspeed
{
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);
virtual void cycle();
virtual int measure();
virtual int collect();
};
/* 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"))
ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address,
CONVERSION_INTERVAL)
{
// 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
@ -464,8 +158,6 @@ ETSAirspeed::collect()
uint16_t diff_pres_pa = val[1] << 8 | val[0];
// XXX move the parameter read out of the driver.
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;
@ -505,47 +197,6 @@ ETSAirspeed::collect()
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()
{
@ -571,7 +222,7 @@ ETSAirspeed::cycle()
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&ETSAirspeed::cycle_trampoline,
(worker_t)&Airspeed::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
@ -589,22 +240,11 @@ ETSAirspeed::cycle()
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&ETSAirspeed::cycle_trampoline,
(worker_t)&Airspeed::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.
*/
@ -642,7 +282,7 @@ start(int i2c_bus)
if (g_dev == nullptr)
goto fail;
if (OK != g_dev->init())
if (OK != g_dev->Airspeed::init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
@ -779,11 +419,11 @@ info()
static void
ets_airspeed_usage()
{
fprintf(stderr, "usage: ets_airspeed command [options]\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");
warnx("usage: ets_airspeed command [options]");
warnx("options:");
warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
warnx("command:");
warnx("\tstart|stop|reset|test|info");
}
int

2
src/drivers/ets_airspeed/module.mk
View File

@ -36,6 +36,6 @@
#
MODULE_COMMAND = ets_airspeed
MODULE_STACKSIZE = 1024
MODULE_STACKSIZE = 2048
SRCS = ets_airspeed.cpp

506
src/drivers/meas_airspeed/meas_airspeed.cpp Normal file
View File

@ -0,0 +1,506 @@
/****************************************************************************
*
* 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 meas_airspeed.cpp
* @author Lorenz Meier
* @author Simon Wilks
*
* Driver for the MEAS Spec series connected via I2C.
*
* Supported sensors:
*
* - MS4525DO (http://www.meas-spec.com/downloads/MS4525DO.pdf)
* - untested: MS5525DSO (http://www.meas-spec.com/downloads/MS5525DSO.pdf)
*
* Interface application notes:
*
* - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
*/
#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>
#include <drivers/airspeed/airspeed.h>
/* I2C bus address is 1010001x */
#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */
/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */
/* Register address */
#define ADDR_READ_MR 0x00 /* write to this address to start conversion */
#define ADDR_READ_DF2 0x00 /* read from this address to read pressure only */
#define ADDR_READ_DF3 0x01
#define ADDR_READ_DF4 0x02 /* read from this address to read pressure and temp */
/* Measurement rate is 100Hz */
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
class MEASAirspeed : public Airspeed
{
public:
MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO);
protected:
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
virtual void cycle();
virtual int measure();
virtual int collect();
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
CONVERSION_INTERVAL)
{
}
int
MEASAirspeed::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
uint8_t cmd = 0;
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
MEASAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[4] = {0, 0, 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;
}
uint8_t status = val[0] & 0xC0;
if (status == 2) {
log("err: stale data");
} else if (status == 3) {
log("err: fault");
}
uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
uint16_t temp = (val[3] & 0xE0) << 8 | val[2];
diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
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].temperature = temp;
_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
MEASAirspeed::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)&Airspeed::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)&Airspeed::cycle_trampoline,
this,
USEC2TICK(CONVERSION_INTERVAL));
}
/**
* Local functions in support of the shell command.
*/
namespace meas_airspeed
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
MEASAirspeed *g_dev = nullptr;
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, try the MS4525DO first */
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO);
/* check if the MS4525DO was instantiated */
if (g_dev == nullptr)
goto fail;
/* try the MS5525DSO next if init fails */
if (OK != g_dev->Airspeed::init()) {
delete g_dev;
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO);
/* check if the MS5525DSO was instantiated */
if (g_dev == nullptr)
goto fail;
/* both versions failed if the init for the MS5525DSO fails, give up */
if (OK != g_dev->Airspeed::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 'meas_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);
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}
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
meas_airspeed_usage()
{
warnx("usage: meas_airspeed command [options]");
warnx("options:");
warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
warnx("command:");
warnx("\tstart|stop|reset|test|info");
}
int
meas_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"))
meas_airspeed::start(i2c_bus);
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop"))
meas_airspeed::stop();
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
meas_airspeed::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
meas_airspeed::reset();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
meas_airspeed::info();
meas_airspeed_usage();
exit(0);
}

41
src/drivers/meas_airspeed/module.mk Normal file
View File

@ -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 MEAS Spec airspeed sensor driver.
#
MODULE_COMMAND = meas_airspeed
MODULE_STACKSIZE = 2048
SRCS = meas_airspeed.cpp

2
src/modules/sensors/sensor_params.c
View File

@ -68,7 +68,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_INT32(SENS_DPRES_OFF, 1667);
PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);

62
src/modules/sensors/sensors.cpp
View File

@ -60,6 +60,7 @@
#include <drivers/drv_baro.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_adc.h>
#include <drivers/drv_airspeed.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@ -197,7 +198,7 @@ private:
float mag_scale[3];
float accel_offset[3];
float accel_scale[3];
int diff_pres_offset_pa;
float diff_pres_offset_pa;
int rc_type;
@ -230,6 +231,7 @@ private:
float battery_voltage_scaling;
int rc_rl1_DSM_VCC_control;
} _parameters; /**< local copies of interesting parameters */
struct {
@ -280,6 +282,7 @@ private:
param_t battery_voltage_scaling;
param_t rc_rl1_DSM_VCC_control;
} _parameter_handles; /**< handles for interesting parameters */
@ -554,25 +557,11 @@ Sensors::parameters_update()
/* rc values */
for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) {
warnx("Failed getting min for chan %d", i);
}
if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) {
warnx("Failed getting trim for chan %d", i);
}
if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) {
warnx("Failed getting max for chan %d", i);
}
if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) {
warnx("Failed getting rev for chan %d", i);
}
if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) {
warnx("Failed getting dead zone for chan %d", i);
}
param_get(_parameter_handles.min[i], &(_parameters.min[i]));
param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
param_get(_parameter_handles.max[i], &(_parameters.max[i]));
param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
@ -662,21 +651,10 @@ Sensors::parameters_update()
warnx("Failed getting mode aux 5 index");
}
if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) {
warnx("Failed getting rc scaling for roll");
}
if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) {
warnx("Failed getting rc scaling for pitch");
}
if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) {
warnx("Failed getting rc scaling for yaw");
}
if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) {
warnx("Failed getting rc scaling for flaps");
}
param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@ -1041,6 +1019,20 @@ Sensors::parameter_update_poll(bool forced)
close(fd);
fd = open(AIRSPEED_DEVICE_PATH, 0);
/* this sensor is optional, abort without error */
if (fd > 0) {
struct airspeed_scale airscale = {
_parameters.diff_pres_offset_pa,
1.0f,
};
if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
warn("WARNING: failed to set scale / offsets for airspeed sensor");
}
#if 0
printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);