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

Merge branch 'mpu6k_queue' of github.com:PX4/Firmware into fmuv2_bringup

This commit is contained in:
Lorenz Meier 2013-08-06 09:15:24 +02:00
parent ec2e02d50e 338e506a28
commit 461b2eb366
6 changed files with 98 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/device/cdev.cpp
View File

@ -395,4 +395,4 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup)
return cdev->poll(filp, fds, setup);
}
} // namespace device
} // namespace device

2
src/drivers/device/device.cpp
View File

@ -241,4 +241,4 @@ Device::ioctl(unsigned operation, unsigned &arg)
return -ENODEV;
}
} // namespace device
} // namespace device

2
src/drivers/device/device.h
View File

@ -488,4 +488,4 @@ private:
} // namespace device
#endif /* _DEVICE_DEVICE_H */
#endif /* _DEVICE_DEVICE_H */

34
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

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

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

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

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

@ -763,11 +763,11 @@ Sensors::accel_init()
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the accel internal sampling rate up to at leat 500Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 500);
/* set the accel internal sampling rate up to at leat 1000Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
/* set the driver to poll at 500Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 500);
/* set the driver to poll at 1000Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 1000);
#else
@ -801,11 +801,13 @@ Sensors::gyro_init()
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* set the gyro internal sampling rate up to at leat 500Hz */
ioctl(fd, GYROIOCSSAMPLERATE, 500);
/* set the gyro internal sampling rate up to at least 1000Hz */
if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
ioctl(fd, GYROIOCSSAMPLERATE, 800);
/* set the driver to poll at 500Hz */
ioctl(fd, SENSORIOCSPOLLRATE, 500);
/* set the driver to poll at 1000Hz */
if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
ioctl(fd, SENSORIOCSPOLLRATE, 800);
#else