aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Debug/olimex-px4fmu-debug.cfg22
-rw-r--r--Debug/openocd.gdbinit21
-rw-r--r--Debug/px4fmu-v1-board.cfg38
-rwxr-xr-xDebug/runopenocd.sh5
-rw-r--r--Debug/stm32f4x.cfg64
-rw-r--r--makefiles/upload.mk2
-rw-r--r--src/drivers/drv_mag.h25
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp122
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp131
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp498
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp87
-rw-r--r--src/drivers/px4io/px4io.cpp57
-rw-r--r--src/modules/mavlink/module.mk1
-rw-r--r--src/modules/px4iofirmware/mixer.cpp3
-rw-r--r--src/modules/px4iofirmware/protocol.h3
-rw-r--r--src/modules/px4iofirmware/registers.c12
-rw-r--r--src/modules/sensors/sensors.cpp33
-rw-r--r--src/modules/systemlib/mavlink_log.c (renamed from src/modules/mavlink/mavlink_log.c)21
-rw-r--r--src/modules/systemlib/module.mk3
-rw-r--r--src/systemcmds/config/config.c189
20 files changed, 803 insertions, 534 deletions
diff --git a/Debug/olimex-px4fmu-debug.cfg b/Debug/olimex-px4fmu-debug.cfg
new file mode 100644
index 000000000..61d70070d
--- /dev/null
+++ b/Debug/olimex-px4fmu-debug.cfg
@@ -0,0 +1,22 @@
+# program a bootable device load on a mavstation
+# To run type openocd -f mavprogram.cfg
+
+source [find interface/olimex-arm-usb-ocd-h.cfg]
+source [find px4fmu-v1-board.cfg]
+
+init
+halt
+
+# Find the flash inside this CPU
+flash probe 0
+
+# erase it (128 pages) then program and exit
+
+#flash erase_sector 0 0 127
+# stm32f1x mass_erase 0
+
+# It seems that Pat's image has a start address offset of 0x1000 but the vectors need to be at zero, so fixbin.sh moves things around
+#flash write_bank 0 fixed.bin 0
+#flash write_image firmware.elf
+#shutdown
+
diff --git a/Debug/openocd.gdbinit b/Debug/openocd.gdbinit
new file mode 100644
index 000000000..92d78b58d
--- /dev/null
+++ b/Debug/openocd.gdbinit
@@ -0,0 +1,21 @@
+target remote :3333
+
+# Don't let GDB get confused while stepping
+define hook-step
+ mon cortex_m maskisr on
+end
+define hookpost-step
+ mon cortex_m maskisr off
+end
+
+mon init
+mon stm32_init
+# mon reset halt
+mon poll
+mon cortex_m maskisr auto
+set mem inaccessible-by-default off
+set print pretty
+source Debug/PX4
+
+echo PX4 resumed, press ctrl-c to interrupt\n
+continue
diff --git a/Debug/px4fmu-v1-board.cfg b/Debug/px4fmu-v1-board.cfg
new file mode 100644
index 000000000..19b862a2d
--- /dev/null
+++ b/Debug/px4fmu-v1-board.cfg
@@ -0,0 +1,38 @@
+# The latest defaults in OpenOCD 0.7.0 are actually prettymuch correct for the px4fmu
+
+# increase working area to 32KB for faster flash programming
+set WORKAREASIZE 0x8000
+
+source [find target/stm32f4x.cfg]
+
+# needed for px4
+reset_config trst_only
+
+proc stm32_reset {} {
+ reset halt
+# FIXME - needed to init periphs on reset
+# 0x40023800 RCC base
+# 0x24 RCC_APB2 0x75933
+# RCC_APB2 0
+}
+
+# perform init that is required on each connection to the target
+proc stm32_init {} {
+
+ # force jtag to not shutdown during sleep
+ #uint32_t cr = getreg32(STM32_DBGMCU_CR);
+ #cr |= DBGMCU_CR_STANDBY | DBGMCU_CR_STOP | DBGMCU_CR_SLEEP;
+ #putreg32(cr, STM32_DBGMCU_CR);
+ mww 0xe0042004 00000007
+}
+
+# if srst is not fitted use SYSRESETREQ to
+# perform a soft reset
+cortex_m reset_config sysresetreq
+
+# Let GDB directly program elf binaries
+gdb_memory_map enable
+
+# doesn't work yet
+gdb_flash_program disable
+
diff --git a/Debug/runopenocd.sh b/Debug/runopenocd.sh
new file mode 100755
index 000000000..6258fccfb
--- /dev/null
+++ b/Debug/runopenocd.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+
+openocd -f interface/olimex-arm-usb-ocd-h.cfg -f $DIR/px4fmu-v1-board.cfg
diff --git a/Debug/stm32f4x.cfg b/Debug/stm32f4x.cfg
deleted file mode 100644
index 28bfcfbbb..000000000
--- a/Debug/stm32f4x.cfg
+++ /dev/null
@@ -1,64 +0,0 @@
-# script for stm32f2xxx
-
-if { [info exists CHIPNAME] } {
- set _CHIPNAME $CHIPNAME
-} else {
- set _CHIPNAME stm32f4xxx
-}
-
-if { [info exists ENDIAN] } {
- set _ENDIAN $ENDIAN
-} else {
- set _ENDIAN little
-}
-
-# Work-area is a space in RAM used for flash programming
-# By default use 64kB
-if { [info exists WORKAREASIZE] } {
- set _WORKAREASIZE $WORKAREASIZE
-} else {
- set _WORKAREASIZE 0x10000
-}
-
-# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz
-#
-# Since we may be running of an RC oscilator, we crank down the speed a
-# bit more to be on the safe side. Perhaps superstition, but if are
-# running off a crystal, we can run closer to the limit. Note
-# that there can be a pretty wide band where things are more or less stable.
-jtag_khz 1000
-
-jtag_nsrst_delay 100
-jtag_ntrst_delay 100
-
-#jtag scan chain
-if { [info exists CPUTAPID ] } {
- set _CPUTAPID $CPUTAPID
-} else {
- # See STM Document RM0033
- # Section 32.6.3 - corresponds to Cortex-M3 r2p0
- set _CPUTAPID 0x4ba00477
-}
-jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
-
-if { [info exists BSTAPID ] } {
- set _BSTAPID $BSTAPID
-} else {
- # See STM Document RM0033
- # Section 32.6.2
- #
- set _BSTAPID 0x06413041
-}
-jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID
-
-set _TARGETNAME $_CHIPNAME.cpu
-target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto
-
-$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
-
-set _FLASHNAME $_CHIPNAME.flash
-flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME
-
-# if srst is not fitted use SYSRESETREQ to
-# perform a soft reset
-cortex_m3 reset_config sysresetreq
diff --git a/makefiles/upload.mk b/makefiles/upload.mk
index c55e3f188..470ddfdf1 100644
--- a/makefiles/upload.mk
+++ b/makefiles/upload.mk
@@ -18,7 +18,7 @@ ifeq ($(SYSTYPE),Linux)
SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0"
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"
+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"
endif
.PHONY: all upload-$(METHOD)-$(BOARD)
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index e95034e8e..3de5625fd 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -90,28 +90,37 @@ ORB_DECLARE(sensor_mag);
/** set the mag internal sample rate to at least (arg) Hz */
#define MAGIOCSSAMPLERATE _MAGIOC(0)
+/** return the mag internal sample rate in Hz */
+#define MAGIOCGSAMPLERATE _MAGIOC(1)
+
/** set the mag internal lowpass filter to no lower than (arg) Hz */
-#define MAGIOCSLOWPASS _MAGIOC(1)
+#define MAGIOCSLOWPASS _MAGIOC(2)
+
+/** return the mag internal lowpass filter in Hz */
+#define MAGIOCGLOWPASS _MAGIOC(3)
/** set the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCSSCALE _MAGIOC(2)
+#define MAGIOCSSCALE _MAGIOC(4)
/** copy the mag scaling constants to the structure pointed to by (arg) */
-#define MAGIOCGSCALE _MAGIOC(3)
+#define MAGIOCGSCALE _MAGIOC(5)
/** set the measurement range to handle (at least) arg Gauss */
-#define MAGIOCSRANGE _MAGIOC(4)
+#define MAGIOCSRANGE _MAGIOC(6)
+
+/** return the current mag measurement range in Gauss */
+#define MAGIOCGRANGE _MAGIOC(7)
/** perform self-calibration, update scale factors to canonical units */
-#define MAGIOCCALIBRATE _MAGIOC(5)
+#define MAGIOCCALIBRATE _MAGIOC(8)
/** excite strap */
-#define MAGIOCEXSTRAP _MAGIOC(6)
+#define MAGIOCEXSTRAP _MAGIOC(9)
/** perform self test and report status */
-#define MAGIOCSELFTEST _MAGIOC(7)
+#define MAGIOCSELFTEST _MAGIOC(10)
/** determine if mag is external or onboard */
-#define MAGIOCGEXTERNAL _MAGIOC(8)
+#define MAGIOCGEXTERNAL _MAGIOC(11)
#endif /* _DRV_MAG_H */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 692f890bd..d77f03bb7 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -77,8 +77,8 @@
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
-/* Max measurement rate is 160Hz */
-#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */
+/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
+#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */
#define ADDR_CONF_A 0x00
#define ADDR_CONF_B 0x01
@@ -192,6 +192,11 @@ private:
void stop();
/**
+ * Reset the device
+ */
+ int reset();
+
+ /**
* Perform the on-sensor scale calibration routine.
*
* @note The sensor will continue to provide measurements, these
@@ -365,6 +370,9 @@ 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];
@@ -381,9 +389,6 @@ HMC5883::init()
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
- /* set range */
- set_range(_range_ga);
-
ret = OK;
/* sensor is ok, but not calibrated */
_sensor_ok = true;
@@ -542,68 +547,67 @@ int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
-
case SENSORIOCSPOLLRATE: {
- switch (arg) {
+ switch (arg) {
- /* switching to manual polling */
- case SENSOR_POLLRATE_MANUAL:
- stop();
- _measure_ticks = 0;
- return OK;
+ /* switching to manual polling */
+ case SENSOR_POLLRATE_MANUAL:
+ stop();
+ _measure_ticks = 0;
+ return OK;
- /* external signalling (DRDY) not supported */
- case SENSOR_POLLRATE_EXTERNAL:
+ /* external signalling (DRDY) not supported */
+ case SENSOR_POLLRATE_EXTERNAL:
- /* zero would be bad */
- case 0:
- return -EINVAL;
+ /* zero would be bad */
+ case 0:
+ return -EINVAL;
- /* set default/max polling rate */
- case SENSOR_POLLRATE_MAX:
- case SENSOR_POLLRATE_DEFAULT: {
- /* do we need to start internal polling? */
- bool want_start = (_measure_ticks == 0);
+ /* set default/max polling rate */
+ case SENSOR_POLLRATE_MAX:
+ case SENSOR_POLLRATE_DEFAULT: {
+ /* do we need to start internal polling? */
+ bool want_start = (_measure_ticks == 0);
- /* set interval for next measurement to minimum legal value */
- _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
+ /* set interval for next measurement to minimum legal value */
+ _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
- /* if we need to start the poll state machine, do it */
- if (want_start)
- start();
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
- return OK;
- }
+ return OK;
+ }
- /* adjust to a legal polling interval in Hz */
- default: {
- /* do we need to start internal polling? */
- bool want_start = (_measure_ticks == 0);
+ /* adjust to a legal polling interval in Hz */
+ default: {
+ /* do we need to start internal polling? */
+ bool want_start = (_measure_ticks == 0);
- /* convert hz to tick interval via microseconds */
- unsigned ticks = USEC2TICK(1000000 / arg);
+ /* convert hz to tick interval via microseconds */
+ unsigned ticks = USEC2TICK(1000000 / arg);
- /* check against maximum rate */
- if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
- return -EINVAL;
+ /* check against maximum rate */
+ if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
+ return -EINVAL;
- /* update interval for next measurement */
- _measure_ticks = ticks;
+ /* update interval for next measurement */
+ _measure_ticks = ticks;
- /* if we need to start the poll state machine, do it */
- if (want_start)
- start();
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
- return OK;
- }
+ return OK;
}
}
+ }
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
- return (1000 / _measure_ticks);
+ return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
@@ -633,17 +637,24 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
return _num_reports - 1;
case SENSORIOCRESET:
- /* XXX implement this */
- return -EINVAL;
+ return reset();
case MAGIOCSSAMPLERATE:
- /* not supported, always 1 sample per poll */
- return -EINVAL;
+ /* same as pollrate because device is in single measurement mode*/
+ return ioctl(filp, SENSORIOCSPOLLRATE, arg);
+
+ case MAGIOCGSAMPLERATE:
+ /* same as pollrate because device is in single measurement mode*/
+ return 1000000/TICK2USEC(_measure_ticks);
case MAGIOCSRANGE:
return set_range(arg);
+ case MAGIOCGRANGE:
+ return _range_ga;
+
case MAGIOCSLOWPASS:
+ case MAGIOCGLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
@@ -697,6 +708,13 @@ HMC5883::stop()
work_cancel(HPWORK, &_work);
}
+int
+HMC5883::reset()
+{
+ /* set range */
+ return set_range(_range_ga);
+}
+
void
HMC5883::cycle_trampoline(void *arg)
{
@@ -861,10 +879,10 @@ HMC5883::collect()
} else {
#endif
/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
- * therefore switch and invert x and y */
+ * 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;
/* 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;
+ _reports[_next_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;
#ifdef PX4_I2C_BUS_ONBOARD
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index de6b753f1..5e0a2119a 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -154,6 +154,10 @@ static const int ERROR = -1;
#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5)
#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
+#define L3GD20_DEFAULT_RATE 760
+#define L3GD20_DEFAULT_RANGE_DPS 2000
+#define L3GD20_DEFAULT_FILTER_FREQ 30
+
extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI
@@ -191,9 +195,10 @@ private:
orb_advert_t _gyro_topic;
unsigned _current_rate;
- unsigned _current_range;
unsigned _orientation;
+ unsigned _read;
+
perf_counter_t _sample_perf;
math::LowPassFilter2p _gyro_filter_x;
@@ -211,6 +216,11 @@ private:
void stop();
/**
+ * Reset the driver
+ */
+ void reset();
+
+ /**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
@@ -274,6 +284,14 @@ private:
int set_samplerate(unsigned frequency);
/**
+ * Set the lowpass filter of the driver
+ *
+ * @param samplerate The current samplerate
+ * @param frequency The cutoff frequency for the lowpass filter
+ */
+ void set_driver_lowpass_filter(float samplerate, float bandwidth);
+
+ /**
* Self test
*
* @return 0 on success, 1 on failure
@@ -296,12 +314,12 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_current_rate(0),
- _current_range(0),
_orientation(SENSOR_BOARD_ROTATION_270_DEG),
- _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
- _gyro_filter_x(250, 30),
- _gyro_filter_y(250, 30),
- _gyro_filter_z(250, 30)
+ _read(0),
+ _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
+ _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+ _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+ _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
{
// enable debug() calls
_debug_enabled = true;
@@ -349,22 +367,7 @@ L3GD20::init()
memset(&_reports[0], 0, sizeof(_reports[0]));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
- /* set default configuration */
- write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
- write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
- write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
- write_reg(ADDR_CTRL_REG4, REG4_BDU);
- write_reg(ADDR_CTRL_REG5, 0);
-
- write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
-
- /* disable FIFO. This makes things simpler and ensures we
- * aren't getting stale data. It means we must run the hrt
- * callback fast enough to not miss data. */
- write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
-
- set_range(2000); /* default to 2000dps */
- set_samplerate(0); /* max sample rate */
+ reset();
ret = OK;
out:
@@ -464,8 +467,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
- /* With internal low pass filters enabled, 250 Hz is sufficient */
- return ioctl(filp, SENSORIOCSPOLLRATE, 250);
+ return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
@@ -483,12 +485,10 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks;
- // adjust filters
- float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
- float sample_rate = 1.0e6f/ticks;
- _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
- _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
- _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+ /* adjust filters */
+ float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
+ float sample_rate = 1.0e6f/ticks;
+ set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
/* if we need to start the poll state machine, do it */
if (want_start)
@@ -533,8 +533,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return _num_reports - 1;
case SENSORIOCRESET:
- /* XXX implement */
- return -EINVAL;
+ reset();
+ return OK;
case GYROIOCSSAMPLERATE:
return set_samplerate(arg);
@@ -543,16 +543,15 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return _current_rate;
case GYROIOCSLOWPASS: {
- float cutoff_freq_hz = arg;
- float sample_rate = 1.0e6f / _call_interval;
- _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
- _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
- _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
- return OK;
- }
+ float cutoff_freq_hz = arg;
+ float sample_rate = 1.0e6f / _call_interval;
+ set_driver_lowpass_filter(sample_rate, cutoff_freq_hz);
+
+ return OK;
+ }
case GYROIOCGLOWPASS:
- return _gyro_filter_x.get_cutoff_freq();
+ return _gyro_filter_x.get_cutoff_freq();
case GYROIOCSSCALE:
/* copy scale in */
@@ -565,10 +564,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return OK;
case GYROIOCSRANGE:
+ /* arg should be in dps */
return set_range(arg);
case GYROIOCGRANGE:
- return _current_range;
+ /* convert to dps and round */
+ return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
case GYROIOCSELFTEST:
return self_test();
@@ -618,22 +619,23 @@ L3GD20::set_range(unsigned max_dps)
{
uint8_t bits = REG4_BDU;
float new_range_scale_dps_digit;
+ float new_range;
if (max_dps == 0) {
max_dps = 2000;
}
if (max_dps <= 250) {
- _current_range = 250;
+ new_range = 250;
bits |= RANGE_250DPS;
new_range_scale_dps_digit = 8.75e-3f;
} else if (max_dps <= 500) {
- _current_range = 500;
+ new_range = 500;
bits |= RANGE_500DPS;
new_range_scale_dps_digit = 17.5e-3f;
} else if (max_dps <= 2000) {
- _current_range = 2000;
+ new_range = 2000;
bits |= RANGE_2000DPS;
new_range_scale_dps_digit = 70e-3f;
@@ -641,7 +643,7 @@ L3GD20::set_range(unsigned max_dps)
return -EINVAL;
}
- _gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
+ _gyro_range_rad_s = new_range / 180.0f * M_PI_F;
_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
write_reg(ADDR_CTRL_REG4, bits);
@@ -656,7 +658,7 @@ L3GD20::set_samplerate(unsigned frequency)
if (frequency == 0)
frequency = 760;
- // use limits good for H or non-H models
+ /* use limits good for H or non-H models */
if (frequency <= 100) {
_current_rate = 95;
bits |= RATE_95HZ_LP_25HZ;
@@ -683,6 +685,14 @@ L3GD20::set_samplerate(unsigned frequency)
}
void
+L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth)
+{
+ _gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+ _gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+ _gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth);
+}
+
+void
L3GD20::start()
{
/* make sure we are stopped first */
@@ -702,6 +712,30 @@ L3GD20::stop()
}
void
+L3GD20::reset()
+{
+ /* set default configuration */
+ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+ write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
+ write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
+ write_reg(ADDR_CTRL_REG4, REG4_BDU);
+ write_reg(ADDR_CTRL_REG5, 0);
+
+ write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
+
+ /* disable FIFO. This makes things simpler and ensures we
+ * aren't getting stale data. It means we must run the hrt
+ * callback fast enough to not miss data. */
+ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+
+ set_samplerate(L3GD20_DEFAULT_RATE);
+ set_range(L3GD20_DEFAULT_RANGE_DPS);
+ set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
+
+ _read = 0;
+}
+
+void
L3GD20::measure_trampoline(void *arg)
{
L3GD20 *dev = (L3GD20 *)arg;
@@ -804,6 +838,8 @@ L3GD20::measure()
if (_gyro_topic > 0)
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
+ _read++;
+
/* stop the perf counter */
perf_end(_sample_perf);
}
@@ -811,6 +847,7 @@ L3GD20::measure()
void
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);
@@ -949,7 +986,7 @@ reset()
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
- err(1, "driver poll restart failed");
+ err(1, "accel pollrate reset failed");
exit(0);
}
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 3e6ce64b8..efe7cf8eb 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -54,6 +54,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
+#include <systemlib/geo/geo.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
@@ -168,6 +169,14 @@ static const int ERROR = -1;
#define INT_CTRL_M 0x12
#define INT_SRC_M 0x13
+/* default values for this device */
+#define LSM303D_ACCEL_DEFAULT_RANGE_G 8
+#define LSM303D_ACCEL_DEFAULT_RATE 800
+#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
+#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define LSM303D_MAG_DEFAULT_RANGE_GA 2
+#define LSM303D_MAG_DEFAULT_RATE 100
extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
@@ -180,56 +189,61 @@ public:
LSM303D(int bus, const char* path, spi_dev_e device);
virtual ~LSM303D();
- virtual int init();
+ virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
- virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
- void print_info();
+ void print_info();
protected:
- virtual int probe();
+ virtual int probe();
friend class LSM303D_mag;
virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen);
- virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg);
+ virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg);
private:
- LSM303D_mag *_mag;
+ LSM303D_mag *_mag;
struct hrt_call _accel_call;
struct hrt_call _mag_call;
- unsigned _call_accel_interval;
- unsigned _call_mag_interval;
+ unsigned _call_accel_interval;
+ unsigned _call_mag_interval;
- unsigned _num_accel_reports;
+ unsigned _num_accel_reports;
volatile unsigned _next_accel_report;
volatile unsigned _oldest_accel_report;
struct accel_report *_accel_reports;
- struct accel_scale _accel_scale;
- float _accel_range_scale;
- float _accel_range_m_s2;
- orb_advert_t _accel_topic;
-
- unsigned _current_samplerate;
-
- unsigned _num_mag_reports;
+ unsigned _num_mag_reports;
volatile unsigned _next_mag_report;
volatile unsigned _oldest_mag_report;
struct mag_report *_mag_reports;
+ struct accel_scale _accel_scale;
+ unsigned _accel_range_m_s2;
+ float _accel_range_scale;
+ unsigned _accel_samplerate;
+ unsigned _accel_onchip_filter_bandwith;
+
struct mag_scale _mag_scale;
- float _mag_range_scale;
- float _mag_range_ga;
+ unsigned _mag_range_ga;
+ float _mag_range_scale;
+ unsigned _mag_samplerate;
+
+ orb_advert_t _accel_topic;
orb_advert_t _mag_topic;
+ unsigned _accel_read;
+ unsigned _mag_read;
+
perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf;
@@ -240,12 +254,19 @@ private:
/**
* Start automatic measurement.
*/
- void start();
+ void start();
/**
* Stop automatic measurement.
*/
- void stop();
+ void stop();
+
+ /**
+ * Reset chip.
+ *
+ * Resets the chip and measurements ranges, but not scale and offset.
+ */
+ void reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
@@ -256,24 +277,38 @@ private:
*
* @param arg Instance pointer for the driver that is polling.
*/
- static void measure_trampoline(void *arg);
+ static void measure_trampoline(void *arg);
/**
* Static trampoline for the mag because it runs at a lower rate
*
* @param arg Instance pointer for the driver that is polling.
*/
- static void mag_measure_trampoline(void *arg);
+ static void mag_measure_trampoline(void *arg);
/**
* Fetch accel measurements from the sensor and update the report ring.
*/
- void measure();
+ void measure();
/**
* Fetch mag measurements from the sensor and update the report ring.
*/
- void mag_measure();
+ void mag_measure();
+
+ /**
+ * Accel self test
+ *
+ * @return 0 on success, 1 on failure
+ */
+ int accel_self_test();
+
+ /**
+ * Mag self test
+ *
+ * @return 0 on success, 1 on failure
+ */
+ int mag_self_test();
/**
* Read a register from the LSM303D
@@ -281,7 +316,7 @@ private:
* @param The register to read.
* @return The value that was read.
*/
- uint8_t read_reg(unsigned reg);
+ uint8_t read_reg(unsigned reg);
/**
* Write a register in the LSM303D
@@ -289,7 +324,7 @@ private:
* @param reg The register to write.
* @param value The new value to write.
*/
- void write_reg(unsigned reg, uint8_t value);
+ void write_reg(unsigned reg, uint8_t value);
/**
* Modify a register in the LSM303D
@@ -300,7 +335,7 @@ private:
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
- void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Set the LSM303D accel measurement range.
@@ -309,7 +344,7 @@ private:
* Zero selects the maximum supported range.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
- int set_range(unsigned max_g);
+ int accel_set_range(unsigned max_g);
/**
* Set the LSM303D mag measurement range.
@@ -318,24 +353,25 @@ private:
* Zero selects the maximum supported range.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
- int mag_set_range(unsigned max_g);
+ int mag_set_range(unsigned max_g);
/**
- * Set the LSM303D accel anti-alias filter.
+ * Set the LSM303D on-chip anti-alias filter bandwith.
*
* @param bandwidth The anti-alias filter bandwidth in Hz
* Zero selects the highest bandwidth
* @return OK if the value can be supported, -ERANGE otherwise.
*/
- int set_antialias_filter_bandwidth(unsigned bandwith);
+ int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
/**
- * Get the LSM303D accel anti-alias filter.
+ * Set the driver lowpass filter bandwidth.
*
* @param bandwidth The anti-alias filter bandwidth in Hz
- * @return OK if the value was read and supported, ERROR otherwise.
+ * Zero selects the highest bandwidth
+ * @return OK if the value can be supported, -ERANGE otherwise.
*/
- int get_antialias_filter_bandwidth(unsigned &bandwidth);
+ int accel_set_driver_lowpass_filter(float samplerate, float bandwidth);
/**
* Set the LSM303D internal accel sampling frequency.
@@ -345,7 +381,7 @@ private:
* Zero selects the maximum rate supported.
* @return OK if the value can be supported.
*/
- int set_samplerate(unsigned frequency);
+ int accel_set_samplerate(unsigned frequency);
/**
* Set the LSM303D internal mag sampling frequency.
@@ -355,7 +391,7 @@ private:
* Zero selects the maximum rate supported.
* @return OK if the value can be supported.
*/
- int mag_set_samplerate(unsigned frequency);
+ int mag_set_samplerate(unsigned frequency);
};
/**
@@ -396,38 +432,43 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_next_accel_report(0),
_oldest_accel_report(0),
_accel_reports(nullptr),
- _accel_range_scale(0.0f),
- _accel_range_m_s2(0.0f),
- _accel_topic(-1),
- _current_samplerate(0),
_num_mag_reports(0),
_next_mag_report(0),
_oldest_mag_report(0),
_mag_reports(nullptr),
- _mag_range_scale(0.0f),
+ _accel_range_m_s2(0.0f),
+ _accel_range_scale(0.0f),
+ _accel_samplerate(0),
+ _accel_onchip_filter_bandwith(0),
_mag_range_ga(0.0f),
+ _mag_range_scale(0.0f),
+ _mag_samplerate(0),
+ _accel_topic(-1),
+ _mag_topic(-1),
+ _accel_read(0),
+ _mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
- _accel_filter_x(800, 30),
- _accel_filter_y(800, 30),
- _accel_filter_z(800, 30)
+ _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
{
// enable debug() calls
_debug_enabled = true;
// default scale factors
- _accel_scale.x_offset = 0;
+ _accel_scale.x_offset = 0.0f;
_accel_scale.x_scale = 1.0f;
- _accel_scale.y_offset = 0;
+ _accel_scale.y_offset = 0.0f;
_accel_scale.y_scale = 1.0f;
- _accel_scale.z_offset = 0;
+ _accel_scale.z_offset = 0.0f;
_accel_scale.z_scale = 1.0f;
- _mag_scale.x_offset = 0;
+ _mag_scale.x_offset = 0.0f;
_mag_scale.x_scale = 1.0f;
- _mag_scale.y_offset = 0;
+ _mag_scale.y_offset = 0.0f;
_mag_scale.y_scale = 1.0f;
- _mag_scale.z_offset = 0;
+ _mag_scale.z_offset = 0.0f;
_mag_scale.z_scale = 1.0f;
}
@@ -478,27 +519,12 @@ LSM303D::init()
if (_mag_reports == nullptr)
goto out;
+ reset();
+
/* advertise mag topic */
memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);
- /* enable accel, XXX do this with an ioctl? */
- write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
-
- /* enable mag, XXX do this with an ioctl? */
- write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
- write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
-
- /* XXX should we enable FIFO? */
-
- set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */
- set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */
- set_samplerate(400); /* max sample rate */
-
- mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
- mag_set_samplerate(100);
-
- /* XXX test this when another mag is used */
/* do CDev init for the mag device node, keep it optional */
mag_ret = _mag->init();
@@ -511,6 +537,28 @@ out:
return ret;
}
+void
+LSM303D::reset()
+{
+ /* enable accel*/
+ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
+
+ /* enable mag */
+ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+ write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
+
+ accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
+ accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE);
+ accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ);
+ accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
+
+ mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
+ mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
+
+ _accel_read = 0;
+ _mag_read = 0;
+}
+
int
LSM303D::probe()
{
@@ -612,64 +660,55 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case SENSORIOCSPOLLRATE: {
- switch (arg) {
+ switch (arg) {
- /* switching to manual polling */
+ /* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_accel_interval = 0;
return OK;
- /* external signalling not supported */
+ /* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
- /* zero would be bad */
+ /* zero would be bad */
case 0:
return -EINVAL;
- /* set default/max polling rate */
+ /* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
case SENSOR_POLLRATE_DEFAULT:
- /* With internal low pass filters enabled, 250 Hz is sufficient */
- /* XXX for vibration tests with 800 Hz */
- return ioctl(filp, SENSORIOCSPOLLRATE, 800);
+ return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
- /* do we need to start internal polling? */
- bool want_start = (_call_accel_interval == 0);
-
- /* convert hz to hrt interval via microseconds */
- unsigned ticks = 1000000 / arg;
+ /* do we need to start internal polling? */
+ bool want_start = (_call_accel_interval == 0);
- /* check against maximum sane rate */
- if (ticks < 1000)
- return -EINVAL;
+ /* convert hz to hrt interval via microseconds */
+ unsigned ticks = 1000000 / arg;
- /* adjust sample rate of sensor */
- set_samplerate(arg);
+ /* check against maximum sane rate */
+ if (ticks < 500)
+ return -EINVAL;
- // adjust filters
- float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
- float sample_rate = 1.0e6f/ticks;
- _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
- _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
- _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
+ /* adjust filters */
+ accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
- /* update interval for next measurement */
- /* XXX this is a bit shady, but no other way to adjust... */
- _accel_call.period = _call_accel_interval = ticks;
+ /* update interval for next measurement */
+ /* XXX this is a bit shady, but no other way to adjust... */
+ _accel_call.period = _call_accel_interval = ticks;
- /* if we need to start the poll state machine, do it */
- if (want_start)
- start();
+ /* if we need to start the poll state machine, do it */
+ if (want_start)
+ start();
- return OK;
- }
+ return OK;
}
}
+ }
case SENSORIOCGPOLLRATE:
if (_call_accel_interval == 0)
@@ -678,66 +717,77 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_accel_interval;
case SENSORIOCSQUEUEDEPTH: {
- /* account for sentinel in the ring */
- arg++;
+ /* account for sentinel in the ring */
+ arg++;
- /* lower bound is mandatory, upper bound is a sanity check */
- if ((arg < 2) || (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 accel_report *buf = new struct accel_report[arg];
+ /* allocate new buffer */
+ struct accel_report *buf = new struct accel_report[arg];
- if (nullptr == buf)
- return -ENOMEM;
+ if (nullptr == buf)
+ 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();
+ /* reset the measurement state machine with the new buffer, free the old */
+ stop();
+ delete[] _accel_reports;
+ _num_accel_reports = arg;
+ _accel_reports = buf;
+ start();
- return OK;
- }
+ return OK;
+ }
case SENSORIOCGQUEUEDEPTH:
return _num_accel_reports - 1;
case SENSORIOCRESET:
- /* XXX implement */
- return -EINVAL;
+ reset();
+ return OK;
+
+ case ACCELIOCSSAMPLERATE:
+ return accel_set_samplerate(arg);
+
+ case ACCELIOCGSAMPLERATE:
+ return _accel_samplerate;
case ACCELIOCSLOWPASS: {
- float cutoff_freq_hz = arg;
- float sample_rate = 1.0e6f / _call_accel_interval;
- _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
- _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
- _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
- return OK;
- }
+ return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
+ }
case ACCELIOCGLOWPASS:
- return _accel_filter_x.get_cutoff_freq();
-
- case ACCELIOCSSCALE:
- {
- /* copy scale, but only if off by a few percent */
- struct accel_scale *s = (struct accel_scale *) arg;
- float sum = s->x_scale + s->y_scale + s->z_scale;
- if (sum > 2.0f && sum < 4.0f) {
- memcpy(&_accel_scale, s, sizeof(_accel_scale));
- return OK;
- } else {
- return -EINVAL;
- }
+ return _accel_filter_x.get_cutoff_freq();
+
+ case ACCELIOCSSCALE: {
+ /* copy scale, but only if off by a few percent */
+ struct accel_scale *s = (struct accel_scale *) arg;
+ float sum = s->x_scale + s->y_scale + s->z_scale;
+ if (sum > 2.0f && sum < 4.0f) {
+ memcpy(&_accel_scale, s, sizeof(_accel_scale));
+ return OK;
+ } else {
+ return -EINVAL;
}
+ }
+
+ case ACCELIOCSRANGE:
+ /* arg needs to be in G */
+ return accel_set_range(arg);
+
+ case ACCELIOCGRANGE:
+ /* convert to m/s^2 and return rounded in G */
+ return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
+ case ACCELIOCSELFTEST:
+ return accel_self_test();
+
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
@@ -768,7 +818,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
- /* 50 Hz is max for mag */
+ /* 100 Hz is max for mag */
return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
/* adjust to a legal polling interval in Hz */
@@ -783,9 +833,6 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
if (ticks < 1000)
return -EINVAL;
- /* adjust sample rate of sensor */
- mag_set_samplerate(arg);
-
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_mag_call.period = _call_mag_interval = ticks;
@@ -833,17 +880,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
return _num_mag_reports - 1;
case SENSORIOCRESET:
- return ioctl(filp, cmd, arg);
+ reset();
+ return OK;
case MAGIOCSSAMPLERATE:
-// case MAGIOCGSAMPLERATE:
- /* XXX not implemented */
- return -EINVAL;
+ return mag_set_samplerate(arg);
+
+ case MAGIOCGSAMPLERATE:
+ return _mag_samplerate;
case MAGIOCSLOWPASS:
-// case MAGIOCGLOWPASS:
- /* XXX not implemented */
-// _set_dlpf_filter((uint16_t)arg);
+ case MAGIOCGLOWPASS:
+ /* not supported, no internal filtering */
return -EINVAL;
case MAGIOCSSCALE:
@@ -857,17 +905,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
return OK;
case MAGIOCSRANGE:
-// case MAGIOCGRANGE:
- /* XXX not implemented */
- // XXX change these two values on set:
- // _mag_range_scale = xx
- // _mag_range_ga = xx
- return -EINVAL;
+ return mag_set_range(arg);
+
+ case MAGIOCGRANGE:
+ return _mag_range_ga;
case MAGIOCSELFTEST:
- /* XXX not implemented */
-// return self_test();
- return -EINVAL;
+ return mag_self_test();
case MAGIOCGEXTERNAL:
/* no external mag board yet */
@@ -879,6 +923,53 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
}
}
+int
+LSM303D::accel_self_test()
+{
+ if (_accel_read == 0)
+ return 1;
+
+ /* inspect accel offsets */
+ if (fabsf(_accel_scale.x_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+ return 1;
+
+ if (fabsf(_accel_scale.y_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+ return 1;
+
+ if (fabsf(_accel_scale.z_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+ return 1;
+
+ return 0;
+}
+
+int
+LSM303D::mag_self_test()
+{
+ if (_mag_read == 0)
+ return 1;
+
+ /**
+ * inspect mag offsets
+ * don't check mag scale because it seems this is calibrated on chip
+ */
+ if (fabsf(_mag_scale.x_offset) < 0.000001f)
+ return 1;
+
+ if (fabsf(_mag_scale.y_offset) < 0.000001f)
+ return 1;
+
+ if (fabsf(_mag_scale.z_offset) < 0.000001f)
+ return 1;
+
+ return 0;
+}
+
uint8_t
LSM303D::read_reg(unsigned reg)
{
@@ -914,38 +1005,37 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
}
int
-LSM303D::set_range(unsigned max_g)
+LSM303D::accel_set_range(unsigned max_g)
{
uint8_t setbits = 0;
uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
- float new_range_g = 0.0f;
float new_scale_g_digit = 0.0f;
if (max_g == 0)
max_g = 16;
if (max_g <= 2) {
- new_range_g = 2.0f;
+ _accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G;
setbits |= REG2_FULL_SCALE_2G_A;
new_scale_g_digit = 0.061e-3f;
} else if (max_g <= 4) {
- new_range_g = 4.0f;
+ _accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G;
setbits |= REG2_FULL_SCALE_4G_A;
new_scale_g_digit = 0.122e-3f;
} else if (max_g <= 6) {
- new_range_g = 6.0f;
+ _accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G;
setbits |= REG2_FULL_SCALE_6G_A;
new_scale_g_digit = 0.183e-3f;
} else if (max_g <= 8) {
- new_range_g = 8.0f;
+ _accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G;
setbits |= REG2_FULL_SCALE_8G_A;
new_scale_g_digit = 0.244e-3f;
} else if (max_g <= 16) {
- new_range_g = 16.0f;
+ _accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G;
setbits |= REG2_FULL_SCALE_16G_A;
new_scale_g_digit = 0.732e-3f;
@@ -953,8 +1043,7 @@ LSM303D::set_range(unsigned max_g)
return -EINVAL;
}
- _accel_range_m_s2 = new_range_g * 9.80665f;
- _accel_range_scale = new_scale_g_digit * 9.80665f;
+ _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G;
modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
@@ -966,29 +1055,28 @@ LSM303D::mag_set_range(unsigned max_ga)
{
uint8_t setbits = 0;
uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
- float new_range = 0.0f;
float new_scale_ga_digit = 0.0f;
if (max_ga == 0)
max_ga = 12;
if (max_ga <= 2) {
- new_range = 2.0f;
+ _mag_range_ga = 2;
setbits |= REG6_FULL_SCALE_2GA_M;
new_scale_ga_digit = 0.080e-3f;
} else if (max_ga <= 4) {
- new_range = 4.0f;
+ _mag_range_ga = 4;
setbits |= REG6_FULL_SCALE_4GA_M;
new_scale_ga_digit = 0.160e-3f;
} else if (max_ga <= 8) {
- new_range = 8.0f;
+ _mag_range_ga = 8;
setbits |= REG6_FULL_SCALE_8GA_M;
new_scale_ga_digit = 0.320e-3f;
} else if (max_ga <= 12) {
- new_range = 12.0f;
+ _mag_range_ga = 12;
setbits |= REG6_FULL_SCALE_12GA_M;
new_scale_ga_digit = 0.479e-3f;
@@ -996,7 +1084,6 @@ LSM303D::mag_set_range(unsigned max_ga)
return -EINVAL;
}
- _mag_range_ga = new_range;
_mag_range_scale = new_scale_ga_digit;
modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
@@ -1005,7 +1092,7 @@ LSM303D::mag_set_range(unsigned max_ga)
}
int
-LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
+LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
{
uint8_t setbits = 0;
uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
@@ -1015,15 +1102,19 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
if (bandwidth <= 50) {
setbits |= REG2_AA_FILTER_BW_50HZ_A;
+ _accel_onchip_filter_bandwith = 50;
} else if (bandwidth <= 194) {
setbits |= REG2_AA_FILTER_BW_194HZ_A;
+ _accel_onchip_filter_bandwith = 194;
} else if (bandwidth <= 362) {
setbits |= REG2_AA_FILTER_BW_362HZ_A;
+ _accel_onchip_filter_bandwith = 362;
} else if (bandwidth <= 773) {
setbits |= REG2_AA_FILTER_BW_773HZ_A;
+ _accel_onchip_filter_bandwith = 773;
} else {
return -EINVAL;
@@ -1035,26 +1126,17 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
}
int
-LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth)
+LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
{
- uint8_t readbits = read_reg(ADDR_CTRL_REG2);
-
- if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A)
- bandwidth = 50;
- else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A)
- bandwidth = 194;
- else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A)
- bandwidth = 362;
- else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A)
- bandwidth = 773;
- else
- return ERROR;
+ _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth);
+ _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth);
+ _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth);
return OK;
}
int
-LSM303D::set_samplerate(unsigned frequency)
+LSM303D::accel_set_samplerate(unsigned frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = REG1_RATE_BITS_A;
@@ -1064,23 +1146,23 @@ LSM303D::set_samplerate(unsigned frequency)
if (frequency <= 100) {
setbits |= REG1_RATE_100HZ_A;
- _current_samplerate = 100;
+ _accel_samplerate = 100;
} else if (frequency <= 200) {
setbits |= REG1_RATE_200HZ_A;
- _current_samplerate = 200;
+ _accel_samplerate = 200;
} else if (frequency <= 400) {
setbits |= REG1_RATE_400HZ_A;
- _current_samplerate = 400;
+ _accel_samplerate = 400;
} else if (frequency <= 800) {
setbits |= REG1_RATE_800HZ_A;
- _current_samplerate = 800;
+ _accel_samplerate = 800;
} else if (frequency <= 1600) {
setbits |= REG1_RATE_1600HZ_A;
- _current_samplerate = 1600;
+ _accel_samplerate = 1600;
} else {
return -EINVAL;
@@ -1102,13 +1184,15 @@ LSM303D::mag_set_samplerate(unsigned frequency)
if (frequency <= 25) {
setbits |= REG5_RATE_25HZ_M;
+ _mag_samplerate = 25;
} else if (frequency <= 50) {
setbits |= REG5_RATE_50HZ_M;
+ _mag_samplerate = 50;
} else if (frequency <= 100) {
setbits |= REG5_RATE_100HZ_M;
-
+ _mag_samplerate = 100;
} else {
return -EINVAL;
@@ -1229,6 +1313,8 @@ LSM303D::measure()
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
+ _accel_read++;
+
/* stop the perf counter */
perf_end(_accel_sample_perf);
}
@@ -1281,7 +1367,7 @@ LSM303D::mag_measure()
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 = _mag_range_ga;
+ mag_report->range_ga = (float)_mag_range_ga;
/* post a report to the ring - note, not locked */
INCREMENT(_next_mag_report, _num_mag_reports);
@@ -1297,6 +1383,8 @@ LSM303D::mag_measure()
/* publish for subscribers */
orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
+ _mag_read++;
+
/* stop the perf counter */
perf_end(_mag_sample_perf);
}
@@ -1304,6 +1392,8 @@ LSM303D::mag_measure()
void
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);
@@ -1466,7 +1556,7 @@ test()
/* check if mag is onboard or external */
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
errx(1, "failed to get if mag is onboard or external");
- warnx("device active: %s", ret ? "external" : "onboard");
+ warnx("mag device active: %s", ret ? "external" : "onboard");
/* do a simple demand read */
sz = read(fd_mag, &m_report, sizeof(m_report));
@@ -1484,7 +1574,7 @@ test()
/* XXX add poll-rate tests here too */
-// reset();
+ reset();
errx(0, "PASS");
}
@@ -1503,7 +1593,17 @@ reset()
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
- err(1, "driver poll restart failed");
+ err(1, "accel pollrate reset failed");
+
+ fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+ if (fd < 0) {
+ warnx("mag could not be opened, external mag might be used");
+ } else {
+ /* no need to reset the mag as well, the reset() is the same */
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+ err(1, "mag pollrate reset failed");
+ }
exit(0);
}
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index bfc74c73e..0e65923db 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -149,6 +149,15 @@
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
+#define MPU6000_ACCEL_DEFAULT_RANGE_G 8
+#define MPU6000_ACCEL_DEFAULT_RATE 1000
+#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define MPU6000_GYRO_DEFAULT_RANGE_G 8
+#define MPU6000_GYRO_DEFAULT_RATE 1000
+#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42
class MPU6000_gyro;
@@ -357,12 +366,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_reads(0),
_sample_rate(1000),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
- _accel_filter_x(1000, 30),
- _accel_filter_y(1000, 30),
- _accel_filter_z(1000, 30),
- _gyro_filter_x(1000, 30),
- _gyro_filter_y(1000, 30),
- _gyro_filter_z(1000, 30)
+ _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+ _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
+ _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ)
{
// disable debug() calls
_debug_enabled = false;
@@ -485,14 +494,13 @@ void MPU6000::reset()
up_udelay(1000);
// SAMPLE RATE
- //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
- _set_sample_rate(_sample_rate); // default sample rate = 200Hz
+ _set_sample_rate(_sample_rate);
usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the
// system response
- _set_dlpf_filter(42);
+ _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
usleep(1000);
// Gyro scale 2000 deg/s ()
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -535,8 +543,8 @@ void MPU6000::reset()
// Correct accel scale factors of 4096 LSB/g
// scale to m/s^2 ( 1g = 9.81 m/s^2)
- _accel_range_scale = (9.81f / 4096.0f);
- _accel_range_m_s2 = 8.0f * 9.81f;
+ _accel_range_scale = (CONSTANTS_ONE_G / 4096.0f);
+ _accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G;
usleep(1000);
@@ -777,9 +785,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
+ return ioctl(filp, SENSORIOCSPOLLRATE, 1000);
+
case SENSOR_POLLRATE_DEFAULT:
- /* set to same as sample rate per default */
- return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate);
+ return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
@@ -867,9 +876,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// XXX decide on relationship of both filters
// i.e. disable the on-chip filter
//_set_dlpf_filter((uint16_t)arg);
- _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
- _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
- _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+ _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+ _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
+ _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
return OK;
case ACCELIOCSSCALE:
@@ -897,7 +906,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// _accel_range_m_s2 = 8.0f * 9.81f;
return -EINVAL;
case ACCELIOCGRANGE:
- return _accel_range_m_s2;
+ return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
case ACCELIOCSELFTEST:
return accel_self_test();
@@ -920,28 +929,28 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
return ioctl(filp, cmd, arg);
case SENSORIOCSQUEUEDEPTH: {
- /* lower bound is mandatory, upper bound is a sanity check */
- if ((arg < 1) || (arg > 100))
- return -EINVAL;
-
- /* allocate new buffer */
- GyroReportBuffer *buf = new GyroReportBuffer(arg);
-
- if (nullptr == buf)
- return -ENOMEM;
- if (buf->size() == 0) {
- delete buf;
- return -ENOMEM;
- }
+ /* lower bound is mandatory, upper bound is a sanity check */
+ if ((arg < 1) || (arg > 100))
+ return -EINVAL;
+
+ /* allocate new buffer */
+ GyroReportBuffer *buf = new GyroReportBuffer(arg);
+
+ if (nullptr == buf)
+ return -ENOMEM;
+ if (buf->size() == 0) {
+ delete buf;
+ return -ENOMEM;
+ }
- /* reset the measurement state machine with the new buffer, free the old */
- stop();
- delete _gyro_reports;
- _gyro_reports = buf;
- start();
+ /* reset the measurement state machine with the new buffer, free the old */
+ stop();
+ delete _gyro_reports;
+ _gyro_reports = buf;
+ start();
- return OK;
- }
+ return OK;
+ }
case SENSORIOCGQUEUEDEPTH:
return _gyro_reports->size();
@@ -980,7 +989,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
// _gyro_range_rad_s = xx
return -EINVAL;
case GYROIOCGRANGE:
- return _gyro_range_rad_s;
+ return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
case GYROIOCSELFTEST:
return gyro_self_test();
@@ -1400,7 +1409,7 @@ test()
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
- (double)(a_report.range_m_s2 / 9.81f));
+ (double)(a_report.range_m_s2 / CONSTANTS_ONE_G));
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index b1712e85e..21847f11b 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -193,6 +193,11 @@ public:
void print_status();
/**
+ * Disable RC input handling
+ */
+ int disable_rc_handling();
+
+ /**
* Set the DSM VCC is controlled by relay one flag
*
* @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
@@ -223,7 +228,8 @@ private:
unsigned _max_relays; ///<Maximum relays supported by PX4IO
unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO
- unsigned _update_interval; ///<Subscription interval limiting send rate
+ unsigned _update_interval; ///< Subscription interval limiting send rate
+ bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
volatile int _task; ///<worker task id
volatile bool _task_should_exit; ///<worker terminate flag
@@ -294,6 +300,11 @@ private:
int io_get_status();
/**
+ * Disable RC input handling
+ */
+ int io_disable_rc_handling();
+
+ /**
* Fetch RC inputs from IO.
*
* @param input_rc Input structure to populate.
@@ -411,6 +422,7 @@ PX4IO::PX4IO(device::Device *interface) :
_max_relays(0),
_max_transfer(16), /* sensible default */
_update_interval(0),
+ _rc_handling_disabled(false),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
@@ -615,12 +627,16 @@ PX4IO::init()
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0);
- /* publish RC config to IO */
- ret = io_set_rc_config();
- if (ret != OK) {
- log("failed to update RC input config");
- mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
- return ret;
+ if (_rc_handling_disabled) {
+ ret = io_disable_rc_handling();
+ } else {
+ /* publish RC config to IO */
+ ret = io_set_rc_config();
+ if (ret != OK) {
+ log("failed to update RC input config");
+ mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
+ return ret;
+ }
}
}
@@ -916,6 +932,21 @@ PX4IO::io_set_arming_state()
}
int
+PX4IO::disable_rc_handling()
+{
+ return io_disable_rc_handling();
+}
+
+int
+PX4IO::io_disable_rc_handling()
+{
+ uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED;
+ uint16_t clear = 0;
+
+ return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
+}
+
+int
PX4IO::io_set_rc_config()
{
unsigned offset = 0;
@@ -1456,7 +1487,7 @@ PX4IO::print_status()
(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
- ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""),
+ ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""),
((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"),
@@ -1866,6 +1897,16 @@ start(int argc, char *argv[])
errx(1, "driver init failed");
}
+ /* disable RC handling on request */
+ if (argc > 0 && !strcmp(argv[0], "norc")) {
+
+ if(g_dev->disable_rc_handling())
+ warnx("Failed disabling RC handling");
+
+ } else {
+ warnx("unknown argument: %s", argv[0]);
+ }
+
int dsm_vcc_ctl;
if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index bfccb2d38..5d3d6a73c 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink
SRCS += mavlink.c \
missionlib.c \
mavlink_parameters.c \
- mavlink_log.c \
mavlink_receiver.cpp \
orb_listener.c \
waypoints.c
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 38727c68c..deed25836 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -148,7 +148,8 @@ mixer_tick(void)
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
- (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+ !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 97809d9c2..f5fa0ba75 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -74,7 +74,7 @@
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
-#define PX4IO_PROTOCOL_VERSION 3
+#define PX4IO_PROTOCOL_VERSION 4
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
@@ -159,6 +159,7 @@
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
+#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 3ff9307cd..655a0c7a8 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -158,9 +158,10 @@ volatile uint16_t r_page_setup[] =
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
- PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \
+ PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
- PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE
+ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
+ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -432,6 +433,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
// r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
// }
+ if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
+ }
+
r_setup_arming = value;
break;
@@ -523,6 +528,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
+ /* clear any existing RC disabled flag */
+ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
+
/* set all options except the enabled option */
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b237bd059..ded39c465 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -912,11 +912,11 @@ Sensors::gyro_init()
#else
- /* set the gyro internal sampling rate up to at leat 800Hz */
- ioctl(fd, GYROIOCSSAMPLERATE, 800);
+ /* set the gyro internal sampling rate up to at least 760Hz */
+ ioctl(fd, GYROIOCSSAMPLERATE, 760);
- /* set the driver to poll at 800Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, 800);
+ /* set the driver to poll at 760Hz */
+ ioctl(fd, SENSORIOCSPOLLRATE, 760);
#endif
@@ -929,6 +929,7 @@ void
Sensors::mag_init()
{
int fd;
+ int ret;
fd = open(MAG_DEVICE_PATH, 0);
@@ -937,13 +938,27 @@ Sensors::mag_init()
errx(1, "FATAL: no magnetometer found");
}
- /* set the mag internal poll rate to at least 150Hz */
- ioctl(fd, MAGIOCSSAMPLERATE, 150);
+ /* try different mag sampling rates */
- /* set the driver to poll at 150Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, 150);
- int ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+ ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+ if (ret == OK) {
+ /* set the pollrate accordingly */
+ ioctl(fd, SENSORIOCSPOLLRATE, 150);
+ } else {
+ ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+ /* if the slower sampling rate still fails, something is wrong */
+ if (ret == OK) {
+ /* set the driver to poll also at the slower rate */
+ ioctl(fd, SENSORIOCSPOLLRATE, 100);
+ } else {
+ errx(1, "FATAL: mag sampling rate could not be set");
+ }
+ }
+
+
+
+ ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
if (ret < 0)
errx(1, "FATAL: unknown if magnetometer is external or onboard");
else if (ret == 1)
diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c
index 192195856..27608bdbf 100644
--- a/src/modules/mavlink/mavlink_log.c
+++ b/src/modules/systemlib/mavlink_log.c
@@ -46,28 +46,25 @@
#include <mavlink/mavlink_log.h>
-static FILE* text_recorder_fd = NULL;
-
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
+__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
{
lb->size = size;
lb->start = 0;
lb->count = 0;
lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
- text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w");
}
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
+__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
{
return lb->count == (int)lb->size;
}
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
+__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
{
return lb->count == 0;
}
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
+__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
{
int end = (lb->start + lb->count) % lb->size;
memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
@@ -80,19 +77,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
}
}
-int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
+__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
{
if (!mavlink_logbuffer_is_empty(lb)) {
memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
lb->start = (lb->start + 1) % lb->size;
--lb->count;
- if (text_recorder_fd) {
- fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd);
- fputc("\n", text_recorder_fd);
- fsync(text_recorder_fd);
- }
-
return 0;
} else {
@@ -100,7 +91,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa
}
}
-void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
+__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index b470c1227..cbf829122 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -48,4 +48,5 @@ SRCS = err.c \
geo/geo.c \
systemlib.c \
airspeed.c \
- system_params.c
+ system_params.c \
+ mavlink_log.c
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 5a02fd620..188dafa4e 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -2,6 +2,7 @@
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
+ * Author: Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,6 +36,7 @@
/**
* @file config.c
* @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
*
* config tool.
*/
@@ -69,27 +71,15 @@ config_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "gyro")) {
- if (argc >= 3) {
- do_gyro(argc - 2, argv + 2);
- } else {
- errx(1, "not enough parameters.");
- }
+ do_gyro(argc - 2, argv + 2);
}
if (!strcmp(argv[1], "accel")) {
- if (argc >= 3) {
- do_accel(argc - 2, argv + 2);
- } else {
- errx(1, "not enough parameters.");
- }
+ do_accel(argc - 2, argv + 2);
}
if (!strcmp(argv[1], "mag")) {
- if (argc >= 3) {
- do_mag(argc - 2, argv + 2);
- } else {
- errx(1, "not enough parameters.");
- }
+ do_mag(argc - 2, argv + 2);
}
}
@@ -100,6 +90,7 @@ static void
do_gyro(int argc, char *argv[])
{
int fd;
+ int ret;
fd = open(GYRO_DEVICE_PATH, 0);
@@ -109,44 +100,45 @@ do_gyro(int argc, char *argv[])
} else {
- if (argc >= 2) {
+ if (argc == 2 && !strcmp(argv[0], "sampling")) {
- char* end;
- int i = strtol(argv[1],&end,10);
+ /* set the gyro internal sampling rate up to at least i Hz */
+ ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
- if (!strcmp(argv[0], "sampling")) {
+ if (ret)
+ errx(ret,"sampling rate could not be set");
- /* set the accel internal sampling rate up to at leat i Hz */
- ioctl(fd, GYROIOCSSAMPLERATE, i);
+ } else if (argc == 2 && !strcmp(argv[0], "rate")) {
- } else if (!strcmp(argv[0], "rate")) {
+ /* set the driver to poll at i Hz */
+ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
- /* set the driver to poll at i Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, i);
- } else if (!strcmp(argv[0], "range")) {
+ if (ret)
+ errx(ret,"pollrate could not be set");
- /* set the range to i dps */
- ioctl(fd, GYROIOCSRANGE, i);
- }
+ } else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+ /* set the range to i dps */
+ ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
- } else if (argc > 0) {
+ if (ret)
+ errx(ret,"range could not be set");
- if(!strcmp(argv[0], "check")) {
- int ret = ioctl(fd, GYROIOCSELFTEST, 0);
+ } else if(argc == 1 && !strcmp(argv[0], "check")) {
+ ret = ioctl(fd, GYROIOCSELFTEST, 0);
- if (ret) {
- warnx("gyro self test FAILED! Check calibration:");
- struct gyro_scale scale;
- ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
- warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
- warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
- } else {
- warnx("gyro calibration and self test OK");
- }
+ if (ret) {
+ warnx("gyro self test FAILED! Check calibration:");
+ struct gyro_scale scale;
+ ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
+ warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+ warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+ } else {
+ warnx("gyro calibration and self test OK");
}
} else {
- warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
+ errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
}
int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
@@ -165,6 +157,7 @@ static void
do_mag(int argc, char *argv[])
{
int fd;
+ int ret;
fd = open(MAG_DEVICE_PATH, 0);
@@ -174,31 +167,52 @@ do_mag(int argc, char *argv[])
} else {
- if (argc > 0) {
+ if (argc == 2 && !strcmp(argv[0], "sampling")) {
+
+ /* set the mag internal sampling rate up to at least i Hz */
+ ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
+
+ if (ret)
+ errx(ret,"sampling rate could not be set");
+
+ } else if (argc == 2 && !strcmp(argv[0], "rate")) {
- if (!strcmp(argv[0], "check")) {
- int ret = ioctl(fd, MAGIOCSELFTEST, 0);
+ /* set the driver to poll at i Hz */
+ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
- if (ret) {
- warnx("mag self test FAILED! Check calibration.");
- struct mag_scale scale;
- ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
- warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
- warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
- } else {
- warnx("mag calibration and self test OK");
- }
+ if (ret)
+ errx(ret,"pollrate could not be set");
+
+ } else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+ /* set the range to i G */
+ ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+ if (ret)
+ errx(ret,"range could not be set");
+
+ } else if(argc == 1 && !strcmp(argv[0], "check")) {
+ ret = ioctl(fd, MAGIOCSELFTEST, 0);
+
+ if (ret) {
+ warnx("mag self test FAILED! Check calibration:");
+ struct mag_scale scale;
+ ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
+ warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+ warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+ } else {
+ warnx("mag calibration and self test OK");
}
} else {
- warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t");
+ errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t");
}
- int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0);
+ int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
- int range = -1;//ioctl(fd, MAGIOCGRANGE, 0);
+ int range = ioctl(fd, MAGIOCGRANGE, 0);
- warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range);
+ warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range);
close(fd);
}
@@ -210,6 +224,7 @@ static void
do_accel(int argc, char *argv[])
{
int fd;
+ int ret;
fd = open(ACCEL_DEVICE_PATH, 0);
@@ -219,50 +234,52 @@ do_accel(int argc, char *argv[])
} else {
- if (argc >= 2) {
+ if (argc == 2 && !strcmp(argv[0], "sampling")) {
- char* end;
- int i = strtol(argv[1],&end,10);
+ /* set the accel internal sampling rate up to at least i Hz */
+ ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
- if (!strcmp(argv[0], "sampling")) {
+ if (ret)
+ errx(ret,"sampling rate could not be set");
- /* set the accel internal sampling rate up to at leat i Hz */
- ioctl(fd, ACCELIOCSSAMPLERATE, i);
+ } else if (argc == 2 && !strcmp(argv[0], "rate")) {
- } else if (!strcmp(argv[0], "rate")) {
+ /* set the driver to poll at i Hz */
+ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
- /* set the driver to poll at i Hz */
- ioctl(fd, SENSORIOCSPOLLRATE, i);
- } else if (!strcmp(argv[0], "range")) {
+ if (ret)
+ errx(ret,"pollrate could not be set");
- /* set the range to i dps */
- ioctl(fd, ACCELIOCSRANGE, i);
- }
- } else if (argc > 0) {
-
- if (!strcmp(argv[0], "check")) {
- int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
-
- if (ret) {
- warnx("accel self test FAILED! Check calibration.");
- struct accel_scale scale;
- ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
- warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
- warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
- } else {
- warnx("accel calibration and self test OK");
- }
+ } else if (argc == 2 && !strcmp(argv[0], "range")) {
+
+ /* set the range to i G */
+ ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
+
+ if (ret)
+ errx(ret,"range could not be set");
+
+ } else if(argc == 1 && !strcmp(argv[0], "check")) {
+ ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+ if (ret) {
+ warnx("accel self test FAILED! Check calibration:");
+ struct accel_scale scale;
+ ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
+ warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
+ warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+ } else {
+ warnx("accel calibration and self test OK");
}
} else {
- warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t");
+ errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t");
}
int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, ACCELIOCGRANGE, 0);
- warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range);
+ warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
close(fd);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-03 19:48:06 +0000