diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-06 09:15:24 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-06 09:15:24 +0200 |
commit | 461b2eb3667838d455efc8946e81498f6a0d58f1 (patch) | |
tree | 61eefabd6488f24443706a3e47d7f776be68f0e0 /src/drivers | |
parent | ec2e02d50ea2f518051b50e4e83e12736526fbc2 (diff) | |
parent | 338e506a28e4233bc8a16493530f3b82a0dd67e9 (diff) | |
download | px4-firmware-461b2eb3667838d455efc8946e81498f6a0d58f1.tar.gz px4-firmware-461b2eb3667838d455efc8946e81498f6a0d58f1.tar.bz2 px4-firmware-461b2eb3667838d455efc8946e81498f6a0d58f1.zip |
Merge branch 'mpu6k_queue' of github.com:PX4/Firmware into fmuv2_bringup
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/device/cdev.cpp | 2 | ||||
-rw-r--r-- | src/drivers/device/device.cpp | 2 | ||||
-rw-r--r-- | src/drivers/device/device.h | 2 | ||||
-rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 34 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 76 |
5 files changed, 88 insertions, 28 deletions
diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 1972d2efc..47ebcd40a 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -395,4 +395,4 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup) return cdev->poll(filp, fds, setup); } -} // namespace device
\ No newline at end of file +} // namespace device diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index 1c6f9b7f4..c3ee77b1c 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -241,4 +241,4 @@ Device::ioctl(unsigned operation, unsigned &arg) return -ENODEV; } -} // namespace device
\ No newline at end of file +} // namespace device diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 75f35ff0f..a9ed5d77c 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -488,4 +488,4 @@ private: } // namespace device -#endif /* _DEVICE_DEVICE_H */
\ No newline at end of file +#endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b19a1a0e6..3dc6159ef 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -34,6 +34,7 @@ /** * @file meas_airspeed.cpp * @author Lorenz Meier + * @author Sarthak Kaingade * @author Simon Wilks * * Driver for the MEAS Spec series connected via I2C. @@ -92,9 +93,6 @@ /* Register address */ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ -#define ADDR_READ_DF2 0x00 /* read from this address to read pressure only */ -#define ADDR_READ_DF3 0x01 -#define ADDR_READ_DF4 0x02 /* read from this address to read pressure and temp */ /* Measurement rate is 100Hz */ #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ @@ -122,7 +120,7 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) + CONVERSION_INTERVAL) { } @@ -160,7 +158,7 @@ MEASAirspeed::collect() perf_begin(_sample_perf); - ret = transfer(nullptr, 0, &val[0], 2); + ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { log("error reading from sensor: %d", ret); @@ -171,25 +169,37 @@ MEASAirspeed::collect() if (status == 2) { log("err: stale data"); + } else if (status == 3) { log("err: fault"); } - uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); + //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; - diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); - diff_pres_pa -= _diff_pres_offset; + // XXX leaving this in until new calculation method has been cross-checked + //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); + //diff_pres_pa -= _diff_pres_offset; + int16_t dp_raw = 0, dT_raw = 0; + dp_raw = (val[0] << 8) + val[1]; + dp_raw = 0x3FFF & dp_raw; + dT_raw = (val[2] << 8) + val[3]; + dT_raw = (0xFFE0 & dT_raw) >> 5; + float temperature = ((200 * dT_raw) / 2047) - 50; // XXX we may want to smooth out the readings to remove noise. + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative, enforce absolute value + uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].temperature = temp; - _reports[_next_report].differential_pressure_pa = diff_pres_pa; + _reports[_next_report].temperature = temperature; + _reports[_next_report].differential_pressure_pa = diff_press_pa; // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_pres_pa; + if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) { + _reports[_next_report].max_differential_pressure_pa = diff_press_pa; } /* announce the airspeed if needed, just publish else */ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bf18e73c0..65fa33530 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -70,6 +70,7 @@ #include <drivers/device/ringbuffer.h> #include <drivers/drv_accel.h> #include <drivers/drv_gyro.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -202,6 +203,13 @@ private: unsigned _sample_rate; perf_counter_t _sample_perf; + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + /** * Start automatic measurement. */ @@ -332,8 +340,14 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _gyro_range_rad_s(0.0f), _gyro_topic(-1), _reads(0), - _sample_rate(500), - _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) + _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) { // disable debug() calls _debug_enabled = false; @@ -463,7 +477,7 @@ void MPU6000::reset() // 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(20); + _set_dlpf_filter(42); usleep(1000); // Gyro scale 2000 deg/s () write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -714,6 +728,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) if (ticks < 1000) 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); + + + float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; @@ -767,9 +794,17 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) _set_sample_rate(arg); return OK; - case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: - _set_dlpf_filter((uint16_t)arg); + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSLOWPASS: + + // 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); return OK; case ACCELIOCSSCALE: @@ -853,9 +888,14 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) _set_sample_rate(arg); return OK; - case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: - _set_dlpf_filter((uint16_t)arg); + return _gyro_filter_x.get_cutoff_freq(); + case GYROIOCSLOWPASS: + _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + // XXX check relation to the internal lowpass + //_set_dlpf_filter((uint16_t)arg); return OK; case GYROIOCSSCALE: @@ -1112,9 +1152,14 @@ MPU6000::measure() arb.y_raw = report.accel_y; arb.z_raw = report.accel_z; - arb.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - arb.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - arb.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + arb.x = _accel_filter_x.apply(x_in_new); + arb.y = _accel_filter_y.apply(y_in_new); + arb.z = _accel_filter_z.apply(z_in_new); + arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1125,9 +1170,14 @@ MPU6000::measure() grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; - grb.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - grb.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - grb.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + + grb.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.z = _gyro_filter_z.apply(z_gyro_in_new); + grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; |