AK09916 move to new WQ, PX4Magnetometer, and cleanup

src/drivers/drv_sensor.h

@@ -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

src/drivers/magnetometer/ak09916/CMakeLists.txt

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

src/drivers/magnetometer/ak09916/ak09916.hpp

@@ -35,47 +35,18 @@
 
 
 #include <px4_config.h>
-
+#include <lib/perf/perf_counter.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 <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 */
-
+static constexpr float AK09916_MAG_RANGE_GA{1.5e-3f};
-#define 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_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;
+	PX4Magnetometer _px4_mag;
-	orb_advert_t _mag_topic;
+
-	int _mag_orb_class_instance;
+	uint32_t _measure_interval{0};
-	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;
 	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 &);