zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #12 from cvg/fmuv2_bringup_lsm303d_config 

Fmuv2 bringup lsm303d config
sbg
Lorenz Meier 12 years ago
parent
1c371a6cf8 4f51f333a9
commit
db1229dca3
7 changed files with 645 additions and 440 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 25
      src/drivers/drv_mag.h
  2. 122
      src/drivers/hmc5883/hmc5883.cpp
  3. 131
      src/drivers/l3gd20/l3gd20.cpp
  4. 498
      src/drivers/lsm303d/lsm303d.cpp
  5. 87
      src/drivers/mpu6000/mpu6000.cpp
  6. 33
      src/modules/sensors/sensors.cpp
  7. 189
      src/systemcmds/config/config.c

25
src/drivers/drv_mag.h
Unescape View File

@ -90,28 +90,37 @@ ORB_DECLARE(sensor_mag); @@ -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 */

122
src/drivers/hmc5883/hmc5883.cpp
Unescape View File

@ -77,8 +77,8 @@ @@ -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: @@ -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() @@ -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() @@ -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 @@ -542,68 +547,67 @@ int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
return -EINVAL;
/* check against maximum rate */
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
return -EINVAL;
/* update interval for next measurement */
_measure_ticks = ticks;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
return (1000 / _measure_ticks);
return 1000000/TICK2USEC(_measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
@ -633,17 +637,24 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) @@ -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() @@ -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() @@ -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
Unescape View File

@ -154,6 +154,10 @@ static const int ERROR = -1; @@ -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: @@ -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: @@ -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: @@ -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) : @@ -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() @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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() @@ -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() @@ -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() @@ -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() @@ -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);
}

498
src/drivers/lsm303d/lsm303d.cpp
Unescape View File

@ -54,6 +54,7 @@ @@ -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; @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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) : @@ -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() @@ -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: @@ -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) @@ -612,64 +660,55 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
switch (arg) {
/* switching to manual polling */
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_accel_interval = 0;
return OK;
/* external signalling not supported */
/* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
case SENSOR_POLLRATE_DEFAULT:
/* With internal low pass filters enabled, 250 Hz is sufficient */
/* XXX for vibration tests with 800 Hz */
return ioctl(filp, SENSORIOCSPOLLRATE, 800);
return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_accel_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* do we need to start internal polling? */
bool want_start = (_call_accel_interval == 0);
/* check against maximum sane rate */
if (ticks < 1000)
return -EINVAL;
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* adjust sample rate of sensor */
set_samplerate(arg);
/* check against maximum sane rate */
if (ticks < 500)
return -EINVAL;
// adjust filters
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f/ticks;
_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
/* adjust filters */
accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_accel_call.period = _call_accel_interval = ticks;
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_accel_call.period = _call_accel_interval = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_accel_interval == 0)
@ -678,66 +717,77 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) @@ -678,66 +717,77 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_accel_interval;
case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */
arg++;
/* account for sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct accel_report *buf = new struct accel_report[arg];
/* allocate new buffer */
struct accel_report *buf = new struct accel_report[arg];
if (nullptr == buf)
return -ENOMEM;
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _accel_reports;
_num_accel_reports = arg;
_accel_reports = buf;
start();
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _accel_reports;
_num_accel_reports = arg;
_accel_reports = buf;
start();
return OK;
}
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_accel_reports - 1;
case SENSORIOCRESET:
/* XXX implement */
return -EINVAL;
reset();
return OK;
case ACCELIOCSSAMPLERATE:
return accel_set_samplerate(arg);
case ACCELIOCGSAMPLERATE:
return _accel_samplerate;
case ACCELIOCSLOWPASS: {
float cutoff_freq_hz = arg;
float sample_rate = 1.0e6f / _call_accel_interval;
_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
return OK;
}
return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
}
case ACCELIOCGLOWPASS:
return _accel_filter_x.get_cutoff_freq();
case ACCELIOCSSCALE:
{
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
memcpy(&_accel_scale, s, sizeof(_accel_scale));
return OK;
} else {
return -EINVAL;
}
return _accel_filter_x.get_cutoff_freq();
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
memcpy(&_accel_scale, s, sizeof(_accel_scale));
return OK;
} else {
return -EINVAL;
}
}
case ACCELIOCSRANGE:
/* arg needs to be in G */
return accel_set_range(arg);
case ACCELIOCGRANGE:
/* convert to m/s^2 and return rounded in G */
return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSELFTEST:
return accel_self_test();
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
@ -768,7 +818,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -1484,7 +1574,7 @@ test()
/* XXX add poll-rate tests here too */
// reset();
reset();
errx(0, "PASS");
}
@ -1503,7 +1593,17 @@ reset() @@ -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);
}

87
src/drivers/mpu6000/mpu6000.cpp
Unescape View File

@ -149,6 +149,15 @@ @@ -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) : @@ -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() @@ -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() @@ -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) @@ -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) @@ -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) @@ -897,7 +906,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// _accel_range_m_s2 = 8.0f * 9.81f;
return -EINVAL;
case ACCELIOCGRANGE:
return _accel_range_m_s2;
return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
case ACCELIOCSELFTEST:
return accel_self_test();
@ -920,28 +929,28 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) @@ -920,28 +929,28 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
return ioctl(filp, cmd, arg);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
GyroReportBuffer *buf = new GyroReportBuffer(arg);
if (nullptr == buf)
return -ENOMEM;
if (buf->size() == 0) {
delete buf;
return -ENOMEM;
}
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
GyroReportBuffer *buf = new GyroReportBuffer(arg);
if (nullptr == buf)
return -ENOMEM;
if (buf->size() == 0) {
delete buf;
return -ENOMEM;
}
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _gyro_reports;
_gyro_reports = buf;
start();
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _gyro_reports;
_gyro_reports = buf;
start();
return OK;
}
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _gyro_reports->size();
@ -980,7 +989,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) @@ -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() @@ -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
Unescape View File

@ -919,11 +919,11 @@ Sensors::gyro_init() @@ -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 @@ -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() @@ -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)

189
src/systemcmds/config/config.c
Unescape View File

@ -2,6 +2,7 @@ @@ -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 @@ @@ -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[]) @@ -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 @@ -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[]) @@ -109,44 +100,45 @@ do_gyro(int argc, char *argv[])
} else {
if (argc >= 2) {
if (argc == 2 && !strcmp(argv[0], "sampling")) {
char* end;
int i = strtol(argv[1],&end,10);
/* set the gyro internal sampling rate up to at least i Hz */
ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
if (!strcmp(argv[0], "sampling")) {
if (ret)
errx(ret,"sampling rate could not be set");
/* set the accel internal sampling rate up to at leat i Hz */
ioctl(fd, GYROIOCSSAMPLERATE, i);
} else if (argc == 2 && !strcmp(argv[0], "rate")) {
} else if (!strcmp(argv[0], "rate")) {
/* set the driver to poll at i Hz */
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
/* set the driver to poll at i Hz */
ioctl(fd, SENSORIOCSPOLLRATE, i);
} else if (!strcmp(argv[0], "range")) {
if (ret)
errx(ret,"pollrate could not be set");
/* set the range to i dps */
ioctl(fd, GYROIOCSRANGE, i);
}
} else if (argc == 2 && !strcmp(argv[0], "range")) {
/* set the range to i dps */
ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0));
} else if (argc > 0) {
if (ret)
errx(ret,"range could not be set");
if(!strcmp(argv[0], "check")) {
int ret = ioctl(fd, GYROIOCSELFTEST, 0);
} else if(argc == 1 && !strcmp(argv[0], "check")) {
ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret) {
warnx("gyro self test FAILED! Check calibration:");
struct gyro_scale scale;
ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
} else {
warnx("gyro calibration and self test OK");
}
if (ret) {
warnx("gyro self test FAILED! Check calibration:");
struct gyro_scale scale;
ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
} else {
warnx("gyro calibration and self test OK");
}
} else {
warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t");
}
int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
@ -165,6 +157,7 @@ static void @@ -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[]) @@ -174,31 +167,52 @@ do_mag(int argc, char *argv[])
} else {
if (argc > 0) {
if (argc == 2 && !strcmp(argv[0], "sampling")) {
/* set the mag internal sampling rate up to at least i Hz */
ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
if (ret)
errx(ret,"sampling rate could not be set");
} else if (argc == 2 && !strcmp(argv[0], "rate")) {
if (!strcmp(argv[0], "check")) {
int ret = ioctl(fd, MAGIOCSELFTEST, 0);
/* set the driver to poll at i Hz */
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
if (ret) {
warnx("mag self test FAILED! Check calibration.");
struct mag_scale scale;
ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
} else {
warnx("mag calibration and self test OK");
}
if (ret)
errx(ret,"pollrate could not be set");
} else if (argc == 2 && !strcmp(argv[0], "range")) {
/* set the range to i G */
ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0));
if (ret)
errx(ret,"range could not be set");
} else if(argc == 1 && !strcmp(argv[0], "check")) {
ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret) {
warnx("mag self test FAILED! Check calibration:");
struct mag_scale scale;
ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
} else {
warnx("mag calibration and self test OK");
}
} else {
warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t");
errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t");
}
int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0);
int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = -1;//ioctl(fd, MAGIOCGRANGE, 0);
int range = ioctl(fd, MAGIOCGRANGE, 0);
warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range);
warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range);
close(fd);
}
@ -210,6 +224,7 @@ static void @@ -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[]) @@ -219,50 +234,52 @@ do_accel(int argc, char *argv[])
} else {
if (argc >= 2) {
if (argc == 2 && !strcmp(argv[0], "sampling")) {
char* end;
int i = strtol(argv[1],&end,10);
/* set the accel internal sampling rate up to at least i Hz */
ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0));
if (!strcmp(argv[0], "sampling")) {
if (ret)
errx(ret,"sampling rate could not be set");
/* set the accel internal sampling rate up to at leat i Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, i);
} else if (argc == 2 && !strcmp(argv[0], "rate")) {
} else if (!strcmp(argv[0], "rate")) {
/* set the driver to poll at i Hz */
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0));
/* set the driver to poll at i Hz */
ioctl(fd, SENSORIOCSPOLLRATE, i);
} else if (!strcmp(argv[0], "range")) {
if (ret)
errx(ret,"pollrate could not be set");
/* set the range to i dps */
ioctl(fd, ACCELIOCSRANGE, i);
}
} else if (argc > 0) {
if (!strcmp(argv[0], "check")) {
int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret) {
warnx("accel self test FAILED! Check calibration.");
struct accel_scale scale;
ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
} else {
warnx("accel calibration and self test OK");
}
} else if (argc == 2 && !strcmp(argv[0], "range")) {
/* set the range to i G */
ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0));
if (ret)
errx(ret,"range could not be set");
} else if(argc == 1 && !strcmp(argv[0], "check")) {
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret) {
warnx("accel self test FAILED! Check calibration:");
struct accel_scale scale;
ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
} else {
warnx("accel calibration and self test OK");
}
} else {
warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t");
errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t");
}
int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, ACCELIOCGRANGE, 0);
warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range);
warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
close(fd);
}

Write Preview
Loading…
Cancel
Save