fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 14:08:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master

Browse Source
This commit is contained in:
Lorenz Meier
2013-08-14 22:53:42 +02:00
parent 27d0885453 d2f19c7d84
commit aebdd6e059
116 changed files with 11509 additions and 1195 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/airspeed/airspeed.cpp
+378
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);
}
src/drivers/airspeed/airspeed.h
+169
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)
src/drivers/airspeed/module.mk
+38
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
src/drivers/device/cdev.cpp
+8 -8
View File
@@ -111,21 +111,21 @@ CDev::~CDev()
int
CDev::init()
{
int ret = OK;
// base class init first
ret = Device::init();
int ret = Device::init();
if (ret != OK)
goto out;
// now register the driver
ret = register_driver(_devname, &fops, 0666, (void *)this);
if (_devname != nullptr) {
ret = register_driver(_devname, &fops, 0666, (void *)this);
if (ret != OK)
goto out;
if (ret != OK)
goto out;
_registered = true;
_registered = true;
}
out:
return ret;
@@ -395,4 +395,4 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup)
return cdev->poll(filp, fds, setup);
}
} // namespace device
} // namespace device
src/drivers/device/device.cpp
+18 -1
View File
@@ -223,5 +223,22 @@ interrupt(int irq, void *context)
return OK;
}
int
Device::read(unsigned offset, void *data, unsigned count)
{
return -ENODEV;
}
} // namespace device
int
Device::write(unsigned offset, void *data, unsigned count)
{
return -ENODEV;
}
int
Device::ioctl(unsigned operation, unsigned &arg)
{
return -ENODEV;
}
} // namespace device
src/drivers/device/device.h
+57 -13
View File
@@ -68,11 +68,62 @@ namespace device __EXPORT
class __EXPORT Device
{
public:
/**
* Destructor.
*
* Public so that anonymous devices can be destroyed.
*/
virtual ~Device();
/**
* Interrupt handler.
*/
virtual void interrupt(void *ctx); /**< interrupt handler */
/*
* Direct access methods.
*/
/**
* Initialise the driver and make it ready for use.
*
* @return OK if the driver initialized OK, negative errno otherwise;
*/
virtual int init();
/**
* Read directly from the device.
*
* The actual size of each unit quantity is device-specific.
*
* @param offset The device address at which to start reading
* @param data The buffer into which the read values should be placed.
* @param count The number of items to read.
* @return The number of items read on success, negative errno otherwise.
*/
virtual int read(unsigned address, void *data, unsigned count);
/**
* Write directly to the device.
*
* The actual size of each unit quantity is device-specific.
*
* @param address The device address at which to start writing.
* @param data The buffer from which values should be read.
* @param count The number of items to write.
* @return The number of items written on success, negative errno otherwise.
*/
virtual int write(unsigned address, void *data, unsigned count);
/**
* Perform a device-specific operation.
*
* @param operation The operation to perform.
* @param arg An argument to the operation.
* @return Negative errno on error, OK or positive value on success.
*/
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
const char *_name; /**< driver name */
bool _debug_enabled; /**< if true, debug messages are printed */
@@ -85,14 +136,6 @@ protected:
*/
Device(const char *name,
int irq = 0);
~Device();
/**
* Initialise the driver and make it ready for use.
*
* @return OK if the driver initialised OK.
*/
virtual int init();
/**
* Enable the device interrupt
@@ -189,7 +232,7 @@ public:
/**
* Destructor
*/
~CDev();
virtual ~CDev();
virtual int init();
@@ -282,6 +325,7 @@ public:
* Test whether the device is currently open.
*
* This can be used to avoid tearing down a device that is still active.
* Note - not virtual, cannot be overridden by a subclass.
*
* @return True if the device is currently open.
*/
@@ -396,9 +440,9 @@ public:
const char *devname,
uint32_t base,
int irq = 0);
~PIO();
virtual ~PIO();
int init();
virtual int init();
protected:
@@ -407,7 +451,7 @@ protected:
*
* @param offset Register offset in bytes from the base address.
*/
uint32_t reg(uint32_t offset) {
uint32_t reg(uint32_t offset) {
return *(volatile uint32_t *)(_base + offset);
}
@@ -444,4 +488,4 @@ private:
} // namespace device
#endif /* _DEVICE_DEVICE_H */
#endif /* _DEVICE_DEVICE_H */
src/drivers/device/i2c.h
+6 -8
View File
@@ -55,13 +55,11 @@ class __EXPORT I2C : public CDev
public:
/**
* Get the address
*/
uint16_t get_address() {
return _address;
}
/**
* Get the address
*/
int16_t get_address() { return _address; }
protected:
/**
* The number of times a read or write operation will be retried on
@@ -90,7 +88,7 @@ protected:
uint16_t address,
uint32_t frequency,
int irq = 0);
~I2C();
virtual ~I2C();
virtual int init();
src/drivers/device/ringbuffer.h
+203
View File
@@ -0,0 +1,203 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* 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 ringbuffer.h
*
* A simple ringbuffer template.
*/
#pragma once
template<typename T>
class RingBuffer {
public:
RingBuffer(unsigned size);
virtual ~RingBuffer();
/**
* Put an item into the buffer.
*
* @param val Item to put
* @return true if the item was put, false if the buffer is full
*/
bool put(T &val);
/**
* Put an item into the buffer.
*
* @param val Item to put
* @return true if the item was put, false if the buffer is full
*/
bool put(const T &val);
/**
* Get an item from the buffer.
*
* @param val Item that was gotten
* @return true if an item was got, false if the buffer was empty.
*/
bool get(T &val);
/**
* Get an item from the buffer (scalars only).
*
* @return The value that was fetched, or zero if the buffer was
* empty.
*/
T get(void);
/*
* Get the number of slots free in the buffer.
*
* @return The number of items that can be put into the buffer before
* it becomes full.
*/
unsigned space(void);
/*
* Get the number of items in the buffer.
*
* @return The number of items that can be got from the buffer before
* it becomes empty.
*/
unsigned count(void);
/*
* Returns true if the buffer is empty.
*/
bool empty() { return _tail == _head; }
/*
* Returns true if the buffer is full.
*/
bool full() { return _next(_head) == _tail; }
/*
* Returns the capacity of the buffer, or zero if the buffer could
* not be allocated.
*/
unsigned size() { return (_buf != nullptr) ? _size : 0; }
/*
* Empties the buffer.
*/
void flush() { _head = _tail = _size; }
private:
T *const _buf;
const unsigned _size;
volatile unsigned _head; /**< insertion point */
volatile unsigned _tail; /**< removal point */
unsigned _next(unsigned index);
};
template <typename T>
RingBuffer<T>::RingBuffer(unsigned with_size) :
_buf(new T[with_size + 1]),
_size(with_size),
_head(with_size),
_tail(with_size)
{}
template <typename T>
RingBuffer<T>::~RingBuffer()
{
if (_buf != nullptr)
delete[] _buf;
}
template <typename T>
bool RingBuffer<T>::put(T &val)
{
unsigned next = _next(_head);
if (next != _tail) {
_buf[_head] = val;
_head = next;
return true;
} else {
return false;
}
}
template <typename T>
bool RingBuffer<T>::put(const T &val)
{
unsigned next = _next(_head);
if (next != _tail) {
_buf[_head] = val;
_head = next;
return true;
} else {
return false;
}
}
template <typename T>
bool RingBuffer<T>::get(T &val)
{
if (_tail != _head) {
val = _buf[_tail];
_tail = _next(_tail);
return true;
} else {
return false;
}
}
template <typename T>
T RingBuffer<T>::get(void)
{
T val;
return get(val) ? val : 0;
}
template <typename T>
unsigned RingBuffer<T>::space(void)
{
return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1);
}
template <typename T>
unsigned RingBuffer<T>::count(void)
{
return _size - space();
}
template <typename T>
unsigned RingBuffer<T>::_next(unsigned index)
{
return (0 == index) ? _size : (index - 1);
}
src/drivers/device/spi.h
+1 -1
View File
@@ -71,7 +71,7 @@ protected:
enum spi_mode_e mode,
uint32_t frequency,
int irq = 0);
~SPI();
virtual ~SPI();
virtual int init();
src/drivers/drv_airspeed.h
+9
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 */
src/drivers/drv_pwm_output.h
+1 -4
View File
@@ -118,11 +118,8 @@ ORB_DECLARE(output_pwm);
/** start DSM bind */
#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7)
/** stop DSM bind */
#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8)
/** Power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9)
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
src/drivers/ets_airspeed/ets_airspeed.cpp
+27 -375
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
@@ -463,9 +157,15 @@ ETSAirspeed::collect()
}
uint16_t diff_pres_pa = val[1] << 8 | val[0];
if (diff_pres_pa == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
// caller could end up using this value as part of an
// average
log("zero value from sensor");
return -1;
}
// 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 +205,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 +230,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 +248,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 +290,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 */
@@ -735,6 +383,10 @@ test()
warnx("diff pressure: %d pa", report.differential_pressure_pa);
}
/* reset the sensor polling to its default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
errx(1, "failed to set default rate");
errx(0, "PASS");
}
@@ -779,11 +431,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
src/drivers/ets_airspeed/module.mk
+1 -1
View File
@@ -36,6 +36,6 @@
#
MODULE_COMMAND = ets_airspeed
MODULE_STACKSIZE = 1024
MODULE_STACKSIZE = 2048
SRCS = ets_airspeed.cpp
src/drivers/l3gd20/l3gd20.cpp
+7 -2
View File
@@ -333,8 +333,13 @@ L3GD20::init()
write_reg(ADDR_CTRL_REG4, REG4_BDU);
write_reg(ADDR_CTRL_REG5, 0);
write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
/* disable FIFO. This makes things simpler and ensures we
* aren't getting stale data. It means we must run the hrt
* callback fast enough to not miss data. */
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
set_range(500); /* default to 500dps */
set_samplerate(0); /* max sample rate */
src/drivers/md25/BlockSysIdent.cpp
+8
View File
@@ -0,0 +1,8 @@
#include "BlockSysIdent.hpp"
BlockSysIdent::BlockSysIdent() :
Block(NULL, "SYSID"),
_freq(this, "FREQ"),
_ampl(this, "AMPL")
{
}
src/drivers/md25/BlockSysIdent.hpp
+10
View File
@@ -0,0 +1,10 @@
#include <controllib/block/Block.hpp>
#include <controllib/block/BlockParam.hpp>
class BlockSysIdent : public control::Block {
public:
BlockSysIdent();
private:
control::BlockParam<float> _freq;
control::BlockParam<float> _ampl;
};
src/drivers/md25/md25.cpp
+84 -3
View File
@@ -45,9 +45,16 @@
#include "md25.hpp"
#include <poll.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <systemlib/err.h>
#include <arch/board/board.h>
#include <mavlink/mavlink_log.h>
#include <controllib/uorb/UOrbPublication.hpp>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_hrt.h>
// registers
enum {
@@ -72,6 +79,9 @@ enum {
REG_COMMAND_RW,
};
// File descriptors
static int mavlink_fd;
MD25::MD25(const char *deviceName, int bus,
uint16_t address, uint32_t speed) :
I2C("MD25", deviceName, bus, address, speed),
@@ -106,7 +116,8 @@ MD25::MD25(const char *deviceName, int bus,
setMotor2Speed(0);
resetEncoders();
_setMode(MD25::MODE_UNSIGNED_SPEED);
setSpeedRegulation(true);
setSpeedRegulation(false);
setMotorAccel(10);
setTimeout(true);
}
@@ -298,6 +309,12 @@ int MD25::setDeviceAddress(uint8_t address)
return OK;
}
int MD25::setMotorAccel(uint8_t accel)
{
return _writeUint8(REG_ACCEL_RATE_RW,
accel);
}
int MD25::setMotor1Speed(float value)
{
return _writeUint8(REG_SPEED1_RW,
@@ -451,12 +468,12 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
MD25 md25("/dev/md25", bus, address);
// print status
char buf[200];
char buf[400];
md25.status(buf, sizeof(buf));
printf("%s\n", buf);
// setup for test
md25.setSpeedRegulation(true);
md25.setSpeedRegulation(false);
md25.setTimeout(true);
float dt = 0.1;
float speed = 0.2;
@@ -550,4 +567,68 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
return 0;
}
int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency)
{
printf("md25 sine: starting\n");
// setup
MD25 md25("/dev/md25", bus, address);
// print status
char buf[400];
md25.status(buf, sizeof(buf));
printf("%s\n", buf);
// setup for test
md25.setSpeedRegulation(false);
md25.setTimeout(true);
float dt = 0.01;
float t_final = 60.0;
float prev_revolution = md25.getRevolutions1();
// debug publication
control::UOrbPublication<debug_key_value_s> debug_msg(NULL,
ORB_ID(debug_key_value));
// sine wave for motor 1
md25.resetEncoders();
while (true) {
// input
uint64_t timestamp = hrt_absolute_time();
float t = timestamp/1000000.0f;
float input_value = amplitude*sinf(2*M_PI*frequency*t);
md25.setMotor1Speed(input_value);
// output
md25.readData();
float current_revolution = md25.getRevolutions1();
// send input message
//strncpy(debug_msg.key, "md25 in ", 10);
//debug_msg.timestamp_ms = 1000*timestamp;
//debug_msg.value = input_value;
//debug_msg.update();
// send output message
strncpy(debug_msg.key, "md25 out ", 10);
debug_msg.timestamp_ms = 1000*timestamp;
debug_msg.value = current_revolution;
debug_msg.update();
if (t > t_final) break;
// update for next step
prev_revolution = current_revolution;
// sleep
usleep(1000000 * dt);
}
md25.setMotor1Speed(0);
printf("md25 sine complete\n");
return 0;
}
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
src/drivers/md25/md25.hpp
+17 -1
View File
@@ -46,7 +46,7 @@
#include <poll.h>
#include <stdio.h>
#include <controllib/block/UOrbSubscription.hpp>
#include <controllib/uorb/UOrbSubscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <drivers/device/i2c.h>
@@ -212,6 +212,19 @@ public:
*/
int setDeviceAddress(uint8_t address);
/**
* set motor acceleration
* @param accel
* controls motor speed change (1-10)
* accel rate | time for full fwd. to full rev.
* 1 | 6.375 s
* 2 | 1.6 s
* 3 | 0.675 s
* 5(default) | 1.275 s
* 10 | 0.65 s
*/
int setMotorAccel(uint8_t accel);
/**
* set motor 1 speed
* @param normSpeed normalize speed between -1 and 1
@@ -290,4 +303,7 @@ private:
// unit testing
int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
// sine testing
int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency);
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
src/drivers/md25/md25_main.cpp
+47 -2
View File
@@ -82,7 +82,7 @@ usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n");
fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n");
exit(1);
}
@@ -136,6 +136,28 @@ int md25_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "sine")) {
if (argc < 6) {
printf("usage: md25 sine bus address amp freq\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t address = strtoul(argv[3], nullptr, 0);
float amplitude = atof(argv[4]);
float frequency = atof(argv[5]);
md25Sine(deviceName, bus, address, amplitude, frequency);
exit(0);
}
if (!strcmp(argv[1], "probe")) {
if (argc < 4) {
printf("usage: md25 probe bus address\n");
@@ -162,6 +184,29 @@ int md25_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "read")) {
if (argc < 4) {
printf("usage: md25 read bus address\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t address = strtoul(argv[3], nullptr, 0);
MD25 md25(deviceName, bus, address);
// print status
char buf[400];
md25.status(buf, sizeof(buf));
printf("%s\n", buf);
exit(0);
}
if (!strcmp(argv[1], "search")) {
if (argc < 3) {
printf("usage: md25 search bus\n");
@@ -246,7 +291,7 @@ int md25_thread_main(int argc, char *argv[])
uint8_t address = strtoul(argv[4], nullptr, 0);
// start
MD25 md25("/dev/md25", bus, address);
MD25 md25(deviceName, bus, address);
thread_running = true;
src/drivers/meas_airspeed/meas_airspeed.cpp
+520
View File
@@ -0,0 +1,520 @@
/****************************************************************************
*
* 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 Sarthak Kaingade
* @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 */
/* 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], 4);
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];
// XXX leaving this in until new calculation method has been cross-checked
//diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
//diff_pres_pa -= _diff_pres_offset;
int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1];
dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50;
// XXX we may want to smooth out the readings to remove noise.
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative, enforce absolute value
uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].temperature = temperature;
_reports[_next_report].differential_pressure_pa = diff_press_pa;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) {
_reports[_next_report].max_differential_pressure_pa = diff_press_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);
}
/* reset the sensor polling to its default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
errx(1, "failed to set default rate");
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);
}
src/drivers/meas_airspeed/module.mk
+41
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
src/drivers/mpu6000/mpu6000.cpp
+300 -104
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* 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
@@ -35,6 +35,9 @@
* @file mpu6000.cpp
*
* Driver for the Invensense MPU6000 connected via SPI.
*
* @author Andrew Tridgell
* @author Pat Hickey
*/
#include <nuttx/config.h>
@@ -64,8 +67,10 @@
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
@@ -178,21 +183,33 @@ private:
struct hrt_call _call;
unsigned _call_interval;
struct accel_report _accel_report;
typedef RingBuffer<accel_report> AccelReportBuffer;
AccelReportBuffer *_accel_reports;
struct accel_scale _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
struct gyro_report _gyro_report;
typedef RingBuffer<gyro_report> GyroReportBuffer;
GyroReportBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
unsigned _reads;
unsigned _sample_rate;
perf_counter_t _sample_perf;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
/**
* Start automatic measurement.
*/
@@ -203,6 +220,13 @@ private:
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
void reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@@ -215,7 +239,7 @@ private:
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report ring.
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
@@ -307,14 +331,23 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_gyro(new MPU6000_gyro(this)),
_product(0),
_call_interval(0),
_accel_reports(nullptr),
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_gyro_reports(nullptr),
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_reads(0),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
_sample_rate(1000),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
_accel_filter_x(1000, 30),
_accel_filter_y(1000, 30),
_accel_filter_z(1000, 30),
_gyro_filter_x(1000, 30),
_gyro_filter_y(1000, 30),
_gyro_filter_z(1000, 30)
{
// disable debug() calls
_debug_enabled = false;
@@ -335,8 +368,6 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
memset(&_accel_report, 0, sizeof(_accel_report));
memset(&_gyro_report, 0, sizeof(_gyro_report));
memset(&_call, 0, sizeof(_call));
}
@@ -348,6 +379,12 @@ MPU6000::~MPU6000()
/* delete the gyro subdriver */
delete _gyro;
/* free any existing reports */
if (_accel_reports != nullptr)
delete _accel_reports;
if (_gyro_reports != nullptr)
delete _gyro_reports;
/* delete the perf counter */
perf_free(_sample_perf);
}
@@ -356,6 +393,7 @@ int
MPU6000::init()
{
int ret;
int gyro_ret;
/* do SPI init (and probe) first */
ret = SPI::init();
@@ -366,9 +404,58 @@ MPU6000::init()
return ret;
}
/* advertise sensor topics */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report);
/* allocate basic report buffers */
_accel_reports = new AccelReportBuffer(2);
if (_accel_reports == nullptr)
goto out;
_gyro_reports = new GyroReportBuffer(2);
if (_gyro_reports == nullptr)
goto out;
reset();
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
gyro_ret = _gyro->init();
/* fetch an initial set of measurements for advertisement */
measure();
if (gyro_ret != OK) {
_gyro_topic = -1;
} else {
gyro_report gr;
_gyro_reports->get(gr);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
}
/* advertise accel topic */
accel_report ar;
_accel_reports->get(ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
out:
return ret;
}
void MPU6000::reset()
{
// Chip reset
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
@@ -384,13 +471,13 @@ MPU6000::init()
// SAMPLE RATE
//write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
_set_sample_rate(200); // default sample rate = 200Hz
_set_sample_rate(_sample_rate); // default sample rate = 200Hz
usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the
// system response
_set_dlpf_filter(20);
_set_dlpf_filter(42);
usleep(1000);
// Gyro scale 2000 deg/s ()
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -401,12 +488,6 @@ MPU6000::init()
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
// scaling factor:
// 1/(2^15)*(2000/180)*PI
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
@@ -439,12 +520,6 @@ MPU6000::init()
// Correct accel scale factors of 4096 LSB/g
// scale to m/s^2 ( 1g = 9.81 m/s^2)
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_accel_range_scale = (9.81f / 4096.0f);
_accel_range_m_s2 = 8.0f * 9.81f;
@@ -460,14 +535,6 @@ MPU6000::init()
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000);
/* do CDev init for the gyro device node, keep it optional */
int gyro_ret = _gyro->init();
if (gyro_ret != OK) {
_gyro_topic = -1;
}
return ret;
}
int
@@ -509,6 +576,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
if(div>200) div=200;
if(div<1) div=1;
write_reg(MPUREG_SMPLRT_DIV, div-1);
_sample_rate = 1000 / div;
}
/*
@@ -545,21 +613,33 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
ssize_t
MPU6000::read(struct file *filp, char *buffer, size_t buflen)
{
int ret = 0;
unsigned count = buflen / sizeof(accel_report);
/* buffer must be large enough */
if (buflen < sizeof(_accel_report))
if (count < 1)
return -ENOSPC;
/* if automatic measurement is not enabled */
if (_call_interval == 0)
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
if (_call_interval == 0) {
_accel_reports->flush();
measure();
}
/* copy out the latest reports */
memcpy(buffer, &_accel_report, sizeof(_accel_report));
ret = sizeof(_accel_report);
/* if no data, error (we could block here) */
if (_accel_reports->empty())
return -EAGAIN;
return ret;
/* copy reports out of our buffer to the caller */
accel_report *arp = reinterpret_cast<accel_report *>(buffer);
int transferred = 0;
while (count--) {
if (!_accel_reports->get(*arp++))
break;
transferred++;
}
/* return the number of bytes transferred */
return (transferred * sizeof(accel_report));
}
int
@@ -576,21 +656,33 @@ MPU6000::self_test()
ssize_t
MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
{
int ret = 0;
unsigned count = buflen / sizeof(gyro_report);
/* buffer must be large enough */
if (buflen < sizeof(_gyro_report))
if (count < 1)
return -ENOSPC;
/* if automatic measurement is not enabled */
if (_call_interval == 0)
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
if (_call_interval == 0) {
_gyro_reports->flush();
measure();
}
/* copy out the latest report */
memcpy(buffer, &_gyro_report, sizeof(_gyro_report));
ret = sizeof(_gyro_report);
/* if no data, error (we could block here) */
if (_gyro_reports->empty())
return -EAGAIN;
return ret;
/* copy reports out of our buffer to the caller */
gyro_report *arp = reinterpret_cast<gyro_report *>(buffer);
int transferred = 0;
while (count--) {
if (!_gyro_reports->get(*arp++))
break;
transferred++;
}
/* return the number of bytes transferred */
return (transferred * sizeof(gyro_report));
}
int
@@ -598,6 +690,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCRESET:
reset();
return OK;
case SENSORIOCSPOLLRATE: {
switch (arg) {
@@ -617,8 +713,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* XXX 500Hz is just a wild guess */
return ioctl(filp, SENSORIOCSPOLLRATE, 500);
/* set to same as sample rate per default */
return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate);
/* adjust to a legal polling interval in Hz */
default: {
@@ -632,6 +728,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
if (ticks < 1000)
return -EINVAL;
// adjust filters
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f/ticks;
_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks;
@@ -651,23 +760,51 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH:
/* XXX not implemented */
return -EINVAL;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
AccelReportBuffer *buf = new AccelReportBuffer(arg);
if (nullptr == buf)
return -ENOMEM;
if (buf->size() == 0) {
delete buf;
return -ENOMEM;
}
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _accel_reports;
_accel_reports = buf;
start();
return OK;
}
case SENSORIOCGQUEUEDEPTH:
/* XXX not implemented */
return -EINVAL;
return _accel_reports->size();
case ACCELIOCGSAMPLERATE:
return _sample_rate;
case ACCELIOCSSAMPLERATE:
case ACCELIOCGSAMPLERATE:
_set_sample_rate(arg);
return OK;
_set_sample_rate(arg);
return OK;
case ACCELIOCGLOWPASS:
return _accel_filter_x.get_cutoff_freq();
case ACCELIOCSLOWPASS:
case ACCELIOCGLOWPASS:
_set_dlpf_filter((uint16_t)arg);
// XXX decide on relationship of both filters
// i.e. disable the on-chip filter
//_set_dlpf_filter((uint16_t)arg);
_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
return OK;
case ACCELIOCSSCALE:
@@ -689,12 +826,13 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return OK;
case ACCELIOCSRANGE:
case ACCELIOCGRANGE:
/* XXX not implemented */
// XXX change these two values on set:
// _accel_range_scale = (9.81f / 4096.0f);
// _accel_range_rad_s = 8.0f * 9.81f;
// _accel_range_m_s2 = 8.0f * 9.81f;
return -EINVAL;
case ACCELIOCGRANGE:
return _accel_range_m_s2;
case ACCELIOCSELFTEST:
return self_test();
@@ -713,19 +851,51 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
/* these are shared with the accel side */
case SENSORIOCSPOLLRATE:
case SENSORIOCGPOLLRATE:
case SENSORIOCSQUEUEDEPTH:
case SENSORIOCGQUEUEDEPTH:
case SENSORIOCRESET:
return ioctl(filp, cmd, arg);
case GYROIOCSSAMPLERATE:
case GYROIOCGSAMPLERATE:
_set_sample_rate(arg);
return OK;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
GyroReportBuffer *buf = new GyroReportBuffer(arg);
if (nullptr == buf)
return -ENOMEM;
if (buf->size() == 0) {
delete buf;
return -ENOMEM;
}
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _gyro_reports;
_gyro_reports = buf;
start();
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _gyro_reports->size();
case GYROIOCGSAMPLERATE:
return _sample_rate;
case GYROIOCSSAMPLERATE:
_set_sample_rate(arg);
return OK;
case GYROIOCSLOWPASS:
case GYROIOCGLOWPASS:
_set_dlpf_filter((uint16_t)arg);
return _gyro_filter_x.get_cutoff_freq();
case GYROIOCSLOWPASS:
_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
// XXX check relation to the internal lowpass
//_set_dlpf_filter((uint16_t)arg);
return OK;
case GYROIOCSSCALE:
@@ -739,12 +909,13 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
return OK;
case GYROIOCSRANGE:
case GYROIOCGRANGE:
/* XXX not implemented */
// XXX change these two values on set:
// _gyro_range_scale = xx
// _gyro_range_m_s2 = xx
// _gyro_range_rad_s = xx
return -EINVAL;
case GYROIOCGRANGE:
return _gyro_range_rad_s;
case GYROIOCSELFTEST:
return self_test();
@@ -849,6 +1020,10 @@ MPU6000::start()
/* make sure we are stopped first */
stop();
/* discard any stale data in the buffers */
_accel_reports->flush();
_gyro_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);
}
@@ -862,7 +1037,7 @@ MPU6000::stop()
void
MPU6000::measure_trampoline(void *arg)
{
MPU6000 *dev = (MPU6000 *)arg;
MPU6000 *dev = reinterpret_cast<MPU6000 *>(arg);
/* make another measurement */
dev->measure();
@@ -943,10 +1118,16 @@ MPU6000::measure()
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
/*
* Report buffers.
*/
accel_report arb;
gyro_report grb;
/*
* Adjust and scale results to m/s^2.
*/
_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
grb.timestamp = arb.timestamp = hrt_absolute_time();
/*
@@ -967,40 +1148,53 @@ MPU6000::measure()
/* NOTE: Axes have been swapped to match the board a few lines above. */
_accel_report.x_raw = report.accel_x;
_accel_report.y_raw = report.accel_y;
_accel_report.z_raw = report.accel_z;
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
_accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
_accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
_accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
_accel_report.scaling = _accel_range_scale;
_accel_report.range_m_s2 = _accel_range_m_s2;
float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
_accel_report.temperature_raw = report.temp;
_accel_report.temperature = (report.temp) / 361.0f + 35.0f;
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
_gyro_report.x_raw = report.gyro_x;
_gyro_report.y_raw = report.gyro_y;
_gyro_report.z_raw = report.gyro_z;
arb.temperature_raw = report.temp;
arb.temperature = (report.temp) / 361.0f + 35.0f;
_gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
_gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
_gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
_gyro_report.scaling = _gyro_range_scale;
_gyro_report.range_rad_s = _gyro_range_rad_s;
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
_gyro_report.temperature_raw = report.temp;
_gyro_report.temperature = (report.temp) / 361.0f + 35.0f;
float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
grb.temperature_raw = report.temp;
grb.temperature = (report.temp) / 361.0f + 35.0f;
_accel_reports->put(arb);
_gyro_reports->put(grb);
/* notify anyone waiting for data */
poll_notify(POLLIN);
_gyro->parent_poll_notify();
/* and publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
if (_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
}
/* stop measuring */
@@ -1103,21 +1297,19 @@ fail:
void
test()
{
int fd = -1;
int fd_gyro = -1;
struct accel_report a_report;
struct gyro_report g_report;
accel_report a_report;
gyro_report g_report;
ssize_t sz;
/* get the driver */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
ACCEL_DEVICE_PATH);
/* get the driver */
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
@@ -1129,8 +1321,10 @@ test()
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report))
if (sz != sizeof(a_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(a_report));
err(1, "immediate acc read failed");
}
warnx("single read");
warnx("time: %lld", a_report.timestamp);
@@ -1146,8 +1340,10 @@ test()
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
if (sz != sizeof(g_report))
if (sz != sizeof(g_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(g_report));
err(1, "immediate gyro read failed");
}
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
src/drivers/px4io/px4io.cpp
+35 -28
View File
@@ -206,6 +206,12 @@ private:
bool _dsm_vcc_ctl;
/**
* System armed
*/
bool _system_armed;
/**
* Trampoline to the worker task
*/
@@ -374,7 +380,8 @@ PX4IO::PX4IO() :
_battery_amp_bias(0),
_battery_mamphour_total(0),
_battery_last_timestamp(0),
_dsm_vcc_ctl(false)
_dsm_vcc_ctl(false),
_system_armed(false)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -633,9 +640,11 @@ void
PX4IO::task_main()
{
hrt_abstime last_poll_time = 0;
int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
log("starting");
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
@@ -735,6 +744,25 @@ PX4IO::task_main()
*/
if (fds[3].revents & POLLIN) {
parameter_update_s pupdate;
int32_t dsm_bind_val;
param_t dsm_bind_param;
// See if bind parameter has been set, and reset it to 0
param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
if (dsm_bind_val) {
if (!_system_armed) {
if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
} else {
mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected");
}
} else {
mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected");
}
dsm_bind_val = 0;
param_set(dsm_bind_param, &dsm_bind_val);
}
/* copy to reset the notification */
orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
@@ -842,6 +870,8 @@ PX4IO::io_set_arming_state()
uint16_t set = 0;
uint16_t clear = 0;
_system_armed = vstatus.flag_system_armed;
if (armed.armed && !armed.lockdown) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
@@ -1578,16 +1608,11 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
usleep(500000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
usleep(1000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
usleep(100000);
usleep(50000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
break;
case DSM_BIND_STOP:
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
usleep(50000);
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
usleep(500000);
break;
case DSM_BIND_POWER_UP:
@@ -1829,30 +1854,12 @@ bind(int argc, char *argv[])
else
errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
errx(1, "failed opening console");
warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
warnx("Press CTRL-C or 'c' when done.");
g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
for (;;) {
usleep(500000L);
/* Check if user wants to quit */
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63) {
warnx("Done\n");
g_dev->ioctl(nullptr, DSM_BIND_STOP, 0);
g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
close(console);
exit(0);
}
}
}
exit(0);
}
void
src/drivers/stm32/drv_hrt.c
+10 -10
View File
@@ -70,7 +70,7 @@
#include "stm32_gpio.h"
#include "stm32_tim.h"
#ifdef CONFIG_HRT_TIMER
#ifdef HRT_TIMER
/* HRT configuration */
#if HRT_TIMER == 1
@@ -155,7 +155,7 @@
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
# endif
#else
# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y
# error HRT_TIMER must be a value between 1 and 11
#endif
/*
@@ -275,7 +275,7 @@ static void hrt_call_invoke(void);
/*
* Specific registers and bits used by PPM sub-functions
*/
#ifdef CONFIG_HRT_PPM
#ifdef HRT_PPM_CHANNEL
/*
* If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
*
@@ -326,7 +326,7 @@ static void hrt_call_invoke(void);
# define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */
# define CCER_PPM_FLIP GTIM_CCER_CC4P
# else
# error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set
# error HRT_PPM_CHANNEL must be a value between 1 and 4
# endif
/*
@@ -377,7 +377,7 @@ static void hrt_ppm_decode(uint32_t status);
# define CCMR1_PPM 0
# define CCMR2_PPM 0
# define CCER_PPM 0
#endif /* CONFIG_HRT_PPM */
#endif /* HRT_PPM_CHANNEL */
/*
* Initialise the timer we are going to use.
@@ -424,7 +424,7 @@ hrt_tim_init(void)
up_enable_irq(HRT_TIMER_VECTOR);
}
#ifdef CONFIG_HRT_PPM
#ifdef HRT_PPM_CHANNEL
/*
* Handle the PPM decoder state machine.
*/
@@ -526,7 +526,7 @@ error:
ppm_decoded_channels = 0;
}
#endif /* CONFIG_HRT_PPM */
#endif /* HRT_PPM_CHANNEL */
/*
* Handle the compare interupt by calling the callout dispatcher
@@ -546,7 +546,7 @@ hrt_tim_isr(int irq, void *context)
/* ack the interrupts we just read */
rSR = ~status;
#ifdef CONFIG_HRT_PPM
#ifdef HRT_PPM_CHANNEL
/* was this a PPM edge? */
if (status & (SR_INT_PPM | SR_OVF_PPM)) {
@@ -686,7 +686,7 @@ hrt_init(void)
sq_init(&callout_queue);
hrt_tim_init();
#ifdef CONFIG_HRT_PPM
#ifdef HRT_PPM_CHANNEL
/* configure the PPM input pin */
stm32_configgpio(GPIO_PPM_IN);
#endif
@@ -907,4 +907,4 @@ hrt_latency_update(void)
}
#endif /* CONFIG_HRT_TIMER */
#endif /* HRT_TIMER */
src/drivers/stm32/drv_pwm_servo.c
+6
View File
@@ -88,6 +88,7 @@
#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET)
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET)
static void pwm_timer_init(unsigned timer);
static void pwm_timer_set_rate(unsigned timer, unsigned rate);
@@ -110,6 +111,11 @@ pwm_timer_init(unsigned timer)
rCCER(timer) = 0;
rDCR(timer) = 0;
if ((pwm_timers[timer].base == STM32_TIM1_BASE) || (pwm_timers[timer].base == STM32_TIM8_BASE)) {
/* master output enable = on */
rBDTR(timer) = ATIM_BDTR_MOE;
}
/* configure the timer to free-run at 1MHz */
rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
src/drivers/stm32/tone_alarm/tone_alarm.cpp
-4
View File
@@ -117,10 +117,6 @@
#include <systemlib/err.h>
#ifndef CONFIG_HRT_TIMER
# error This driver requires CONFIG_HRT_TIMER
#endif
/* Tone alarm configuration */
#if TONE_ALARM_TIMER == 2
# define TONE_ALARM_BASE STM32_TIM2_BASE