diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-12 13:54:59 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-12 13:54:59 +0200 |
commit | df763ff7e24a8885313f6e8d472b834bd0a5e72d (patch) | |
tree | 0277f3aa8c84943aa9600a869e6d8a9b96a805e5 | |
parent | da3620bd53ebe2276a491c89ca02247bbe97ca73 (diff) | |
parent | 379623596715d0dce47192329ae9aceabb9ded11 (diff) | |
download | px4-firmware-df763ff7e24a8885313f6e8d472b834bd0a5e72d.tar.gz px4-firmware-df763ff7e24a8885313f6e8d472b834bd0a5e72d.tar.bz2 px4-firmware-df763ff7e24a8885313f6e8d472b834bd0a5e72d.zip |
Merged master
22 files changed, 489 insertions, 483 deletions
diff --git a/Debug/ARMv7M b/Debug/ARMv7M index 803d96453..4c539b345 100644 --- a/Debug/ARMv7M +++ b/Debug/ARMv7M @@ -56,7 +56,7 @@ define vecstate if $mmfsr & (1<<3) printf " during exception return" end - if $mmfsr & (1<<0) + if $mmfsr & (1<<1) printf " during data access" end if $mmfsr & (1<<0) diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index f6d82a5ec..7dbbb8284 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -8,7 +8,18 @@ echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index 794342a0b..6333aae56 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -8,7 +8,18 @@ echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1f466e49f..4c4b0129e 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -207,7 +207,9 @@ then if [ $MODE == autostart ] then # Telemetry port is on both FMU boards ttyS1 - mavlink start -b 57600 -d /dev/ttyS1 + # but the AR.Drone motors can be get 'flashed' + # if starting MAVLink on them - so do not + # start it as default (default link: USB) # Start commander commander start diff --git a/Tools/com b/Tools/com Binary files differdeleted file mode 100755 index 1d80d4aa5..000000000 --- a/Tools/com +++ /dev/null diff --git a/Tools/com.c b/Tools/com.c deleted file mode 100644 index fa52dcb61..000000000 --- a/Tools/com.c +++ /dev/null @@ -1,191 +0,0 @@ -/* - * Building: cc -o com com.c - * Usage : ./com /dev/device [speed] - * Example : ./com /dev/ttyS0 [115200] - * Keys : Ctrl-A - exit, Ctrl-X - display control lines status - * Darcs : darcs get http://tinyserial.sf.net/ - * Homepage: http://tinyserial.sourceforge.net - * Version : 2009-03-05 - * - * Ivan Tikhonov, http://www.brokestream.com, kefeer@brokestream.com - * Patches by Jim Kou, Henry Nestler, Jon Miner, Alan Horstmann - * - */ - - -/* Copyright (C) 2007 Ivan Tikhonov - - This software is provided 'as-is', without any express or implied - warranty. In no event will the authors be held liable for any damages - arising from the use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software - in a product, an acknowledgment in the product documentation would be - appreciated but is not required. - 2. Altered source versions must be plainly marked as such, and must not be - misrepresented as being the original software. - 3. This notice may not be removed or altered from any source distribution. - - Ivan Tikhonov, kefeer@brokestream.com - -*/ - -#include <termios.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <unistd.h> -#include <sys/signal.h> -#include <sys/types.h> -#include <sys/ioctl.h> -#include <fcntl.h> -#include <errno.h> - -int transfer_byte(int from, int to, int is_control); - -typedef struct {char *name; int flag; } speed_spec; - - -void print_status(int fd) { - int status; - unsigned int arg; - status = ioctl(fd, TIOCMGET, &arg); - fprintf(stderr, "[STATUS]: "); - if(arg & TIOCM_RTS) fprintf(stderr, "RTS "); - if(arg & TIOCM_CTS) fprintf(stderr, "CTS "); - if(arg & TIOCM_DSR) fprintf(stderr, "DSR "); - if(arg & TIOCM_CAR) fprintf(stderr, "DCD "); - if(arg & TIOCM_DTR) fprintf(stderr, "DTR "); - if(arg & TIOCM_RNG) fprintf(stderr, "RI "); - fprintf(stderr, "\r\n"); -} - -int main(int argc, char *argv[]) -{ - int comfd; - struct termios oldtio, newtio; //place for old and new port settings for serial port - struct termios oldkey, newkey; //place tor old and new port settings for keyboard teletype - char *devicename = argv[1]; - int need_exit = 0; - speed_spec speeds[] = - { - {"1200", B1200}, - {"2400", B2400}, - {"4800", B4800}, - {"9600", B9600}, - {"19200", B19200}, - {"38400", B38400}, - {"57600", B57600}, - {"115200", B115200}, - {NULL, 0} - }; - int speed = B9600; - - if(argc < 2) { - fprintf(stderr, "example: %s /dev/ttyS0 [115200]\n", argv[0]); - exit(1); - } - - comfd = open(devicename, O_RDWR | O_NOCTTY | O_NONBLOCK); - if (comfd < 0) - { - perror(devicename); - exit(-1); - } - - if(argc > 2) { - speed_spec *s; - for(s = speeds; s->name; s++) { - if(strcmp(s->name, argv[2]) == 0) { - speed = s->flag; - fprintf(stderr, "setting speed %s\n", s->name); - break; - } - } - } - - fprintf(stderr, "C-a exit, C-x modem lines status\n"); - - tcgetattr(STDIN_FILENO,&oldkey); - newkey.c_cflag = B9600 | CRTSCTS | CS8 | CLOCAL | CREAD; - newkey.c_iflag = IGNPAR; - newkey.c_oflag = 0; - newkey.c_lflag = 0; - newkey.c_cc[VMIN]=1; - newkey.c_cc[VTIME]=0; - tcflush(STDIN_FILENO, TCIFLUSH); - tcsetattr(STDIN_FILENO,TCSANOW,&newkey); - - - tcgetattr(comfd,&oldtio); // save current port settings - newtio.c_cflag = speed | CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - newtio.c_cc[VMIN]=1; - newtio.c_cc[VTIME]=0; - tcflush(comfd, TCIFLUSH); - tcsetattr(comfd,TCSANOW,&newtio); - - print_status(comfd); - - while(!need_exit) { - fd_set fds; - int ret; - - FD_ZERO(&fds); - FD_SET(STDIN_FILENO, &fds); - FD_SET(comfd, &fds); - - - ret = select(comfd+1, &fds, NULL, NULL, NULL); - if(ret == -1) { - perror("select"); - } else if (ret > 0) { - if(FD_ISSET(STDIN_FILENO, &fds)) { - need_exit = transfer_byte(STDIN_FILENO, comfd, 1); - } - if(FD_ISSET(comfd, &fds)) { - need_exit = transfer_byte(comfd, STDIN_FILENO, 0); - } - } - } - - tcsetattr(comfd,TCSANOW,&oldtio); - tcsetattr(STDIN_FILENO,TCSANOW,&oldkey); - close(comfd); - - return 0; -} - - -int transfer_byte(int from, int to, int is_control) { - char c; - int ret; - do { - ret = read(from, &c, 1); - } while (ret < 0 && errno == EINTR); - if(ret == 1) { - if(is_control) { - if(c == '\x01') { // C-a - return -1; - } else if(c == '\x18') { // C-x - print_status(to); - return 0; - } - } - while(write(to, &c, 1) == -1) { - if(errno!=EAGAIN && errno!=EINTR) { perror("write failed"); break; } - } - } else { - fprintf(stderr, "\nnothing to read. probably port disconnected.\n"); - return -2; - } - return 0; -} - diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 52d089360..64af672a3 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -400,7 +400,19 @@ print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property( # Spin waiting for a device to show up while True: - for port in args.port.split(","): + portlist = [] + patterns = args.port.split(",") + # on unix-like platforms use glob to support wildcard ports. This allows + # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from + # causing modem hangups etc + if "linux" in _platform or "darwin" in _platform: + import glob + for pattern in patterns: + portlist += glob.glob(pattern) + else: + portlist = patterns + + for port in portlist: #print("Trying %s" % port) diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 470ddfdf1..bc26d743d 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -12,10 +12,10 @@ SYSTYPE := $(shell uname -s) # XXX The uploader should be smarter than this. # ifeq ($(SYSTYPE),Darwin) -SERIAL_PORTS ?= "/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4" +SERIAL_PORTS ?= "/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" endif ifeq ($(SYSTYPE),Linux) -SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" +SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*" endif ifeq ($(SERIAL_PORTS),) SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 2a6b190de..5e45cc936 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -79,6 +79,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), _reports(nullptr), + _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), _sensor_ok(false), _measure_ticks(0), @@ -87,8 +88,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _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")) + _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls _debug_enabled = true; @@ -122,7 +122,7 @@ Airspeed::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer<differential_pressure_s>(2); + _reports = new RingBuffer(2, sizeof(differential_pressure_s)); if (_reports == nullptr) goto out; @@ -282,7 +282,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*abuf)) { + if (_reports->get(abuf)) { ret += sizeof(*abuf); abuf++; } @@ -312,7 +312,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(*abuf)) { + if (_reports->get(abuf)) { ret = sizeof(*abuf); } @@ -375,6 +375,6 @@ Airspeed::print_info() void Airspeed::new_report(const differential_pressure_s &report) { - if (!_reports->force(report)) + if (!_reports->force(&report)) perf_count(_buffer_overflows); } diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 7850ccc7e..048784813 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -104,7 +104,7 @@ public: virtual void print_info(); private: - RingBuffer<differential_pressure_s> *_reports; + RingBuffer *_reports; perf_counter_t _buffer_overflows; protected: diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index f14c008ce..f0044d36f 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -147,14 +147,7 @@ private: struct hrt_call _call; unsigned _call_interval; - /* - this wrapper type is needed to avoid a linker error for - RingBuffer instances which appear in two drivers. - */ - struct _accel_report { - accel_report r; - }; - RingBuffer<struct _accel_report> *_reports; + RingBuffer *_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -284,7 +277,7 @@ BMA180::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer<_accel_report>(2); + _reports = new RingBuffer(2, sizeof(accel_report)); if (_reports == nullptr) goto out; @@ -349,7 +342,7 @@ ssize_t BMA180::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); - struct _accel_report *arp = reinterpret_cast<struct _accel_report *>(buffer); + struct accel_report *arp = reinterpret_cast<struct accel_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -365,7 +358,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_reports->get(*arp)) { + if (_reports->get(arp)) { ret += sizeof(*arp); arp++; } @@ -380,7 +373,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_reports->get(*arp)) + if (_reports->get(arp)) ret = sizeof(*arp); return ret; @@ -676,7 +669,7 @@ BMA180::measure() // } raw_report; // #pragma pack(pop) - struct _accel_report report; + struct accel_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -696,40 +689,40 @@ BMA180::measure() * them before. There is no good way to synchronise with the internal * measurement flow without using the external interrupt. */ - report.r.timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report.r.y_raw = read_reg(ADDR_ACC_X_LSB + 0); - report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; - report.r.x_raw = read_reg(ADDR_ACC_X_LSB + 2); - report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; - report.r.z_raw = read_reg(ADDR_ACC_X_LSB + 4); - report.r.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; + report.y_raw = read_reg(ADDR_ACC_X_LSB + 0); + report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + report.x_raw = read_reg(ADDR_ACC_X_LSB + 2); + report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + report.z_raw = read_reg(ADDR_ACC_X_LSB + 4); + report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; /* discard two non-value bits in the 16 bit measurement */ - report.r.x_raw = (report.r.x_raw / 4); - report.r.y_raw = (report.r.y_raw / 4); - report.r.z_raw = (report.r.z_raw / 4); + report.x_raw = (report.x_raw / 4); + report.y_raw = (report.y_raw / 4); + report.z_raw = (report.z_raw / 4); /* invert y axis, due to 14 bit data no overflow can occur in the negation */ - report.r.y_raw = -report.r.y_raw; + report.y_raw = -report.y_raw; - report.r.x = ((report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - report.r.y = ((report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - report.r.z = ((report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - report.r.scaling = _accel_range_scale; - report.r.range_m_s2 = _accel_range_m_s2; + report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + report.scaling = _accel_range_scale; + report.range_m_s2 = _accel_range_m_s2; - _reports->force(report); + _reports->force(&report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &report.r); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); /* stop the perf counter */ perf_end(_sample_perf); diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index e3c5a20bd..a9e22eaa6 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -1,7 +1,6 @@ /**************************************************************************** * * 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 @@ -35,15 +34,14 @@ /** * @file ringbuffer.h * - * A simple ringbuffer template. + * A flexible ringbuffer class. */ #pragma once -template<typename T> class RingBuffer { public: - RingBuffer(unsigned size); + RingBuffer(unsigned ring_size, size_t entry_size); virtual ~RingBuffer(); /** @@ -52,15 +50,18 @@ public: * @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 if there is space. - * - * @param val Item to put - * @return true if the item was put, false if the buffer is full - */ - bool put(const T &val); + bool put(const void *val, size_t val_size = 0); + + bool put(int8_t val); + bool put(uint8_t val); + bool put(int16_t val); + bool put(uint16_t val); + bool put(int32_t val); + bool put(uint32_t val); + bool put(int64_t val); + bool put(uint64_t val); + bool put(float val); + bool put(double val); /** * Force an item into the buffer, discarding an older item if there is not space. @@ -68,15 +69,18 @@ public: * @param val Item to put * @return true if an item was discarded to make space */ - bool force(T &val); - - /** - * Force an item into the buffer, discarding an older item if there is not space. - * - * @param val Item to put - * @return true if an item was discarded to make space - */ - bool force(const T &val); + bool force(const void *val, size_t val_size = 0); + + bool force(int8_t val); + bool force(uint8_t val); + bool force(int16_t val); + bool force(uint16_t val); + bool force(int32_t val); + bool force(uint32_t val); + bool force(int64_t val); + bool force(uint64_t val); + bool force(float val); + bool force(double val); /** * Get an item from the buffer. @@ -84,15 +88,18 @@ public: * @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. If the buffer is empty, - * returns zero. - */ - T get(void); + bool get(void *val, size_t val_size = 0); + + bool get(int8_t &val); + bool get(uint8_t &val); + bool get(int16_t &val); + bool get(uint16_t &val); + bool get(int32_t &val); + bool get(uint32_t &val); + bool get(int64_t &val); + bool get(uint64_t &val); + bool get(float &val); + bool get(double &val); /* * Get the number of slots free in the buffer. @@ -148,67 +155,68 @@ public: void print_info(const char *name); private: - T *_buf; - unsigned _size; - volatile unsigned _head; /**< insertion point */ - volatile unsigned _tail; /**< removal point */ + unsigned _num_items; + const size_t _item_size; + char *_buf; + volatile unsigned _head; /**< insertion point in _item_size units */ + volatile unsigned _tail; /**< removal point in _item_size units */ 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) +RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : + _num_items(num_items), + _item_size(item_size), + _buf(new char[(_num_items+1) * item_size]), + _head(_num_items), + _tail(_num_items) {} -template <typename T> -RingBuffer<T>::~RingBuffer() +RingBuffer::~RingBuffer() { if (_buf != nullptr) delete[] _buf; } -template <typename T> -bool RingBuffer<T>::empty() +unsigned +RingBuffer::_next(unsigned index) { - return _tail == _head; + return (0 == index) ? _num_items : (index - 1); } -template <typename T> -bool RingBuffer<T>::full() +bool +RingBuffer::empty() { - return _next(_head) == _tail; + return _tail == _head; } -template <typename T> -unsigned RingBuffer<T>::size() +bool +RingBuffer::full() { - return (_buf != nullptr) ? _size : 0; + return _next(_head) == _tail; } -template <typename T> -void RingBuffer<T>::flush() +unsigned +RingBuffer::size() { - T junk; - while (!empty()) - get(junk); + return (_buf != nullptr) ? _num_items : 0; } -template <typename T> -unsigned RingBuffer<T>::_next(unsigned index) +void +RingBuffer::flush() { - return (0 == index) ? _size : (index - 1); + while (!empty()) + get(NULL); } -template <typename T> -bool RingBuffer<T>::put(T &val) +bool +RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); if (next != _tail) { - _buf[_head] = val; + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + memcpy(&_buf[_head * _item_size], val, val_size); _head = next; return true; } else { @@ -216,55 +224,150 @@ bool RingBuffer<T>::put(T &val) } } -template <typename T> -bool RingBuffer<T>::put(const T &val) +bool +RingBuffer::put(int8_t val) { - unsigned next = _next(_head); - if (next != _tail) { - _buf[_head] = val; - _head = next; - return true; - } else { - return false; - } + return put(&val, sizeof(val)); } -template <typename T> -bool RingBuffer<T>::force(T &val) +bool +RingBuffer::put(uint8_t val) { - bool overwrote = false; + return put(&val, sizeof(val)); +} - for (;;) { - if (put(val)) - break; - T junk; - get(junk); - overwrote = true; - } - return overwrote; +bool +RingBuffer::put(int16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(float val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(double val) +{ + return put(&val, sizeof(val)); } -template <typename T> -bool RingBuffer<T>::force(const T &val) +bool +RingBuffer::force(const void *val, size_t val_size) { bool overwrote = false; for (;;) { - if (put(val)) + if (put(val, val_size)) break; - T junk; - get(junk); + get(NULL); overwrote = true; } return overwrote; } -template <typename T> -bool RingBuffer<T>::get(T &val) +bool +RingBuffer::force(int8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(float val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(double val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::get(void *val, size_t val_size) { if (_tail != _head) { unsigned candidate; unsigned next; + + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + do { /* decide which element we think we're going to read */ candidate = _tail; @@ -273,7 +376,8 @@ bool RingBuffer<T>::get(T &val) next = _next(candidate); /* go ahead and read from this index */ - val = _buf[candidate]; + if (val != NULL) + memcpy(val, &_buf[candidate * _item_size], val_size); /* if the tail pointer didn't change, we got our item */ } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); @@ -284,15 +388,68 @@ bool RingBuffer<T>::get(T &val) } } -template <typename T> -T RingBuffer<T>::get(void) +bool +RingBuffer::get(int8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int16_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint16_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(float &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(double &val) { - T val; - return get(val) ? val : 0; + return get(&val, sizeof(val)); } -template <typename T> -unsigned RingBuffer<T>::space(void) +unsigned +RingBuffer::space(void) { unsigned tail, head; @@ -309,39 +466,44 @@ unsigned RingBuffer<T>::space(void) tail = _tail; } while (head != _head); - return (tail >= head) ? (_size - (tail - head)) : (head - tail - 1); + return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1); } -template <typename T> -unsigned RingBuffer<T>::count(void) +unsigned +RingBuffer::count(void) { /* * Note that due to the conservative nature of space(), this may * over-estimate the number of items in the buffer. */ - return _size - space(); + return _num_items - space(); } -template <typename T> -bool RingBuffer<T>::resize(unsigned new_size) +bool +RingBuffer::resize(unsigned new_size) { - T *old_buffer; - T *new_buffer = new T[new_size + 1]; + char *old_buffer; + char *new_buffer = new char [(new_size+1) * _item_size]; if (new_buffer == nullptr) { return false; } old_buffer = _buf; _buf = new_buffer; - _size = new_size; + _num_items = new_size; _head = new_size; _tail = new_size; delete[] old_buffer; return true; } -template <typename T> -void RingBuffer<T>::print_info(const char *name) +void +RingBuffer::print_info(const char *name) { - printf("%s %u (%u/%u @ %p)\n", - name, _size, _head, _tail, _buf); + printf("%s %u/%u (%u/%u @ %p)\n", + name, + _num_items, + _num_items * _item_size, + _head, + _tail, + _buf); } diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index b838bf16b..58a1593ed 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -149,7 +149,7 @@ private: work_s _work; unsigned _measure_ticks; - RingBuffer<struct mag_report> *_reports; + RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -367,7 +367,7 @@ HMC5883::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer<struct mag_report>(2); + _reports = new RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; @@ -496,7 +496,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*mag_buf)) { + if (_reports->get(mag_buf)) { ret += sizeof(struct mag_report); mag_buf++; } @@ -526,7 +526,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) break; } - if (_reports->get(*mag_buf)) { + if (_reports->get(mag_buf)) { ret = sizeof(struct mag_report); } } while (0); @@ -878,7 +878,7 @@ HMC5883::collect() orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); /* post a report to the ring */ - if (_reports->force(new_report)) { + if (_reports->force(&new_report)) { perf_count(_buffer_overflows); } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 7fb1cbbbc..4c3b0ce51 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -185,7 +185,7 @@ private: struct hrt_call _call; unsigned _call_interval; - RingBuffer<gyro_report> *_reports; + RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -347,7 +347,7 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer<struct gyro_report>(2); + _reports = new RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) goto out; @@ -367,9 +367,12 @@ out: int L3GD20::probe() { + irqstate_t flags = irqsave(); /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); + bool success = false; + /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { @@ -380,15 +383,21 @@ L3GD20::probe() #else #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 #endif - return OK; + + success = true; } if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; - return OK; + success = true; } + irqrestore(flags); + + if (success) + return OK; + return -EIO; } @@ -412,7 +421,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_reports->get(*gbuf)) { + if (_reports->get(gbuf)) { ret += sizeof(*gbuf); gbuf++; } @@ -427,7 +436,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_reports->get(*gbuf)) { + if (_reports->get(gbuf)) { ret = sizeof(*gbuf); } @@ -806,7 +815,7 @@ L3GD20::measure() report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; - _reports->force(report); + _reports->force(&report); /* notify anyone waiting for data */ poll_notify(POLLIN); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 947e17712..a90cd0a3d 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -219,19 +219,8 @@ private: unsigned _call_accel_interval; unsigned _call_mag_interval; - /* - these wrapper types are needed to avoid a linker error for - RingBuffer instances which appear in two drivers. - */ - struct _accel_report { - struct accel_report r; - }; - RingBuffer<struct _accel_report> *_accel_reports; - - struct _mag_report { - struct mag_report r; - }; - RingBuffer<struct _mag_report> *_mag_reports; + RingBuffer *_accel_reports; + RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -409,7 +398,7 @@ public: LSM303D_mag(LSM303D *parent); ~LSM303D_mag(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); protected: @@ -493,11 +482,13 @@ LSM303D::init() int mag_ret; /* do SPI init (and probe) first */ - if (SPI::init() != OK) + if (SPI::init() != OK) { + warnx("SPI init failed"); goto out; + } /* allocate basic report buffers */ - _accel_reports = new RingBuffer<struct _accel_report>(2); + _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; @@ -507,7 +498,7 @@ LSM303D::init() memset(&zero_report, 0, sizeof(zero_report)); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _mag_reports = new RingBuffer<struct _mag_report>(2); + _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) goto out; @@ -534,6 +525,7 @@ out: void LSM303D::reset() { + irqstate_t flags = irqsave(); /* enable accel*/ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); @@ -548,6 +540,7 @@ LSM303D::reset() mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); + irqrestore(flags); _accel_read = 0; _mag_read = 0; @@ -556,11 +549,16 @@ LSM303D::reset() int LSM303D::probe() { + irqstate_t flags = irqsave(); /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); + + irqrestore(flags); + + if (success) return OK; return -EIO; @@ -570,7 +568,7 @@ ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); - struct _accel_report *arb = reinterpret_cast<struct _accel_report *>(buffer); + accel_report *arb = reinterpret_cast<accel_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -583,7 +581,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) * While there is space in the caller's buffer, and reports, copy them. */ while (count--) { - if (_accel_reports->get(*arb)) { + if (_accel_reports->get(arb)) { ret += sizeof(*arb); arb++; } @@ -597,7 +595,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(*arb)) + if (_accel_reports->get(arb)) ret = sizeof(*arb); return ret; @@ -607,7 +605,7 @@ ssize_t LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); - struct _mag_report *mrb = reinterpret_cast<struct _mag_report *>(buffer); + mag_report *mrb = reinterpret_cast<mag_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -621,7 +619,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) * While there is space in the caller's buffer, and reports, copy them. */ while (count--) { - if (_mag_reports->get(*mrb)) { + if (_mag_reports->get(mrb)) { ret += sizeof(*mrb); mrb++; } @@ -636,7 +634,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(*mrb)) + if (_mag_reports->get(mrb)) ret = sizeof(*mrb); return ret; @@ -1228,7 +1226,7 @@ LSM303D::measure() } raw_accel_report; #pragma pack(pop) - struct _accel_report accel_report; + accel_report accel_report; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1253,30 +1251,30 @@ LSM303D::measure() */ - accel_report.r.timestamp = hrt_absolute_time(); + accel_report.timestamp = hrt_absolute_time(); - accel_report.r.x_raw = raw_accel_report.x; - accel_report.r.y_raw = raw_accel_report.y; - accel_report.r.z_raw = raw_accel_report.z; + accel_report.x_raw = raw_accel_report.x; + accel_report.y_raw = raw_accel_report.y; + accel_report.z_raw = raw_accel_report.z; - float x_in_new = ((accel_report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - accel_report.r.x = _accel_filter_x.apply(x_in_new); - accel_report.r.y = _accel_filter_y.apply(y_in_new); - accel_report.r.z = _accel_filter_z.apply(z_in_new); + accel_report.x = _accel_filter_x.apply(x_in_new); + accel_report.y = _accel_filter_y.apply(y_in_new); + accel_report.z = _accel_filter_z.apply(z_in_new); - accel_report.r.scaling = _accel_range_scale; - accel_report.r.range_m_s2 = _accel_range_m_s2; + accel_report.scaling = _accel_range_scale; + accel_report.range_m_s2 = _accel_range_m_s2; - _accel_reports->force(accel_report); + _accel_reports->force(&accel_report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report.r); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); _accel_read++; @@ -1298,7 +1296,7 @@ LSM303D::mag_measure() } raw_mag_report; #pragma pack(pop) - struct _mag_report mag_report; + mag_report mag_report; /* start the performance counter */ perf_begin(_mag_sample_perf); @@ -1323,25 +1321,25 @@ LSM303D::mag_measure() */ - mag_report.r.timestamp = hrt_absolute_time(); + mag_report.timestamp = hrt_absolute_time(); - mag_report.r.x_raw = raw_mag_report.x; - mag_report.r.y_raw = raw_mag_report.y; - mag_report.r.z_raw = raw_mag_report.z; - mag_report.r.x = ((mag_report.r.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.r.y = ((mag_report.r.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.r.z = ((mag_report.r.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - mag_report.r.scaling = _mag_range_scale; - mag_report.r.range_ga = (float)_mag_range_ga; + mag_report.x_raw = raw_mag_report.x; + mag_report.y_raw = raw_mag_report.y; + mag_report.z_raw = raw_mag_report.z; + mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report.scaling = _mag_range_scale; + mag_report.range_ga = (float)_mag_range_ga; - _mag_reports->force(mag_report); + _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report.r); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); _mag_read++; @@ -1426,8 +1424,10 @@ start() /* create the driver */ g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); - if (g_dev == nullptr) + if (g_dev == nullptr) { + warnx("failed instantiating LSM303D obj"); goto fail; + } if (OK != g_dev->init()) goto fail; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index fabe10b87..ccc5bc15e 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -120,7 +120,7 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer<struct range_finder_report> *_reports; + RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -226,7 +226,7 @@ MB12XX::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer<struct range_finder_report>(2); + _reports = new RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) goto out; @@ -403,7 +403,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*rbuf)) { + if (_reports->get(rbuf)) { ret += sizeof(*rbuf); rbuf++; } @@ -433,7 +433,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(*rbuf)) { + if (_reports->get(rbuf)) { ret = sizeof(*rbuf); } @@ -496,7 +496,7 @@ MB12XX::collect() /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - if (_reports->force(report)) { + if (_reports->force(&report)) { perf_count(_buffer_overflows); } diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 66d36826a..14a3571de 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -194,26 +194,14 @@ private: struct hrt_call _call; unsigned _call_interval; - /* - these wrapper types are needed to avoid a linker error for - RingBuffer instances which appear in two drivers. - */ - struct _accel_report { - struct accel_report r; - }; - typedef RingBuffer<_accel_report> AccelReportBuffer; - AccelReportBuffer *_accel_reports; + RingBuffer *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - struct _gyro_report { - struct gyro_report r; - }; - typedef RingBuffer<_gyro_report> GyroReportBuffer; - GyroReportBuffer *_gyro_reports; + RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -441,11 +429,11 @@ MPU6000::init() } /* allocate basic report buffers */ - _accel_reports = new AccelReportBuffer(2); + _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _gyro_reports = new GyroReportBuffer(2); + _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); if (_gyro_reports == nullptr) goto out; @@ -475,16 +463,16 @@ MPU6000::init() if (gyro_ret != OK) { _gyro_topic = -1; } else { - _gyro_report gr; - _gyro_reports->get(gr); + gyro_report gr; + _gyro_reports->get(&gr); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r); + _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.r); + accel_report ar; + _accel_reports->get(&ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); out: return ret; @@ -665,10 +653,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - _accel_report *arp = reinterpret_cast<_accel_report *>(buffer); + accel_report *arp = reinterpret_cast<accel_report *>(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(*arp)) + if (!_accel_reports->get(arp)) break; transferred++; arp++; @@ -759,10 +747,10 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - _gyro_report *grp = reinterpret_cast<_gyro_report *>(buffer); + gyro_report *grp = reinterpret_cast<gyro_report *>(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(*grp)) + if (!_gyro_reports->get(grp)) break; transferred++; grp++; @@ -1191,13 +1179,13 @@ MPU6000::measure() /* * Report buffers. */ - _accel_report arb; - _gyro_report grb; + accel_report arb; + gyro_report grb; /* * Adjust and scale results to m/s^2. */ - grb.r.timestamp = arb.r.timestamp = hrt_absolute_time(); + grb.timestamp = arb.timestamp = hrt_absolute_time(); /* @@ -1218,53 +1206,53 @@ MPU6000::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - arb.r.x_raw = report.accel_x; - arb.r.y_raw = report.accel_y; - arb.r.z_raw = report.accel_z; + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; 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.r.x = _accel_filter_x.apply(x_in_new); - arb.r.y = _accel_filter_y.apply(y_in_new); - arb.r.z = _accel_filter_z.apply(z_in_new); + 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); - arb.r.scaling = _accel_range_scale; - arb.r.range_m_s2 = _accel_range_m_s2; + arb.scaling = _accel_range_scale; + arb.range_m_s2 = _accel_range_m_s2; - arb.r.temperature_raw = report.temp; - arb.r.temperature = (report.temp) / 361.0f + 35.0f; + arb.temperature_raw = report.temp; + arb.temperature = (report.temp) / 361.0f + 35.0f; - grb.r.x_raw = report.gyro_x; - grb.r.y_raw = report.gyro_y; - grb.r.z_raw = report.gyro_z; + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; 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.r.x = _gyro_filter_x.apply(x_gyro_in_new); - grb.r.y = _gyro_filter_y.apply(y_gyro_in_new); - grb.r.z = _gyro_filter_z.apply(z_gyro_in_new); + 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.r.scaling = _gyro_range_scale; - grb.r.range_rad_s = _gyro_range_rad_s; + grb.scaling = _gyro_range_scale; + grb.range_rad_s = _gyro_range_rad_s; - grb.r.temperature_raw = report.temp; - grb.r.temperature = (report.temp) / 361.0f + 35.0f; + grb.temperature_raw = report.temp; + grb.temperature = (report.temp) / 361.0f + 35.0f; - _accel_reports->force(arb); - _gyro_reports->force(grb); + _accel_reports->force(&arb); + _gyro_reports->force(&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, &arb.r); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); } /* stop measuring */ diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 3f57cd68f..1c8a4d776 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -115,7 +115,7 @@ protected: struct work_s _work; unsigned _measure_ticks; - RingBuffer<struct baro_report> *_reports; + RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -241,7 +241,7 @@ MS5611::init() } /* allocate basic report buffers */ - _reports = new RingBuffer<struct baro_report>(2); + _reports = new RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { debug("can't get memory for reports"); @@ -285,7 +285,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_reports->get(*brp)) { + if (_reports->get(brp)) { ret += sizeof(*brp); brp++; } @@ -327,7 +327,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(*brp)) + if (_reports->get(brp)) ret = sizeof(*brp); } while (0); @@ -664,7 +664,7 @@ MS5611::collect() /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - if (_reports->force(report)) { + if (_reports->force(&report)) { perf_count(_buffer_overflows); } diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index f6c624340..21caed2ff 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -134,6 +134,7 @@ int MS5611_SPI::init() { int ret; + irqstate_t flags; ret = SPI::init(); if (ret != OK) { @@ -141,15 +142,23 @@ MS5611_SPI::init() goto out; } + /* disable interrupts, make this section atomic */ + flags = irqsave(); /* send reset command */ ret = _reset(); + /* re-enable interrupts */ + irqrestore(flags); if (ret != OK) { debug("reset failed"); goto out; } + /* disable interrupts, make this section atomic */ + flags = irqsave(); /* read PROM */ ret = _read_prom(); + /* re-enable interrupts */ + irqrestore(flags); if (ret != OK) { debug("prom readout failed"); goto out; diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index c78232f11..8245aa560 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -62,15 +62,15 @@ #include <systemlib/param/param.h> #include <drivers/drv_hrt.h> -PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); //PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); //PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); -PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); +PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); +PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f); //PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); //PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 0a336be47..adb63186c 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -59,14 +59,14 @@ #include <systemlib/err.h> #include <drivers/drv_hrt.h> -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); //PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); //PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.0f); /* 0.15 F405 Flamewheel */ -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); //PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); //PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index bf9578ea3..b7041e4d5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -54,7 +54,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) |