fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 17:43:34 +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
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+2 -2
View File
@@ -47,8 +47,8 @@
#include <mathlib/mathlib.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <controllib/block/UOrbSubscription.hpp>
#include <controllib/block/UOrbPublication.hpp>
#include <controllib/uorb/UOrbSubscription.hpp>
#include <controllib/uorb/UOrbPublication.hpp>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+5 -5
View File
@@ -224,8 +224,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* rate-limit raw data updates to 200Hz */
orb_set_interval(sub_raw, 4);
/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
orb_set_interval(sub_raw, 3);
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
@@ -236,7 +236,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
int printcounter = 0;
@@ -384,7 +383,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.005f) {
if (!const_initialized && dt < 0.05f && dt > 0.001f) {
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
@@ -424,7 +423,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
continue;
}
if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
if (last_data > 0 && raw.timestamp - last_data > 12000)
printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
last_data = raw.timestamp;
src/modules/commander/accelerometer_calibration.cpp
+2 -2
View File
@@ -275,8 +275,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
/* EMA time constant in seconds*/
float ema_len = 0.2f;
/* set "still" threshold to 0.1 m/s^2 */
float still_thr2 = pow(0.1f, 2);
/* set "still" threshold to 0.25 m/s^2 */
float still_thr2 = pow(0.25f, 2);
/* set accel error threshold to 5m/s^2 */
float accel_err_thr = 5.0f;
/* still time required in us */
src/modules/controllib/block/Block.cpp
+2 -2
View File
@@ -43,8 +43,8 @@
#include "Block.hpp"
#include "BlockParam.hpp"
#include "UOrbSubscription.hpp"
#include "UOrbPublication.hpp"
#include "../uorb/UOrbSubscription.hpp"
#include "../uorb/UOrbPublication.hpp"
namespace control
{
src/modules/controllib/blocks.hpp
+1
View File
@@ -42,6 +42,7 @@
#include <assert.h>
#include <time.h>
#include <stdlib.h>
#include <math.h>
#include <mathlib/math/test/test.hpp>
#include "block/Block.hpp"
src/modules/controllib/module.mk
+4 -4
View File
@@ -37,7 +37,7 @@
SRCS = test_params.c \
block/Block.cpp \
block/BlockParam.cpp \
block/UOrbPublication.cpp \
block/UOrbSubscription.cpp \
blocks.cpp \
fixedwing.cpp
uorb/UOrbPublication.cpp \
uorb/UOrbSubscription.cpp \
uorb/blocks.cpp \
blocks.cpp
src/modules/controllib/block/UOrbPublication.cpp → src/modules/controllib/uorb/UOrbPublication.cpp
View File
src/modules/controllib/block/UOrbPublication.hpp → src/modules/controllib/uorb/UOrbPublication.hpp
+2 -2
View File
@@ -39,8 +39,8 @@
#pragma once
#include <uORB/uORB.h>
#include "Block.hpp"
#include "List.hpp"
#include "../block/Block.hpp"
#include "../block/List.hpp"
namespace control
src/modules/controllib/block/UOrbSubscription.cpp → src/modules/controllib/uorb/UOrbSubscription.cpp
View File
src/modules/controllib/block/UOrbSubscription.hpp → src/modules/controllib/uorb/UOrbSubscription.hpp
+2 -2
View File
@@ -39,8 +39,8 @@
#pragma once
#include <uORB/uORB.h>
#include "Block.hpp"
#include "List.hpp"
#include "../block/Block.hpp"
#include "../block/List.hpp"
namespace control
src/modules/controllib/uorb/blocks.cpp
+101
View File
@@ -0,0 +1,101 @@
/****************************************************************************
*
* Copyright (C) 2012 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 uorb_blocks.cpp
*
* uorb block library code
*/
#include "blocks.hpp"
namespace control
{
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_xtYawLimit(this, "XT2YAW"),
_xt2Yaw(this, "XT2YAW"),
_psiCmd(0)
{
}
BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd)
{
// heading to waypoint
float psiTrack = get_bearing_to_next_waypoint(
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
// cross track
struct crosstrack_error_s xtrackError;
get_distance_to_line(&xtrackError,
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)lastPosCmd.lat / (double)1e7d,
(double)lastPosCmd.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
_psiCmd = _wrap_2pi(psiTrack -
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
}
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_actuators(&getPublications(), ORB_ID(actuator_controls_0))
{
}
BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
} // namespace control
src/modules/controllib/uorb/blocks.hpp
+113
View File
@@ -0,0 +1,113 @@
/****************************************************************************
*
* Copyright (C) 2012 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 uorb_blocks.h
*
* uorb block library code
*/
#pragma once
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <poll.h>
extern "C" {
#include <systemlib/geo/geo.h>
}
#include "../blocks.hpp"
#include "UOrbSubscription.hpp"
#include "UOrbPublication.hpp"
namespace control
{
/**
* Waypoint Guidance block
*/
class __EXPORT BlockWaypointGuidance : public SuperBlock
{
private:
BlockLimitSym _xtYawLimit;
BlockP _xt2Yaw;
float _psiCmd;
public:
BlockWaypointGuidance(SuperBlock *parent, const char *name);
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd);
float getPsiCmd() { return _psiCmd; }
};
/**
* UorbEnabledAutopilot
*/
class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
{
protected:
// subscriptions
UOrbSubscription<vehicle_attitude_s> _att;
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
UOrbSubscription<vehicle_global_position_s> _pos;
UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
UOrbSubscription<manual_control_setpoint_s> _manual;
UOrbSubscription<vehicle_status_s> _status;
UOrbSubscription<parameter_update_s> _param_update;
// publications
UOrbPublication<actuator_controls_s> _actuators;
public:
BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
virtual ~BlockUorbEnabledAutopilot();
};
} // namespace control
src/modules/controllib/fixedwing.cpp → src/modules/fixedwing_backside/fixedwing.cpp
+1 -56
View File
@@ -88,61 +88,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
_yawDamper.update(rCmd, r, outputScale);
}
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_xtYawLimit(this, "XT2YAW"),
_xt2Yaw(this, "XT2YAW"),
_psiCmd(0)
{
}
BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd)
{
// heading to waypoint
float psiTrack = get_bearing_to_next_waypoint(
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
// cross track
struct crosstrack_error_s xtrackError;
get_distance_to_line(&xtrackError,
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)lastPosCmd.lat / (double)1e7d,
(double)lastPosCmd.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
_psiCmd = _wrap_2pi(psiTrack -
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
}
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_actuators(&getPublications(), ORB_ID(actuator_controls_0))
{
}
BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
BlockUorbEnabledAutopilot(parent, name),
_stabilization(this, ""), // no name needed, already unique
@@ -385,4 +330,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
} // namespace control
#endif
#endif
src/modules/controllib/fixedwing.hpp → src/modules/fixedwing_backside/fixedwing.hpp
+2 -66
View File
@@ -39,31 +39,8 @@
#pragma once
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <poll.h>
#include "blocks.hpp"
#include "block/UOrbSubscription.hpp"
#include "block/UOrbPublication.hpp"
extern "C" {
#include <systemlib/geo/geo.h>
}
#include <controllib/blocks.hpp>
#include <controllib/uorb/blocks.hpp>
namespace control
{
@@ -250,47 +227,6 @@ public:
* than frontside at high speeds.
*/
/**
* Waypoint Guidance block
*/
class __EXPORT BlockWaypointGuidance : public SuperBlock
{
private:
BlockLimitSym _xtYawLimit;
BlockP _xt2Yaw;
float _psiCmd;
public:
BlockWaypointGuidance(SuperBlock *parent, const char *name);
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd);
float getPsiCmd() { return _psiCmd; }
};
/**
* UorbEnabledAutopilot
*/
class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
{
protected:
// subscriptions
UOrbSubscription<vehicle_attitude_s> _att;
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
UOrbSubscription<vehicle_global_position_s> _pos;
UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
UOrbSubscription<manual_control_setpoint_s> _manual;
UOrbSubscription<vehicle_status_s> _status;
UOrbSubscription<parameter_update_s> _param_update;
// publications
UOrbPublication<actuator_controls_s> _actuators;
public:
BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
virtual ~BlockUorbEnabledAutopilot();
};
/**
* Multi-mode Autopilot
*/
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+2 -1
View File
@@ -45,12 +45,13 @@
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <controllib/fixedwing.hpp>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "fixedwing.hpp"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
src/modules/fixedwing_backside/module.mk
+1
View File
@@ -38,4 +38,5 @@
MODULE_COMMAND = fixedwing_backside
SRCS = fixedwing_backside_main.cpp \
fixedwing.cpp \
params.c
src/modules/mathlib/math/filter/LowPassFilter2p.cpp
+77
View File
@@ -0,0 +1,77 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/****************************************************************************
*
* Copyright (C) 2012 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 LowPassFilter.cpp
/// @brief A class to implement a second order low pass filter
/// Author: Leonard Hall <LeonardTHall@gmail.com>
#include "LowPassFilter2p.hpp"
#include "math.h"
namespace math
{
void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
_cutoff_freq = cutoff_freq;
float fr = sample_freq/_cutoff_freq;
float ohm = tanf(M_PI_F/fr);
float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
_b0 = ohm*ohm/c;
_b1 = 2.0f*_b0;
_b2 = _b0;
_a1 = 2.0f*(ohm*ohm-1.0f)/c;
_a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
}
float LowPassFilter2p::apply(float sample)
{
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) {
// don't allow bad values to propogate via the filter
delay_element_0 = sample;
}
float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
// return the value. Should be no need to check limits
return output;
}
} // namespace math
src/modules/mathlib/math/filter/LowPassFilter2p.hpp
+78
View File
@@ -0,0 +1,78 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/****************************************************************************
*
* Copyright (C) 2012 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 LowPassFilter.h
/// @brief A class to implement a second order low pass filter
/// Author: Leonard Hall <LeonardTHall@gmail.com>
/// Adapted for PX4 by Andrew Tridgell
#pragma once
namespace math
{
class __EXPORT LowPassFilter2p
{
public:
// constructor
LowPassFilter2p(float sample_freq, float cutoff_freq) {
// set initial parameters
set_cutoff_frequency(sample_freq, cutoff_freq);
_delay_element_1 = _delay_element_2 = 0;
}
// change parameters
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
// apply - Add a new raw value to the filter
// and retrieve the filtered result
float apply(float sample);
// return the cutoff frequency
float get_cutoff_freq(void) const {
return _cutoff_freq;
}
private:
float _cutoff_freq;
float _a1;
float _a2;
float _b0;
float _b1;
float _b2;
float _delay_element_1; // buffered sample -1
float _delay_element_2; // buffered sample -2
};
} // namespace math
src/modules/mathlib/math/filter/module.mk
+44
View File
@@ -0,0 +1,44 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# filter library
#
SRCS = LowPassFilter2p.cpp
#
# In order to include .config we first have to save off the
# current makefile name, since app.mk needs it.
#
APP_MAKEFILE := $(lastword $(MAKEFILE_LIST))
-include $(TOPDIR)/.config
src/modules/px4iofirmware/controls.c
+2
View File
@@ -300,6 +300,8 @@ controls_tick() {
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
}
src/modules/px4iofirmware/dsm.c
+2 -2
View File
@@ -125,9 +125,9 @@ dsm_bind(uint16_t cmd, int pulses)
case dsm_bind_send_pulses:
for (int i = 0; i < pulses; i++) {
stm32_gpiowrite(usart1RxAsOutp, false);
up_udelay(50);
up_udelay(25);
stm32_gpiowrite(usart1RxAsOutp, true);
up_udelay(50);
up_udelay(25);
}
break;
case dsm_bind_reinit_uart:
src/modules/sdlog2/sdlog2.c
+10 -1
View File
@@ -606,6 +606,15 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "unable to create logging folder, exiting.");
}
const char *converter_in = "/etc/logging/conv.zip";
char* converter_out = malloc(150);
sprintf(converter_out, "%s/conv.zip", folder_path);
if (file_copy(converter_in, converter_out)) {
errx(1, "unable to copy conversion scripts, exiting.");
}
free(converter_out);
/* only print logging path, important to find log file later */
warnx("logging to directory: %s", folder_path);
@@ -1318,7 +1327,7 @@ int file_copy(const char *file_old, const char *file_new)
fclose(source);
fclose(target);
return ret;
return OK;
}
void handle_command(struct vehicle_command_s *cmd)
src/modules/segway/BlockSegwayController.cpp
+57
View File
@@ -0,0 +1,57 @@
#include "BlockSegwayController.hpp"
void BlockSegwayController::update() {
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
// set dt for all child blocks
setDt(dt);
// check for new updates
if (_param_update.updated()) updateParams();
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) {
// update guidance
}
// compute speed command
float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO ||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
_actuators.control[0] = spdCmd;
_actuators.control[1] = spdCmd;
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
_actuators.control[CH_LEFT] = _manual.throttle;
_actuators.control[CH_RIGHT] = _manual.pitch;
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
_actuators.control[0] = spdCmd;
_actuators.control[1] = spdCmd;
}
}
// update all publications
updatePublications();
}
src/modules/segway/BlockSegwayController.hpp
+27
View File
@@ -0,0 +1,27 @@
#pragma once
#include <controllib/uorb/blocks.hpp>
using namespace control;
class BlockSegwayController : public control::BlockUorbEnabledAutopilot {
public:
BlockSegwayController() :
BlockUorbEnabledAutopilot(NULL,"SEG"),
th2v(this, "TH2V"),
q2v(this, "Q2V"),
_attPoll(),
_timeStamp(0)
{
_attPoll.fd = _att.getHandle();
_attPoll.events = POLLIN;
}
void update();
private:
enum {CH_LEFT, CH_RIGHT};
BlockPI th2v;
BlockP q2v;
struct pollfd _attPoll;
uint64_t _timeStamp;
};
src/modules/segway/module.mk
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# segway controller
#
MODULE_COMMAND = segway
SRCS = segway_main.cpp \
BlockSegwayController.cpp \
params.c
src/modules/segway/params.c
+8
View File
@@ -0,0 +1,8 @@
#include <systemlib/param/param.h>
// 16 is max name length
PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage
PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage
PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter
PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage
src/modules/segway/segway_main.cpp
+157
View File
@@ -0,0 +1,157 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: James Goppert
*
* 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 segway_main.cpp
* @author James Goppert
*
* Segway controller using control library
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "BlockSegwayController.hpp"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int segway_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int segway_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: segway {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int segway_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("segway",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
5120,
segway_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("is running");
} else {
warnx("not started");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int segway_thread_main(int argc, char *argv[])
{
warnx("starting");
using namespace control;
BlockSegwayController autopilot;
thread_running = true;
while (!thread_should_exit) {
autopilot.update();
}
warnx("exiting.");
thread_running = false;
return 0;
}
src/modules/sensors/sensor_params.c
+2 -1
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);
@@ -156,6 +156,7 @@ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
src/modules/sensors/sensors.cpp
+99 -57
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>
@@ -91,8 +92,35 @@
#define BARO_HEALTH_COUNTER_LIMIT_OK 5
#define ADC_HEALTH_COUNTER_LIMIT_OK 5
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
/**
* Analog layout:
* FMU:
* IN2 - battery voltage
* IN3 - battery current
* IN4 - 5V sense
* IN10 - spare (we could actually trim these from the set)
* IN11 - spare (we could actually trim these from the set)
* IN12 - spare (we could actually trim these from the set)
* IN13 - aux1
* IN14 - aux2
* IN15 - pressure sensor
*
* IO:
* IN4 - servo supply rail
* IN5 - analog RSSI
*/
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#define ADC_BATTERY_VOLTAGE_CHANNEL 2
#define ADC_BATTERY_CURRENT_CHANNEL 3
#define ADC_5V_RAIL_SENSE 4
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
#define BAT_VOL_INITIAL 0.f
#define BAT_VOL_LOWPASS_1 0.99f
@@ -197,7 +225,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;
@@ -228,7 +256,6 @@ private:
float battery_voltage_scaling;
int rc_rl1_DSM_VCC_control;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -277,7 +304,6 @@ private:
param_t battery_voltage_scaling;
param_t rc_rl1_DSM_VCC_control;
} _parameter_handles; /**< handles for interesting parameters */
@@ -389,7 +415,7 @@ namespace sensors
#endif
static const int ERROR = -1;
Sensors *g_sensors;
Sensors *g_sensors = nullptr;
}
Sensors::Sensors() :
@@ -512,9 +538,6 @@ Sensors::Sensors() :
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
/* DSM VCC relay control */
_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
/* fetch initial parameter values */
parameters_update();
}
@@ -552,25 +575,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]);
@@ -656,21 +665,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;
@@ -726,11 +724,6 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
/* relay 1 DSM VCC control */
if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
warnx("Failed updating relay 1 DSM VCC control");
}
return OK;
}
@@ -746,11 +739,26 @@ Sensors::accel_init()
errx(1, "FATAL: no accelerometer found");
} else {
/* set the accel internal sampling rate up to at leat 500Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 500);
/* set the driver to poll at 500Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 500);
// XXX do the check more elegantly
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the accel internal sampling rate up to at leat 1000Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
/* set the driver to poll at 1000Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 1000);
#else
/* set the accel internal sampling rate up to at leat 800Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 800);
/* set the driver to poll at 800Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 800);
#endif
warnx("using system accel");
close(fd);
@@ -769,11 +777,28 @@ Sensors::gyro_init()
errx(1, "FATAL: no gyro found");
} else {
/* set the gyro internal sampling rate up to at leat 500Hz */
ioctl(fd, GYROIOCSSAMPLERATE, 500);
/* set the driver to poll at 500Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 500);
// XXX do the check more elegantly
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the gyro internal sampling rate up to at least 1000Hz */
if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
ioctl(fd, GYROIOCSSAMPLERATE, 800);
/* set the driver to poll at 1000Hz */
if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
ioctl(fd, SENSORIOCSPOLLRATE, 800);
#else
/* set the gyro internal sampling rate up to at leat 800Hz */
ioctl(fd, GYROIOCSSAMPLERATE, 800);
/* set the driver to poll at 800Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 800);
#endif
warnx("using system gyro");
close(fd);
@@ -1034,6 +1059,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]);
@@ -1364,6 +1403,9 @@ Sensors::task_main()
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
orb_set_interval(_gyro_sub, 4);
/*
* do advertisements
*/
@@ -1399,7 +1441,7 @@ Sensors::task_main()
while (!_task_should_exit) {
/* wait for up to 500ms for data */
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
src/modules/systemlib/hx_stream.c
+126 -65
View File
@@ -53,14 +53,26 @@
struct hx_stream {
uint8_t buf[HX_STREAM_MAX_FRAME + 4];
unsigned frame_bytes;
bool escaped;
bool txerror;
/* RX state */
uint8_t rx_buf[HX_STREAM_MAX_FRAME + 4];
unsigned rx_frame_bytes;
bool rx_escaped;
hx_stream_rx_callback rx_callback;
void *rx_callback_arg;
/* TX state */
int fd;
hx_stream_rx_callback callback;
void *callback_arg;
bool tx_error;
uint8_t *tx_buf;
unsigned tx_resid;
uint32_t tx_crc;
enum {
TX_IDLE = 0,
TX_SEND_START,
TX_SEND_DATA,
TX_SENT_ESCAPE,
TX_SEND_END
} tx_state;
perf_counter_t pc_tx_frames;
perf_counter_t pc_rx_frames;
@@ -81,21 +93,7 @@ static void
hx_tx_raw(hx_stream_t stream, uint8_t c)
{
if (write(stream->fd, &c, 1) != 1)
stream->txerror = true;
}
static void
hx_tx_byte(hx_stream_t stream, uint8_t c)
{
switch (c) {
case FBO:
case CEO:
hx_tx_raw(stream, CEO);
c ^= 0x20;
break;
}
hx_tx_raw(stream, c);
stream->tx_error = true;
}
static int
@@ -105,11 +103,11 @@ hx_rx_frame(hx_stream_t stream)
uint8_t b[4];
uint32_t w;
} u;
unsigned length = stream->frame_bytes;
unsigned length = stream->rx_frame_bytes;
/* reset the stream */
stream->frame_bytes = 0;
stream->escaped = false;
stream->rx_frame_bytes = 0;
stream->rx_escaped = false;
/* not a real frame - too short */
if (length < 4) {
@@ -122,11 +120,11 @@ hx_rx_frame(hx_stream_t stream)
length -= 4;
/* compute expected CRC */
u.w = crc32(&stream->buf[0], length);
u.w = crc32(&stream->rx_buf[0], length);
/* compare computed and actual CRC */
for (unsigned i = 0; i < 4; i++) {
if (u.b[i] != stream->buf[length + i]) {
if (u.b[i] != stream->rx_buf[length + i]) {
perf_count(stream->pc_rx_errors);
return 0;
}
@@ -134,7 +132,7 @@ hx_rx_frame(hx_stream_t stream)
/* frame is good */
perf_count(stream->pc_rx_frames);
stream->callback(stream->callback_arg, &stream->buf[0], length);
stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length);
return 1;
}
@@ -150,8 +148,8 @@ hx_stream_init(int fd,
if (stream != NULL) {
memset(stream, 0, sizeof(struct hx_stream));
stream->fd = fd;
stream->callback = callback;
stream->callback_arg = arg;
stream->rx_callback = callback;
stream->rx_callback_arg = arg;
}
return stream;
@@ -179,49 +177,112 @@ hx_stream_set_counters(hx_stream_t stream,
stream->pc_rx_errors = rx_errors;
}
void
hx_stream_reset(hx_stream_t stream)
{
stream->rx_frame_bytes = 0;
stream->rx_escaped = false;
stream->tx_buf = NULL;
stream->tx_resid = 0;
stream->tx_state = TX_IDLE;
}
int
hx_stream_start(hx_stream_t stream,
const void *data,
size_t count)
{
if (count > HX_STREAM_MAX_FRAME)
return -EINVAL;
stream->tx_buf = data;
stream->tx_resid = count;
stream->tx_state = TX_SEND_START;
stream->tx_crc = crc32(data, count);
return OK;
}
int
hx_stream_send_next(hx_stream_t stream)
{
int c;
/* sort out what we're going to send */
switch (stream->tx_state) {
case TX_SEND_START:
stream->tx_state = TX_SEND_DATA;
return FBO;
case TX_SEND_DATA:
c = *stream->tx_buf;
switch (c) {
case FBO:
case CEO:
stream->tx_state = TX_SENT_ESCAPE;
return CEO;
}
break;
case TX_SENT_ESCAPE:
c = *stream->tx_buf ^ 0x20;
stream->tx_state = TX_SEND_DATA;
break;
case TX_SEND_END:
stream->tx_state = TX_IDLE;
return FBO;
case TX_IDLE:
default:
return -1;
}
/* if we are here, we have consumed a byte from the buffer */
stream->tx_resid--;
stream->tx_buf++;
/* buffer exhausted */
if (stream->tx_resid == 0) {
uint8_t *pcrc = (uint8_t *)&stream->tx_crc;
/* was the buffer the frame CRC? */
if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) {
stream->tx_state = TX_SEND_END;
} else {
/* no, it was the payload - switch to sending the CRC */
stream->tx_buf = pcrc;
stream->tx_resid = sizeof(stream->tx_crc);
}
}
return c;
}
int
hx_stream_send(hx_stream_t stream,
const void *data,
size_t count)
{
union {
uint8_t b[4];
uint32_t w;
} u;
const uint8_t *p = (const uint8_t *)data;
unsigned resid = count;
int result;
if (resid > HX_STREAM_MAX_FRAME)
return -EINVAL;
result = hx_stream_start(stream, data, count);
if (result != OK)
return result;
/* start the frame */
hx_tx_raw(stream, FBO);
/* transmit the data */
while (resid--)
hx_tx_byte(stream, *p++);
/* compute the CRC */
u.w = crc32(data, count);
/* send the CRC */
p = &u.b[0];
resid = 4;
while (resid--)
hx_tx_byte(stream, *p++);
/* and the trailing frame separator */
hx_tx_raw(stream, FBO);
int c;
while ((c = hx_stream_send_next(stream)) >= 0)
hx_tx_raw(stream, c);
/* check for transmit error */
if (stream->txerror) {
stream->txerror = false;
if (stream->tx_error) {
stream->tx_error = false;
return -EIO;
}
perf_count(stream->pc_tx_frames);
return 0;
return OK;
}
void
@@ -234,17 +295,17 @@ hx_stream_rx(hx_stream_t stream, uint8_t c)
}
/* escaped? */
if (stream->escaped) {
stream->escaped = false;
if (stream->rx_escaped) {
stream->rx_escaped = false;
c ^= 0x20;
} else if (c == CEO) {
/* now escaped, ignore the byte */
stream->escaped = true;
/* now rx_escaped, ignore the byte */
stream->rx_escaped = true;
return;
}
/* save for later */
if (stream->frame_bytes < sizeof(stream->buf))
stream->buf[stream->frame_bytes++] = c;
if (stream->rx_frame_bytes < sizeof(stream->rx_buf))
stream->rx_buf[stream->rx_frame_bytes++] = c;
}
src/modules/systemlib/hx_stream.h
+41 -1
View File
@@ -58,7 +58,8 @@ __BEGIN_DECLS
* Allocate a new hx_stream object.
*
* @param fd The file handle over which the protocol will
* communicate.
* communicate, or -1 if the protocol will use
* hx_stream_start/hx_stream_send_next.
* @param callback Called when a frame is received.
* @param callback_arg Passed to the callback.
* @return A handle to the stream, or NULL if memory could
@@ -80,6 +81,7 @@ __EXPORT extern void hx_stream_free(hx_stream_t stream);
*
* Any counter may be set to NULL to disable counting that datum.
*
* @param stream A handle returned from hx_stream_init.
* @param tx_frames Counter for transmitted frames.
* @param rx_frames Counter for received frames.
* @param rx_errors Counter for short and corrupt received frames.
@@ -89,6 +91,44 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream,
perf_counter_t rx_frames,
perf_counter_t rx_errors);
/**
* Reset a stream.
*
* Forces the local stream state to idle.
*
* @param stream A handle returned from hx_stream_init.
*/
__EXPORT extern void hx_stream_reset(hx_stream_t stream);
/**
* Prepare to send a frame.
*
* Use this in conjunction with hx_stream_send_next to
* set the frame to be transmitted.
*
* Use hx_stream_send() to write to the stream fd directly.
*
* @param stream A handle returned from hx_stream_init.
* @param data Pointer to the data to send.
* @param count The number of bytes to send.
* @return Zero on success, -errno on error.
*/
__EXPORT extern int hx_stream_start(hx_stream_t stream,
const void *data,
size_t count);
/**
* Get the next byte to send for a stream.
*
* This requires that the stream be prepared for sending by
* calling hx_stream_start first.
*
* @param stream A handle returned from hx_stream_init.
* @return The byte to send, or -1 if there is
* nothing left to send.
*/
__EXPORT extern int hx_stream_send_next(hx_stream_t stream);
/**
* Send a frame.
*
src/modules/systemlib/perf_counter.c
+34 -7
View File
@@ -201,23 +201,50 @@ perf_end(perf_counter_t handle)
switch (handle->type) {
case PC_ELAPSED: {
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
pce->event_count++;
pce->time_total += elapsed;
if (pce->time_start != 0) {
hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
if ((pce->time_least > elapsed) || (pce->time_least == 0))
pce->time_least = elapsed;
pce->event_count++;
pce->time_total += elapsed;
if (pce->time_most < elapsed)
pce->time_most = elapsed;
if ((pce->time_least > elapsed) || (pce->time_least == 0))
pce->time_least = elapsed;
if (pce->time_most < elapsed)
pce->time_most = elapsed;
pce->time_start = 0;
}
}
break;
default:
break;
}
}
void
perf_cancel(perf_counter_t handle)
{
if (handle == NULL)
return;
switch (handle->type) {
case PC_ELAPSED: {
struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
pce->time_start = 0;
}
break;
default:
break;
}
}
void
perf_reset(perf_counter_t handle)
{
src/modules/systemlib/perf_counter.h
+13 -1
View File
@@ -92,13 +92,25 @@ __EXPORT extern void perf_begin(perf_counter_t handle);
* End a performance event.
*
* This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
* If a call is made without a corresopnding perf_begin call, or if perf_cancel
* has been called subsequently, no change is made to the counter.
*
* @param handle The handle returned from perf_alloc.
*/
__EXPORT extern void perf_end(perf_counter_t handle);
/**
* Reset a performance event.
* Cancel a performance event.
*
* This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
* It reverts the effect of a previous perf_begin.
*
* @param handle The handle returned from perf_alloc.
*/
__EXPORT extern void perf_cancel(perf_counter_t handle);
/**
* Reset a performance counter.
*
* This call resets performance counter to initial state
*
src/systemcmds/config/config.c
+200
View File
@@ -0,0 +1,200 @@
/****************************************************************************
*
* Copyright (c) 2012, 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 config.c
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* config tool.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <arch/board/board.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
__EXPORT int config_main(int argc, char *argv[]);
static void do_gyro(int argc, char *argv[]);
static void do_accel(int argc, char *argv[]);
static void do_mag(int argc, char *argv[]);
int
config_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "gyro")) {
if (argc >= 3) {
do_gyro(argc - 2, argv + 2);
} else {
errx(1, "not enough parameters.");
}
}
if (!strcmp(argv[1], "accel")) {
if (argc >= 3) {
do_accel(argc - 2, argv + 2);
} else {
errx(1, "not enough parameters.");
}
}
if (!strcmp(argv[1], "mag")) {
if (argc >= 3) {
do_mag(argc - 2, argv + 2);
} else {
errx(1, "not enough parameters.");
}
}
}
errx(1, "expected a command, try 'gyro', 'accel', 'mag'");
}
static void
do_gyro(int argc, char *argv[])
{
int fd;
fd = open(GYRO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", GYRO_DEVICE_PATH);
errx(1, "FATAL: no gyro found");
} else {
if (argc >= 2) {
char* end;
int i = strtol(argv[1],&end,10);
if (!strcmp(argv[0], "sampling")) {
/* set the accel internal sampling rate up to at leat i Hz */
ioctl(fd, GYROIOCSSAMPLERATE, i);
} else if (!strcmp(argv[0], "rate")) {
/* set the driver to poll at i Hz */
ioctl(fd, SENSORIOCSPOLLRATE, i);
} else if (!strcmp(argv[0], "range")) {
/* set the range to i dps */
ioctl(fd, GYROIOCSRANGE, i);
}
} else if (!(argc > 0 && !strcmp(argv[0], "info"))) {
warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
}
int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, GYROIOCGRANGE, 0);
warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range);
close(fd);
}
exit(0);
}
static void
do_mag(int argc, char *argv[])
{
exit(0);
}
static void
do_accel(int argc, char *argv[])
{
int fd;
fd = open(ACCEL_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", ACCEL_DEVICE_PATH);
errx(1, "FATAL: no accelerometer found");
} else {
if (argc >= 2) {
char* end;
int i = strtol(argv[1],&end,10);
if (!strcmp(argv[0], "sampling")) {
/* set the accel internal sampling rate up to at leat i Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, i);
} else if (!strcmp(argv[0], "rate")) {
/* set the driver to poll at i Hz */
ioctl(fd, SENSORIOCSPOLLRATE, i);
} else if (!strcmp(argv[0], "range")) {
/* set the range to i dps */
ioctl(fd, ACCELIOCSRANGE, i);
}
} else if (!(argc > 0 && !strcmp(argv[0], "info"))) {
warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t");
}
int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, ACCELIOCGRANGE, 0);
warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range);
close(fd);
}
exit(0);
}
src/systemcmds/config/module.mk
+44
View File
@@ -0,0 +1,44 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Build the config tool.
#
MODULE_COMMAND = config
SRCS = config.c
MODULE_STACKSIZE = 4096
MAXOPTIMIZATION = -Os
src/systemcmds/ramtron/module.mk
+6
View File
@@ -0,0 +1,6 @@
#
# RAMTRON file system driver
#
MODULE_COMMAND = ramtron
SRCS = ramtron.c
src/systemcmds/ramtron/ramtron.c
+279
View File
@@ -0,0 +1,279 @@
/****************************************************************************
*
* 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 ramtron.c
*
* ramtron service and utility app.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/spi.h>
#include <nuttx/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
__EXPORT int ramtron_main(int argc, char *argv[]);
#ifndef CONFIG_MTD_RAMTRON
/* create a fake command with decent message to not confuse users */
int ramtron_main(int argc, char *argv[])
{
errx(1, "RAMTRON not enabled, skipping.");
}
#else
static void ramtron_attach(void);
static void ramtron_start(void);
static void ramtron_erase(void);
static void ramtron_ioctl(unsigned operation);
static void ramtron_save(const char *name);
static void ramtron_load(const char *name);
static void ramtron_test(void);
static bool attached = false;
static bool started = false;
static struct mtd_dev_s *ramtron_mtd;
int ramtron_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "start"))
ramtron_start();
if (!strcmp(argv[1], "save_param"))
ramtron_save(argv[2]);
if (!strcmp(argv[1], "load_param"))
ramtron_load(argv[2]);
if (!strcmp(argv[1], "erase"))
ramtron_erase();
if (!strcmp(argv[1], "test"))
ramtron_test();
if (0) { /* these actually require a file on the filesystem... */
if (!strcmp(argv[1], "reformat"))
ramtron_ioctl(FIOC_REFORMAT);
if (!strcmp(argv[1], "repack"))
ramtron_ioctl(FIOC_OPTIMIZE);
}
}
errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n");
}
struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
static void
ramtron_attach(void)
{
/* find the right spi */
struct spi_dev_s *spi = up_spiinitialize(2);
/* this resets the spi bus, set correct bus speed again */
// xxx set in ramtron driver, leave this out
// SPI_SETFREQUENCY(spi, 4000000);
SPI_SETFREQUENCY(spi, 375000000);
SPI_SETBITS(spi, 8);
SPI_SETMODE(spi, SPIDEV_MODE3);
SPI_SELECT(spi, SPIDEV_FLASH, false);
if (spi == NULL)
errx(1, "failed to locate spi bus");
/* start the MTD driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
ramtron_mtd = ramtron_initialize(spi);
if (ramtron_mtd) {
/* abort on first valid result */
if (i > 0) {
warnx("warning: ramtron needed %d attempts to attach", i+1);
}
break;
}
}
/* if last attempt is still unsuccessful, abort */
if (ramtron_mtd == NULL)
errx(1, "failed to initialize ramtron driver");
attached = true;
}
static void
ramtron_start(void)
{
int ret;
if (started)
errx(1, "ramtron already mounted");
if (!attached)
ramtron_attach();
/* start NXFFS */
ret = nxffs_initialize(ramtron_mtd);
if (ret < 0)
errx(1, "failed to initialize NXFFS - erase ramtron to reformat");
/* mount the ramtron */
ret = mount(NULL, "/ramtron", "nxffs", 0, NULL);
if (ret < 0)
errx(1, "failed to mount /ramtron - erase ramtron to reformat");
started = true;
warnx("mounted ramtron at /ramtron");
exit(0);
}
//extern int at24c_nuke(void);
static void
ramtron_erase(void)
{
if (!attached)
ramtron_attach();
// if (at24c_nuke())
errx(1, "erase failed");
errx(0, "erase done, reboot now");
}
static void
ramtron_ioctl(unsigned operation)
{
int fd;
fd = open("/ramtron/.", 0);
if (fd < 0)
err(1, "open /ramtron");
if (ioctl(fd, operation, 0) < 0)
err(1, "ioctl");
exit(0);
}
static void
ramtron_save(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/ramtron/parameters'");
warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead");
/* delete the file in case it exists */
unlink(name);
/* create the file */
int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0)
err(1, "opening '%s' failed", name);
int result = param_export(fd, false);
close(fd);
if (result < 0) {
unlink(name);
errx(1, "error exporting to '%s'", name);
}
exit(0);
}
static void
ramtron_load(const char *name)
{
if (!started)
errx(1, "must be started first");
if (!name)
err(1, "missing argument for device name, try '/ramtron/parameters'");
warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead");
int fd = open(name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", name);
int result = param_load(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", name);
exit(0);
}
//extern void at24c_test(void);
static void
ramtron_test(void)
{
// at24c_test();
exit(0);
}
#endif
src/systemcmds/tests/test_hrt.c
+1 -1
View File
@@ -94,7 +94,7 @@ extern uint16_t ppm_pulse_history[];
int test_ppm(int argc, char *argv[])
{
#ifdef CONFIG_HRT_PPM
#ifdef HRT_PPM_CHANNEL
unsigned i;
printf("channels: %u\n", ppm_decoded_channels);