mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 00:31:36 +08:00
Merged fmuv2_bringup
This commit is contained in:
Lorenz Meier
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
Executable
+5
@@ -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
|
||||
@@ -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
|
||||
+1
-1
@@ -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)
|
||||
|
||||
+17
-8
@@ -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 */
|
||||
|
||||
@@ -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
|
||||
@@ -191,6 +191,11 @@ private:
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* Reset the device
|
||||
*/
|
||||
int reset();
|
||||
|
||||
/**
|
||||
* Perform the on-sensor scale calibration routine.
|
||||
*
|
||||
@@ -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;
|
||||
|
||||
/* external signalling (DRDY) not supported */
|
||||
case SENSOR_POLLRATE_EXTERNAL:
|
||||
|
||||
/* 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 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();
|
||||
|
||||
/* switching to manual polling */
|
||||
case SENSOR_POLLRATE_MANUAL:
|
||||
stop();
|
||||
_measure_ticks = 0;
|
||||
return OK;
|
||||
}
|
||||
|
||||
/* external signalling (DRDY) not supported */
|
||||
case SENSOR_POLLRATE_EXTERNAL:
|
||||
/* adjust to a legal polling interval in Hz */
|
||||
default: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* zero would be bad */
|
||||
case 0:
|
||||
return -EINVAL;
|
||||
/* convert hz to tick interval via microseconds */
|
||||
unsigned ticks = USEC2TICK(1000000 / arg);
|
||||
|
||||
/* 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);
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
|
||||
return -EINVAL;
|
||||
|
||||
/* set interval for next measurement to minimum legal value */
|
||||
_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/* 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);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
|
||||
return -EINVAL;
|
||||
|
||||
/* update interval for next measurement */
|
||||
_measure_ticks = ticks;
|
||||
|
||||
/* 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
|
||||
|
||||
@@ -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;
|
||||
@@ -210,6 +215,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.
|
||||
@@ -273,6 +283,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
|
||||
*
|
||||
@@ -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;
|
||||
@@ -682,6 +684,14 @@ L3GD20::set_samplerate(unsigned frequency)
|
||||
return OK;
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
@@ -701,6 +711,30 @@ L3GD20::stop()
|
||||
hrt_cancel(&_call);
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
+298
-198
File diff suppressed because it is too large
Load Diff
@@ -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,29 +929,29 @@ 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;
|
||||
/* 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);
|
||||
/* allocate new buffer */
|
||||
GyroReportBuffer *buf = new GyroReportBuffer(arg);
|
||||
|
||||
if (nullptr == buf)
|
||||
return -ENOMEM;
|
||||
if (buf->size() == 0) {
|
||||
delete buf;
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* reset the measurement state machine with the new buffer, free the old */
|
||||
stop();
|
||||
delete _gyro_reports;
|
||||
_gyro_reports = buf;
|
||||
start();
|
||||
|
||||
return OK;
|
||||
if (nullptr == buf)
|
||||
return -ENOMEM;
|
||||
if (buf->size() == 0) {
|
||||
delete buf;
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* reset the measurement state machine with the new buffer, free the old */
|
||||
stop();
|
||||
delete _gyro_reports;
|
||||
_gyro_reports = buf;
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case SENSORIOCGQUEUEDEPTH:
|
||||
return _gyro_reports->size();
|
||||
|
||||
@@ -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));
|
||||
|
||||
@@ -192,6 +192,11 @@ public:
|
||||
*/
|
||||
void print_status();
|
||||
|
||||
/**
|
||||
* Disable RC input handling
|
||||
*/
|
||||
int disable_rc_handling();
|
||||
|
||||
/**
|
||||
* Set the DSM VCC is controlled by relay one flag
|
||||
*
|
||||
@@ -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
|
||||
@@ -293,6 +299,11 @@ private:
|
||||
*/
|
||||
int io_get_status();
|
||||
|
||||
/**
|
||||
* Disable RC input handling
|
||||
*/
|
||||
int io_disable_rc_handling();
|
||||
|
||||
/**
|
||||
* Fetch RC inputs from IO.
|
||||
*
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -915,6 +931,21 @@ PX4IO::io_set_arming_state()
|
||||
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
@@ -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) {
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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);
|
||||
@@ -48,4 +48,5 @@ SRCS = err.c \
|
||||
geo/geo.c \
|
||||
systemlib.c \
|
||||
airspeed.c \
|
||||
system_params.c
|
||||
system_params.c \
|
||||
mavlink_log.c
|
||||
|
||||
+101
-84
@@ -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")) {
|
||||
|
||||
} else if (argc > 0) {
|
||||
/* set the range to i dps */
|
||||
ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
if(!strcmp(argv[0], "check")) {
|
||||
int ret = ioctl(fd, GYROIOCSELFTEST, 0);
|
||||
if (ret)
|
||||
errx(ret,"range could not be set");
|
||||
|
||||
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 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");
|
||||
}
|
||||
|
||||
} 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")) {
|
||||
|
||||
if (!strcmp(argv[0], "check")) {
|
||||
int ret = ioctl(fd, MAGIOCSELFTEST, 0);
|
||||
/* set the mag internal sampling rate up to at least i Hz */
|
||||
ret = ioctl(fd, MAGIOCSSAMPLERATE, 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,"sampling rate could not be set");
|
||||
|
||||
} else if (argc == 2 && !strcmp(argv[0], "rate")) {
|
||||
|
||||
/* set the driver to poll at i Hz */
|
||||
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
|
||||
|
||||
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) {
|
||||
} else if (argc == 2 && !strcmp(argv[0], "range")) {
|
||||
|
||||
if (!strcmp(argv[0], "check")) {
|
||||
int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
|
||||
/* set the range to i G */
|
||||
ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 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");
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user