fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mpu9250: fix mag spikes on fmu-v4pro

Browse Source 
This should fix spikes in the mag data on MPU9250 found inside Drotek
Pixhawk 3Pro.

The problem turned out to be that we are not checking the DRDY bit
before reading the data. We presumably threw away most of the stale data
by doing a duplicate check, however, sometimes we might have run into a
race where the mag data was already being updated in the chip while
still being read.
This commit is contained in:
Julian Oes 2019-11-01 17:29:24 +01:00 committed by Daniel Agar
parent 3e189889ef
commit a08abccdd5
2 changed files with 50 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
src/drivers/imu/mpu9250/MPU9250_mag.cpp
View File

@ -52,7 +52,7 @@ MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotati
_parent(parent),
_mag_overruns(perf_alloc(PC_COUNT, "mpu9250_mag_overruns")),
_mag_overflows(perf_alloc(PC_COUNT, "mpu9250_mag_overflows")),
_mag_duplicates(perf_alloc(PC_COUNT, "mpu9250_mag_duplicates"))
_mag_errors(perf_alloc(PC_COUNT, "mpu9250_mag_errors"))
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_MPU9250);
_px4_mag.set_scale(MPU9250_MAG_RANGE_GA);
@ -62,64 +62,75 @@ MPU9250_mag::~MPU9250_mag()
{
perf_free(_mag_overruns);
perf_free(_mag_overflows);
perf_free(_mag_duplicates);
}
bool MPU9250_mag::check_duplicate(uint8_t *mag_data)
{
if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) {
// it isn't new data - wait for next timer
return true;
} else {
memcpy(&_last_mag_data, mag_data, sizeof(_last_mag_data));
return false;
}
perf_free(_mag_errors);
}
void
MPU9250_mag::measure()
{
union raw_data_t {
struct ak8963_regs ak8963_data;
struct ak09916_regs ak09916_data;
} raw_data;
const hrt_abstime timestamp_sample = hrt_absolute_time();
int ret = _interface->read(AK8963REG_ST1, &raw_data, sizeof(struct ak8963_regs));
if (ret == OK) {
_measure(timestamp_sample, raw_data.ak8963_data);
}
}
uint8_t st1;
int ret = _interface->read(AK8963REG_ST1, &st1, sizeof(st1));
void
MPU9250_mag::_measure(hrt_abstime timestamp_sample, struct ak8963_regs data)
{
if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) {
perf_count(_mag_duplicates);
if (ret != OK) {
_px4_mag.set_error_count(perf_event_count(_mag_errors));
return;
}
/* monitor for if data overrun flag is ever set */
if (data.st1 & 0x02) {
/* Check if data ready is set.
* This is not described to be set in continuous mode according to the
* MPU9250 datasheet. However, the datasheet of the 8963 recommends to
* check data ready before doing the read and before triggering the
* next measurement by reading ST2. */
if (!(st1 & AK09916_ST1_DRDY)) {
return;
}
/* Monitor if data overrun flag is ever set. */
if (st1 & 0x02) {
perf_count(_mag_overruns);
}
/* monitor for if magnetic sensor overflow flag is ever set noting that st2
* is usually not even refreshed, but will always be in the same place in the
* mpu's buffers regardless, hence the actual count would be bogus
*/
ak8963_regs data;
ret = _interface->read(AK8963REG_ST1, &data, sizeof(data));
if (ret != OK) {
_px4_mag.set_error_count(perf_event_count(_mag_errors));
return;
}
/* Monitor magnetic sensor overflow flag. */
if (data.st2 & 0x08) {
perf_count(_mag_overflows);
}
_measure(timestamp_sample, data);
}
void
MPU9250_mag::_measure(hrt_abstime timestamp_sample, ak8963_regs data)
{
/* Check if data ready is set.
* This is not described to be set in continuous mode according to the
* MPU9250 datasheet. However, the datasheet of the 8963 recommends to
* check data ready before doing the read and before triggering the
* next measurement by reading ST2.
*
* If _measure is used in passthrough mode, all the data is already
* fetched, however, we should still not use the data if the data ready
* is not set. This has lead to intermittent spikes when the data was
* being updated while getting read.
*/
if (!(data.st1 & AK09916_ST1_DRDY)) {
return;
}
_px4_mag.set_external(_parent->is_external());
_px4_mag.set_temperature(_parent->_last_temperature);
//_px4_mag.set_error_count(perf_event_count(_mag_errors));
/*
* Align axes - note the accel & gryo are also re-aligned so this
* Align axes - note the accel & gyro are also re-aligned so this
* doesn't look obvious with the datasheet
*/
_px4_mag.update(timestamp_sample, data.x, -data.y, -data.z);

25
src/drivers/imu/mpu9250/MPU9250_mag.h
View File

@ -106,18 +106,6 @@ struct ak8963_regs {
};
#pragma pack(pop)
#pragma pack(push, 1)
struct ak09916_regs {
uint8_t st1;
int16_t x;
int16_t y;
int16_t z;
uint8_t tmps;
uint8_t st2;
};
#pragma pack(pop)
extern device::Device *AK8963_I2C_interface(int bus, bool external_bus);
typedef device::Device *(*MPU9250_mag_constructor)(int, bool);
@ -150,11 +138,8 @@ protected:
friend class MPU9250;
/* Directly measure from the _interface if possible */
void measure();
/* Update the state with prefetched data (internally called by the regular measure() )*/
void _measure(hrt_abstime timestamp, struct ak8963_regs data);
void _measure(hrt_abstime timestamp_sample, ak8963_regs data);
uint8_t read_reg(unsigned reg);
void write_reg(unsigned reg, uint8_t value);
@ -162,7 +147,6 @@ protected:
bool is_passthrough() { return _interface == nullptr; }
private:
PX4Magnetometer _px4_mag;
MPU9250 *_parent;
@ -171,12 +155,7 @@ private:
perf_counter_t _mag_overruns;
perf_counter_t _mag_overflows;
perf_counter_t _mag_duplicates;
bool check_duplicate(uint8_t *mag_data);
// keep last mag reading for duplicate detection
uint8_t _last_mag_data[6] {};
perf_counter_t _mag_errors;
/* do not allow to copy this class due to pointer data members */
MPU9250_mag(const MPU9250_mag &);