aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/device/cdev.cpp16
-rw-r--r--src/drivers/device/device.cpp19
-rw-r--r--src/drivers/device/device.h70
-rw-r--r--src/drivers/device/i2c.h14
-rw-r--r--src/drivers/device/ringbuffer.h203
-rw-r--r--src/drivers/device/spi.h2
-rw-r--r--src/drivers/drv_pwm_output.h5
-rw-r--r--src/drivers/ets_airspeed/ets_airspeed.cpp4
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp9
-rw-r--r--src/drivers/md25/BlockSysIdent.cpp8
-rw-r--r--src/drivers/md25/BlockSysIdent.hpp10
-rw-r--r--src/drivers/md25/md25.cpp87
-rw-r--r--src/drivers/md25/md25.hpp18
-rw-r--r--src/drivers/md25/md25_main.cpp49
-rw-r--r--src/drivers/meas_airspeed/meas_airspeed.cpp38
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp380
-rw-r--r--src/drivers/px4io/px4io.cpp181
-rw-r--r--src/drivers/stm32/drv_hrt.c20
-rw-r--r--src/drivers/stm32/drv_pwm_servo.c6
-rw-r--r--src/drivers/stm32/tone_alarm/tone_alarm.cpp4
20 files changed, 907 insertions, 236 deletions
diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp
index 422321850..47ebcd40a 100644
--- a/src/drivers/device/cdev.cpp
+++ b/src/drivers/device/cdev.cpp
@@ -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 \ No newline at end of file
+} // namespace device
diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp
index 04a5222c3..c3ee77b1c 100644
--- a/src/drivers/device/device.cpp
+++ b/src/drivers/device/device.cpp
@@ -223,5 +223,22 @@ interrupt(int irq, void *context)
return OK;
}
+int
+Device::read(unsigned offset, void *data, unsigned count)
+{
+ return -ENODEV;
+}
+
+int
+Device::write(unsigned offset, void *data, unsigned count)
+{
+ return -ENODEV;
+}
+
+int
+Device::ioctl(unsigned operation, unsigned &arg)
+{
+ return -ENODEV;
+}
-} // namespace device \ No newline at end of file
+} // namespace device
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index 7d375aab9..a9ed5d77c 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -69,10 +69,61 @@ 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 */ \ No newline at end of file
+#endif /* _DEVICE_DEVICE_H */
diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h
index b4a9cdd53..549879352 100644
--- a/src/drivers/device/i2c.h
+++ b/src/drivers/device/i2c.h
@@ -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();
diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h
new file mode 100644
index 000000000..dc0c84052
--- /dev/null
+++ b/src/drivers/device/ringbuffer.h
@@ -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);
+}
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index d2d01efb3..e0122372a 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -71,7 +71,7 @@ protected:
enum spi_mode_e mode,
uint32_t frequency,
int irq = 0);
- ~SPI();
+ virtual ~SPI();
virtual int init();
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 52a667403..ec9d4ca09 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -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)
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index de8028b0f..cd72d9d23 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -383,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");
}
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 98098c83b..1ffca2f43 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -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 */
diff --git a/src/drivers/md25/BlockSysIdent.cpp b/src/drivers/md25/BlockSysIdent.cpp
new file mode 100644
index 000000000..23b0724d8
--- /dev/null
+++ b/src/drivers/md25/BlockSysIdent.cpp
@@ -0,0 +1,8 @@
+#include "BlockSysIdent.hpp"
+
+BlockSysIdent::BlockSysIdent() :
+ Block(NULL, "SYSID"),
+ _freq(this, "FREQ"),
+ _ampl(this, "AMPL")
+{
+}
diff --git a/src/drivers/md25/BlockSysIdent.hpp b/src/drivers/md25/BlockSysIdent.hpp
new file mode 100644
index 000000000..270635f40
--- /dev/null
+++ b/src/drivers/md25/BlockSysIdent.hpp
@@ -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;
+};
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
index 71932ad65..d43e3aef9 100644
--- a/src/drivers/md25/md25.cpp
+++ b/src/drivers/md25/md25.cpp
@@ -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
diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp
index e77511b16..1661f67f9 100644
--- a/src/drivers/md25/md25.hpp
+++ b/src/drivers/md25/md25.hpp
@@ -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>
@@ -213,6 +213,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
* @return non-zero -> error
@@ -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
diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp
index e62c46b0d..7e5904d05 100644
--- a/src/drivers/md25/md25_main.cpp
+++ b/src/drivers/md25/md25_main.cpp
@@ -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;
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 7a2e22c01..68d2c5d65 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -34,6 +34,7 @@
/**
* @file meas_airspeed.cpp
* @author Lorenz Meier
+ * @author Sarthak Kaingade
* @author Simon Wilks
*
* Driver for the MEAS Spec series connected via I2C.
@@ -92,9 +93,6 @@
/* Register address */
#define ADDR_READ_MR 0x00 /* write to this address to start conversion */
-#define ADDR_READ_DF2 0x00 /* read from this address to read pressure only */
-#define ADDR_READ_DF3 0x01
-#define ADDR_READ_DF4 0x02 /* read from this address to read pressure and temp */
/* Measurement rate is 100Hz */
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
@@ -122,7 +120,7 @@ protected:
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address,
- CONVERSION_INTERVAL)
+ CONVERSION_INTERVAL)
{
}
@@ -160,7 +158,7 @@ MEASAirspeed::collect()
perf_begin(_sample_perf);
- ret = transfer(nullptr, 0, &val[0], 2);
+ ret = transfer(nullptr, 0, &val[0], 4);
if (ret < 0) {
log("error reading from sensor: %d", ret);
@@ -171,25 +169,37 @@ MEASAirspeed::collect()
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 diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
uint16_t temp = (val[3] & 0xE0) << 8 | val[2];
- diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
- diff_pres_pa -= _diff_pres_offset;
+ // XXX 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 = temp;
- _reports[_next_report].differential_pressure_pa = diff_pres_pa;
+ _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_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
- _reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
+ 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 */
@@ -405,6 +415,10 @@ test()
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");
}
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 1fd6cb17e..c4e331a30 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -67,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
@@ -181,13 +183,17 @@ 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;
@@ -197,6 +203,13 @@ private:
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.
*/
@@ -208,6 +221,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.
*
@@ -219,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();
@@ -311,15 +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_rate(500),
- _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;
@@ -340,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));
}
@@ -353,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);
}
@@ -361,6 +393,7 @@ int
MPU6000::init()
{
int ret;
+ int gyro_ret;
/* do SPI init (and probe) first */
ret = SPI::init();
@@ -371,6 +404,59 @@ MPU6000::init()
return ret;
}
+ /* 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);
up_udelay(10000);
@@ -391,7 +477,7 @@ MPU6000::init()
// 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);
@@ -402,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;
@@ -440,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;
@@ -461,22 +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();
-
- /* ensure we got real values to share */
- measure();
-
- if (gyro_ret != OK) {
- _gyro_topic = -1;
- } else {
- _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report);
- }
-
- /* advertise sensor topics */
- _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report);
-
- return ret;
}
int
@@ -555,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;
+
+ /* 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 ret;
+ /* return the number of bytes transferred */
+ return (transferred * sizeof(accel_report));
}
int
@@ -586,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;
+
+ /* 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 ret;
+ /* return the number of bytes transferred */
+ return (transferred * sizeof(gyro_report));
}
int
@@ -608,6 +690,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
+ case SENSORIOCRESET:
+ reset();
+ return OK;
+
case SENSORIOCSPOLLRATE: {
switch (arg) {
@@ -627,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: {
@@ -642,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;
@@ -661,25 +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;
- case SENSORIOCGQUEUEDEPTH:
- /* XXX not implemented */
- 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:
+ return _accel_reports->size();
case ACCELIOCGSAMPLERATE:
return _sample_rate;
case ACCELIOCSSAMPLERATE:
- _set_sample_rate(arg);
- return OK;
+ _set_sample_rate(arg);
+ return OK;
- case ACCELIOCSLOWPASS:
case ACCELIOCGLOWPASS:
- _set_dlpf_filter((uint16_t)arg);
+ return _accel_filter_x.get_cutoff_freq();
+
+ case ACCELIOCSLOWPASS:
+
+ // 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:
@@ -726,11 +851,36 @@ 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 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;
@@ -738,9 +888,14 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
_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:
@@ -865,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);
}
@@ -878,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();
@@ -960,9 +1119,15 @@ MPU6000::measure()
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();
/*
@@ -983,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 */
@@ -1119,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);
@@ -1145,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);
@@ -1162,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);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index ae56b70b3..fed83ea1a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -89,21 +89,61 @@
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
+/**
+ * The PX4IO class.
+ *
+ * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
+ */
class PX4IO : public device::I2C
{
public:
+ /**
+ * Constructor.
+ *
+ * Initialize all class variables.
+ */
PX4IO();
+ /**
+ * Destructor.
+ *
+ * Wait for worker thread to terminate.
+ */
virtual ~PX4IO();
+ /**
+ * Initialize the PX4IO class.
+ *
+ * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
+ */
virtual int init();
+ /**
+ * IO Control handler.
+ *
+ * Handle all IOCTL calls to the PX4IO file descriptor.
+ *
+ * @param[in] filp file handle (not used). This function is always called directly through object reference
+ * @param[in] cmd the IOCTL command
+ * @param[in] the IOCTL command parameter (optional)
+ */
virtual int ioctl(file *filp, int cmd, unsigned long arg);
+
+ /**
+ * write handler.
+ *
+ * Handle writes to the PX4IO file descriptor.
+ *
+ * @param[in] filp file handle (not used). This function is always called directly through object reference
+ * @param[in] buffer pointer to the data buffer to be written
+ * @param[in] len size in bytes to be written
+ * @return number of bytes written
+ */
virtual ssize_t write(file *filp, const char *buffer, size_t len);
/**
* Set the update rate for actuator outputs from FMU to IO.
*
- * @param rate The rate in Hz actuator outpus are sent to IO.
+ * @param[in] rate The rate in Hz actuator output are sent to IO.
* Min 10 Hz, max 400 Hz
*/
int set_update_rate(int rate);
@@ -111,29 +151,41 @@ public:
/**
* Set the battery current scaling and bias
*
- * @param amp_per_volt
- * @param amp_bias
+ * @param[in] amp_per_volt
+ * @param[in] amp_bias
*/
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
/**
* Push failsafe values to IO.
*
- * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
- * @param len Number of channels, could up to 8
+ * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+ * @param[in] len Number of channels, could up to 8
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
- * Print the current status of IO
- */
+ * Print IO status.
+ *
+ * Print all relevant IO status information
+ */
void print_status();
+ /**
+ * Set the DSM VCC is controlled by relay one flag
+ *
+ * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+ */
inline void set_dsm_vcc_ctl(bool enable)
{
_dsm_vcc_ctl = enable;
};
+ /**
+ * Get the DSM VCC is controlled by relay one flag
+ *
+ * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+ */
inline bool get_dsm_vcc_ctl()
{
return _dsm_vcc_ctl;
@@ -141,52 +193,48 @@ public:
private:
// XXX
- unsigned _max_actuators;
- unsigned _max_controls;
- unsigned _max_rc_input;
- unsigned _max_relays;
- unsigned _max_transfer;
+ unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO
+ unsigned _max_controls; ///<Maximum # of controls supported by PX4IO
+ unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO
+ unsigned _max_relays; ///<Maximum relays supported by PX4IO
+ unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO
- unsigned _update_interval; ///< subscription interval limiting send rate
+ unsigned _update_interval; ///<Subscription interval limiting send rate
- volatile int _task; ///< worker task
- volatile bool _task_should_exit;
+ volatile int _task; ///<worker task id
+ volatile bool _task_should_exit; ///<worker terminate flag
- int _mavlink_fd;
+ int _mavlink_fd; ///<mavlink file descriptor
- perf_counter_t _perf_update;
+ perf_counter_t _perf_update; ///<local performance counter
/* cached IO state */
- uint16_t _status;
- uint16_t _alarms;
+ uint16_t _status; ///<Various IO status flags
+ uint16_t _alarms; ///<Various IO alarms
/* subscribed topics */
- int _t_actuators; ///< actuator controls topic
- int _t_armed; ///< system armed control topic
- int _t_vstatus; ///< system / vehicle status
- int _t_param; ///< parameter update topic
+ int _t_actuators; ///<actuator controls topic
+ int _t_armed; ///<system armed control topic
+ int _t_vstatus; ///<system / vehicle status
+ int _t_param; ///<parameter update topic
/* advertised topics */
- orb_advert_t _to_input_rc; ///< rc inputs from io
- orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
- orb_advert_t _to_outputs; ///< mixed servo outputs topic
- orb_advert_t _to_battery; ///< battery status / voltage
+ orb_advert_t _to_input_rc; ///<rc inputs from IO topic
+ orb_advert_t _to_actuators_effective; ///<effective actuator controls topic
+ orb_advert_t _to_outputs; ///<mixed servo outputs topic
+ orb_advert_t _to_battery; ///<battery status / voltage topic
- actuator_outputs_s _outputs; ///< mixed outputs
- actuator_controls_effective_s _controls_effective; ///< effective controls
+ actuator_outputs_s _outputs; ///<mixed outputs
+ actuator_controls_effective_s _controls_effective; ///<effective controls
- bool _primary_pwm_device; ///< true if we are the default PWM output
+ bool _primary_pwm_device; ///<true if we are the default PWM output
- float _battery_amp_per_volt;
- float _battery_amp_bias;
- float _battery_mamphour_total;
- uint64_t _battery_last_timestamp;
+ float _battery_amp_per_volt; ///<current sensor amps/volt
+ float _battery_amp_bias; ///<current sensor bias
+ float _battery_mamphour_total;///<amp hours consumed so far
+ uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp
- /**
- * Relay1 is dedicated to controlling DSM receiver power
- */
-
- bool _dsm_vcc_ctl;
+ bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
/**
* Trampoline to the worker task
@@ -570,9 +618,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.
@@ -672,6 +722,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 (!(_status & PX4IO_P_STATUS_FLAGS_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);
@@ -1445,16 +1514,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:
@@ -1603,7 +1667,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
}
ssize_t
-PX4IO::write(file *filp, const char *buffer, size_t len)
+PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
+/* Make it obvious that file * isn't used here */
{
unsigned count = len / 2;
@@ -1696,30 +1761,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
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 7ef3db970..83a1a1abb 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -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 */
diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c
index 7b060412c..dbb45a138 100644
--- a/src/drivers/stm32/drv_pwm_servo.c
+++ b/src/drivers/stm32/drv_pwm_servo.c
@@ -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;
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 167ef30a8..2284be84d 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -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
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-28 12:23:04 +0000