mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
ak09916: cleanup only
Note: the author name was removed because this file has almost no resemblence with the code written by that author 4 years ago, has been copied to new places, and was initially commited without author anyway. Also, my opinion is that the version control system should take care of attribution, and not outdated comments.
This commit is contained in:
Julian Oes
parent
748b664ad0
commit
474b8028d0
@ -32,12 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mag.cpp
|
||||
*
|
||||
* Driver for the ak09916 magnetometer within the Invensense mpu9250
|
||||
*
|
||||
* @author Robert Dickenson
|
||||
*
|
||||
* Driver for the standalone AK09916 magnetometer.
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
@ -51,10 +46,8 @@
|
||||
#include "ak09916.hpp"
|
||||
|
||||
|
||||
/** driver 'main' command */
|
||||
extern "C" { __EXPORT int ak09916_main(int argc, char *argv[]); }
|
||||
|
||||
#define AK09916_CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
|
||||
|
||||
namespace ak09916
|
||||
{
|
||||
@ -66,25 +59,16 @@ void start(bool, enum Rotation);
|
||||
void info(bool);
|
||||
void usage();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*
|
||||
* This function only returns if the driver is up and running
|
||||
* or failed to detect the sensor.
|
||||
*/
|
||||
void start(bool external_bus, enum Rotation rotation)
|
||||
{
|
||||
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);
|
||||
|
||||
if (*g_dev_ptr != nullptr)
|
||||
/* if already started, the still command succeeded */
|
||||
{
|
||||
if (*g_dev_ptr != nullptr) {
|
||||
PX4_ERR("already started");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
if (external_bus) {
|
||||
#if defined(PX4_I2C_BUS_EXPANSION)
|
||||
*g_dev_ptr = new AK09916(PX4_I2C_BUS_EXPANSION, path, rotation);
|
||||
@ -98,25 +82,14 @@ void start(bool external_bus, enum Rotation rotation)
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (*g_dev_ptr == nullptr) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
|
||||
if (OK != (*g_dev_ptr)->init()) {
|
||||
goto fail;
|
||||
if (*g_dev_ptr == nullptr || (OK != (*g_dev_ptr)->init())) {
|
||||
PX4_ERR("driver start failed");
|
||||
delete (*g_dev_ptr);
|
||||
*g_dev_ptr = nullptr;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
exit(0);
|
||||
fail:
|
||||
|
||||
if (*g_dev_ptr != nullptr) {
|
||||
delete (*g_dev_ptr);
|
||||
*g_dev_ptr = nullptr;
|
||||
}
|
||||
|
||||
PX4_ERR("driver start failed");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
void
|
||||
@ -153,16 +126,14 @@ info(bool external_bus)
|
||||
void
|
||||
usage()
|
||||
{
|
||||
PX4_WARN("missing command: try 'start', 'info', stop'");
|
||||
PX4_WARN("options:");
|
||||
PX4_WARN(" -X (external bus)");
|
||||
|
||||
PX4_INFO("missing command: try 'start', 'info', stop'");
|
||||
PX4_INFO("options:");
|
||||
PX4_INFO(" -X (external bus)");
|
||||
PX4_INFO(" -R (rotation)");
|
||||
}
|
||||
|
||||
} // namespace AK09916
|
||||
} // namespace ak09916
|
||||
|
||||
// If interface is non-null, then it will used for interacting with the device.
|
||||
// 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),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
|
||||
@ -191,17 +162,15 @@ AK09916::init()
|
||||
{
|
||||
int ret = I2C::init();
|
||||
|
||||
/* if cdev init failed, bail now */
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("AK09916 mag init failed");
|
||||
|
||||
PX4_WARN("AK09916 mag init failed");
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = reset();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
return PX4_ERROR;
|
||||
return ret;
|
||||
}
|
||||
|
||||
start();
|
||||
@ -212,7 +181,7 @@ AK09916::init()
|
||||
bool AK09916::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
|
||||
// It isn't new data - wait for next timer.
|
||||
return true;
|
||||
|
||||
} else {
|
||||
@ -238,15 +207,15 @@ AK09916::measure()
|
||||
return;
|
||||
}
|
||||
|
||||
/* monitor for if data overrun flag is ever set */
|
||||
/* Monitor for if data overrun flag is ever set. */
|
||||
if (raw_data.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
|
||||
*/
|
||||
/* 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.
|
||||
*/
|
||||
if (raw_data.st2 & 0x08) {
|
||||
perf_count(_mag_overflows);
|
||||
}
|
||||
@ -278,9 +247,9 @@ AK09916::read_reg(uint8_t reg)
|
||||
}
|
||||
|
||||
bool
|
||||
AK09916::check_id(uint8_t &deviceid)
|
||||
AK09916::check_id()
|
||||
{
|
||||
deviceid = read_reg(AK09916REG_WIA);
|
||||
const uint8_t deviceid = read_reg(AK09916REG_WIA);
|
||||
|
||||
return (AK09916_DEVICE_ID_A == deviceid);
|
||||
}
|
||||
@ -298,10 +267,10 @@ AK09916::reset()
|
||||
int rv = probe();
|
||||
|
||||
if (rv == OK) {
|
||||
// Now reset the mag
|
||||
// Now reset the mag.
|
||||
write_reg(AK09916REG_CNTL3, AK09916_RESET);
|
||||
|
||||
// Then re-initialize the bus/mag
|
||||
// Then re-initialize the bus/mag.
|
||||
rv = setup();
|
||||
}
|
||||
|
||||
@ -316,9 +285,7 @@ AK09916::probe()
|
||||
do {
|
||||
write_reg(AK09916REG_CNTL3, AK09916_RESET);
|
||||
|
||||
uint8_t id = 0;
|
||||
|
||||
if (check_id(id)) {
|
||||
if (check_id()) {
|
||||
return OK;
|
||||
}
|
||||
|
||||
@ -339,17 +306,16 @@ AK09916::setup()
|
||||
void
|
||||
AK09916::start()
|
||||
{
|
||||
_measure_interval = AK09916_CONVERSION_INTERVAL;
|
||||
_cycle_interval = AK09916_CONVERSION_INTERVAL_us;
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
void
|
||||
AK09916::stop()
|
||||
{
|
||||
/* ensure no new items are queued while we cancel this one */
|
||||
_measure_interval = 0;
|
||||
// Ensure no new items are queued while we cancel this one.
|
||||
_cycle_interval = 0;
|
||||
|
||||
ScheduleClear();
|
||||
}
|
||||
@ -357,16 +323,14 @@ AK09916::stop()
|
||||
void
|
||||
AK09916::Run()
|
||||
{
|
||||
if (_measure_interval == 0) {
|
||||
if (_cycle_interval == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* measurement phase */
|
||||
measure();
|
||||
|
||||
if (_measure_interval > 0) {
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
ScheduleDelayed(_measure_interval);
|
||||
if (_cycle_interval > 0) {
|
||||
ScheduleDelayed(_cycle_interval);
|
||||
}
|
||||
}
|
||||
|
||||
@ -402,23 +366,14 @@ ak09916_main(int argc, char *argv[])
|
||||
|
||||
const char *verb = argv[myoptind];
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(verb, "start")) {
|
||||
ak09916::start(external_bus, rotation);
|
||||
}
|
||||
|
||||
/*
|
||||
* Stop the driver.
|
||||
*/
|
||||
if (!strcmp(verb, "stop")) {
|
||||
ak09916::stop(external_bus);
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(verb, "info")) {
|
||||
ak09916::info(external_bus);
|
||||
}
|
||||
|
||||
@ -42,27 +42,26 @@
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
|
||||
#define AK09916_DEVICE_PATH_MAG "/dev/ak09916_i2c_int"
|
||||
#define AK09916_DEVICE_PATH_MAG_EXT "/dev/ak09916_i2c_ext"
|
||||
static constexpr auto AK09916_DEVICE_PATH_MAG = "/dev/ak09916_i2c_int";
|
||||
static constexpr auto AK09916_DEVICE_PATH_MAG_EXT = "/dev/ak09916_i2c_ext";
|
||||
|
||||
/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
|
||||
static constexpr float AK09916_MAG_RANGE_GA{1.5e-3f};
|
||||
// in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit.
|
||||
static constexpr float AK09916_MAG_RANGE_GA = 1.5e-3f;
|
||||
|
||||
/* ak09916 deviating register addresses and bit definitions */
|
||||
#define AK09916_I2C_ADDR 0x0C
|
||||
static constexpr uint8_t AK09916_I2C_ADDR = 0x0C;
|
||||
|
||||
#define AK09916_DEVICE_ID_A 0x48
|
||||
#define AK09916_DEVICE_ID_B 0x09 // additional ID byte ("INFO" on AK9063 without content specification.)
|
||||
static constexpr uint8_t AK09916_DEVICE_ID_A = 0x48;
|
||||
static constexpr uint8_t AK09916REG_WIA = 0x00;
|
||||
|
||||
#define AK09916REG_WIA 0x00
|
||||
static constexpr uint8_t AK09916REG_ST1 = 0x10;
|
||||
static constexpr uint8_t AK09916REG_CNTL2 = 0x31;
|
||||
static constexpr uint8_t AK09916REG_CNTL3 = 0x32;
|
||||
|
||||
#define AK09916REG_ST1 0x10
|
||||
#define AK09916REG_CNTL2 0x31
|
||||
#define AK09916REG_CNTL3 0x32
|
||||
static constexpr uint8_t AK09916_RESET = 0x01;
|
||||
static constexpr uint8_t AK09916_CNTL2_CONTINOUS_MODE_100HZ = 0x08;
|
||||
|
||||
#define AK09916_RESET 0x01
|
||||
#define AK09916_CNTL2_SINGLE_MODE 0x01 /* default */
|
||||
#define AK09916_CNTL2_CONTINOUS_MODE_100HZ 0x08
|
||||
// Run at 100 Hz.
|
||||
static constexpr unsigned AK09916_CONVERSION_INTERVAL_us = 1000000 / 100;
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct ak09916_regs {
|
||||
@ -75,49 +74,43 @@ struct ak09916_regs {
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/**
|
||||
* Helper class implementing the magnetometer driver node.
|
||||
*/
|
||||
|
||||
class AK09916 : public device::I2C, px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
AK09916(int bus, const char *path, enum Rotation rotation);
|
||||
~AK09916();
|
||||
|
||||
virtual int init();
|
||||
|
||||
void read_block(uint8_t reg, uint8_t *val, uint8_t count);
|
||||
|
||||
int reset(void);
|
||||
int probe(void);
|
||||
int setup(void);
|
||||
void print_info(void);
|
||||
int setup_master_i2c(void);
|
||||
bool check_id(uint8_t &id);
|
||||
|
||||
void Run();
|
||||
|
||||
void start(void);
|
||||
void stop(void);
|
||||
virtual int init() override;
|
||||
void start();
|
||||
void stop();
|
||||
void cycle();
|
||||
void print_info();
|
||||
int probe();
|
||||
|
||||
protected:
|
||||
|
||||
/* Directly measure from the _interface if possible */
|
||||
int setup();
|
||||
int setup_master_i2c();
|
||||
bool check_id();
|
||||
void Run();
|
||||
void measure();
|
||||
int reset();
|
||||
|
||||
uint8_t read_reg(uint8_t reg);
|
||||
void read_block(uint8_t reg, uint8_t *val, uint8_t count);
|
||||
void write_reg(uint8_t reg, uint8_t value);
|
||||
|
||||
private:
|
||||
|
||||
PX4Magnetometer _px4_mag;
|
||||
|
||||
uint32_t _measure_interval{0};
|
||||
uint32_t _cycle_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;
|
||||
|
||||
bool check_duplicate(uint8_t *mag_data);
|
||||
@ -125,7 +118,6 @@ private:
|
||||
// keep last mag reading for duplicate detection
|
||||
uint8_t _last_mag_data[6] {};
|
||||
|
||||
/* do not allow to copy this class due to pointer data members */
|
||||
AK09916(const AK09916 &);
|
||||
AK09916 operator=(const AK09916 &);
|
||||
AK09916(const AK09916 &) = delete;
|
||||
AK09916 operator=(const AK09916 &) = delete;
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user