Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

InvenSense MPU9250 move to PX4Accelerometer and PX4Gyroscope helpers #12079

Merged
merged 1 commit into from
Jun 1, 2019
+135 −1,215
Merged

InvenSense MPU9250 move to PX4Accelerometer and PX4Gyroscope helpers #12079

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 3 additions & 5 deletions src/drivers/imu/mpu9250/mag_i2c.cpp → src/drivers/imu/mpu9250/AK8963_I2C.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,11 +39,10 @@

#include <px4_config.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>

#include "mpu9250.h"
#include "mag.h"
#include "MPU9250_mag.h"

#ifdef USE_I2C

Expand All @@ -69,10 +68,9 @@ AK8963_I2C_interface(int bus, bool external_bus)
return new AK8963_I2C(bus);
}

AK8963_I2C::AK8963_I2C(int bus) :
I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, 400000)
AK8963_I2C::AK8963_I2C(int bus) : I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, 400000)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250;
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250;
}

int
Expand Down
11 changes: 6 additions & 5 deletions src/drivers/imu/mpu9250/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,14 +36,15 @@ px4_add_module(
STACK_MAIN 1500
COMPILE_FLAGS
SRCS
AK8963_I2C.cpp
mpu9250.cpp
mpu9250_i2c.cpp
MPU9250_mag.cpp
mpu9250_main.cpp
mpu9250_spi.cpp
main.cpp
accel.cpp
gyro.cpp
mag.cpp
mag_i2c.cpp
DEPENDS
px4_work_queue
drivers_accelerometer
drivers_gyroscope
drivers_magnetometer
)
191 changes: 18 additions & 173 deletions src/drivers/imu/mpu9250/mag.cpp → src/drivers/imu/mpu9250/MPU9250_mag.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,111 +45,36 @@
#include <px4_time.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/integrator.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>

#include "mag.h"
#include "mpu9250.h"

#include "MPU9250_mag.h"
#include "mpu9250.h"

// If interface is non-null, then it will used for interacting with the device.
// Otherwise, it will passthrough the parent MPU9250
MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, const char *path) :
CDev("MPU9250_mag", path),
MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotation rotation) :
_interface(interface),
_px4_mag(parent->_interface->get_device_id(), (parent->_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH),
rotation),
_parent(parent),
_mag_topic(nullptr),
_mag_orb_class_instance(-1),
_mag_class_instance(-1),
_mag_reading_data(false),
_mag_reports(nullptr),
_mag_scale{},
_mag_range_scale(),
_mag_reads(perf_alloc(PC_COUNT, "mpu9250_mag_reads")),
_mag_errors(perf_alloc(PC_COUNT, "mpu9250_mag_errors")),
_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_asa_x(1.0),
_mag_asa_y(1.0),
_mag_asa_z(1.0),
_last_mag_data{}
_mag_duplicates(perf_alloc(PC_COUNT, "mpu9250_mag_duplicates"))
{
// default mag scale factors
_mag_scale.x_offset = 0;
_mag_scale.x_scale = 1.0f;
_mag_scale.y_offset = 0;
_mag_scale.y_scale = 1.0f;
_mag_scale.z_offset = 0;
_mag_scale.z_scale = 1.0f;

_mag_range_scale = MPU9250_MAG_RANGE_GA;
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_MPU9250);
_px4_mag.set_scale(MPU9250_MAG_RANGE_GA);
}

MPU9250_mag::~MPU9250_mag()
{
if (_mag_class_instance != -1) {
unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance);
}

if (_mag_reports != nullptr) {
delete _mag_reports;
}

orb_unadvertise(_mag_topic);

perf_free(_mag_reads);
perf_free(_mag_errors);
perf_free(_mag_overruns);
perf_free(_mag_overflows);
perf_free(_mag_duplicates);
}

int
MPU9250_mag::init()
{
int ret = CDev::init();

/* if cdev init failed, bail now */
if (ret != OK) {
if (_parent->_whoami == MPU_WHOAMI_9250) {
PX4_ERR("mag init failed");
}

return ret;
}

_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));

if (_mag_reports == nullptr) {
return -ENOMEM;
}

_mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);


/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);

_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
&_mag_orb_class_instance, (_parent->is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
// &_mag_orb_class_instance, ORB_PRIO_LOW);

if (_mag_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
return -ENOMEM;
}

return OK;
}

bool MPU9250_mag::check_duplicate(uint8_t *mag_data)
{
if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) {
Expand All @@ -170,18 +95,17 @@ MPU9250_mag::measure()
struct ak09916_regs ak09916_data;
} raw_data;

uint8_t ret = _interface->read(AK8963REG_ST1, &raw_data, sizeof(struct ak8963_regs));
const hrt_abstime timestamp_sample = hrt_absolute_time();
int ret = _interface->read(AK8963REG_ST1, &raw_data, sizeof(struct ak8963_regs));

if (ret == OK) {
_measure(raw_data.ak8963_data);
_measure(timestamp_sample, raw_data.ak8963_data);
}
}

void
MPU9250_mag::_measure(struct ak8963_regs data)
MPU9250_mag::_measure(hrt_abstime timestamp_sample, struct ak8963_regs data)
{
bool mag_notify = true;

if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) {
perf_count(_mag_duplicates);
return;
Expand All @@ -200,92 +124,15 @@ MPU9250_mag::_measure(struct ak8963_regs data)
perf_count(_mag_overflows);
}

mag_report mrb;
mrb.timestamp = hrt_absolute_time();

mrb.is_external = _parent->is_external();
_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
* doesn't look obvious with the datasheet
*/
float xraw_f, yraw_f, zraw_f;

mrb.x_raw = data.x;
mrb.y_raw = -data.y;
mrb.z_raw = -data.z;

xraw_f = data.x;
yraw_f = -data.y;
zraw_f = -data.z;

/* apply user specified rotation */
rotate_3f(_parent->_rotation, xraw_f, yraw_f, zraw_f);

mrb.x = ((xraw_f * _mag_range_scale * _mag_asa_x) - _mag_scale.x_offset) * _mag_scale.x_scale;
mrb.y = ((yraw_f * _mag_range_scale * _mag_asa_y) - _mag_scale.y_offset) * _mag_scale.y_scale;
mrb.z = ((zraw_f * _mag_range_scale * _mag_asa_z) - _mag_scale.z_offset) * _mag_scale.z_scale;
mrb.scaling = _mag_range_scale;
mrb.temperature = _parent->_last_temperature;
mrb.device_id = _parent->_mag->_device_id.devid;

mrb.error_count = perf_event_count(_mag_errors);

_mag_reports->force(&mrb);

/* notify anyone waiting for data */
if (mag_notify) {
poll_notify(POLLIN);
}

if (mag_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mrb);
}
}

int
MPU9250_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
{
/*
* Repeated in MPU9250_accel::ioctl
* Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500
*/

switch (cmd) {

case SENSORIOCRESET:
return ak8963_reset();

case SENSORIOCSPOLLRATE: {
switch (arg) {

/* zero would be bad */
case 0:
return -EINVAL;

case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE);

/* adjust to a legal polling interval in Hz */
default:
return _parent->_set_pollrate(arg);
}
}

case MAGIOCSSCALE:
/* copy scale in */
memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale));
return OK;

case MAGIOCGSCALE:
/* copy scale out */
memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale));
return OK;

default:
return (int)CDev::ioctl(filp, cmd, arg);
}
_px4_mag.update(timestamp_sample, data.x, -data.y, -data.z);
}

void
Expand Down Expand Up @@ -326,7 +173,7 @@ MPU9250_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
uint8_t
MPU9250_mag::read_reg(unsigned int reg)
{
uint8_t buf;
uint8_t buf{};

if (_interface == nullptr) {
passthrough_read(reg, &buf, 0x01);
Expand Down Expand Up @@ -413,9 +260,7 @@ MPU9250_mag::ak8963_read_adjustments(void)
}
}

_mag_asa_x = ak8963_ASA[0];
_mag_asa_y = ak8963_ASA[1];
_mag_asa_z = ak8963_ASA[2];
_px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]);

return true;
}
Expand Down
Loading