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

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

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

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

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

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

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

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

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

@ -919,11 +919,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
@ -936,6 +936,7 @@ void
Sensors::mag_init()
{
int fd;
int ret;
fd = open(MAG_DEVICE_PATH, 0);
@ -944,13 +945,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)

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

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