dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' of github.com:PX4/Firmware into fat-dma

This commit is contained in:
Lorenz Meier 2013-09-12 09:52:49 +02:00
parent c92e3f3f4b 5ece19f66a
commit be436d3a99
43 changed files with 3414 additions and 779 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
ROMFS/px4fmu_common/init.d/100_mpx_easystar
View File

@ -1,6 +1,6 @@
#!nsh
echo "[init] Multiplex Easystar"
echo "[init] PX4FMU v1, v2 with or without IO on EasyStar"
#
# Load default params for this platform
@ -20,28 +20,31 @@ fi
#
param set MAV_TYPE 1
#
# Start MAVLink (depends on orb)
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
set EXIT_ON_END no
#
# Start and configure PX4IO interface
# Start and configure PX4IO or FMU interface
#
sh /etc/init.d/rc.io
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
#
# Set actuator limit to 100 Hz update (50 Hz PWM)
px4io limit 100
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Start the commander
#
commander start
#
# Start the sensors
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
@ -49,9 +52,14 @@ sh /etc/init.d/rc.sensors
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#
gps start
@ -65,4 +73,10 @@ att_pos_estimator_ekf start
#
mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
fw_att_control start
fw_pos_control_l1 start
# Not ready yet for prime-time
#fw_pos_control_l1 start
if [ $EXIT_ON_END == yes ]
then
exit
fi

82
ROMFS/px4fmu_common/init.d/101_hk_bixler Normal file
View File

@ -0,0 +1,82 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
fw_att_control start
# Not ready yet for prime-time
#fw_pos_control_l1 start
if [ $EXIT_ON_END == yes ]
then
exit
fi

63
ROMFS/px4fmu_common/init.d/30_io_camflyer
View File

@ -1,6 +1,6 @@
#!nsh
echo "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer"
echo "[init] PX4FMU v1, v2 with or without IO on Camflyer"
#
# Load default params for this platform
@ -19,34 +19,32 @@ fi
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
#
# Start MAVLink (depends on orb)
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
#
# Start the commander (depends on orb, mavlink)
#
commander start
#
# Start PX4IO interface (depends on orb, commander)
#
px4io start
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
px4io recovery
set EXIT_ON_END no
#
# Set actuator limit to 100 Hz update (50 Hz PWM)
px4io limit 100
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Start the sensors (depends on orb, px4io)
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
@ -54,7 +52,12 @@ sh /etc/init.d/rc.sensors
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#
@ -70,4 +73,10 @@ att_pos_estimator_ekf start
#
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fw_att_control start
fw_pos_control_l1 start
# Not ready yet for prime-time
#fw_pos_control_l1 start
if [ $EXIT_ON_END == yes ]
then
exit
fi

54
ROMFS/px4fmu_common/init.d/31_io_phantom
View File

@ -1,6 +1,6 @@
#!nsh
echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom"
echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV"
#
# Load default params for this platform
@ -20,28 +20,31 @@ fi
#
param set MAV_TYPE 1
#
# Start MAVLink (depends on orb)
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
set EXIT_ON_END no
#
# Start and configure PX4IO interface
# Start and configure PX4IO or FMU interface
#
sh /etc/init.d/rc.io
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
#
# Set actuator limit to 100 Hz update (50 Hz PWM)
px4io limit 100
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Start the commander
#
commander start
#
# Start the sensors
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
@ -49,9 +52,14 @@ sh /etc/init.d/rc.sensors
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#
gps start
@ -65,4 +73,10 @@ att_pos_estimator_ekf start
#
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fw_att_control start
fw_pos_control_l1 start
# Not ready yet for prime-time
#fw_pos_control_l1 start
if [ $EXIT_ON_END == yes ]
then
exit
fi

8
ROMFS/px4fmu_common/init.d/rc.hil
View File

@ -5,10 +5,9 @@
echo "[HIL] starting.."
uorb start
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyS1
sleep 2
mavlink start -b 230400 -d /dev/ttyACM0
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
@ -50,7 +49,8 @@ att_pos_estimator_ekf start
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
fw_pos_control_l1 start
#fw_pos_control_l1 start
fw_att_control start
echo "[HIL] setup done, running"

8
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -21,7 +21,6 @@ if mavlink stop
then
echo "stopped other MAVLink instance"
fi
sleep 2
mavlink start -b 230400 -d /dev/ttyACM0
# Stop commander
@ -37,13 +36,6 @@ then
echo "Commander started"
fi
# Stop px4io
if px4io stop
then
echo "PX4IO stopped"
fi
sleep 1
# Start px4io if present
if px4io start
then

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

@ -101,8 +101,14 @@ then
fi
fi
# Try to get an USB console
nshterm /dev/ttyACM0 &
if param compare SYS_AUTOSTART 1000
then
sh /etc/init.d/rc.hil
set MODE custom
else
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi
#
# Upgrade PX4IO firmware
@ -178,6 +184,18 @@ then
sh /etc/init.d/31_io_phantom
set MODE custom
fi
if param compare SYS_AUTOSTART 100
then
sh /etc/init.d/100_mpx_easystar
set MODE custom
fi
if param compare SYS_AUTOSTART 101
then
sh /etc/init.d/101_hk_bixler
set MODE custom
fi
# Start any custom extensions that might be missing
if [ -f /fs/microsd/etc/rc.local ]
@ -190,7 +208,6 @@ then
then
# Telemetry port is on both FMU boards ttyS1
mavlink start -b 57600 -d /dev/ttyS1
usleep 5000
# Start commander
commander start

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/config_px4fmu-v1_default.mk
View File

@ -80,7 +80,7 @@ MODULES += examples/flow_position_estimator
#
#MODULES += modules/segway # XXX needs state machine update
#MODULES += modules/fw_pos_control_l1
#MODULES += modules/fw_att_control
MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
MODULES += modules/multirotor_pos_control
MODULES += examples/flow_position_control
@ -111,7 +111,7 @@ MODULES += modules/uORB
LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
#MODULES += lib/ecl
MODULES += lib/ecl
MODULES += lib/geo
#

5
makefiles/config_px4fmu-v2_default.mk
View File

@ -75,8 +75,9 @@ MODULES += examples/flow_position_estimator
#
# Vehicle Control
#
#MODULES += modules/segway # XXX needs state machine update
#MODULES += modules/fw_pos_control_l1
#MODULES += modules/fw_att_control
MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
MODULES += modules/multirotor_pos_control
@ -105,7 +106,7 @@ MODULES += modules/uORB
LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
#MODULES += lib/ecl
MODULES += lib/ecl
MODULES += lib/geo
#

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"

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

@ -68,6 +68,7 @@
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
@ -77,10 +78,9 @@
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
_max_differential_pressure_pa(0),
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
@ -88,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;
@ -105,7 +104,7 @@ Airspeed::~Airspeed()
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
delete _reports;
// free perf counters
perf_free(_sample_perf);
@ -123,20 +122,14 @@ Airspeed::init()
goto out;
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct differential_pressure_s[_num_reports];
for (unsigned i = 0; i < _num_reports; i++)
_reports[i].max_differential_pressure_pa = 0;
_reports = new RingBuffer(2, sizeof(differential_pressure_s));
if (_reports == nullptr)
goto out;
_oldest_report = _next_report = 0;
/* get a publish handle on the airspeed topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
differential_pressure_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report);
if (_airspeed_pub < 0)
warnx("failed to create airspeed sensor object. Did you start uOrb?");
@ -229,31 +222,22 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
if (nullptr == buf)
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
@ -281,7 +265,8 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
Airspeed::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct differential_pressure_s);
unsigned count = buflen / sizeof(differential_pressure_s);
differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
@ -297,10 +282,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with them.
*/
while (count--) {
if (_oldest_report != _next_report) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
ret += sizeof(_reports[0]);
INCREMENT(_oldest_report, _num_reports);
if (_reports->get(abuf)) {
ret += sizeof(*abuf);
abuf++;
}
}
@ -309,9 +293,8 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
}
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_oldest_report = _next_report = 0;
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
@ -329,8 +312,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
}
/* state machine will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
if (_reports->get(abuf)) {
ret = sizeof(*abuf);
}
} while (0);
@ -342,7 +326,7 @@ Airspeed::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_oldest_report = _next_report = 0;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
@ -385,6 +369,12 @@ Airspeed::print_info()
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
warnx("poll interval: %u ticks", _measure_ticks);
warnx("report queue: %u (%u/%u @ %p)",
_num_reports, _oldest_report, _next_report, _reports);
_reports->print_info("report queue");
}
void
Airspeed::new_report(const differential_pressure_s &report)
{
if (!_reports->force(&report))
perf_count(_buffer_overflows);
}

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

@ -68,6 +68,7 @@
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
@ -102,6 +103,10 @@ public:
*/
virtual void print_info();
private:
RingBuffer *_reports;
perf_counter_t _buffer_overflows;
protected:
virtual int probe();
@ -114,10 +119,7 @@ protected:
virtual int collect() = 0;
work_s _work;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
differential_pressure_s *_reports;
uint16_t _max_differential_pressure_pa;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
@ -129,7 +131,6 @@ protected:
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
@ -162,8 +163,12 @@ protected:
*/
static void cycle_trampoline(void *arg);
/**
* add a new report to the reports queue
*
* @param report differential_pressure_s report
*/
void new_report(const differential_pressure_s &report);
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)

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

@ -63,6 +63,7 @@
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <drivers/device/ringbuffer.h>
/* oddly, ERROR is not defined for c++ */
@ -146,10 +147,7 @@ private:
struct hrt_call _call;
unsigned _call_interval;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
struct accel_report *_reports;
RingBuffer *_reports;
struct accel_scale _accel_scale;
float _accel_range_scale;
@ -233,16 +231,9 @@ private:
int set_lowpass(unsigned frequency);
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
BMA180::BMA180(int bus, spi_dev_e device) :
SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
_call_interval(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
@ -270,7 +261,7 @@ BMA180::~BMA180()
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
delete _reports;
/* delete the perf counter */
perf_free(_sample_perf);
@ -286,16 +277,15 @@ BMA180::init()
goto out;
/* allocate basic report buffers */
_num_reports = 2;
_oldest_report = _next_report = 0;
_reports = new struct accel_report[_num_reports];
_reports = new RingBuffer(2, sizeof(accel_report));
if (_reports == nullptr)
goto out;
/* advertise sensor topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]);
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
/* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET);
@ -352,6 +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);
int ret = 0;
/* buffer must be large enough */
@ -367,10 +358,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with it.
*/
while (count--) {
if (_oldest_report != _next_report) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
ret += sizeof(_reports[0]);
INCREMENT(_oldest_report, _num_reports);
if (_reports->get(arp)) {
ret += sizeof(*arp);
arp++;
}
}
@ -379,12 +369,12 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
}
/* manual measurement */
_oldest_report = _next_report = 0;
_reports->flush();
measure();
/* measurement will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
if (_reports->get(arp))
ret = sizeof(*arp);
return ret;
}
@ -449,31 +439,22 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct accel_report *buf = new struct accel_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
return _reports->size();
case SENSORIOCRESET:
/* XXX implement */
@ -654,7 +635,7 @@ BMA180::start()
stop();
/* reset the report ring */
_oldest_report = _next_report = 0;
_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
@ -688,7 +669,7 @@ BMA180::measure()
// } raw_report;
// #pragma pack(pop)
accel_report *report = &_reports[_next_report];
struct accel_report report;
/* start the performance counter */
perf_begin(_sample_perf);
@ -708,45 +689,40 @@ BMA180::measure()
* them before. There is no good way to synchronise with the internal
* measurement flow without using the external interrupt.
*/
_reports[_next_report].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->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;
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->x_raw = (report->x_raw / 4);
report->y_raw = (report->y_raw / 4);
report->z_raw = (report->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->y_raw = -report->y_raw;
report.y_raw = -report.y_raw;
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;
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;
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, fix it */
if (_next_report == _oldest_report)
INCREMENT(_oldest_report, _num_reports);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, report);
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
/* stop the perf counter */
perf_end(_sample_perf);
@ -756,8 +732,7 @@ void
BMA180::print_info()
{
perf_print_counter(_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
_reports->print_info("report queue");
}
/**

430
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,37 @@ 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);
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);
/**
* Put an item into the buffer.
* Force an item into the buffer, discarding an older item if there is not space.
*
* @param val Item to put
* @return true if the item was put, false if the buffer is full
* @return true if an item was discarded to make space
*/
bool put(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.
@ -68,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, or zero if the buffer was
* empty.
*/
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.
@ -97,54 +120,103 @@ public:
/*
* Returns true if the buffer is empty.
*/
bool empty() { return _tail == _head; }
bool empty();
/*
* Returns true if the buffer is full.
*/
bool full() { return _next(_head) == _tail; }
bool full();
/*
* Returns the capacity of the buffer, or zero if the buffer could
* not be allocated.
*/
unsigned size() { return (_buf != nullptr) ? _size : 0; }
unsigned size();
/*
* Empties the buffer.
*/
void flush() { _head = _tail = _size; }
void flush();
/*
* resize the buffer. This is unsafe to be called while
* a producer or consuming is running. Caller is responsible
* for any locking needed
*
* @param new_size new size for buffer
* @return true if the resize succeeds, false if
* not (allocation error)
*/
bool resize(unsigned new_size);
/*
* printf() some info on the buffer
*/
void print_info(const char *name);
private:
T *const _buf;
const 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>::put(T &val)
unsigned
RingBuffer::_next(unsigned index)
{
return (0 == index) ? _num_items : (index - 1);
}
bool
RingBuffer::empty()
{
return _tail == _head;
}
bool
RingBuffer::full()
{
return _next(_head) == _tail;
}
unsigned
RingBuffer::size()
{
return (_buf != nullptr) ? _num_items : 0;
}
void
RingBuffer::flush()
{
while (!empty())
get(NULL);
}
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 {
@ -152,52 +224,286 @@ 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>::get(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, val_size))
break;
get(NULL);
overwrote = true;
}
return overwrote;
}
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) {
val = _buf[_tail];
_tail = _next(_tail);
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;
/* and what the corresponding next index will be */
next = _next(candidate);
/* go ahead and read from this index */
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));
return true;
} else {
return false;
}
}
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 (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1);
return get(&val, sizeof(val));
}
template <typename T>
unsigned RingBuffer<T>::count(void)
bool
RingBuffer::get(int16_t &val)
{
return _size - space();
return get(&val, sizeof(val));
}
template <typename T>
unsigned RingBuffer<T>::_next(unsigned index)
bool
RingBuffer::get(uint16_t &val)
{
return (0 == index) ? _size : (index - 1);
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;
/*
* Make a copy of the head/tail pointers in a fashion that
* may err on the side of under-estimating the free space
* in the buffer in the case that the buffer is being updated
* asynchronously with our check.
* If the head pointer changes (reducing space) while copying,
* re-try the copy.
*/
do {
head = _head;
tail = _tail;
} while (head != _head);
return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1);
}
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 _num_items - space();
}
bool
RingBuffer::resize(unsigned new_size)
{
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;
_num_items = new_size;
_head = new_size;
_tail = new_size;
delete[] old_buffer;
return true;
}
void
RingBuffer::print_info(const char *name)
{
printf("%s %u/%u (%u/%u @ %p)\n",
name,
_num_items,
_num_items * _item_size,
_head,
_tail,
_buf);
}

28
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -68,6 +68,7 @@
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
@ -173,27 +174,22 @@ ETSAirspeed::collect()
diff_pres_pa -= _diff_pres_offset;
}
// XXX we may want to smooth out the readings to remove noise.
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].differential_pressure_pa = diff_pres_pa;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
if (diff_pres_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa;
}
// XXX we may want to smooth out the readings to remove noise.
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.differential_pressure_pa = diff_pres_pa;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);

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

@ -65,6 +65,7 @@
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
@ -148,10 +149,7 @@ private:
work_s _work;
unsigned _measure_ticks;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
mag_report *_reports;
RingBuffer *_reports;
mag_scale _scale;
float _range_scale;
float _range_ga;
@ -310,9 +308,6 @@ private:
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
/*
* Driver 'main' command.
*/
@ -322,9 +317,6 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
HMC5883::HMC5883(int bus) :
I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
_measure_ticks(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
@ -356,9 +348,8 @@ HMC5883::~HMC5883()
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
delete _reports;
// free perf counters
perf_free(_sample_perf);
@ -375,21 +366,18 @@ HMC5883::init()
if (I2C::init() != OK)
goto out;
/* reset the device configuration */
reset();
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct mag_report[_num_reports];
_reports = new RingBuffer(2, sizeof(mag_report));
if (_reports == nullptr)
goto out;
_oldest_report = _next_report = 0;
/* reset the device configuration */
reset();
/* get a publish handle on the mag topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
struct mag_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
@ -493,6 +481,7 @@ ssize_t
HMC5883::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct mag_report);
struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
@ -501,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_oldest_report != _next_report) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
ret += sizeof(_reports[0]);
INCREMENT(_oldest_report, _num_reports);
if (_reports->get(mag_buf)) {
ret += sizeof(struct mag_report);
mag_buf++;
}
}
@ -522,7 +509,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_oldest_report = _next_report = 0;
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
@ -539,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
break;
}
/* state machine will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
if (_reports->get(mag_buf)) {
ret = sizeof(struct mag_report);
}
} while (0);
return ret;
@ -615,31 +601,22 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct mag_report *buf = new struct mag_report[arg];
if (nullptr == buf)
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
return _reports->size();
case SENSORIOCRESET:
return reset();
@ -701,7 +678,7 @@ HMC5883::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_oldest_report = _next_report = 0;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
@ -810,9 +787,10 @@ HMC5883::collect()
perf_begin(_sample_perf);
struct mag_report new_report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time();
new_report.timestamp = hrt_absolute_time();
/*
* @note We could read the status register here, which could tell us that
@ -842,8 +820,10 @@ HMC5883::collect()
*/
if ((abs(report.x) > 2048) ||
(abs(report.y) > 2048) ||
(abs(report.z) > 2048))
(abs(report.z) > 2048)) {
perf_count(_comms_errors);
goto out;
}
/*
* RAW outputs
@ -851,10 +831,10 @@ HMC5883::collect()
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
_reports[_next_report].x_raw = report.y;
_reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x);
new_report.x_raw = report.y;
new_report.y_raw = -report.x;
/* z remains z */
_reports[_next_report].z_raw = report.z;
new_report.z_raw = report.z;
/* scale values for output */
@ -876,34 +856,30 @@ HMC5883::collect()
#ifdef PX4_I2C_BUS_ONBOARD
if (_bus == PX4_I2C_BUS_ONBOARD) {
/* to align the sensor axes with the board, x and y need to be flipped */
_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
} else {
#endif
/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
* therefore switch x and y and invert y */
_reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale;
new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
_reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
#ifdef PX4_I2C_BUS_ONBOARD
}
#endif
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
/* post a report to the ring */
if (_reports->force(&new_report)) {
perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
}
/* notify anyone waiting for data */
@ -1222,8 +1198,7 @@ HMC5883::print_info()
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
_reports->print_info("report queue");
}
/**

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

@ -61,6 +61,7 @@
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <drivers/drv_gyro.h>
#include <drivers/device/ringbuffer.h>
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
@ -183,11 +184,8 @@ private:
struct hrt_call _call;
unsigned _call_interval;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
struct gyro_report *_reports;
RingBuffer *_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
@ -299,16 +297,9 @@ private:
int self_test();
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000),
_call_interval(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
@ -340,7 +331,7 @@ L3GD20::~L3GD20()
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
delete _reports;
/* delete the perf counter */
perf_free(_sample_perf);
@ -356,16 +347,15 @@ L3GD20::init()
goto out;
/* allocate basic report buffers */
_num_reports = 2;
_oldest_report = _next_report = 0;
_reports = new struct gyro_report[_num_reports];
_reports = new RingBuffer(2, sizeof(gyro_report));
if (_reports == nullptr)
goto out;
/* advertise sensor topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
struct gyro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
reset();
@ -415,6 +405,7 @@ ssize_t
L3GD20::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct gyro_report);
struct gyro_report *gbuf = reinterpret_cast<struct gyro_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
@ -430,10 +421,9 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with it.
*/
while (count--) {
if (_oldest_report != _next_report) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
ret += sizeof(_reports[0]);
INCREMENT(_oldest_report, _num_reports);
if (_reports->get(gbuf)) {
ret += sizeof(*gbuf);
gbuf++;
}
}
@ -442,12 +432,13 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
}
/* manual measurement */
_oldest_report = _next_report = 0;
_reports->flush();
measure();
/* measurement will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
if (_reports->get(gbuf)) {
ret = sizeof(*gbuf);
}
return ret;
}
@ -515,31 +506,22 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct gyro_report *buf = new struct gyro_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
return _reports->size();
case SENSORIOCRESET:
reset();
@ -708,7 +690,7 @@ L3GD20::start()
stop();
/* reset the report ring */
_oldest_report = _next_report = 0;
_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
@ -768,7 +750,7 @@ L3GD20::measure()
} raw_report;
#pragma pack(pop)
gyro_report *report = &_reports[_next_report];
gyro_report report;
/* start the performance counter */
perf_begin(_sample_perf);
@ -791,61 +773,56 @@ L3GD20::measure()
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
report->timestamp = hrt_absolute_time();
report.timestamp = hrt_absolute_time();
switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG:
/* keep axes in place */
report->x_raw = raw_report.x;
report->y_raw = raw_report.y;
report.x_raw = raw_report.x;
report.y_raw = raw_report.y;
break;
case SENSOR_BOARD_ROTATION_090_DEG:
/* swap x and y */
report->x_raw = raw_report.y;
report->y_raw = raw_report.x;
report.x_raw = raw_report.y;
report.y_raw = raw_report.x;
break;
case SENSOR_BOARD_ROTATION_180_DEG:
/* swap x and y and negate both */
report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
break;
case SENSOR_BOARD_ROTATION_270_DEG:
/* swap x and y and negate y */
report->x_raw = raw_report.y;
report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
report.x_raw = raw_report.y;
report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
break;
}
report->z_raw = raw_report.z;
report.z_raw = raw_report.z;
report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report->x = _gyro_filter_x.apply(report->x);
report->y = _gyro_filter_y.apply(report->y);
report->z = _gyro_filter_z.apply(report->z);
report.x = _gyro_filter_x.apply(report.x);
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
report->scaling = _gyro_range_scale;
report->range_rad_s = _gyro_range_rad_s;
report.scaling = _gyro_range_scale;
report.range_rad_s = _gyro_range_rad_s;
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, fix it */
if (_next_report == _oldest_report)
INCREMENT(_oldest_report, _num_reports);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
if (_gyro_topic > 0)
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
_read++;
@ -858,8 +835,7 @@ L3GD20::print_info()
{
printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
_reports->print_info("report queue");
}
int

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

@ -62,6 +62,7 @@
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/device/ringbuffer.h>
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
@ -218,15 +219,8 @@ private:
unsigned _call_accel_interval;
unsigned _call_mag_interval;
unsigned _num_accel_reports;
volatile unsigned _next_accel_report;
volatile unsigned _oldest_accel_report;
struct accel_report *_accel_reports;
unsigned _num_mag_reports;
volatile unsigned _next_mag_report;
volatile unsigned _oldest_mag_report;
struct mag_report *_mag_reports;
RingBuffer *_accel_reports;
RingBuffer *_mag_reports;
struct accel_scale _accel_scale;
unsigned _accel_range_m_s2;
@ -420,22 +414,12 @@ private:
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000),
_mag(new LSM303D_mag(this)),
_call_accel_interval(0),
_call_mag_interval(0),
_num_accel_reports(0),
_next_accel_report(0),
_oldest_accel_report(0),
_accel_reports(nullptr),
_num_mag_reports(0),
_next_mag_report(0),
_oldest_mag_report(0),
_mag_reports(nullptr),
_accel_range_m_s2(0.0f),
_accel_range_scale(0.0f),
@ -480,9 +464,9 @@ LSM303D::~LSM303D()
/* free any existing reports */
if (_accel_reports != nullptr)
delete[] _accel_reports;
delete _accel_reports;
if (_mag_reports != nullptr)
delete[] _mag_reports;
delete _mag_reports;
delete _mag;
@ -504,20 +488,17 @@ LSM303D::init()
}
/* allocate basic report buffers */
_num_accel_reports = 2;
_oldest_accel_report = _next_accel_report = 0;
_accel_reports = new struct accel_report[_num_accel_reports];
_accel_reports = new RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr)
goto out;
/* advertise accel topic */
memset(&_accel_reports[0], 0, sizeof(_accel_reports[0]));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]);
struct accel_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
_num_mag_reports = 2;
_oldest_mag_report = _next_mag_report = 0;
_mag_reports = new struct mag_report[_num_mag_reports];
_mag_reports = new RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr)
goto out;
@ -525,8 +506,9 @@ LSM303D::init()
reset();
/* advertise mag topic */
memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
struct mag_report zero_mag_report;
memset(&zero_mag_report, 0, sizeof(zero_mag_report));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report);
/* do CDev init for the mag device node, keep it optional */
mag_ret = _mag->init();
@ -586,6 +568,7 @@ ssize_t
LSM303D::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct accel_report);
accel_report *arb = reinterpret_cast<accel_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
@ -594,17 +577,13 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
/* if automatic measurement is enabled */
if (_call_accel_interval > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the measurement code while we are doing this;
* we are careful to avoid racing with it.
*/
while (count--) {
if (_oldest_accel_report != _next_accel_report) {
memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports));
ret += sizeof(_accel_reports[0]);
INCREMENT(_oldest_accel_report, _num_accel_reports);
if (_accel_reports->get(arb)) {
ret += sizeof(*arb);
arb++;
}
}
@ -613,12 +592,11 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
}
/* manual measurement */
_oldest_accel_report = _next_accel_report = 0;
measure();
/* measurement will have generated a report, copy it out */
memcpy(buffer, _accel_reports, sizeof(*_accel_reports));
ret = sizeof(*_accel_reports);
if (_accel_reports->get(arb))
ret = sizeof(*arb);
return ret;
}
@ -627,6 +605,7 @@ ssize_t
LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct mag_report);
mag_report *mrb = reinterpret_cast<mag_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
@ -638,14 +617,11 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the measurement code while we are doing this;
* we are careful to avoid racing with it.
*/
while (count--) {
if (_oldest_mag_report != _next_mag_report) {
memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports));
ret += sizeof(_mag_reports[0]);
INCREMENT(_oldest_mag_report, _num_mag_reports);
if (_mag_reports->get(mrb)) {
ret += sizeof(*mrb);
mrb++;
}
}
@ -654,12 +630,12 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
}
/* manual measurement */
_oldest_mag_report = _next_mag_report = 0;
_mag_reports->flush();
measure();
/* measurement will have generated a report, copy it out */
memcpy(buffer, _mag_reports, sizeof(*_mag_reports));
ret = sizeof(*_mag_reports);
if (_mag_reports->get(mrb))
ret = sizeof(*mrb);
return ret;
}
@ -727,31 +703,22 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_accel_interval;
case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct accel_report *buf = new struct accel_report[arg];
if (nullptr == buf)
irqstate_t flags = irqsave();
if (!_accel_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _accel_reports;
_num_accel_reports = arg;
_accel_reports = buf;
start();
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_accel_reports - 1;
return _accel_reports->size();
case SENSORIOCRESET:
reset();
@ -863,31 +830,22 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_mag_interval;
case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct mag_report *buf = new struct mag_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _mag_reports;
_num_mag_reports = arg;
_mag_reports = buf;
start();
return OK;
irqstate_t flags = irqsave();
if (!_mag_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_mag_reports - 1;
return _mag_reports->size();
case SENSORIOCRESET:
reset();
@ -1220,8 +1178,8 @@ LSM303D::start()
stop();
/* reset the report ring */
_oldest_accel_report = _next_accel_report = 0;
_oldest_mag_report = _next_mag_report = 0;
_accel_reports->flush();
_mag_reports->flush();
/* start polling at the specified rate */
hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this);
@ -1268,7 +1226,7 @@ LSM303D::measure()
} raw_accel_report;
#pragma pack(pop)
accel_report *accel_report = &_accel_reports[_next_accel_report];
accel_report accel_report;
/* start the performance counter */
perf_begin(_accel_sample_perf);
@ -1293,35 +1251,30 @@ LSM303D::measure()
*/
accel_report->timestamp = hrt_absolute_time();
accel_report.timestamp = hrt_absolute_time();
accel_report->x_raw = raw_accel_report.x;
accel_report->y_raw = raw_accel_report.y;
accel_report->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->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;
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->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.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->scaling = _accel_range_scale;
accel_report->range_m_s2 = _accel_range_m_s2;
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
/* post a report to the ring - note, not locked */
INCREMENT(_next_accel_report, _num_accel_reports);
/* if we are running up against the oldest report, fix it */
if (_next_accel_report == _oldest_accel_report)
INCREMENT(_oldest_accel_report, _num_accel_reports);
_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);
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
_accel_read++;
@ -1343,7 +1296,7 @@ LSM303D::mag_measure()
} raw_mag_report;
#pragma pack(pop)
mag_report *mag_report = &_mag_reports[_next_mag_report];
mag_report mag_report;
/* start the performance counter */
perf_begin(_mag_sample_perf);
@ -1368,30 +1321,25 @@ LSM303D::mag_measure()
*/
mag_report->timestamp = hrt_absolute_time();
mag_report.timestamp = hrt_absolute_time();
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_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;
/* post a report to the ring - note, not locked */
INCREMENT(_next_mag_report, _num_mag_reports);
/* if we are running up against the oldest report, fix it */
if (_next_mag_report == _oldest_mag_report)
INCREMENT(_oldest_mag_report, _num_mag_reports);
_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);
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
_mag_read++;
@ -1405,11 +1353,8 @@ LSM303D::print_info()
printf("accel reads: %u\n", _accel_read);
printf("mag reads: %u\n", _mag_read);
perf_print_counter(_accel_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n",
_num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
perf_print_counter(_mag_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n",
_num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports);
_accel_reports->print_info("accel reports");
_mag_reports->print_info("mag reports");
}
LSM303D_mag::LSM303D_mag(LSM303D *parent) :

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

@ -64,6 +64,7 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
@ -119,10 +120,7 @@ private:
float _min_distance;
float _max_distance;
work_s _work;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
range_finder_report *_reports;
RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
@ -183,9 +181,6 @@ private:
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0)
/*
* Driver 'main' command.
*/
@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) :
I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
_min_distance(MB12XX_MIN_DISTANCE),
_max_distance(MB12XX_MAX_DISTANCE),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
@ -221,7 +213,7 @@ MB12XX::~MB12XX()
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
delete _reports;
}
int
@ -234,17 +226,15 @@ MB12XX::init()
goto out;
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct range_finder_report[_num_reports];
_reports = new RingBuffer(2, sizeof(range_finder_report));
if (_reports == nullptr)
goto out;
_oldest_report = _next_report = 0;
/* get a publish handle on the range finder topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]);
struct range_finder_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
if (_range_finder_topic < 0)
debug("failed to create sensor_range_finder object. Did you start uOrb?");
@ -354,31 +344,22 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct range_finder_report *buf = new struct range_finder_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
@ -406,6 +387,7 @@ ssize_t
MB12XX::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
@ -421,10 +403,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with them.
*/
while (count--) {
if (_oldest_report != _next_report) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
ret += sizeof(_reports[0]);
INCREMENT(_oldest_report, _num_reports);
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
@ -433,9 +414,8 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
}
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_oldest_report = _next_report = 0;
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
@ -453,8 +433,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
}
/* state machine will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
@ -498,26 +479,25 @@ MB12XX::collect()
if (ret < 0)
{
log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint16_t distance = val[0] << 8 | val[1];
float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].distance = si_units;
_reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
report.timestamp = hrt_absolute_time();
report.distance = si_units;
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
/* publish it */
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]);
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
}
/* notify anyone waiting for data */
@ -525,11 +505,8 @@ MB12XX::collect()
ret = OK;
out:
perf_end(_sample_perf);
return ret;
return ret;
}
void
@ -537,7 +514,7 @@ MB12XX::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_oldest_report = _next_report = 0;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
@ -626,8 +603,7 @@ MB12XX::print_info()
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
_reports->print_info("report queue");
}
/**

26
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

@ -199,27 +199,23 @@ MEASAirspeed::collect()
// Calculate differential pressure. As its centered around 8000
// and can go positive or negative, enforce absolute value
uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].temperature = temperature;
_reports[_next_report].differential_pressure_pa = diff_press_pa;
struct differential_pressure_s report;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) {
_reports[_next_report].max_differential_pressure_pa = diff_press_pa;
if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa;
}
report.timestamp = hrt_absolute_time();
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
}
new_report(report);
/* notify anyone waiting for data */
poll_notify(POLLIN);

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

@ -194,16 +194,14 @@ private:
struct hrt_call _call;
unsigned _call_interval;
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;
typedef RingBuffer<gyro_report> GyroReportBuffer;
GyroReportBuffer *_gyro_reports;
RingBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
@ -431,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;
@ -466,14 +464,14 @@ MPU6000::init()
_gyro_topic = -1;
} else {
gyro_report gr;
_gyro_reports->get(gr);
_gyro_reports->get(&gr);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
}
/* advertise accel topic */
accel_report ar;
_accel_reports->get(ar);
_accel_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
out:
@ -658,9 +656,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
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++;
}
/* return the number of bytes transferred */
@ -748,12 +747,13 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
return -EAGAIN;
/* copy reports out of our buffer to the caller */
gyro_report *arp = reinterpret_cast<gyro_report *>(buffer);
gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
int transferred = 0;
while (count--) {
if (!_gyro_reports->get(*arp++))
if (!_gyro_reports->get(grp))
break;
transferred++;
grp++;
}
/* return the number of bytes transferred */
@ -837,28 +837,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
AccelReportBuffer *buf = new AccelReportBuffer(arg);
if (nullptr == buf)
return -ENOMEM;
if (buf->size() == 0) {
delete buf;
return -ENOMEM;
}
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _accel_reports;
_accel_reports = buf;
start();
return OK;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_accel_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _accel_reports->size();
@ -935,21 +926,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
GyroReportBuffer *buf = new GyroReportBuffer(arg);
if (nullptr == buf)
return -ENOMEM;
if (buf->size() == 0) {
delete buf;
irqstate_t flags = irqsave();
if (!_gyro_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _gyro_reports;
_gyro_reports = buf;
start();
irqrestore(flags);
return OK;
}
@ -1260,8 +1242,8 @@ MPU6000::measure()
grb.temperature_raw = report.temp;
grb.temperature = (report.temp) / 361.0f + 35.0f;
_accel_reports->put(arb);
_gyro_reports->put(grb);
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
poll_notify(POLLIN);
@ -1280,7 +1262,10 @@ MPU6000::measure()
void
MPU6000::print_info()
{
perf_print_counter(_sample_perf);
printf("reads: %u\n", _reads);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
}
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :

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

@ -60,6 +60,7 @@
#include <drivers/device/device.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@ -114,10 +115,7 @@ protected:
struct work_s _work;
unsigned _measure_ticks;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
struct baro_report *_reports;
RingBuffer *_reports;
bool _collect_phase;
unsigned _measure_phase;
@ -196,9 +194,6 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
_interface(interface),
_prom(prom_buf.s),
_measure_ticks(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_collect_phase(false),
_measure_phase(0),
@ -223,7 +218,7 @@ MS5611::~MS5611()
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
delete _reports;
// free perf counters
perf_free(_sample_perf);
@ -246,8 +241,7 @@ MS5611::init()
}
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct baro_report[_num_reports];
_reports = new RingBuffer(2, sizeof(baro_report));
if (_reports == nullptr) {
debug("can't get memory for reports");
@ -255,11 +249,10 @@ MS5611::init()
goto out;
}
_oldest_report = _next_report = 0;
/* get a publish handle on the baro topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]);
struct baro_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report);
if (_baro_topic < 0) {
debug("failed to create sensor_baro object");
@ -276,6 +269,7 @@ ssize_t
MS5611::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct baro_report);
struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
@ -291,10 +285,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
* we are careful to avoid racing with them.
*/
while (count--) {
if (_oldest_report != _next_report) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
ret += sizeof(_reports[0]);
INCREMENT(_oldest_report, _num_reports);
if (_reports->get(brp)) {
ret += sizeof(*brp);
brp++;
}
}
@ -303,10 +296,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
}
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_measure_phase = 0;
_oldest_report = _next_report = 0;
_reports->flush();
/* do temperature first */
if (OK != measure()) {
@ -335,8 +327,8 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
}
/* state machine will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
if (_reports->get(brp))
ret = sizeof(*brp);
} while (0);
@ -411,31 +403,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct baro_report *buf = new struct baro_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop_cycle();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start_cycle();
return OK;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
@ -469,7 +451,7 @@ MS5611::start_cycle()
/* reset the report ring and state machine */
_collect_phase = false;
_measure_phase = 0;
_oldest_report = _next_report = 0;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
@ -588,8 +570,9 @@ MS5611::collect()
perf_begin(_sample_perf);
struct baro_report report;
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time();
report.timestamp = hrt_absolute_time();
/* read the most recent measurement - read offset/size are hardcoded in the interface */
ret = _interface->read(0, (void *)&raw, 0);
@ -638,8 +621,8 @@ MS5611::collect()
int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15;
/* generate a new report */
_reports[_next_report].temperature = _TEMP / 100.0f;
_reports[_next_report].pressure = P / 100.0f; /* convert to millibar */
report.temperature = _TEMP / 100.0f;
report.pressure = P / 100.0f; /* convert to millibar */
/* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */
@ -676,18 +659,13 @@ MS5611::collect()
* h = ------------------------------- + h1
* a
*/
_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
}
/* notify anyone waiting for data */
@ -709,8 +687,7 @@ MS5611::print_info()
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
_reports->print_info("report queue");
printf("TEMP: %d\n", _TEMP);
printf("SENS: %lld\n", _SENS);
printf("OFF: %lld\n", _OFF);

32
src/drivers/px4io/px4io.cpp
View File

@ -487,25 +487,27 @@ PX4IO::detect()
{
int ret;
ASSERT(_task == -1);
if (_task == -1) {
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
/* get some parameters */
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
if (protocol != PX4IO_PROTOCOL_VERSION) {
if (protocol == _io_reg_get_error) {
log("IO not installed");
} else {
log("IO version error");
mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
/* get some parameters */
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
if (protocol != PX4IO_PROTOCOL_VERSION) {
if (protocol == _io_reg_get_error) {
log("IO not installed");
} else {
log("IO version error");
mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
}
return -1;
}
return -1;
}
log("IO found");
return 0;

127
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp Normal file
View File

@ -0,0 +1,127 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name APL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ecl_pitch_controller.cpp
* Implementation of a simple orthogonal pitch PID controller.
*
*/
#include "ecl_pitch_controller.h"
#include <math.h>
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
ECL_PitchController::ECL_PitchController() :
_last_run(0),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_max_deflection_rad(math::radians(45.0f))
{
}
float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = dt_micros / 1000000;
/* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = true;
float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_i_rate = _k_i * _tc;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
/* calculate the offset in the rate resulting from rolling */
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(roll) * sinf(roll)) * _roll_ff;
float pitch_error = pitch_setpoint - pitch;
/* rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
/* add turn offset */
_rate_setpoint += turn_offset;
_rate_error = _rate_setpoint - pitch_rate;
float ilimit_scaled = 0.0f;
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * k_i_rate * dt * scaler;
/*
* anti-windup: do not allow integrator to increase into the
* wrong direction if actuator is at limit
*/
if (_last_output < -_max_deflection_rad) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > _max_deflection_rad) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
/* integrator limit */
_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
/* store non-limited output */
_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
}
void ECL_PitchController::reset_integrator()
{
_integrator = 0.0f;
}

107
src/lib/ecl/attitude_fw/ecl_pitch_controller.h Normal file
View File

@ -0,0 +1,107 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name APL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ecl_pitch_controller.h
* Definition of a simple orthogonal pitch PID controller.
*
*/
#ifndef ECL_PITCH_CONTROLLER_H
#define ECL_PITCH_CONTROLLER_H
#include <stdbool.h>
#include <stdint.h>
class __EXPORT ECL_PitchController
{
public:
ECL_PitchController();
float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
void reset_integrator();
void set_time_constant(float time_constant) {
_tc = time_constant;
}
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate_pos(float max_rate_pos) {
_max_rate_pos = max_rate_pos;
}
void set_max_rate_neg(float max_rate_neg) {
_max_rate_neg = max_rate_neg;
}
void set_roll_ff(float roll_ff) {
_roll_ff = roll_ff;
}
float get_rate_error() {
return _rate_error;
}
float get_desired_rate() {
return _rate_setpoint;
}
private:
uint64_t _last_run;
float _tc;
float _k_p;
float _k_i;
float _k_d;
float _integrator_max;
float _max_rate_pos;
float _max_rate_neg;
float _roll_ff;
float _last_output;
float _integrator;
float _rate_error;
float _rate_setpoint;
float _max_deflection_rad;
};
#endif // ECL_PITCH_CONTROLLER_H

125
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp Normal file
View File

@ -0,0 +1,125 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name APL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ecl_roll_controller.cpp
* Implementation of a simple orthogonal roll PID controller.
*
*/
#include "../ecl.h"
#include "ecl_roll_controller.h"
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
ECL_RollController::ECL_RollController() :
_last_run(0),
_tc(0.1f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_max_deflection_rad(math::radians(45.0f))
{
}
float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_i_rate = _k_i * _tc;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
float roll_error = roll_setpoint - roll;
_rate_setpoint = roll_error / _tc;
/* limit the rate */
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
_rate_error = _rate_setpoint - roll_rate;
float ilimit_scaled = 0.0f;
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * k_i_rate * dt * scaler;
/*
* anti-windup: do not allow integrator to increase into the
* wrong direction if actuator is at limit
*/
if (_last_output < -_max_deflection_rad) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > _max_deflection_rad) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
/* integrator limit */
_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
/* store non-limited output */
_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
}
void ECL_RollController::reset_integrator()
{
_integrator = 0.0f;
}

100
src/lib/ecl/attitude_fw/ecl_roll_controller.h Normal file
View File

@ -0,0 +1,100 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name APL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ecl_roll_controller.h
* Definition of a simple orthogonal roll PID controller.
*
*/
#ifndef ECL_ROLL_CONTROLLER_H
#define ECL_ROLL_CONTROLLER_H
#include <stdbool.h>
#include <stdint.h>
class __EXPORT ECL_RollController
{
public:
ECL_RollController();
float control(float roll_setpoint, float roll, float roll_rate,
float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
void reset_integrator();
void set_time_constant(float time_constant) {
if (time_constant > 0.1f && time_constant < 3.0f) {
_tc = time_constant;
}
}
void set_k_p(float k_p) {
_k_p = k_p;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
void set_max_rate(float max_rate) {
_max_rate = max_rate;
}
float get_rate_error() {
return _rate_error;
}
float get_desired_rate() {
return _rate_setpoint;
}
private:
uint64_t _last_run;
float _tc;
float _k_p;
float _k_i;
float _k_d;
float _integrator_max;
float _max_rate;
float _last_output;
float _integrator;
float _rate_error;
float _rate_setpoint;
float _max_deflection_rad;
};
#endif // ECL_ROLL_CONTROLLER_H

74
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp Normal file
View File

@ -0,0 +1,74 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name APL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ecl_yaw_controller.cpp
* Implementation of a simple orthogonal coordinated turn yaw PID controller.
*
*/
#include "ecl_yaw_controller.h"
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
ECL_YawController::ECL_YawController() :
_last_run(0),
_last_error(0.0f),
_last_output(0.0f),
_last_rate_hp_out(0.0f),
_last_rate_hp_in(0.0f),
_k_d_last(0.0f),
_integrator(0.0f)
{
}
float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
float airspeed_min, float airspeed_max, float aspeed)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
return 0.0f;
}
void ECL_YawController::reset_integrator()
{
_integrator = 0.0f;
}

89
src/lib/ecl/attitude_fw/ecl_yaw_controller.h Normal file
View File

@ -0,0 +1,89 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name APL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ecl_yaw_controller.h
* Definition of a simple orthogonal coordinated turn yaw PID controller.
*
*/
#ifndef ECL_YAW_CONTROLLER_H
#define ECL_YAW_CONTROLLER_H
#include <stdbool.h>
#include <stdint.h>
class __EXPORT ECL_YawController
{
public:
ECL_YawController();
float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
void reset_integrator();
void set_k_side(float k_a) {
_k_side = k_a;
}
void set_k_i(float k_i) {
_k_i = k_i;
}
void set_k_d(float k_d) {
_k_d = k_d;
}
void set_k_roll_ff(float k_roll_ff) {
_k_roll_ff = k_roll_ff;
}
void set_integrator_max(float max) {
_integrator_max = max;
}
private:
uint64_t _last_run;
float _k_side;
float _k_i;
float _k_d;
float _k_roll_ff;
float _integrator_max;
float _last_error;
float _last_output;
float _last_rate_hp_out;
float _last_rate_hp_in;
float _k_d_last;
float _integrator;
};
#endif // ECL_YAW_CONTROLLER_H

43
src/lib/ecl/ecl.h Normal file
View File

@ -0,0 +1,43 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name APL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ecl.h
* Adapter / shim layer for system calls needed by ECL
*
*/
#include <drivers/drv_hrt.h>
#define ecl_absolute_time hrt_absolute_time
#define ecl_elapsed_time hrt_elapsed_time

40
src/lib/ecl/module.mk Normal file
View File

@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Estimation and Control Library
#
SRCS = attitude_fw/ecl_pitch_controller.cpp \
attitude_fw/ecl_roll_controller.cpp \
attitude_fw/ecl_yaw_controller.cpp

30
src/lib/geo/geo.c
View File

@ -210,6 +210,36 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
return theta;
}
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
*vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon);
}
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
*vy = CONSTANTS_RADIUS_OF_EARTH * d_lon;
*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
}
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)

8
src/lib/geo/geo.h
View File

@ -57,10 +57,6 @@ __BEGIN_DECLS
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */
#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */
/* compatibility aliases */
#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH
#define GRAVITY_MSS CONSTANTS_ONE_G
// XXX remove
struct crosstrack_error_s {
bool past_end; // Flag indicating we are past the end of the line/arc segment
@ -116,6 +112,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
*/
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,

10
src/modules/commander/commander.cpp
View File

@ -343,6 +343,9 @@ void print_status()
warnx("arming: %s", armed_str);
}
static orb_advert_t control_mode_pub;
static orb_advert_t status_pub;
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
@ -356,6 +359,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
uint8_t base_mode = (uint8_t) cmd->param1;
uint8_t custom_main_mode = (uint8_t) cmd->param2;
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
// TODO remove debug code
//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
/* set arming state */
@ -543,7 +550,6 @@ int commander_thread_main(int argc, char *argv[])
}
/* Main state machine */
orb_advert_t status_pub;
/* make sure we are in preflight state */
memset(&status, 0, sizeof(status));
status.condition_landed = true; // initialize to safe value
@ -555,7 +561,7 @@ int commander_thread_main(int argc, char *argv[])
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
orb_advert_t control_mode_pub;
/* Initialize all flags to false */
memset(&control_mode, 0, sizeof(control_mode));

12
src/modules/commander/state_machine_helper.cpp
View File

@ -228,8 +228,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
case MAIN_STATE_SEATBELT:
/* need altitude estimate */
if (current_state->condition_local_altitude_valid) {
/* need at minimum altitude estimate */
if (current_state->condition_local_altitude_valid ||
current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
@ -237,8 +238,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
case MAIN_STATE_EASY:
/* need local position estimate */
if (current_state->condition_local_position_valid) {
/* need at minimum local position estimate */
if (current_state->condition_local_position_valid ||
current_state->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
@ -502,6 +504,8 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
current_control_mode->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
// XXX also set lockdown here
ret = OK;
} else {

770
src/modules/fw_att_control/fw_att_control_main.cpp Normal file
View File

@ -0,0 +1,770 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fw_att_control_main.c
* Implementation of a generic attitude controller based on classic orthogonal PIDs.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <ecl/attitude_fw/ecl_pitch_controller.h>
#include <ecl/attitude_fw/ecl_roll_controller.h>
#include <ecl/attitude_fw/ecl_yaw_controller.h>
/**
* Fixedwing attitude control app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
class FixedwingAttitudeControl
{
public:
/**
* Constructor
*/
FixedwingAttitudeControl();
/**
* Destructor, also kills the sensors task.
*/
~FixedwingAttitudeControl();
/**
* Start the sensors task.
*
* @return OK on success.
*/
int start();
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _control_task; /**< task handle for sensor task */
int _att_sub; /**< vehicle attitude subscription */
int _accel_sub; /**< accelerometer subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _vcontrol_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator control inputs */
perf_counter_t _loop_perf; /**< loop performance counter */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
bool _airspeed_valid; /**< flag if the airspeed measurement is valid */
struct {
float tconst;
float p_p;
float p_d;
float p_i;
float p_rmax_pos;
float p_rmax_neg;
float p_integrator_max;
float p_roll_feedforward;
float r_p;
float r_d;
float r_i;
float r_integrator_max;
float r_rmax;
float y_p;
float y_i;
float y_d;
float y_roll_feedforward;
float y_integrator_max;
float airspeed_min;
float airspeed_trim;
float airspeed_max;
} _parameters; /**< local copies of interesting parameters */
struct {
param_t tconst;
param_t p_p;
param_t p_d;
param_t p_i;
param_t p_rmax_pos;
param_t p_rmax_neg;
param_t p_integrator_max;
param_t p_roll_feedforward;
param_t r_p;
param_t r_d;
param_t r_i;
param_t r_integrator_max;
param_t r_rmax;
param_t y_p;
param_t y_i;
param_t y_d;
param_t y_roll_feedforward;
param_t y_integrator_max;
param_t airspeed_min;
param_t airspeed_trim;
param_t airspeed_max;
} _parameter_handles; /**< handles for interesting parameters */
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl;
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Update control outputs
*
*/
void control_update();
/**
* Check for changes in vehicle control mode.
*/
void vehicle_control_mode_poll();
/**
* Check for changes in manual inputs.
*/
void vehicle_manual_poll();
/**
* Check for airspeed updates.
*/
bool vehicle_airspeed_poll();
/**
* Check for accel updates.
*/
void vehicle_accel_poll();
/**
* Check for set triplet updates.
*/
void vehicle_setpoint_poll();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
};
namespace att_control
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
FixedwingAttitudeControl *g_control;
}
FixedwingAttitudeControl::FixedwingAttitudeControl() :
_task_should_exit(false),
_control_task(-1),
/* subscriptions */
_att_sub(-1),
_accel_sub(-1),
_airspeed_sub(-1),
_vcontrol_mode_sub(-1),
_params_sub(-1),
_manual_sub(-1),
/* publications */
_rate_sp_pub(-1),
_actuators_0_pub(-1),
_attitude_sp_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
/* states */
_setpoint_valid(false),
_airspeed_valid(false)
{
_parameter_handles.tconst = param_find("FW_ATT_TC");
_parameter_handles.p_p = param_find("FW_P_P");
_parameter_handles.p_d = param_find("FW_P_D");
_parameter_handles.p_i = param_find("FW_P_I");
_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
_parameter_handles.p_integrator_max = param_find("FW_P_integrator_max");
_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
_parameter_handles.r_p = param_find("FW_R_P");
_parameter_handles.r_d = param_find("FW_R_D");
_parameter_handles.r_i = param_find("FW_R_I");
_parameter_handles.r_integrator_max = param_find("FW_R_integrator_max");
_parameter_handles.r_rmax = param_find("FW_R_RMAX");
_parameter_handles.y_p = param_find("FW_Y_P");
_parameter_handles.y_i = param_find("FW_Y_I");
_parameter_handles.y_d = param_find("FW_Y_D");
_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
_parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max");
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
/* fetch initial parameter values */
parameters_update();
}
FixedwingAttitudeControl::~FixedwingAttitudeControl()
{
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
att_control::g_control = nullptr;
}
int
FixedwingAttitudeControl::parameters_update()
{
param_get(_parameter_handles.tconst, &(_parameters.tconst));
param_get(_parameter_handles.p_p, &(_parameters.p_p));
param_get(_parameter_handles.p_d, &(_parameters.p_d));
param_get(_parameter_handles.p_i, &(_parameters.p_i));
param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
param_get(_parameter_handles.r_p, &(_parameters.r_p));
param_get(_parameter_handles.r_d, &(_parameters.r_d));
param_get(_parameter_handles.r_i, &(_parameters.r_i));
param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max));
param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax));
param_get(_parameter_handles.y_p, &(_parameters.y_p));
param_get(_parameter_handles.y_i, &(_parameters.y_i));
param_get(_parameter_handles.y_d, &(_parameters.y_d));
param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward));
param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst);
_pitch_ctrl.set_k_p(math::radians(_parameters.p_p));
_pitch_ctrl.set_k_i(math::radians(_parameters.p_i));
_pitch_ctrl.set_k_d(math::radians(_parameters.p_d));
_pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max));
_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
_pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward));
/* roll control parameters */
_roll_ctrl.set_time_constant(_parameters.tconst);
_roll_ctrl.set_k_p(math::radians(_parameters.r_p));
_roll_ctrl.set_k_i(math::radians(_parameters.r_i));
_roll_ctrl.set_k_d(math::radians(_parameters.r_d));
_roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max));
_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
/* yaw control parameters */
_yaw_ctrl.set_k_side(math::radians(_parameters.y_p));
_yaw_ctrl.set_k_i(math::radians(_parameters.y_i));
_yaw_ctrl.set_k_d(math::radians(_parameters.y_d));
_yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward));
_yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max));
return OK;
}
void
FixedwingAttitudeControl::vehicle_control_mode_poll()
{
bool vcontrol_mode_updated;
/* Check HIL state if vehicle status has changed */
orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
if (vcontrol_mode_updated) {
orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
}
}
void
FixedwingAttitudeControl::vehicle_manual_poll()
{
bool manual_updated;
/* get pilots inputs */
orb_check(_manual_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
}
}
bool
FixedwingAttitudeControl::vehicle_airspeed_poll()
{
/* check if there is a new position */
bool airspeed_updated;
orb_check(_airspeed_sub, &airspeed_updated);
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
return true;
}
return false;
}
void
FixedwingAttitudeControl::vehicle_accel_poll()
{
/* check if there is a new position */
bool accel_updated;
orb_check(_accel_sub, &accel_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
}
}
void
FixedwingAttitudeControl::vehicle_setpoint_poll()
{
/* check if there is a new setpoint */
bool att_sp_updated;
orb_check(_att_sp_sub, &att_sp_updated);
if (att_sp_updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
_setpoint_valid = true;
}
}
void
FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
att_control::g_control->task_main();
}
void
FixedwingAttitudeControl::task_main()
{
/* inform about start */
warnx("Initializing..");
fflush(stdout);
/*
* do subscriptions
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
orb_set_interval(_att_sub, 100);
parameters_update();
/* get an initial update for all sensor and status data */
(void)vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _att_sub;
fds[1].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
_airspeed_valid = vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
/* lock integrator until control is started */
bool lock_integrator;
if (_vcontrol_mode.flag_control_attitude_enabled) {
lock_integrator = false;
} else {
lock_integrator = true;
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
/* scale from radians to normalized -1 .. 1 range */
const float actuator_scaling = 1.0f / (M_PI_F / 4.0f);
/* scale around tuning airspeed */
float airspeed;
/* if airspeed is smaller than min, the sensor is not giving good readings */
if (!_airspeed_valid ||
(_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) ||
!isfinite(_airspeed.indicated_airspeed_m_s)) {
airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
} else {
airspeed = _airspeed.indicated_airspeed_m_s;
}
float airspeed_scaling = _parameters.airspeed_trim / airspeed;
//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
float roll_sp, pitch_sp;
float throttle_sp = 0.0f;
if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
roll_sp = _att_sp.roll_body;
pitch_sp = _att_sp.pitch_body;
throttle_sp = _att_sp.thrust;
} else {
/*
* Scale down roll and pitch as the setpoints are radians
* and a typical remote can only do 45 degrees, the mapping is
* -1..+1 to -45..+45 degrees or -0.75..+0.75 radians.
*
* With this mapping the stick angle is a 1:1 representation of
* the commanded attitude. If more than 45 degrees are desired,
* a scaling parameter can be applied to the remote.
*/
roll_sp = _manual.roll * 0.75f;
pitch_sp = _manual.pitch * 0.75f;
throttle_sp = _manual.throttle;
_actuators.control[4] = _manual.flaps;
/*
* in manual mode no external source should / does emit attitude setpoints.
* emit the manual setpoint here to allow attitude controller tuning
* in attitude control mode.
*/
struct vehicle_attitude_setpoint_s att_sp;
att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = roll_sp;
att_sp.pitch_body = pitch_sp;
att_sp.yaw_body = 0.0f;
att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
} else {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
}
}
float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
_parameters.airspeed_min, _parameters.airspeed_max, airspeed);
_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
// _actuators.control[2], _actuators.control[3]);
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
*/
vehicle_rates_setpoint_s rates_sp;
rates_sp.roll = _roll_ctrl.get_desired_rate();
rates_sp.pitch = _pitch_ctrl.get_desired_rate();
rates_sp.yaw = 0.0f; // XXX not yet implemented
rates_sp.timestamp = hrt_absolute_time();
if (_rate_sp_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
} else {
/* advertise and publish */
_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
}
} else {
/* manual/direct control */
_actuators.control[0] = _manual.roll;
_actuators.control[1] = _manual.pitch;
_actuators.control[2] = _manual.yaw;
_actuators.control[3] = _manual.throttle;
_actuators.control[4] = _manual.flaps;
}
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
if (_actuators_0_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else {
/* advertise and publish */
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
}
perf_end(_loop_perf);
}
warnx("exiting.\n");
_control_task = -1;
_exit(0);
}
int
FixedwingAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = task_spawn_cmd("fw_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
(main_t)&FixedwingAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int fw_att_control_main(int argc, char *argv[])
{
if (argc < 1)
errx(1, "usage: fw_att_control {start|stop|status}");
if (!strcmp(argv[1], "start")) {
if (att_control::g_control != nullptr)
errx(1, "already running");
att_control::g_control = new FixedwingAttitudeControl;
if (att_control::g_control == nullptr)
errx(1, "alloc failed");
if (OK != att_control::g_control->start()) {
delete att_control::g_control;
att_control::g_control = nullptr;
err(1, "start failed");
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (att_control::g_control == nullptr)
errx(1, "not running");
delete att_control::g_control;
att_control::g_control = nullptr;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (att_control::g_control) {
errx(0, "running");
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
return 1;
}

136
src/modules/fw_att_control/fw_att_control_params.c Normal file
View File

@ -0,0 +1,136 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fw_pos_control_l1_params.c
*
* Parameters defined by the L1 position control task
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Controller parameters, accessible via MAVLink
*
*/
// @DisplayName Attitude Time Constant
// @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
// @Range 0.4 to 1.0 seconds, in tens of seconds
PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
// @DisplayName Proportional gain.
// @Description This defines how much the elevator input will be commanded dependend on the current pitch error.
// @Range 10 to 200, 1 increments
PARAM_DEFINE_FLOAT(FW_P_P, 40.0f);
// @DisplayName Damping gain.
// @Description This gain damps the airframe pitch rate. In particular relevant for flying wings.
// @Range 0.0 to 10.0, 0.1 increments
PARAM_DEFINE_FLOAT(FW_P_D, 0.0f);
// @DisplayName Integrator gain.
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error.
// @Range 0 to 50.0
PARAM_DEFINE_FLOAT(FW_P_I, 0.0f);
// @DisplayName Maximum positive / up pitch rate.
// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds, in 1 increments
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
// @DisplayName Maximum negative / down pitch rate.
// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds, in 1 increments
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
// @DisplayName Pitch Integrator Anti-Windup
// @Description This limits the range in degrees the integrator can wind up to.
// @Range 0.0 to 45.0
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f);
// @DisplayName Roll feedforward gain.
// @Description This compensates during turns and ensures the nose stays level.
// @Range 0.5 2.0
// @Increment 0.05
// @User User
PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f);
// @DisplayName Proportional Gain.
// @Description This gain controls the roll angle to roll actuator output.
// @Range 10.0 200.0
// @Increment 10.0
// @User User
PARAM_DEFINE_FLOAT(FW_R_P, 40.0f);
// @DisplayName Damping Gain
// @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence.
// @Range 0.0 10.0
// @Increment 1.0
// @User User
PARAM_DEFINE_FLOAT(FW_R_D, 0.0f);
// @DisplayName Integrator Gain
// @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors.
// @Range 0.0 100.0
// @Increment 5.0
// @User User
PARAM_DEFINE_FLOAT(FW_R_I, 0.0f);
// @DisplayName Roll Integrator Anti-Windup
// @Description This limits the range in degrees the integrator can wind up to.
// @Range 0.0 to 45.0
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f);
// @DisplayName Maximum Roll Rate
// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
// @Range 0 to 90.0 degrees per seconds
// @Increment 1.0
PARAM_DEFINE_FLOAT(FW_R_RMAX, 60);
PARAM_DEFINE_FLOAT(FW_Y_P, 0);
PARAM_DEFINE_FLOAT(FW_Y_I, 0);
PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f);
PARAM_DEFINE_FLOAT(FW_Y_D, 0);
PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1);
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f);
PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f);
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f);

41
src/modules/fw_att_control/module.mk Normal file
View File

@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Fixedwing attitude control application
#
MODULE_COMMAND = fw_att_control
SRCS = fw_att_control_main.cpp \
fw_att_control_params.c

41
src/modules/navigator/module.mk Normal file
View File

@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Main Navigation Controller
#
MODULE_COMMAND = navigator
SRCS = navigator_main.cpp \
navigator_params.c

604
src/modules/navigator/navigator_main.cpp Normal file
View File

@ -0,0 +1,604 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_main.c
* Implementation of the main navigation state machine.
*
* Handles missions, geo fencing and failsafe navigation behavior.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
/**
* navigator app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
class Navigator
{
public:
/**
* Constructor
*/
Navigator();
/**
* Destructor, also kills the sensors task.
*/
~Navigator();
/**
* Start the sensors task.
*
* @return OK on success.
*/
int start();
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _navigator_task; /**< task handle for sensor task */
int _global_pos_sub;
int _att_sub; /**< vehicle attitude subscription */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub;
orb_advert_t _triplet_pub; /**< position setpoint */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_status_s _vstatus; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */
perf_counter_t _loop_perf; /**< loop performance counter */
unsigned _mission_items_maxcount; /**< maximum number of mission items supported */
struct mission_item_s * _mission_items; /**< storage for mission items */
bool _mission_valid; /**< flag if mission is valid */
/** manual control states */
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
float _loiter_hold_lat;
float _loiter_hold_lon;
float _loiter_hold_alt;
bool _loiter_hold;
struct {
float throttle_cruise;
} _parameters; /**< local copies of interesting parameters */
struct {
param_t throttle_cruise;
} _parameter_handles; /**< handles for interesting parameters */
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Update control outputs
*
*/
void control_update();
/**
* Check for changes in vehicle status.
*/
void vehicle_status_poll();
/**
* Check for position updates.
*/
void vehicle_attitude_poll();
/**
* Check for set triplet updates.
*/
void mission_poll();
/**
* Control throttle.
*/
float control_throttle(float energy_error);
/**
* Control pitch.
*/
float control_pitch(float altitude_error);
void calculate_airspeed_errors();
void calculate_gndspeed_undershoot();
void calculate_altitude_error();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
};
namespace navigator
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Navigator *g_navigator;
}
Navigator::Navigator() :
_task_should_exit(false),
_navigator_task(-1),
/* subscriptions */
_global_pos_sub(-1),
_att_sub(-1),
_airspeed_sub(-1),
_vstatus_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
/* publications */
_triplet_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
/* states */
_mission_items_maxcount(20),
_mission_valid(false),
_loiter_hold(false)
{
_mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount);
if (!_mission_items) {
_mission_items_maxcount = 0;
warnx("no free RAM to allocate mission, rejecting any waypoints");
}
_parameter_handles.throttle_cruise = param_find("NAV_DUMMY");
/* fetch initial parameter values */
parameters_update();
}
Navigator::~Navigator()
{
if (_navigator_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_navigator_task);
break;
}
} while (_navigator_task != -1);
}
navigator::g_navigator = nullptr;
}
int
Navigator::parameters_update()
{
//param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
return OK;
}
void
Navigator::vehicle_status_poll()
{
bool vstatus_updated;
/* Check HIL state if vehicle status has changed */
orb_check(_vstatus_sub, &vstatus_updated);
if (vstatus_updated) {
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
}
}
void
Navigator::vehicle_attitude_poll()
{
/* check if there is a new position */
bool att_updated;
orb_check(_att_sub, &att_updated);
if (att_updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
}
}
void
Navigator::mission_poll()
{
/* check if there is a new setpoint */
bool mission_updated;
orb_check(_mission_sub, &mission_updated);
if (mission_updated) {
struct mission_s mission;
orb_copy(ORB_ID(mission), _mission_sub, &mission);
// XXX this is not optimal yet, but a first prototype /
// test implementation
if (mission.count <= _mission_items_maxcount) {
/*
* Perform an atomic copy & state update
*/
irqstate_t flags = irqsave();
memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s));
_mission_valid = true;
irqrestore(flags);
} else {
warnx("mission larger than storage space");
}
}
}
void
Navigator::task_main_trampoline(int argc, char *argv[])
{
navigator::g_navigator->task_main();
}
void
Navigator::task_main()
{
/* inform about start */
warnx("Initializing..");
fflush(stdout);
/*
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_mission_sub = orb_subscribe(ORB_ID(mission));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
parameters_update();
/* wakeup source(s) */
struct pollfd fds[2];
/* Setup of loop */
fds[0].fd = _params_sub;
fds[0].events = POLLIN;
fds[1].fd = _global_pos_sub;
fds[1].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
vehicle_status_poll();
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
/* update parameters from storage */
parameters_update();
}
/* only run controller if position changed */
if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
deltaT = 0.01f;
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
vehicle_attitude_poll();
mission_poll();
math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
// Current waypoint
math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
// Global position
math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
/* AUTONOMOUS FLIGHT */
if (1 /* autonomous flight */) {
/* execute navigation once we have a setpoint */
if (_mission_valid) {
// Next waypoint
math::Vector2f prev_wp;
if (_global_triplet.previous_valid) {
prev_wp.setX(_global_triplet.previous.lat / 1e7f);
prev_wp.setY(_global_triplet.previous.lon / 1e7f);
} else {
/*
* No valid next waypoint, go for heading hold.
* This is automatically handled by the L1 library.
*/
prev_wp.setX(_global_triplet.current.lat / 1e7f);
prev_wp.setY(_global_triplet.current.lon / 1e7f);
}
/******** MAIN NAVIGATION STATE MACHINE ********/
// XXX to be put in its own class
if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
/* waypoint is a plain navigation waypoint */
} else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
/* waypoint is a loiter waypoint */
}
// XXX at this point we always want no loiter hold if a
// mission is active
_loiter_hold = false;
} else {
if (!_loiter_hold) {
_loiter_hold_lat = _global_pos.lat / 1e7f;
_loiter_hold_lon = _global_pos.lon / 1e7f;
_loiter_hold_alt = _global_pos.alt;
_loiter_hold = true;
}
//_parameters.loiter_hold_radius
}
} else if (0/* seatbelt mode enabled */) {
/** SEATBELT FLIGHT **/
continue;
} else {
/** MANUAL FLIGHT **/
/* no flight mode applies, do not publish an attitude setpoint */
continue;
}
/******** MAIN NAVIGATION STATE MACHINE ********/
if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
// XXX define launch position and return
} else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) {
// XXX flared descent on final approach
} else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
/* apply minimum pitch if altitude has not yet been reached */
if (_global_pos.alt < _global_triplet.current.altitude) {
_att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1);
}
}
/* lazily publish the setpoint only once available */
if (_triplet_pub > 0) {
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet);
} else {
/* advertise and publish */
_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet);
}
}
perf_end(_loop_perf);
}
warnx("exiting.\n");
_navigator_task = -1;
_exit(0);
}
int
Navigator::start()
{
ASSERT(_navigator_task == -1);
/* start the task */
_navigator_task = task_spawn_cmd("navigator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2048,
(main_t)&Navigator::task_main_trampoline,
nullptr);
if (_navigator_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int navigator_main(int argc, char *argv[])
{
if (argc < 1)
errx(1, "usage: navigator {start|stop|status}");
if (!strcmp(argv[1], "start")) {
if (navigator::g_navigator != nullptr)
errx(1, "already running");
navigator::g_navigator = new Navigator;
if (navigator::g_navigator == nullptr)
errx(1, "alloc failed");
if (OK != navigator::g_navigator->start()) {
delete navigator::g_navigator;
navigator::g_navigator = nullptr;
err(1, "start failed");
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (navigator::g_navigator == nullptr)
errx(1, "not running");
delete navigator::g_navigator;
navigator::g_navigator = nullptr;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (navigator::g_navigator) {
errx(0, "running");
} else {
errx(1, "not running");
}
}
warnx("unrecognized command");
return 1;
}

53
src/modules/navigator/navigator_params.c Normal file
View File

@ -0,0 +1,53 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_params.c
*
* Parameters defined by the navigator task.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Navigator parameters, accessible via MAVLink
*
*/
PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f);

14
src/modules/uORB/topics/vehicle_global_position.h
View File

@ -62,17 +62,17 @@
struct vehicle_global_position_s
{
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
bool valid; /**< true if position satisfies validity criteria of estimator */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
bool valid; /**< true if position satisfies validity criteria of estimator */
int32_t lat; /**< Latitude in 1E7 degrees */
int32_t lon; /**< Longitude in 1E7 degrees */
float alt; /**< Altitude in meters */
float alt; /**< Altitude in meters */
float relative_alt; /**< Altitude above home position in meters, */
float vx; /**< Ground X velocity, m/s in NED */
float vy; /**< Ground Y velocity, m/s in NED */
float vz; /**< Ground Z velocity, m/s in NED */
float yaw; /**< Compass heading in radians -PI..+PI. */
float vx; /**< Ground X velocity, m/s in NED */
float vy; /**< Ground Y velocity, m/s in NED */
float vz; /**< Ground Z velocity, m/s in NED */
float yaw; /**< Compass heading in radians -PI..+PI. */
};
/**