mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-20 20:03:54 +08:00
AK09916 move to new WQ, PX4Magnetometer, and cleanup
This commit is contained in:
Daniel Agar
@@ -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
|
||||
|
||||
@@ -39,5 +39,7 @@ px4_add_module(
|
||||
SRCS
|
||||
ak09916.cpp
|
||||
DEPENDS
|
||||
drivers_magnetometer
|
||||
px4_work_queue
|
||||
)
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 @@
|
||||
|
||||
#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_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 {
|
||||
};
|
||||
#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:
|
||||
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 &);
|
||||
|
||||
Reference in New Issue
Block a user