zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AK09916 move to new WQ, PX4Magnetometer, and cleanup

sbg
Daniel Agar 6 years ago
parent
a523e18c13
commit
f593e3de9c
4 changed files with 72 additions and 448 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      src/drivers/drv_sensor.h
  2. 2
      src/drivers/magnetometer/ak09916/CMakeLists.txt
  3. 399
      src/drivers/magnetometer/ak09916/ak09916.cpp
  4. 90
      src/drivers/magnetometer/ak09916/ak09916.hpp

1
src/drivers/drv_sensor.h
Unescape View File

@ -60,6 +60,7 @@ @@ -60,6 +60,7 @@
#define DRV_MAG_DEVTYPE_IST8310 0x06
#define DRV_MAG_DEVTYPE_RM3100 0x07
#define DRV_MAG_DEVTYPE_QMC5883 0x08
#define DRV_MAG_DEVTYPE_AK09916 0x09
#define DRV_ACC_DEVTYPE_LSM303D 0x11
#define DRV_ACC_DEVTYPE_BMA180 0x12
#define DRV_ACC_DEVTYPE_MPU6000 0x13

2
src/drivers/magnetometer/ak09916/CMakeLists.txt
Unescape View File

@ -39,5 +39,7 @@ px4_add_module( @@ -39,5 +39,7 @@ px4_add_module(
SRCS
ak09916.cpp
DEPENDS
drivers_magnetometer
px4_work_queue
)

399
src/drivers/magnetometer/ak09916/ak09916.cpp
Unescape View File

@ -45,22 +45,11 @@ @@ -45,22 +45,11 @@
#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 <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <px4_getopt.h>
#include "ak09916.hpp"
#include <px4_getopt.h>
/** driver 'main' command */
extern "C" { __EXPORT int ak09916_main(int argc, char *argv[]); }
@ -74,12 +63,9 @@ AK09916 *g_dev_ext; @@ -74,12 +63,9 @@ AK09916 *g_dev_ext;
AK09916 *g_dev_int;
void start(bool, enum Rotation);
void test(bool);
void reset(bool);
void info(bool);
void usage();
/**
* Start the driver.
*
@ -88,7 +74,6 @@ void usage(); @@ -88,7 +74,6 @@ void usage();
*/
void start(bool external_bus, enum Rotation rotation)
{
int fd;
AK09916 **g_dev_ptr = (external_bus ? &g_dev_ext : &g_dev_int);
const char *path = (external_bus ? AK09916_DEVICE_PATH_MAG_EXT : AK09916_DEVICE_PATH_MAG);
@ -122,20 +107,6 @@ void start(bool external_bus, enum Rotation rotation) @@ -122,20 +107,6 @@ void start(bool external_bus, enum Rotation rotation)
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(path, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
close(fd);
PX4_ERR("Failed to setup poll rate");
}
close(fd);
exit(0);
fail:
@ -146,72 +117,21 @@ fail: @@ -146,72 +117,21 @@ fail:
PX4_ERR("driver start failed");
exit(1);
}
void test(bool external_bus)
{
int fd = -1;
const char *path = (external_bus ? AK09916_DEVICE_PATH_MAG_EXT : AK09916_DEVICE_PATH_MAG);
struct mag_report m_report;
ssize_t sz;
/* get the driver */
fd = open(path, O_RDONLY);
if (fd < 0) {
PX4_ERR("%s open failed (try 'AK09916 start' if the driver is not running)", path);
exit(1);
}
/* do a simple demand read */
sz = read(fd, &m_report, sizeof(m_report));
if (sz != sizeof(m_report)) {
PX4_ERR("immediate mag read failed");
exit(1);
}
PX4_WARN("single read");
PX4_WARN("time: %lld", m_report.timestamp);
PX4_WARN("mag x: \t%8.4f\t", (double)m_report.x);
PX4_WARN("mag y: \t%8.4f\t", (double)m_report.y);
PX4_WARN("mag z: \t%8.4f\t", (double)m_report.z);
PX4_WARN("mag x: \t%d\traw 0x%0x", (short)m_report.x_raw, (unsigned short)m_report.x_raw);
PX4_WARN("mag y: \t%d\traw 0x%0x", (short)m_report.y_raw, (unsigned short)m_report.y_raw);
PX4_WARN("mag z: \t%d\traw 0x%0x", (short)m_report.z_raw, (unsigned short)m_report.z_raw);
PX4_ERR("PASS");
exit(0);
}
void
reset(bool external_bus)
stop(bool external_bus)
{
const char *path = external_bus ? AK09916_DEVICE_PATH_MAG_EXT : AK09916_DEVICE_PATH_MAG;
int fd = open(path, O_RDONLY);
if (fd < 0) {
PX4_ERR("failed");
exit(1);
}
AK09916 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
if (*g_dev_ptr == nullptr) {
PX4_ERR("driver not running");
exit(1);
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("driver poll restart failed");
exit(1);
}
(*g_dev_ptr)->stop();
exit(0);
}
void
@ -228,13 +148,12 @@ info(bool external_bus) @@ -228,13 +148,12 @@ info(bool external_bus)
(*g_dev_ptr)->print_info();
exit(0);
}
void
usage()
{
PX4_WARN("missing command: try 'start', 'info', 'test', 'stop',\n'reset'");
PX4_WARN("missing command: try 'start', 'info', stop'");
PX4_WARN("options:");
PX4_WARN(" -X (external bus)");
@ -246,52 +165,20 @@ usage() @@ -246,52 +165,20 @@ usage()
// Otherwise, it will passthrough the parent AK09916
AK09916::AK09916(int bus, const char *path, enum Rotation rotation) :
I2C("AK09916", path, bus, AK09916_I2C_ADDR, 400000),
_measure_ticks(0),
_rotation(rotation),
_mag_topic(nullptr),
_mag_orb_class_instance(-1),
_mag_class_instance(-1),
_mag_reading_data(false),
_mag_reports(nullptr),
_mag_scale{},
_mag_range_scale(),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
_px4_mag(get_device_id(), ORB_PRIO_MAX, rotation),
_mag_reads(perf_alloc(PC_COUNT, "ak09916_mag_reads")),
_mag_errors(perf_alloc(PC_COUNT, "ak09916_mag_errors")),
_mag_overruns(perf_alloc(PC_COUNT, "ak09916_mag_overruns")),
_mag_overflows(perf_alloc(PC_COUNT, "ak09916_mag_overflows")),
_mag_duplicates(perf_alloc(PC_COUNT, "ak09916_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, "ak09916_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 = AK09916_MAG_RANGE_GA;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916);
_px4_mag.set_scale(AK09916_MAG_RANGE_GA);
}
AK09916::~AK09916()
{
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);
@ -311,30 +198,15 @@ AK09916::init() @@ -311,30 +198,15 @@ AK09916::init()
return ret;
}
reset();
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
ret = reset();
if (_mag_reports == nullptr) {
return -ENOMEM;
if (ret != PX4_OK) {
return PX4_ERROR;
}
_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, ORB_PRIO_VERY_HIGH);
// &_mag_orb_class_instance, ORB_PRIO_LOW);
if (_mag_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
return -ENOMEM;
}
start();
return OK;
return PX4_OK;
}
bool AK09916::check_duplicate(uint8_t *mag_data)
@ -352,30 +224,22 @@ bool AK09916::check_duplicate(uint8_t *mag_data) @@ -352,30 +224,22 @@ bool AK09916::check_duplicate(uint8_t *mag_data)
void
AK09916::measure()
{
uint8_t ret, cmd = AK09916REG_ST1;
uint8_t cmd = AK09916REG_ST1;
struct ak09916_regs raw_data;
ret = transfer(&cmd, 1, (uint8_t *)(&raw_data), sizeof(struct ak09916_regs));
const hrt_abstime timestamp_sample = hrt_absolute_time();
uint8_t ret = transfer(&cmd, 1, (uint8_t *)(&raw_data), sizeof(struct ak09916_regs));
if (ret == OK) {
raw_data.st2 = raw_data.st2;
_measure(raw_data);
}
}
void
AK09916::_measure(struct ak09916_regs data)
{
bool mag_notify = true;
if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) {
if (check_duplicate((uint8_t *)&raw_data.x) && !(raw_data.st1 & 0x02)) {
perf_count(_mag_duplicates);
return;
}
/* monitor for if data overrun flag is ever set */
if (data.st1 & 0x02) {
if (raw_data.st1 & 0x02) {
perf_count(_mag_overruns);
}
@ -383,182 +247,29 @@ AK09916::_measure(struct ak09916_regs data) @@ -383,182 +247,29 @@ AK09916::_measure(struct ak09916_regs data)
* 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
*/
if (data.st2 & 0x08) {
if (raw_data.st2 & 0x08) {
perf_count(_mag_overflows);
}
mag_report mrb;
mrb.timestamp = hrt_absolute_time();
mrb.is_external = true;
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(_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;
_last_report.x = mrb.x;
_last_report.y = mrb.y;
_last_report.z = mrb.z;
mrb.scaling = _mag_range_scale;
mrb.device_id = _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);
_px4_mag.set_error_count(perf_event_count(_mag_errors));
_px4_mag.update(timestamp_sample, raw_data.x, raw_data.y, raw_data.z);
}
}
ssize_t
AK09916::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(mag_report);
struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_mag_reports->flush();
/* trigger a measurement */
measure();
if (_mag_reports->get(mag_buf)) {
ret = sizeof(struct mag_report);
}
} while (0);
/* return the number of bytes transferred */
return ret;
}
void
AK09916::print_info()
{
perf_print_counter(_mag_reads);
perf_print_counter(_mag_errors);
perf_print_counter(_mag_overruns);
_mag_reports->print_info("mag queue");
printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z);
printf("\n");
}
int
AK09916::ioctl(struct file *filp, int cmd, unsigned long arg)
{
/*
* Repeated in ICM20948_accel::ioctl
* Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500
*/
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(AK09916_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(AK09916_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCRESET:
return reset();
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;
case MAGIOCCALIBRATE:
return OK;
case MAGIOCEXSTRAP:
return OK;
default:
return (int)I2C::ioctl(filp, cmd, arg);
}
_px4_mag.print_status();
}
uint8_t
AK09916::read_reg(uint8_t reg)
{
const uint8_t cmd = reg;
uint8_t ret;
uint8_t ret{};
transfer(&cmd, 1, &ret, 1);
@ -581,7 +292,7 @@ AK09916::write_reg(uint8_t reg, uint8_t value) @@ -581,7 +292,7 @@ AK09916::write_reg(uint8_t reg, uint8_t value)
}
int
AK09916::reset(void)
AK09916::reset()
{
int rv = probe();
@ -597,7 +308,7 @@ AK09916::reset(void) @@ -597,7 +308,7 @@ AK09916::reset(void)
}
int
AK09916::probe(void)
AK09916::probe()
{
int retries = 10;
@ -617,60 +328,44 @@ AK09916::probe(void) @@ -617,60 +328,44 @@ AK09916::probe(void)
}
int
AK09916::setup(void)
AK09916::setup()
{
write_reg(AK09916REG_CNTL2, AK09916_CNTL2_CONTINOUS_MODE_100HZ);
return OK;
}
void
AK09916::cycle_trampoline(void *arg)
{
AK09916 *dev = (AK09916 *)arg;
dev->cycle();
}
void
AK09916::start()
{
/* reset the report ring and state machine */
_mag_reports->flush();
_measure_interval = AK09916_CONVERSION_INTERVAL;
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&AK09916::cycle_trampoline, this, 1);
ScheduleNow();
}
void
AK09916::stop()
{
if (_measure_ticks > 0) {
/* ensure no new items are queued while we cancel this one */
_measure_ticks = 0;
work_cancel(HPWORK, &_work);
}
_measure_interval = 0;
ScheduleClear();
}
void
AK09916::cycle()
AK09916::Run()
{
if (_measure_ticks == 0) {
if (_measure_interval == 0) {
return;
}
/* measurement phase */
measure();
if (_measure_ticks > 0) {
if (_measure_interval > 0) {
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&AK09916::cycle_trampoline,
this,
USEC2TICK(AK09916_CONVERSION_INTERVAL));
ScheduleDelayed(_measure_interval);
}
}
@ -713,19 +408,11 @@ ak09916_main(int argc, char *argv[]) @@ -713,19 +408,11 @@ ak09916_main(int argc, char *argv[])
ak09916::start(external_bus, rotation);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
ak09916::test(external_bus);
}
/*
* Reset the driver.
* Stop the driver.
*/
if (!strcmp(verb, "reset")) {
ak09916::reset(external_bus);
if (!strcmp(verb, "stop")) {
ak09916::stop(external_bus);
}
/*

90
src/drivers/magnetometer/ak09916/ak09916.hpp
Unescape View File

@ -35,47 +35,18 @@ @@ -35,47 +35,18 @@
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <px4_log.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <nuttx/wqueue.h>
#include <lib/perf/perf_counter.h>
#include <systemlib/conversions.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/integrator.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_mag.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#define AK09916_SAMPLE_RATE 100
#define AK09916_DEVICE_PATH_MAG "/dev/ak09916_i2c_int"
#define AK09916_DEVICE_PATH_MAG_EXT "/dev/ak09916_i2c_ext"
/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
#define AK09916_MAG_RANGE_GA 1.5e-3f;
static constexpr float AK09916_MAG_RANGE_GA{1.5e-3f};
/* ak09916 deviating register addresses and bit definitions */
#define AK09916_I2C_ADDR 0x0C
@ -85,30 +56,13 @@ @@ -85,30 +56,13 @@
#define AK09916REG_WIA 0x00
#define AK09916REG_HXL 0x11
#define AK09916REG_HXH 0x12
#define AK09916REG_HYL 0x13
#define AK09916REG_HYH 0x14
#define AK09916REG_HZL 0x15
#define AK09916REG_HZH 0x16
#define AK09916REG_ST1 0x10
#define AK09916REG_ST2 0x18
#define AK09916REG_CNTL2 0x31
#define AK09916REG_CNTL3 0x32
#define AK09916_CNTL2_POWERDOWN_MODE 0x00
#define AK09916_RESET 0x01
#define AK09916_CNTL2_SINGLE_MODE 0x01 /* default */
#define AK09916_CNTL2_CONTINOUS_MODE_10HZ 0x02
#define AK09916_CNTL2_CONTINOUS_MODE_20HZ 0x04
#define AK09916_CNTL2_CONTINOUS_MODE_50HZ 0x06
#define AK09916_CNTL2_CONTINOUS_MODE_100HZ 0x08
#define AK09916_CNTL2_SELFTEST_MODE 0x10
#define AK09916_CNTL3_SRST 0x01
#define AK09916_ST1_DRDY 0x01
#define AK09916_ST1_DOR 0x02
#pragma pack(push, 1)
struct ak09916_regs {
@ -121,20 +75,16 @@ struct ak09916_regs { @@ -121,20 +75,16 @@ struct ak09916_regs {
};
#pragma pack(pop)
/**
* Helper class implementing the magnetometer driver node.
*/
class AK09916 : public device::I2C
class AK09916 : public device::I2C, px4::ScheduledWorkItem
{
public:
AK09916(int bus, const char *path, enum Rotation rotation);
~AK09916();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
void read_block(uint8_t reg, uint8_t *val, uint8_t count);
@ -145,51 +95,35 @@ public: @@ -145,51 +95,35 @@ public:
int setup_master_i2c(void);
bool check_id(uint8_t &id);
void Run();
static void cycle_trampoline(void *arg);
void start(void);
void stop(void);
void cycle(void);
protected:
friend class ICM20948;
/* Directly measure from the _interface if possible */
void measure();
/* Update the state with prefetched data (internally called by the regular measure() )*/
void _measure(struct ak09916_regs data);
uint8_t read_reg(uint8_t reg);
void write_reg(uint8_t reg, uint8_t value);
private:
work_s _work{};
unsigned _measure_ticks;
enum Rotation _rotation;
orb_advert_t _mag_topic;
int _mag_orb_class_instance;
int _mag_class_instance;
bool _mag_reading_data;
ringbuffer::RingBuffer *_mag_reports;
mag_report _last_report {}; /**< used for info() */
struct mag_calibration_s _mag_scale;
float _mag_range_scale;
PX4Magnetometer _px4_mag;
uint32_t _measure_interval{0};
perf_counter_t _mag_reads;
perf_counter_t _mag_errors;
perf_counter_t _mag_overruns;
perf_counter_t _mag_overflows;
perf_counter_t _mag_duplicates;
float _mag_asa_x;
float _mag_asa_y;
float _mag_asa_z;
bool check_duplicate(uint8_t *mag_data);
// keep last mag reading for duplicate detection
uint8_t _last_mag_data[6];
uint8_t _last_mag_data[6] {};
/* do not allow to copy this class due to pointer data members */
AK09916(const AK09916 &);

Write Preview
Loading…
Cancel
Save