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

L3GD20 and LSM303D reset and range config working properly now

This commit is contained in:
Julian Oes 2013-08-21 14:21:54 +02:00
parent 8083efb60c
commit 1ede95d252
2 changed files with 136 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

@ -170,13 +170,13 @@ static const int ERROR = -1;
#define INT_SRC_M 0x13
/* default values for this device */
#define ACCEL_DEFAULT_RANGE_G 8
#define ACCEL_DEFAULT_SAMPLERATE 800
#define ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
#define ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#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 MAG_DEFAULT_RANGE_GA 2
#define MAG_DEFAULT_SAMPLERATE 100
#define LSM303D_MAG_DEFAULT_RANGE_GA 2
#define LSM303D_MAG_DEFAULT_RATE 100
extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
@ -231,7 +231,7 @@ private:
unsigned _accel_range_m_s2;
float _accel_range_scale;
unsigned _accel_samplerate;
unsigned _accel_filter_bandwith;
unsigned _accel_onchip_filter_bandwith;
struct mag_scale _mag_scale;
unsigned _mag_range_ga;
@ -356,13 +356,22 @@ private:
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 accel_set_antialias_filter_bandwidth(unsigned bandwith);
int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
/**
* Set the driver lowpass filter bandwidth.
*
* @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 accel_set_driver_lowpass_filter(float samplerate, float bandwidth);
/**
* Set the LSM303D internal accel sampling frequency.
@ -430,7 +439,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_accel_range_m_s2(0.0f),
_accel_range_scale(0.0f),
_accel_samplerate(0),
_accel_filter_bandwith(0),
_accel_onchip_filter_bandwith(0),
_mag_range_ga(0.0f),
_mag_range_scale(0.0f),
_mag_samplerate(0),
@ -440,9 +449,9 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_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(0, 0),
_accel_filter_y(0, 0),
_accel_filter_z(0, 0)
_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;
@ -538,14 +547,13 @@ LSM303D::reset()
write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
_accel_filter_bandwith = ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
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);
accel_set_range(ACCEL_DEFAULT_RANGE_G);
accel_set_samplerate(ACCEL_DEFAULT_SAMPLERATE);
accel_set_antialias_filter_bandwidth(ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
mag_set_range(MAG_DEFAULT_RANGE_GA);
mag_set_samplerate(MAG_DEFAULT_SAMPLERATE);
mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
_accel_read = 0;
_mag_read = 0;
@ -672,7 +680,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
return ioctl(filp, SENSORIOCSPOLLRATE, 1600);
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, ACCEL_DEFAULT_SAMPLERATE);
return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
@ -687,11 +695,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
return -EINVAL;
/* adjust filters */
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
_accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz);
_accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz);
_accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz);
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... */
@ -750,10 +754,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
return _accel_samplerate;
case ACCELIOCSLOWPASS: {
_accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
_accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
_accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg);
return OK;
return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
}
case ACCELIOCGLOWPASS:
@ -1091,7 +1092,7 @@ LSM303D::mag_set_range(unsigned max_ga)
}
int
LSM303D::accel_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;
@ -1101,19 +1102,19 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth)
if (bandwidth <= 50) {
setbits |= REG2_AA_FILTER_BW_50HZ_A;
_accel_filter_bandwith = 50;
_accel_onchip_filter_bandwith = 50;
} else if (bandwidth <= 194) {
setbits |= REG2_AA_FILTER_BW_194HZ_A;
_accel_filter_bandwith = 194;
_accel_onchip_filter_bandwith = 194;
} else if (bandwidth <= 362) {
setbits |= REG2_AA_FILTER_BW_362HZ_A;
_accel_filter_bandwith = 362;
_accel_onchip_filter_bandwith = 362;
} else if (bandwidth <= 773) {
setbits |= REG2_AA_FILTER_BW_773HZ_A;
_accel_filter_bandwith = 773;
_accel_onchip_filter_bandwith = 773;
} else {
return -EINVAL;
@ -1124,6 +1125,16 @@ LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth)
return OK;
}
int
LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth)
{
_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::accel_set_samplerate(unsigned frequency)
{
@ -1582,15 +1593,16 @@ reset()
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "accel driver poll rate reset failed");
err(1, "accel pollrate reset failed");
int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY);
fd = open(MAG_DEVICE_PATH, O_RDONLY);
if (fd_mag < 0) {
warnx("could not open to mag " MAG_DEVICE_PATH);
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 driver poll rate reset failed");
err(1, "mag pollrate reset failed");
}
exit(0);