fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 09:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cleanup mpu9250 driver in preparation for PR

Browse Source
This commit is contained in:
Robert Dickenson
2016-03-08 09:29:17 +01:00
committed by Lorenz Meier
parent 4ef9148517
commit 95e51ed8c6
8 changed files with 53 additions and 405 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/boards/px4fmu-v4/board_config.h
+2 -2
View File
@@ -123,8 +123,8 @@ __BEGIN_DECLS
#define PX4_SPI_BUS_BARO PX4_SPI_BUS_RAMTRON
/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
#define PX4_SPIDEV_GYRO 1
#define PX4_SPIDEV_ACCEL_MAG 2
#define PX4_SPIDEV_GYRO 1
#define PX4_SPIDEV_ACCEL_MAG 2
#define PX4_SPIDEV_MPU 4
#define PX4_SPIDEV_HMC 5
#define PX4_SPIDEV_ICM 6
src/drivers/mpu9250/mag.cpp
+32 -44
View File
@@ -34,7 +34,7 @@
/**
* @file mag.cpp
*
* Driver for the ak8963 magnetometer within the Invensense mpu9250.
* Driver for the ak8963 magnetometer within the Invensense mpu9250
*
* @author Robert Dickenson
*
@@ -72,6 +72,10 @@
#define MPU9250_MAG_RANGE_GA 1.5e-3f;
/* we are using the continuous fixed sampling rate of 100Hz */
#define MPU9250_AK8963_SAMPLE_RATE 100
/* mpu9250 master i2c bus specific register address and bit definitions */
@@ -126,11 +130,9 @@ MPU9250_mag::MPU9250_mag(MPU9250 *parent, const char *path) :
_mag_orb_class_instance(-1),
_mag_class_instance(-1),
_mag_reading_data(false),
_mag_call_interval(0),
_mag_reports(nullptr),
_mag_scale{},
_mag_range_scale(),
_mag_sample_rate(100),
_mag_reads(perf_alloc(PC_COUNT, "mpu9250_mag_read")),
_mag_asa_x(1.0),
_mag_asa_y(1.0),
@@ -180,8 +182,6 @@ MPU9250_mag::init()
ak8963_setup();
// measure(0);
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
@@ -198,13 +198,6 @@ out:
return ret;
}
uint8_t cnt0 = 0;
uint8_t cnt1 = 0;
uint8_t cnt2 = 0;
uint8_t cnt3 = 0;
uint8_t cnt4 = 0;
void
MPU9250_mag::measure(struct ak8963_regs data)
{
@@ -212,38 +205,33 @@ MPU9250_mag::measure(struct ak8963_regs data)
if (data.st1 & 0x01) {
if (false == _mag_reading_data) {
cnt1++;
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, BIT_I2C_SLVO_EN | sizeof(struct ak8963_regs));
_mag_reading_data = true;
return;
} else {
cnt2++;
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, BIT_I2C_SLVO_EN | 1);
_mag_reading_data = false;
}
} else {
if (true == _mag_reading_data) {
cnt4++;
_parent->write_reg(MPUREG_I2C_SLV0_CTRL, BIT_I2C_SLVO_EN | 1);
_mag_reading_data = false;
} else {
cnt3++;
}
return;
}
mag_report mrb;
mag_report mrb;
mrb.timestamp = hrt_absolute_time();
/*
* Align axes - note the accel & gryo are also re-aligned so this
* doesn't look obvious with the datasheet
*/
mrb.x_raw = data.x;
mrb.x_raw = data.x;
mrb.y_raw = -data.y;
mrb.z_raw = -data.z;
float xraw_f = data.x;
float xraw_f = data.x;
float yraw_f = -data.y;
float zraw_f = -data.z;
@@ -257,13 +245,7 @@ MPU9250_mag::measure(struct ak8963_regs data)
mrb.scaling = _mag_range_scale;
mrb.temperature = _parent->_last_temperature;
static uint64_t prev_mag_timestamp = 0;
mrb.error_count = ((uint64_t)(mrb.timestamp - prev_mag_timestamp) << 40)
+ ((uint64_t)cnt0 << 32)
+ ((uint32_t)cnt1 << 24) + ((uint32_t)cnt2 << 16)
+ ((uint32_t)cnt3 << 8) + cnt4;
cnt0 = cnt1 = cnt2 = cnt3 = cnt4 = 0;
prev_mag_timestamp = mrb.timestamp;
mrb.error_count = 0;
_mag_reports->force(&mrb);
@@ -289,8 +271,11 @@ MPU9250_mag::read(struct file *filp, char *buffer, size_t buflen)
}
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
if (_mag_call_interval == 0) {
if (_parent->_call_interval == 0) {
_mag_reports->flush();
/* TODO: this won't work as getting valid magnetometer
* data requires more than one measure cycle
*/
_parent->measure();
}
@@ -323,8 +308,10 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case SENSORIOCRESET:
// TODO: we could implement a reset of the AK8963 registers
// return reset();
/*
* TODO: we could implement a reset of the AK8963 registers
*/
//return reset();
return _parent->ioctl(filp, cmd, arg);
case SENSORIOCSPOLLRATE: {
@@ -332,8 +319,11 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
// stop();
_mag_call_interval = 0;
/*
* TODO: investigate being able to stop
* the continuous sampling
*/
//stop();
return OK;
/* external signalling not supported */
@@ -347,15 +337,12 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSOR_POLLRATE_MAX:
return ioctl(filp, SENSORIOCSPOLLRATE, 100);
#define MPU9250_AK8963_DEFAULT_RATE 100
#define MPU9250_AK8963_TIMER_REDUCTION 0
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_AK8963_DEFAULT_RATE);
return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_AK8963_SAMPLE_RATE);
/* adjust to a legal polling interval in Hz */
default: {
if (MPU9250_AK8963_DEFAULT_RATE != arg) {
if (MPU9250_AK8963_SAMPLE_RATE != arg) {
return -EINVAL;
}
return OK;
@@ -364,7 +351,7 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
}
case SENSORIOCGPOLLRATE:
return MPU9250_AK8963_DEFAULT_RATE;
return MPU9250_AK8963_SAMPLE_RATE;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
@@ -388,13 +375,14 @@ MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
return _mag_reports->size();
case MAGIOCGSAMPLERATE:
return _mag_sample_rate;
return MPU9250_AK8963_SAMPLE_RATE;
case MAGIOCSSAMPLERATE:
/*
* We don't currently support any means of changing the sampling rate of the mag
*/
if (MPU9250_AK8963_DEFAULT_RATE != arg) {
/*
* We don't currently support any means of changing
* the sampling rate of the mag
*/
if (MPU9250_AK8963_SAMPLE_RATE != arg) {
return -EINVAL;
}
return OK;
@@ -482,7 +470,7 @@ MPU9250_mag::ak8963_check_id(void)
for (int i = 0; i < 5; i++) {
uint8_t deviceid = 0;
passthrough_read(AK8963REG_WIA, &deviceid, 0x01);
if (deviceid == AK8963_DEVICE_ID) {
if (AK8963_DEVICE_ID == deviceid) {
return true;
}
}
src/drivers/mpu9250/mag.h
-2
View File
@@ -78,11 +78,9 @@ private:
int _mag_orb_class_instance;
int _mag_class_instance;
bool _mag_reading_data;
unsigned _mag_call_interval;
ringbuffer::RingBuffer *_mag_reports;
struct mag_scale _mag_scale;
float _mag_range_scale;
unsigned _mag_sample_rate;
perf_counter_t _mag_reads;
float _mag_asa_x;
float _mag_asa_y;
src/drivers/mpu9250/main.cpp
+1 -3
View File
@@ -78,7 +78,7 @@
#define MPU_DEVICE_PATH_MAG "/dev/mpu9250_mag"
#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu9250_accel_ext"
#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu9250_gyro_ext"
#define MPU_DEVICE_PATH_MAG_EXT "/dev/mpu9250_mag_ext"
#define MPU_DEVICE_PATH_MAG_EXT "/dev/mpu9250_mag_ext"
/** driver 'main' command */
extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); }
@@ -100,8 +100,6 @@ void info(bool);
void regdump(bool);
void testerror(bool);
void usage();
void w(bool);
void g(bool, char *);
/**
* Start the driver.
src/drivers/mpu9250/mpu9250.cpp
+18 -19
View File
@@ -182,10 +182,6 @@
#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE
#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_MAG_DEFAULT_RATE 1000
#define MPU9250_MAG_MAX_OUTPUT_RATE MPU9250_GYRO_MAX_OUTPUT_RATE
#define MPU9250_MAG_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 41
#define MPU9250_ONE_G 9.80665f
@@ -259,7 +255,6 @@ MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, const c
_rotation(rotation),
_checked_next(0),
_last_temperature(0),
// _last_accel{},
_last_accel_data{},
_got_duplicate(false)
{
@@ -391,7 +386,7 @@ MPU9250::init()
return ret;
}
/* do CDev init for the gyro device node, keep it optional */
/* do CDev init for the mag device node, keep it optional */
ret = _mag->init();
/* if probe/setup failed, bail now */
@@ -736,19 +731,20 @@ MPU9250::gyro_self_test()
void
MPU9250::test_error()
{
// deliberately trigger an error. This was noticed during
// development as a handy way to test the reset logic
uint8_t data[16];
memset(data, 0, sizeof(data));
transfer(data, data, sizeof(data));
::printf("error triggered\n");
print_registers();
// deliberately trigger an error. This was noticed during
// development as a handy way to test the reset logic
uint8_t data[16];
memset(data, 0, sizeof(data));
transfer(data, data, sizeof(data));
::printf("error triggered\n");
print_registers();
}
ssize_t
MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(gyro_report);
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
@@ -775,6 +771,7 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen)
if (!_gyro_reports->get(grp)) {
break;
}
transferred++;
grp++;
}
@@ -938,12 +935,14 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg)
return accel_self_test();
#ifdef ACCELIOCSHWLOWPASS
case ACCELIOCSHWLOWPASS:
_set_dlpf_filter(arg);
return OK;
#endif
#ifdef ACCELIOCGHWLOWPASS
case ACCELIOCGHWLOWPASS:
return _dlpf_freq;
#endif
@@ -1027,19 +1026,21 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
return gyro_self_test();
#ifdef GYROIOCSHWLOWPASS
case GYROIOCSHWLOWPASS:
_set_dlpf_filter(arg);
return OK;
#endif
#ifdef GYROIOCGHWLOWPASS
case GYROIOCGHWLOWPASS:
return _dlpf_freq;
#endif
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
@@ -1312,8 +1313,6 @@ MPU9250::measure()
check_registers();
if (check_duplicate(&mpu_report.accel_x[0])) {
extern uint8_t cnt0;
cnt0++;
return;
}
@@ -1361,7 +1360,7 @@ cnt0++;
/*
* Report buffers.
*/
accel_report arb;
accel_report arb;
gyro_report grb;
/*
@@ -1419,7 +1418,7 @@ cnt0++;
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
_last_temperature = (report.temp) / 361.0f + 35.0f;