yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merged master

Browse Source
This commit is contained in:
Lorenz Meier
2013-09-12 13:54:59 +02:00
parent da3620bd53 3796235967
commit df763ff7e2
22 changed files with 487 additions and 481 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Debug/ARMv7M
View File

@@ -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)

13
ROMFS/px4fmu_common/init.d/08_ardrone
View File

@@ -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

13
ROMFS/px4fmu_common/init.d/09_ardrone_flow
View File

@@ -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

4
ROMFS/px4fmu_common/init.d/rcS
View File

@@ -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

BIN
Tools/com
View File

Binary file not shown.

191
Tools/com.c
View File

@@ -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;
}

14
Tools/px_uploader.py
View File

@@ -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)

4
makefiles/upload.mk
View File

@@ -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"

12
src/drivers/airspeed/airspeed.cpp
View File

@@ -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);
}

2
src/drivers/airspeed/airspeed.h
View File

@@ -104,7 +104,7 @@ public:
virtual void print_info();
private:
RingBuffer<differential_pressure_s> *_reports;
RingBuffer *_reports;
perf_counter_t _buffer_overflows;
protected:

55
src/drivers/bma180/bma180.cpp
View File

@@ -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);

380
src/drivers/device/ringbuffer.h
View File

@@ -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);
bool put(const void *val, size_t val_size = 0);
/**
* 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(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);
bool force(const void *val, size_t val_size = 0);
/**
* 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(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);
bool get(void *val, size_t val_size = 0);
/**
* 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(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 (0 == index) ? _num_items : (index - 1);
}
bool
RingBuffer::empty()
{
return _tail == _head;
}
template <typename T>
bool RingBuffer<T>::full()
bool
RingBuffer::full()
{
return _next(_head) == _tail;
}
template <typename T>
unsigned RingBuffer<T>::size()
unsigned
RingBuffer::size()
{
return (_buf != nullptr) ? _size : 0;
return (_buf != nullptr) ? _num_items : 0;
}
template <typename T>
void RingBuffer<T>::flush()
void
RingBuffer::flush()
{
T junk;
while (!empty())
get(junk);
get(NULL);
}
template <typename T>
unsigned RingBuffer<T>::_next(unsigned index)
{
return (0 == index) ? _size : (index - 1);
}
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)
{
return put(&val, sizeof(val));
}
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));
}
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>::force(const T &val)
bool
RingBuffer::force(int8_t val)
{
bool overwrote = false;
for (;;) {
if (put(val))
break;
T junk;
get(junk);
overwrote = true;
}
return overwrote;
return force(&val, sizeof(val));
}
template <typename T>
bool RingBuffer<T>::get(T &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)
{
T val;
return get(val) ? val : 0;
return get(&val, sizeof(val));
}
template <typename T>
unsigned RingBuffer<T>::space(void)
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)
{
return get(&val, sizeof(val));
}
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);
}

10
src/drivers/hmc5883/hmc5883.cpp
View File

@@ -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);
}

23
src/drivers/l3gd20/l3gd20.cpp
View File

@@ -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);

104
src/drivers/lsm303d/lsm303d.cpp
View File

@@ -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;

10
src/drivers/mb12xx/mb12xx.cpp
View File

@@ -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);
}

94
src/drivers/mpu6000/mpu6000.cpp
View File

@@ -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 */

10
src/drivers/ms5611/ms5611.cpp
View File

@@ -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);
}

9
src/drivers/ms5611/ms5611_spi.cpp
View File

@@ -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;

6
src/modules/multirotor_att_control/multirotor_attitude_control.c
View File

@@ -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);

10
src/modules/multirotor_att_control/multirotor_rate_control.c
View File

@@ -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 */

2
src/modules/multirotor_pos_control/multirotor_pos_control_params.c
View File

@@ -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)