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

Merge pull request #12 from cvg/fmuv2_bringup_lsm303d_config 

Fmuv2 bringup lsm303d config
This commit is contained in:
Lorenz Meier 2013-08-21 06:10:33 -07:00
parent 1c371a6cf8 4f51f333a9
commit db1229dca3
7 changed files with 646 additions and 441 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/drivers/drv_mag.h
View File

@ -90,28 +90,37 @@ ORB_DECLARE(sensor_mag);
/** set the mag internal sample rate to at least (arg) Hz */ /** set the mag internal sample rate to at least (arg) Hz */
#define MAGIOCSSAMPLERATE _MAGIOC(0) #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 */ /** 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) */ /** 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) */ /** 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 */ /** 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 */ /** perform self-calibration, update scale factors to canonical units */
#define MAGIOCCALIBRATE _MAGIOC(5) #define MAGIOCCALIBRATE _MAGIOC(8)
/** excite strap */ /** excite strap */
#define MAGIOCEXSTRAP _MAGIOC(6) #define MAGIOCEXSTRAP _MAGIOC(9)
/** perform self test and report status */ /** perform self test and report status */
#define MAGIOCSELFTEST _MAGIOC(7) #define MAGIOCSELFTEST _MAGIOC(10)
/** determine if mag is external or onboard */ /** determine if mag is external or onboard */
#define MAGIOCGEXTERNAL _MAGIOC(8) #define MAGIOCGEXTERNAL _MAGIOC(11)
#endif /* _DRV_MAG_H */ #endif /* _DRV_MAG_H */

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

@ -77,8 +77,8 @@
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
/* Max measurement rate is 160Hz */ /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */
#define ADDR_CONF_A 0x00 #define ADDR_CONF_A 0x00
#define ADDR_CONF_B 0x01 #define ADDR_CONF_B 0x01
@ -191,6 +191,11 @@ private:
*/ */
void stop(); void stop();
/**
* Reset the device
*/
int reset();
/** /**
* Perform the on-sensor scale calibration routine. * Perform the on-sensor scale calibration routine.
* *
@ -365,6 +370,9 @@ HMC5883::init()
if (I2C::init() != OK) if (I2C::init() != OK)
goto out; goto out;
/* reset the device configuration */
reset();
/* allocate basic report buffers */ /* allocate basic report buffers */
_num_reports = 2; _num_reports = 2;
_reports = new struct mag_report[_num_reports]; _reports = new struct mag_report[_num_reports];
@ -381,9 +389,6 @@ HMC5883::init()
if (_mag_topic < 0) if (_mag_topic < 0)
debug("failed to create sensor_mag object"); debug("failed to create sensor_mag object");
/* set range */
set_range(_range_ga);
ret = OK; ret = OK;
/* sensor is ok, but not calibrated */ /* sensor is ok, but not calibrated */
_sensor_ok = true; _sensor_ok = true;
@ -542,68 +547,67 @@ int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{ {
switch (cmd) { switch (cmd) {
case SENSORIOCSPOLLRATE: { 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; return OK;
}
/* external signalling (DRDY) not supported */ /* adjust to a legal polling interval in Hz */
case SENSOR_POLLRATE_EXTERNAL: default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* zero would be bad */ /* convert hz to tick interval via microseconds */
case 0: unsigned ticks = USEC2TICK(1000000 / arg);
return -EINVAL;
/* set default/max polling rate */ /* check against maximum rate */
case SENSOR_POLLRATE_MAX: if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
case SENSOR_POLLRATE_DEFAULT: { return -EINVAL;
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */ /* update interval for next measurement */
_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); _measure_ticks = ticks;
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) if (want_start)
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);
/* 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;
}
} }
} }
}
case SENSORIOCGPOLLRATE: case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL; return SENSOR_POLLRATE_MANUAL;
return (1000 / _measure_ticks); return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */ /* 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; return _num_reports - 1;
case SENSORIOCRESET: case SENSORIOCRESET:
/* XXX implement this */ return reset();
return -EINVAL;
case MAGIOCSSAMPLERATE: case MAGIOCSSAMPLERATE:
/* not supported, always 1 sample per poll */ /* same as pollrate because device is in single measurement mode*/
return -EINVAL; return ioctl(filp, SENSORIOCSPOLLRATE, arg);
case MAGIOCGSAMPLERATE:
/* same as pollrate because device is in single measurement mode*/
return 1000000/TICK2USEC(_measure_ticks);
case MAGIOCSRANGE: case MAGIOCSRANGE:
return set_range(arg); return set_range(arg);
case MAGIOCGRANGE:
return _range_ga;
case MAGIOCSLOWPASS: case MAGIOCSLOWPASS:
case MAGIOCGLOWPASS:
/* not supported, no internal filtering */ /* not supported, no internal filtering */
return -EINVAL; return -EINVAL;
@ -697,6 +708,13 @@ HMC5883::stop()
work_cancel(HPWORK, &_work); work_cancel(HPWORK, &_work);
} }
int
HMC5883::reset()
{
/* set range */
return set_range(_range_ga);
}
void void
HMC5883::cycle_trampoline(void *arg) HMC5883::cycle_trampoline(void *arg)
{ {
@ -861,10 +879,10 @@ HMC5883::collect()
} else { } else {
#endif #endif
/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, /* 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; _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 */ /* 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 */ /* z remains z */
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
#ifdef PX4_I2C_BUS_ONBOARD #ifdef PX4_I2C_BUS_ONBOARD

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

@ -154,6 +154,10 @@ static const int ERROR = -1;
#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) #define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5)
#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) #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[]); } extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI class L3GD20 : public device::SPI
@ -191,9 +195,10 @@ private:
orb_advert_t _gyro_topic; orb_advert_t _gyro_topic;
unsigned _current_rate; unsigned _current_rate;
unsigned _current_range;
unsigned _orientation; unsigned _orientation;
unsigned _read;
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_x;
@ -210,6 +215,11 @@ private:
*/ */
void stop(); void stop();
/**
* Reset the driver
*/
void reset();
/** /**
* Static trampoline from the hrt_call context; because we don't have a * Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet. * generic hrt wrapper yet.
@ -273,6 +283,14 @@ private:
*/ */
int set_samplerate(unsigned frequency); 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 * Self test
* *
@ -296,12 +314,12 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
_gyro_range_rad_s(0.0f), _gyro_range_rad_s(0.0f),
_gyro_topic(-1), _gyro_topic(-1),
_current_rate(0), _current_rate(0),
_current_range(0),
_orientation(SENSOR_BOARD_ROTATION_270_DEG), _orientation(SENSOR_BOARD_ROTATION_270_DEG),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _read(0),
_gyro_filter_x(250, 30), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
_gyro_filter_y(250, 30), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_z(250, 30) _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
{ {
// enable debug() calls // enable debug() calls
_debug_enabled = true; _debug_enabled = true;
@ -349,22 +367,7 @@ L3GD20::init()
memset(&_reports[0], 0, sizeof(_reports[0])); memset(&_reports[0], 0, sizeof(_reports[0]));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
/* set default configuration */ reset();
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 */
ret = OK; ret = OK;
out: out:
@ -464,8 +467,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */ /* set default/max polling rate */
case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: case SENSOR_POLLRATE_DEFAULT:
/* With internal low pass filters enabled, 250 Hz is sufficient */ return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
return ioctl(filp, SENSORIOCSPOLLRATE, 250);
/* adjust to a legal polling interval in Hz */ /* adjust to a legal polling interval in Hz */
default: { 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... */ /* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks; _call.period = _call_interval = ticks;
// adjust filters /* adjust filters */
float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f/ticks; float sample_rate = 1.0e6f/ticks;
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); set_driver_lowpass_filter(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);
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) if (want_start)
@ -533,8 +533,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return _num_reports - 1; return _num_reports - 1;
case SENSORIOCRESET: case SENSORIOCRESET:
/* XXX implement */ reset();
return -EINVAL; return OK;
case GYROIOCSSAMPLERATE: case GYROIOCSSAMPLERATE:
return set_samplerate(arg); return set_samplerate(arg);
@ -543,16 +543,15 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return _current_rate; return _current_rate;
case GYROIOCSLOWPASS: { case GYROIOCSLOWPASS: {
float cutoff_freq_hz = arg; float cutoff_freq_hz = arg;
float sample_rate = 1.0e6f / _call_interval; float sample_rate = 1.0e6f / _call_interval;
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); set_driver_lowpass_filter(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;
return OK; }
}
case GYROIOCGLOWPASS: case GYROIOCGLOWPASS:
return _gyro_filter_x.get_cutoff_freq(); return _gyro_filter_x.get_cutoff_freq();
case GYROIOCSSCALE: case GYROIOCSSCALE:
/* copy scale in */ /* copy scale in */
@ -565,10 +564,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return OK; return OK;
case GYROIOCSRANGE: case GYROIOCSRANGE:
/* arg should be in dps */
return set_range(arg); return set_range(arg);
case GYROIOCGRANGE: 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: case GYROIOCSELFTEST:
return self_test(); return self_test();
@ -618,22 +619,23 @@ L3GD20::set_range(unsigned max_dps)
{ {
uint8_t bits = REG4_BDU; uint8_t bits = REG4_BDU;
float new_range_scale_dps_digit; float new_range_scale_dps_digit;
float new_range;
if (max_dps == 0) { if (max_dps == 0) {
max_dps = 2000; max_dps = 2000;
} }
if (max_dps <= 250) { if (max_dps <= 250) {
_current_range = 250; new_range = 250;
bits |= RANGE_250DPS; bits |= RANGE_250DPS;
new_range_scale_dps_digit = 8.75e-3f; new_range_scale_dps_digit = 8.75e-3f;
} else if (max_dps <= 500) { } else if (max_dps <= 500) {
_current_range = 500; new_range = 500;
bits |= RANGE_500DPS; bits |= RANGE_500DPS;
new_range_scale_dps_digit = 17.5e-3f; new_range_scale_dps_digit = 17.5e-3f;
} else if (max_dps <= 2000) { } else if (max_dps <= 2000) {
_current_range = 2000; new_range = 2000;
bits |= RANGE_2000DPS; bits |= RANGE_2000DPS;
new_range_scale_dps_digit = 70e-3f; new_range_scale_dps_digit = 70e-3f;
@ -641,7 +643,7 @@ L3GD20::set_range(unsigned max_dps)
return -EINVAL; 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; _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
write_reg(ADDR_CTRL_REG4, bits); write_reg(ADDR_CTRL_REG4, bits);
@ -656,7 +658,7 @@ L3GD20::set_samplerate(unsigned frequency)
if (frequency == 0) if (frequency == 0)
frequency = 760; frequency = 760;
// use limits good for H or non-H models /* use limits good for H or non-H models */
if (frequency <= 100) { if (frequency <= 100) {
_current_rate = 95; _current_rate = 95;
bits |= RATE_95HZ_LP_25HZ; bits |= RATE_95HZ_LP_25HZ;
@ -682,6 +684,14 @@ L3GD20::set_samplerate(unsigned frequency)
return OK; 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 void
L3GD20::start() L3GD20::start()
{ {
@ -701,6 +711,30 @@ L3GD20::stop()
hrt_cancel(&_call); 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 void
L3GD20::measure_trampoline(void *arg) L3GD20::measure_trampoline(void *arg)
{ {
@ -804,6 +838,8 @@ L3GD20::measure()
if (_gyro_topic > 0) if (_gyro_topic > 0)
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
_read++;
/* stop the perf counter */ /* stop the perf counter */
perf_end(_sample_perf); perf_end(_sample_perf);
} }
@ -811,6 +847,7 @@ L3GD20::measure()
void void
L3GD20::print_info() L3GD20::print_info()
{ {
printf("gyro reads: %u\n", _read);
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n", printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports); _num_reports, _oldest_report, _next_report, _reports);
@ -949,7 +986,7 @@ reset()
err(1, "driver reset failed"); err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed"); err(1, "accel pollrate reset failed");
exit(0); exit(0);
} }

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

@ -54,6 +54,7 @@
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/geo/geo.h>
#include <nuttx/arch.h> #include <nuttx/arch.h>
#include <nuttx/clock.h> #include <nuttx/clock.h>
@ -168,6 +169,14 @@ static const int ERROR = -1;
#define INT_CTRL_M 0x12 #define INT_CTRL_M 0x12
#define INT_SRC_M 0x13 #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[]); } 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); LSM303D(int bus, const char* path, spi_dev_e device);
virtual ~LSM303D(); virtual ~LSM303D();
virtual int init(); virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg); virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/** /**
* Diagnostics - print some basic information about the driver. * Diagnostics - print some basic information about the driver.
*/ */
void print_info(); void print_info();
protected: protected:
virtual int probe(); virtual int probe();
friend class LSM303D_mag; friend class LSM303D_mag;
virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); 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: private:
LSM303D_mag *_mag; LSM303D_mag *_mag;
struct hrt_call _accel_call; struct hrt_call _accel_call;
struct hrt_call _mag_call; struct hrt_call _mag_call;
unsigned _call_accel_interval; unsigned _call_accel_interval;
unsigned _call_mag_interval; unsigned _call_mag_interval;
unsigned _num_accel_reports; unsigned _num_accel_reports;
volatile unsigned _next_accel_report; volatile unsigned _next_accel_report;
volatile unsigned _oldest_accel_report; volatile unsigned _oldest_accel_report;
struct accel_report *_accel_reports; struct accel_report *_accel_reports;
struct accel_scale _accel_scale; unsigned _num_mag_reports;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
unsigned _current_samplerate;
unsigned _num_mag_reports;
volatile unsigned _next_mag_report; volatile unsigned _next_mag_report;
volatile unsigned _oldest_mag_report; volatile unsigned _oldest_mag_report;
struct mag_report *_mag_reports; 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; struct mag_scale _mag_scale;
float _mag_range_scale; unsigned _mag_range_ga;
float _mag_range_ga; float _mag_range_scale;
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
orb_advert_t _mag_topic; orb_advert_t _mag_topic;
unsigned _accel_read;
unsigned _mag_read;
perf_counter_t _accel_sample_perf; perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf; perf_counter_t _mag_sample_perf;
@ -240,12 +254,19 @@ private:
/** /**
* Start automatic measurement. * Start automatic measurement.
*/ */
void start(); void start();
/** /**
* Stop automatic measurement. * 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 * 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. * @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 * Static trampoline for the mag because it runs at a lower rate
* *
* @param arg Instance pointer for the driver that is polling. * @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. * 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. * 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 * Read a register from the LSM303D
@ -281,7 +316,7 @@ private:
* @param The register to read. * @param The register to read.
* @return The value that was 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 * Write a register in the LSM303D
@ -289,7 +324,7 @@ private:
* @param reg The register to write. * @param reg The register to write.
* @param value The new value 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 * Modify a register in the LSM303D
@ -300,7 +335,7 @@ private:
* @param clearbits Bits in the register to clear. * @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set. * @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. * Set the LSM303D accel measurement range.
@ -309,7 +344,7 @@ private:
* Zero selects the maximum supported range. * Zero selects the maximum supported range.
* @return OK if the value can be supported, -ERANGE otherwise. * @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. * Set the LSM303D mag measurement range.
@ -318,24 +353,25 @@ private:
* Zero selects the maximum supported range. * Zero selects the maximum supported range.
* @return OK if the value can be supported, -ERANGE otherwise. * @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 * @param bandwidth The anti-alias filter bandwidth in Hz
* Zero selects the highest bandwidth * Zero selects the highest bandwidth
* @return OK if the value can be supported, -ERANGE otherwise. * @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 * @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. * Set the LSM303D internal accel sampling frequency.
@ -345,7 +381,7 @@ private:
* Zero selects the maximum rate supported. * Zero selects the maximum rate supported.
* @return OK if the value can be 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. * Set the LSM303D internal mag sampling frequency.
@ -355,7 +391,7 @@ private:
* Zero selects the maximum rate supported. * Zero selects the maximum rate supported.
* @return OK if the value can be 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), _next_accel_report(0),
_oldest_accel_report(0), _oldest_accel_report(0),
_accel_reports(nullptr), _accel_reports(nullptr),
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_current_samplerate(0),
_num_mag_reports(0), _num_mag_reports(0),
_next_mag_report(0), _next_mag_report(0),
_oldest_mag_report(0), _oldest_mag_report(0),
_mag_reports(nullptr), _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_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")), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
_accel_filter_x(800, 30), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(800, 30), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(800, 30) _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
{ {
// enable debug() calls // enable debug() calls
_debug_enabled = true; _debug_enabled = true;
// default scale factors // default scale factors
_accel_scale.x_offset = 0; _accel_scale.x_offset = 0.0f;
_accel_scale.x_scale = 1.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.y_scale = 1.0f;
_accel_scale.z_offset = 0; _accel_scale.z_offset = 0.0f;
_accel_scale.z_scale = 1.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.x_scale = 1.0f;
_mag_scale.y_offset = 0; _mag_scale.y_offset = 0.0f;
_mag_scale.y_scale = 1.0f; _mag_scale.y_scale = 1.0f;
_mag_scale.z_offset = 0; _mag_scale.z_offset = 0.0f;
_mag_scale.z_scale = 1.0f; _mag_scale.z_scale = 1.0f;
} }
@ -478,27 +519,12 @@ LSM303D::init()
if (_mag_reports == nullptr) if (_mag_reports == nullptr)
goto out; goto out;
reset();
/* advertise mag topic */ /* advertise mag topic */
memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_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 */ /* do CDev init for the mag device node, keep it optional */
mag_ret = _mag->init(); mag_ret = _mag->init();
@ -511,6 +537,28 @@ out:
return ret; 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 int
LSM303D::probe() LSM303D::probe()
{ {
@ -612,64 +660,55 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) { switch (cmd) {
case SENSORIOCSPOLLRATE: { case SENSORIOCSPOLLRATE: {
switch (arg) { switch (arg) {
/* switching to manual polling */ /* switching to manual polling */
case SENSOR_POLLRATE_MANUAL: case SENSOR_POLLRATE_MANUAL:
stop(); stop();
_call_accel_interval = 0; _call_accel_interval = 0;
return OK; return OK;
/* external signalling not supported */ /* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL: case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */ /* zero would be bad */
case 0: case 0:
return -EINVAL; return -EINVAL;
/* set default/max polling rate */ /* set default/max polling rate */
case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_MAX:
return ioctl(filp, SENSORIOCSPOLLRATE, 1600); return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
case SENSOR_POLLRATE_DEFAULT: case SENSOR_POLLRATE_DEFAULT:
/* With internal low pass filters enabled, 250 Hz is sufficient */ return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
/* XXX for vibration tests with 800 Hz */
return ioctl(filp, SENSORIOCSPOLLRATE, 800);
/* adjust to a legal polling interval in Hz */ /* adjust to a legal polling interval in Hz */
default: { default: {
/* do we need to start internal polling? */ /* do we need to start internal polling? */
bool want_start = (_call_accel_interval == 0); bool want_start = (_call_accel_interval == 0);
/* convert hz to hrt interval via microseconds */ /* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg; unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */ /* check against maximum sane rate */
if (ticks < 1000) if (ticks < 500)
return -EINVAL; return -EINVAL;
/* adjust sample rate of sensor */ /* adjust filters */
set_samplerate(arg); accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
// adjust filters /* update interval for next measurement */
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); /* XXX this is a bit shady, but no other way to adjust... */
float sample_rate = 1.0e6f/ticks; _accel_call.period = _call_accel_interval = 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);
/* update interval for next measurement */ /* if we need to start the poll state machine, do it */
/* XXX this is a bit shady, but no other way to adjust... */ if (want_start)
_accel_call.period = _call_accel_interval = ticks; start();
/* if we need to start the poll state machine, do it */ return OK;
if (want_start)
start();
return OK;
}
} }
} }
}
case SENSORIOCGPOLLRATE: case SENSORIOCGPOLLRATE:
if (_call_accel_interval == 0) if (_call_accel_interval == 0)
@ -678,66 +717,77 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_accel_interval; return 1000000 / _call_accel_interval;
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */ /* account for sentinel in the ring */
arg++; arg++;
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100)) if ((arg < 2) || (arg > 100))
return -EINVAL; return -EINVAL;
/* allocate new buffer */ /* allocate new buffer */
struct accel_report *buf = new struct accel_report[arg]; struct accel_report *buf = new struct accel_report[arg];
if (nullptr == buf) if (nullptr == buf)
return -ENOMEM; return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */ /* reset the measurement state machine with the new buffer, free the old */
stop(); stop();
delete[] _accel_reports; delete[] _accel_reports;
_num_accel_reports = arg; _num_accel_reports = arg;
_accel_reports = buf; _accel_reports = buf;
start(); start();
return OK; return OK;
} }
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _num_accel_reports - 1; return _num_accel_reports - 1;
case SENSORIOCRESET: case SENSORIOCRESET:
/* XXX implement */ reset();
return -EINVAL; return OK;
case ACCELIOCSSAMPLERATE:
return accel_set_samplerate(arg);
case ACCELIOCGSAMPLERATE:
return _accel_samplerate;
case ACCELIOCSLOWPASS: { case ACCELIOCSLOWPASS: {
float cutoff_freq_hz = arg; return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)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;
}
case ACCELIOCGLOWPASS: case ACCELIOCGLOWPASS:
return _accel_filter_x.get_cutoff_freq(); return _accel_filter_x.get_cutoff_freq();
case ACCELIOCSSCALE: case ACCELIOCSSCALE: {
{ /* copy scale, but only if off by a few percent */
/* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg;
struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale;
float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) {
if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale));
memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK;
return OK; } else {
} else { return -EINVAL;
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: case ACCELIOCGSCALE:
/* copy scale out */ /* copy scale out */
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
return OK; return OK;
case ACCELIOCSELFTEST:
return accel_self_test();
default: default:
/* give it to the superclass */ /* give it to the superclass */
return SPI::ioctl(filp, cmd, arg); 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 */ /* set default/max polling rate */
case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: case SENSOR_POLLRATE_DEFAULT:
/* 50 Hz is max for mag */ /* 100 Hz is max for mag */
return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100);
/* adjust to a legal polling interval in Hz */ /* 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) if (ticks < 1000)
return -EINVAL; return -EINVAL;
/* adjust sample rate of sensor */
mag_set_samplerate(arg);
/* update interval for next measurement */ /* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */ /* XXX this is a bit shady, but no other way to adjust... */
_mag_call.period = _call_mag_interval = ticks; _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; return _num_mag_reports - 1;
case SENSORIOCRESET: case SENSORIOCRESET:
return ioctl(filp, cmd, arg); reset();
return OK;
case MAGIOCSSAMPLERATE: case MAGIOCSSAMPLERATE:
// case MAGIOCGSAMPLERATE: return mag_set_samplerate(arg);
/* XXX not implemented */
return -EINVAL; case MAGIOCGSAMPLERATE:
return _mag_samplerate;
case MAGIOCSLOWPASS: case MAGIOCSLOWPASS:
// case MAGIOCGLOWPASS: case MAGIOCGLOWPASS:
/* XXX not implemented */ /* not supported, no internal filtering */
// _set_dlpf_filter((uint16_t)arg);
return -EINVAL; return -EINVAL;
case MAGIOCSSCALE: case MAGIOCSSCALE:
@ -857,17 +905,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
return OK; return OK;
case MAGIOCSRANGE: case MAGIOCSRANGE:
// case MAGIOCGRANGE: return mag_set_range(arg);
/* XXX not implemented */
// XXX change these two values on set: case MAGIOCGRANGE:
// _mag_range_scale = xx return _mag_range_ga;
// _mag_range_ga = xx
return -EINVAL;
case MAGIOCSELFTEST: case MAGIOCSELFTEST:
/* XXX not implemented */ return mag_self_test();
// return self_test();
return -EINVAL;
case MAGIOCGEXTERNAL: case MAGIOCGEXTERNAL:
/* no external mag board yet */ /* 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 uint8_t
LSM303D::read_reg(unsigned reg) LSM303D::read_reg(unsigned reg)
{ {
@ -914,38 +1005,37 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
} }
int int
LSM303D::set_range(unsigned max_g) LSM303D::accel_set_range(unsigned max_g)
{ {
uint8_t setbits = 0; uint8_t setbits = 0;
uint8_t clearbits = REG2_FULL_SCALE_BITS_A; uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
float new_range_g = 0.0f;
float new_scale_g_digit = 0.0f; float new_scale_g_digit = 0.0f;
if (max_g == 0) if (max_g == 0)
max_g = 16; max_g = 16;
if (max_g <= 2) { if (max_g <= 2) {
new_range_g = 2.0f; _accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G;
setbits |= REG2_FULL_SCALE_2G_A; setbits |= REG2_FULL_SCALE_2G_A;
new_scale_g_digit = 0.061e-3f; new_scale_g_digit = 0.061e-3f;
} else if (max_g <= 4) { } 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; setbits |= REG2_FULL_SCALE_4G_A;
new_scale_g_digit = 0.122e-3f; new_scale_g_digit = 0.122e-3f;
} else if (max_g <= 6) { } 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; setbits |= REG2_FULL_SCALE_6G_A;
new_scale_g_digit = 0.183e-3f; new_scale_g_digit = 0.183e-3f;
} else if (max_g <= 8) { } 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; setbits |= REG2_FULL_SCALE_8G_A;
new_scale_g_digit = 0.244e-3f; new_scale_g_digit = 0.244e-3f;
} else if (max_g <= 16) { } 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; setbits |= REG2_FULL_SCALE_16G_A;
new_scale_g_digit = 0.732e-3f; new_scale_g_digit = 0.732e-3f;
@ -953,8 +1043,7 @@ LSM303D::set_range(unsigned max_g)
return -EINVAL; return -EINVAL;
} }
_accel_range_m_s2 = new_range_g * 9.80665f; _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G;
_accel_range_scale = new_scale_g_digit * 9.80665f;
modify_reg(ADDR_CTRL_REG2, clearbits, setbits); modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
@ -966,29 +1055,28 @@ LSM303D::mag_set_range(unsigned max_ga)
{ {
uint8_t setbits = 0; uint8_t setbits = 0;
uint8_t clearbits = REG6_FULL_SCALE_BITS_M; uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
float new_range = 0.0f;
float new_scale_ga_digit = 0.0f; float new_scale_ga_digit = 0.0f;
if (max_ga == 0) if (max_ga == 0)
max_ga = 12; max_ga = 12;
if (max_ga <= 2) { if (max_ga <= 2) {
new_range = 2.0f; _mag_range_ga = 2;
setbits |= REG6_FULL_SCALE_2GA_M; setbits |= REG6_FULL_SCALE_2GA_M;
new_scale_ga_digit = 0.080e-3f; new_scale_ga_digit = 0.080e-3f;
} else if (max_ga <= 4) { } else if (max_ga <= 4) {
new_range = 4.0f; _mag_range_ga = 4;
setbits |= REG6_FULL_SCALE_4GA_M; setbits |= REG6_FULL_SCALE_4GA_M;
new_scale_ga_digit = 0.160e-3f; new_scale_ga_digit = 0.160e-3f;
} else if (max_ga <= 8) { } else if (max_ga <= 8) {
new_range = 8.0f; _mag_range_ga = 8;
setbits |= REG6_FULL_SCALE_8GA_M; setbits |= REG6_FULL_SCALE_8GA_M;
new_scale_ga_digit = 0.320e-3f; new_scale_ga_digit = 0.320e-3f;
} else if (max_ga <= 12) { } else if (max_ga <= 12) {
new_range = 12.0f; _mag_range_ga = 12;
setbits |= REG6_FULL_SCALE_12GA_M; setbits |= REG6_FULL_SCALE_12GA_M;
new_scale_ga_digit = 0.479e-3f; new_scale_ga_digit = 0.479e-3f;
@ -996,7 +1084,6 @@ LSM303D::mag_set_range(unsigned max_ga)
return -EINVAL; return -EINVAL;
} }
_mag_range_ga = new_range;
_mag_range_scale = new_scale_ga_digit; _mag_range_scale = new_scale_ga_digit;
modify_reg(ADDR_CTRL_REG6, clearbits, setbits); modify_reg(ADDR_CTRL_REG6, clearbits, setbits);
@ -1005,7 +1092,7 @@ LSM303D::mag_set_range(unsigned max_ga)
} }
int int
LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
{ {
uint8_t setbits = 0; uint8_t setbits = 0;
uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
@ -1015,15 +1102,19 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
if (bandwidth <= 50) { if (bandwidth <= 50) {
setbits |= REG2_AA_FILTER_BW_50HZ_A; setbits |= REG2_AA_FILTER_BW_50HZ_A;
_accel_onchip_filter_bandwith = 50;
} else if (bandwidth <= 194) { } else if (bandwidth <= 194) {
setbits |= REG2_AA_FILTER_BW_194HZ_A; setbits |= REG2_AA_FILTER_BW_194HZ_A;
_accel_onchip_filter_bandwith = 194;
} else if (bandwidth <= 362) { } else if (bandwidth <= 362) {
setbits |= REG2_AA_FILTER_BW_362HZ_A; setbits |= REG2_AA_FILTER_BW_362HZ_A;
_accel_onchip_filter_bandwith = 362;
} else if (bandwidth <= 773) { } else if (bandwidth <= 773) {
setbits |= REG2_AA_FILTER_BW_773HZ_A; setbits |= REG2_AA_FILTER_BW_773HZ_A;
_accel_onchip_filter_bandwith = 773;
} else { } else {
return -EINVAL; return -EINVAL;
@ -1035,26 +1126,17 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth)
} }
int 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); _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth);
_accel_filter_y.set_cutoff_frequency(samplerate, bandwidth);
if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A) _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth);
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;
return OK; return OK;
} }
int int
LSM303D::set_samplerate(unsigned frequency) LSM303D::accel_set_samplerate(unsigned frequency)
{ {
uint8_t setbits = 0; uint8_t setbits = 0;
uint8_t clearbits = REG1_RATE_BITS_A; uint8_t clearbits = REG1_RATE_BITS_A;
@ -1064,23 +1146,23 @@ LSM303D::set_samplerate(unsigned frequency)
if (frequency <= 100) { if (frequency <= 100) {
setbits |= REG1_RATE_100HZ_A; setbits |= REG1_RATE_100HZ_A;
_current_samplerate = 100; _accel_samplerate = 100;
} else if (frequency <= 200) { } else if (frequency <= 200) {
setbits |= REG1_RATE_200HZ_A; setbits |= REG1_RATE_200HZ_A;
_current_samplerate = 200; _accel_samplerate = 200;
} else if (frequency <= 400) { } else if (frequency <= 400) {
setbits |= REG1_RATE_400HZ_A; setbits |= REG1_RATE_400HZ_A;
_current_samplerate = 400; _accel_samplerate = 400;
} else if (frequency <= 800) { } else if (frequency <= 800) {
setbits |= REG1_RATE_800HZ_A; setbits |= REG1_RATE_800HZ_A;
_current_samplerate = 800; _accel_samplerate = 800;
} else if (frequency <= 1600) { } else if (frequency <= 1600) {
setbits |= REG1_RATE_1600HZ_A; setbits |= REG1_RATE_1600HZ_A;
_current_samplerate = 1600; _accel_samplerate = 1600;
} else { } else {
return -EINVAL; return -EINVAL;
@ -1102,13 +1184,15 @@ LSM303D::mag_set_samplerate(unsigned frequency)
if (frequency <= 25) { if (frequency <= 25) {
setbits |= REG5_RATE_25HZ_M; setbits |= REG5_RATE_25HZ_M;
_mag_samplerate = 25;
} else if (frequency <= 50) { } else if (frequency <= 50) {
setbits |= REG5_RATE_50HZ_M; setbits |= REG5_RATE_50HZ_M;
_mag_samplerate = 50;
} else if (frequency <= 100) { } else if (frequency <= 100) {
setbits |= REG5_RATE_100HZ_M; setbits |= REG5_RATE_100HZ_M;
_mag_samplerate = 100;
} else { } else {
return -EINVAL; return -EINVAL;
@ -1229,6 +1313,8 @@ LSM303D::measure()
/* publish for subscribers */ /* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report);
_accel_read++;
/* stop the perf counter */ /* stop the perf counter */
perf_end(_accel_sample_perf); 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->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->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
mag_report->scaling = _mag_range_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 */ /* post a report to the ring - note, not locked */
INCREMENT(_next_mag_report, _num_mag_reports); INCREMENT(_next_mag_report, _num_mag_reports);
@ -1297,6 +1383,8 @@ LSM303D::mag_measure()
/* publish for subscribers */ /* publish for subscribers */
orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report);
_mag_read++;
/* stop the perf counter */ /* stop the perf counter */
perf_end(_mag_sample_perf); perf_end(_mag_sample_perf);
} }
@ -1304,6 +1392,8 @@ LSM303D::mag_measure()
void void
LSM303D::print_info() LSM303D::print_info()
{ {
printf("accel reads: %u\n", _accel_read);
printf("mag reads: %u\n", _mag_read);
perf_print_counter(_accel_sample_perf); perf_print_counter(_accel_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n", printf("report queue: %u (%u/%u @ %p)\n",
_num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports);
@ -1466,7 +1556,7 @@ test()
/* check if mag is onboard or external */ /* check if mag is onboard or external */
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
errx(1, "failed to get if mag is onboard or external"); 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 */ /* do a simple demand read */
sz = read(fd_mag, &m_report, sizeof(m_report)); sz = read(fd_mag, &m_report, sizeof(m_report));
@ -1484,7 +1574,7 @@ test()
/* XXX add poll-rate tests here too */ /* XXX add poll-rate tests here too */
// reset(); reset();
errx(0, "PASS"); errx(0, "PASS");
} }
@ -1503,7 +1593,17 @@ reset()
err(1, "driver reset failed"); err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) 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); exit(0);
} }

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

@ -149,6 +149,15 @@
#define MPU6000_REV_D9 0x59 #define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A #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; class MPU6000_gyro;
@ -357,12 +366,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_reads(0), _reads(0),
_sample_rate(1000), _sample_rate(1000),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
_accel_filter_x(1000, 30), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(1000, 30), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(1000, 30), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_x(1000, 30), _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(1000, 30), _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(1000, 30) _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ)
{ {
// disable debug() calls // disable debug() calls
_debug_enabled = false; _debug_enabled = false;
@ -485,14 +494,13 @@ void MPU6000::reset()
up_udelay(1000); up_udelay(1000);
// SAMPLE RATE // SAMPLE RATE
//write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz _set_sample_rate(_sample_rate);
_set_sample_rate(_sample_rate); // default sample rate = 200Hz
usleep(1000); usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the // was 90 Hz, but this ruins quality and does not improve the
// system response // system response
_set_dlpf_filter(42); _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
usleep(1000); usleep(1000);
// Gyro scale 2000 deg/s () // Gyro scale 2000 deg/s ()
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@ -535,8 +543,8 @@ void MPU6000::reset()
// Correct accel scale factors of 4096 LSB/g // Correct accel scale factors of 4096 LSB/g
// scale to m/s^2 ( 1g = 9.81 m/s^2) // scale to m/s^2 ( 1g = 9.81 m/s^2)
_accel_range_scale = (9.81f / 4096.0f); _accel_range_scale = (CONSTANTS_ONE_G / 4096.0f);
_accel_range_m_s2 = 8.0f * 9.81f; _accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G;
usleep(1000); usleep(1000);
@ -777,9 +785,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */ /* set default/max polling rate */
case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_MAX:
return ioctl(filp, SENSORIOCSPOLLRATE, 1000);
case SENSOR_POLLRATE_DEFAULT: case SENSOR_POLLRATE_DEFAULT:
/* set to same as sample rate per default */ return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE);
return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate);
/* adjust to a legal polling interval in Hz */ /* adjust to a legal polling interval in Hz */
default: { default: {
@ -867,9 +876,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// XXX decide on relationship of both filters // XXX decide on relationship of both filters
// i.e. disable the on-chip filter // i.e. disable the on-chip filter
//_set_dlpf_filter((uint16_t)arg); //_set_dlpf_filter((uint16_t)arg);
_accel_filter_x.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_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
return OK; return OK;
case ACCELIOCSSCALE: case ACCELIOCSSCALE:
@ -897,7 +906,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// _accel_range_m_s2 = 8.0f * 9.81f; // _accel_range_m_s2 = 8.0f * 9.81f;
return -EINVAL; return -EINVAL;
case ACCELIOCGRANGE: case ACCELIOCGRANGE:
return _accel_range_m_s2; return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
case ACCELIOCSELFTEST: case ACCELIOCSELFTEST:
return accel_self_test(); return accel_self_test();
@ -920,29 +929,29 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
return ioctl(filp, cmd, arg); return ioctl(filp, cmd, arg);
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) if ((arg < 1) || (arg > 100))
return -EINVAL; return -EINVAL;
/* allocate new buffer */ /* allocate new buffer */
GyroReportBuffer *buf = new GyroReportBuffer(arg); GyroReportBuffer *buf = new GyroReportBuffer(arg);
if (nullptr == buf) if (nullptr == buf)
return -ENOMEM; return -ENOMEM;
if (buf->size() == 0) { if (buf->size() == 0) {
delete buf; delete buf;
return -ENOMEM; return -ENOMEM;
}
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _gyro_reports;
_gyro_reports = buf;
start();
return OK;
} }
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _gyro_reports;
_gyro_reports = buf;
start();
return OK;
}
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _gyro_reports->size(); return _gyro_reports->size();
@ -980,7 +989,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
// _gyro_range_rad_s = xx // _gyro_range_rad_s = xx
return -EINVAL; return -EINVAL;
case GYROIOCGRANGE: case GYROIOCGRANGE:
return _gyro_range_rad_s; return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
case GYROIOCSELFTEST: case GYROIOCSELFTEST:
return gyro_self_test(); 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 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 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, 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 */ /* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report)); sz = read(fd_gyro, &g_report, sizeof(g_report));

33
src/modules/sensors/sensors.cpp
View File

@ -919,11 +919,11 @@ Sensors::gyro_init()
#else #else
/* set the gyro internal sampling rate up to at leat 800Hz */ /* set the gyro internal sampling rate up to at least 760Hz */
ioctl(fd, GYROIOCSSAMPLERATE, 800); ioctl(fd, GYROIOCSSAMPLERATE, 760);
/* set the driver to poll at 800Hz */ /* set the driver to poll at 760Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 800); ioctl(fd, SENSORIOCSPOLLRATE, 760);
#endif #endif
@ -936,6 +936,7 @@ void
Sensors::mag_init() Sensors::mag_init()
{ {
int fd; int fd;
int ret;
fd = open(MAG_DEVICE_PATH, 0); fd = open(MAG_DEVICE_PATH, 0);
@ -944,13 +945,27 @@ Sensors::mag_init()
errx(1, "FATAL: no magnetometer found"); errx(1, "FATAL: no magnetometer found");
} }
/* set the mag internal poll rate to at least 150Hz */ /* try different mag sampling rates */
ioctl(fd, MAGIOCSSAMPLERATE, 150);
/* 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) if (ret < 0)
errx(1, "FATAL: unknown if magnetometer is external or onboard"); errx(1, "FATAL: unknown if magnetometer is external or onboard");
else if (ret == 1) else if (ret == 1)

185
src/systemcmds/config/config.c
View File

@ -2,6 +2,7 @@
* *
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch> * 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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -35,6 +36,7 @@
/** /**
* @file config.c * @file config.c
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* *
* config tool. * config tool.
*/ */
@ -69,27 +71,15 @@ config_main(int argc, char *argv[])
{ {
if (argc >= 2) { if (argc >= 2) {
if (!strcmp(argv[1], "gyro")) { if (!strcmp(argv[1], "gyro")) {
if (argc >= 3) { do_gyro(argc - 2, argv + 2);
do_gyro(argc - 2, argv + 2);
} else {
errx(1, "not enough parameters.");
}
} }
if (!strcmp(argv[1], "accel")) { if (!strcmp(argv[1], "accel")) {
if (argc >= 3) { do_accel(argc - 2, argv + 2);
do_accel(argc - 2, argv + 2);
} else {
errx(1, "not enough parameters.");
}
} }
if (!strcmp(argv[1], "mag")) { if (!strcmp(argv[1], "mag")) {
if (argc >= 3) { do_mag(argc - 2, argv + 2);
do_mag(argc - 2, argv + 2);
} else {
errx(1, "not enough parameters.");
}
} }
} }
@ -100,6 +90,7 @@ static void
do_gyro(int argc, char *argv[]) do_gyro(int argc, char *argv[])
{ {
int fd; int fd;
int ret;
fd = open(GYRO_DEVICE_PATH, 0); fd = open(GYRO_DEVICE_PATH, 0);
@ -109,44 +100,45 @@ do_gyro(int argc, char *argv[])
} else { } else {
if (argc >= 2) { if (argc == 2 && !strcmp(argv[0], "sampling")) {
char* end; /* set the gyro internal sampling rate up to at least i Hz */
int i = strtol(argv[1],&end,10); 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 */ } else if (argc == 2 && !strcmp(argv[0], "rate")) {
ioctl(fd, GYROIOCSSAMPLERATE, i);
} 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 */ if (ret)
ioctl(fd, SENSORIOCSPOLLRATE, i); errx(ret,"pollrate could not be set");
} else if (!strcmp(argv[0], "range")) {
/* set the range to i dps */ } else if (argc == 2 && !strcmp(argv[0], "range")) {
ioctl(fd, GYROIOCSRANGE, i);
}
} else if (argc > 0) { /* set the range to i dps */
ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
if(!strcmp(argv[0], "check")) { if (ret)
int ret = ioctl(fd, GYROIOCSELFTEST, 0); errx(ret,"range could not be set");
if (ret) { } else if(argc == 1 && !strcmp(argv[0], "check")) {
warnx("gyro self test FAILED! Check calibration:"); ret = ioctl(fd, GYROIOCSELFTEST, 0);
struct gyro_scale scale;
ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); if (ret) {
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); warnx("gyro self test FAILED! Check calibration:");
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); struct gyro_scale scale;
} else { ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
warnx("gyro calibration and self test OK"); 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 { } 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); int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
@ -165,6 +157,7 @@ static void
do_mag(int argc, char *argv[]) do_mag(int argc, char *argv[])
{ {
int fd; int fd;
int ret;
fd = open(MAG_DEVICE_PATH, 0); fd = open(MAG_DEVICE_PATH, 0);
@ -174,31 +167,52 @@ do_mag(int argc, char *argv[])
} else { } else {
if (argc > 0) { if (argc == 2 && !strcmp(argv[0], "sampling")) {
if (!strcmp(argv[0], "check")) { /* set the mag internal sampling rate up to at least i Hz */
int ret = ioctl(fd, MAGIOCSELFTEST, 0); ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
if (ret) { if (ret)
warnx("mag self test FAILED! Check calibration."); errx(ret,"sampling rate could not be set");
struct mag_scale scale;
ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); } else if (argc == 2 && !strcmp(argv[0], "rate")) {
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); /* set the driver to poll at i Hz */
} else { ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
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 { } 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 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); close(fd);
} }
@ -210,6 +224,7 @@ static void
do_accel(int argc, char *argv[]) do_accel(int argc, char *argv[])
{ {
int fd; int fd;
int ret;
fd = open(ACCEL_DEVICE_PATH, 0); fd = open(ACCEL_DEVICE_PATH, 0);
@ -219,50 +234,52 @@ do_accel(int argc, char *argv[])
} else { } else {
if (argc >= 2) { if (argc == 2 && !strcmp(argv[0], "sampling")) {
char* end; /* set the accel internal sampling rate up to at least i Hz */
int i = strtol(argv[1],&end,10); 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 */ } else if (argc == 2 && !strcmp(argv[0], "rate")) {
ioctl(fd, ACCELIOCSSAMPLERATE, i);
} 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 */ if (ret)
ioctl(fd, SENSORIOCSPOLLRATE, i); errx(ret,"pollrate could not be set");
} else if (!strcmp(argv[0], "range")) {
/* set the range to i dps */ } else if (argc == 2 && !strcmp(argv[0], "range")) {
ioctl(fd, ACCELIOCSRANGE, i);
}
} else if (argc > 0) {
if (!strcmp(argv[0], "check")) { /* set the range to i G */
int ret = ioctl(fd, ACCELIOCSELFTEST, 0); ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
if (ret) { if (ret)
warnx("accel self test FAILED! Check calibration."); errx(ret,"range could not be set");
struct accel_scale scale;
ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); } else if(argc == 1 && !strcmp(argv[0], "check")) {
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); ret = ioctl(fd, ACCELIOCSELFTEST, 0);
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
} else { if (ret) {
warnx("accel calibration and self test OK"); 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 { } 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 srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, ACCELIOCGRANGE, 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); close(fd);
} }