AP_Compass: implement device IDs for compass drivers

libraries/AP_Compass/AP_Compass.h

@@ -16,11 +16,9 @@
 #define AP_COMPASS_TYPE_UNKNOWN         0x00
 #define AP_COMPASS_TYPE_HIL             0x01
 #define AP_COMPASS_TYPE_HMC5843         0x02
-#define AP_COMPASS_TYPE_HMC5883L        0x03
 #define AP_COMPASS_TYPE_PX4             0x04
 #define AP_COMPASS_TYPE_VRBRAIN         0x05
-#define AP_COMPASS_TYPE_AK8963_MPU9250  0x06
+#define AP_COMPASS_TYPE_AK8963          0x06
-#define AP_COMPASS_TYPE_AK8963_I2C      0x07
 #define AP_COMPASS_TYPE_LSM303D         0x08
 #define AP_COMPASS_TYPE_LSM9DS1         0x09
 #define AP_COMPASS_TYPE_BMM150          0x0A

libraries/AP_Compass/AP_Compass_AK8963.cpp

@@ -52,11 +52,9 @@ struct PACKED sample_regs {
 
 extern const AP_HAL::HAL &hal;
 
-AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus,
+AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus)
-                                     uint32_t dev_id)
     : AP_Compass_Backend(compass)
     , _bus(bus)
-    , _dev_id(dev_id)
 {
 }
 
@@ -73,7 +71,7 @@ AP_Compass_Backend *AP_Compass_AK8963::probe(Compass &compass,
         return nullptr;
     }
 
-    AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, AP_COMPASS_TYPE_AK8963_I2C);
+    AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus);
     if (!sensor || !sensor->init()) {
         delete sensor;
         return nullptr;
@@ -103,7 +101,7 @@ AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t m
         return nullptr;
     }
 
-    AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, AP_COMPASS_TYPE_AK8963_MPU9250);
+    AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus);
     if (!sensor || !sensor->init()) {
         delete sensor;
         return nullptr;
@@ -150,7 +148,9 @@ bool AP_Compass_AK8963::init()
 
     /* register the compass instance in the frontend */
     _compass_instance = register_compass();
-    set_dev_id(_compass_instance, _dev_id);
+
+    _bus->set_device_type(AP_COMPASS_TYPE_AK8963);
+    set_dev_id(_compass_instance, _bus->get_bus_id());
 
     bus_sem->give();
 
@@ -431,3 +431,14 @@ AP_HAL::Device::PeriodicHandle AP_AK8963_BusDriver_Auxiliary::register_periodic_
     return _bus->register_periodic_callback(period_usec, cb);
 }
 
+// set device type within a device class
+void AP_AK8963_BusDriver_Auxiliary::set_device_type(uint8_t devtype)
+{
+    _bus->set_device_type(devtype);
+}
+
+// return 24 bit bus identifier
+uint32_t AP_AK8963_BusDriver_Auxiliary::get_bus_id(void) const
+{
+    return _bus->get_bus_id();
+}

libraries/AP_Compass/AP_Compass_AK8963.h

@@ -36,8 +36,7 @@ public:
     void read() override;
 
 private:
-    AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus,
+    AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus);
-                      uint32_t dev_id);
 
     void _make_factory_sensitivity_adjustment(Vector3f &field) const;
     void _make_adc_sensitivity_adjustment(Vector3f &field) const;
@@ -58,7 +57,6 @@ private:
     float _mag_z_accum;
     uint32_t _accum_count;
     uint32_t _last_update_timestamp;
-    uint32_t _dev_id;
 
     uint8_t _compass_instance;
     bool _initialized;
@@ -79,6 +77,12 @@ public:
     virtual bool configure() { return true; }
     virtual bool start_measurements() { return true; }
     virtual AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t, AP_HAL::Device::PeriodicCb) = 0;
+
+    // set device type within a device class
+    virtual void set_device_type(uint8_t devtype) = 0;
+
+    // return 24 bit bus identifier
+    virtual uint32_t get_bus_id(void) const = 0;
 };
 
 class AP_AK8963_BusDriver_HALDevice: public AP_AK8963_BusDriver
@@ -93,6 +97,16 @@ public:
     virtual AP_HAL::Semaphore  *get_semaphore() override;
     AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
 
+    // set device type within a device class
+    void set_device_type(uint8_t devtype) override {
+        _dev->set_device_type(devtype);
+    }
+
+    // return 24 bit bus identifier
+    uint32_t get_bus_id(void) const override {
+        return _dev->get_bus_id();
+    }
+    
 private:
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
 };
@@ -115,6 +129,12 @@ public:
     bool configure();
     bool start_measurements();
 
+    // set device type within a device class
+    void set_device_type(uint8_t devtype) override;
+
+    // return 24 bit bus identifier
+    uint32_t get_bus_id(void) const override;
+    
 private:
     AuxiliaryBus *_bus;
     AuxiliaryBusSlave *_slave;

libraries/AP_Compass/AP_Compass_BMM150.cpp

@@ -182,7 +182,9 @@ bool AP_Compass_BMM150::init()
 
     /* register the compass instance in the frontend */
     _compass_instance = register_compass();
-    set_dev_id(_compass_instance, AP_COMPASS_TYPE_BMM150);
+
+    _dev->set_device_type(AP_COMPASS_TYPE_BMM150);
+    set_dev_id(_compass_instance, _dev->get_bus_id());
 
     _dev->register_periodic_callback(MEASURE_TIME_USEC,
             FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, bool));

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -180,7 +180,9 @@ bool AP_Compass_HMC5843::init()
     read();
 
     _compass_instance = register_compass();
-    set_dev_id(_compass_instance, _product_id);
+
+    _bus->set_device_type(AP_COMPASS_TYPE_HMC5843);
+    set_dev_id(_compass_instance, _bus->get_bus_id());
 
     if (_force_external) {
         set_external(_compass_instance, true);
@@ -221,8 +223,7 @@ bool AP_Compass_HMC5843::_timer()
     raw_field *= _gain_scale;
     
     // rotate to the desired orientation
-    if (is_external(_compass_instance) &&
+    if (is_external(_compass_instance) && _is_hmc5883L) {
-        _product_id == AP_COMPASS_TYPE_HMC5883L) {
         raw_field.rotate(ROTATION_YAW_90);
     }
 
@@ -246,7 +247,6 @@ bool AP_Compass_HMC5843::_timer()
             _mag_z_accum /= 2;
             _accum_count = 7;
         }
-        _last_accum_time = tnow;
         _sem->give();
     }
     
@@ -325,7 +325,7 @@ bool AP_Compass_HMC5843::_read_sample()
     ry = be16toh(val.ry);
     rz = be16toh(val.rz);
 
-    if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
+    if (_is_hmc5883L) {
         std::swap(ry, rz);
     }
 
@@ -355,11 +355,11 @@ bool AP_Compass_HMC5843::_detect_version()
 
     if (_base_config == try_config) {
         /* a 5883L supports the sample averaging config */
-        _product_id = AP_COMPASS_TYPE_HMC5883L;
+        _is_hmc5883L = true;
         _gain_config = HMC5883L_GAIN_1_30_GA;
         _gain_scale = (1.0f / 1090) * 1000;
     } else if (_base_config == (HMC5843_OPMODE_NORMAL | HMC5843_OSR_75HZ)) {
-        _product_id = AP_COMPASS_TYPE_HMC5843;
+        _is_hmc5883L = false;
         _gain_config = HMC5843_GAIN_1_00_GA;
         _gain_scale = (1.0f / 1300) * 1000;
     } else {
@@ -378,7 +378,7 @@ bool AP_Compass_HMC5843::_calibrate()
     int numAttempts = 0, good_count = 0;
     bool success = false;
 
-    if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
+    if (_is_hmc5883L) {
         calibration_gain = HMC5883L_GAIN_2_50_GA;
         /*
          * note that the HMC5883 datasheet gives the x and y expected
@@ -589,4 +589,16 @@ AP_HAL::Device::PeriodicHandle AP_HMC5843_BusDriver_Auxiliary::register_periodic
     return _bus->register_periodic_callback(period_usec, cb);
 }
 
+// set device type within a device class
+void AP_HMC5843_BusDriver_Auxiliary::set_device_type(uint8_t devtype)
+{
+    _bus->set_device_type(devtype);
+}
+
+// return 24 bit bus identifier
+uint32_t AP_HMC5843_BusDriver_Auxiliary::get_bus_id(void) const
+{
+    return _bus->get_bus_id();
+}
+
 #endif

libraries/AP_Compass/AP_Compass_HMC5843.h

@@ -46,7 +46,6 @@ private:
 
     float _scaling[3];
     float _gain_scale;
-    uint32_t _last_accum_time;
 
     // when unhealthy the millis() value to retry at
     uint32_t _retry_time;
@@ -62,10 +61,10 @@ private:
     uint8_t _base_config;
     uint8_t _compass_instance;
     uint8_t _gain_config;
-    uint8_t _product_id;
 
-    bool _initialised;
+    bool _is_hmc5883L:1;
-    bool _force_external;
+    bool _initialised:1;
+    bool _force_external:1;
 };
 
 class AP_HMC5843_BusDriver
@@ -83,6 +82,12 @@ public:
     virtual bool start_measurements() { return true; }
 
     virtual AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t, AP_HAL::Device::PeriodicCb) = 0;
+
+    // set device type within a device class
+    virtual void set_device_type(uint8_t devtype) = 0;
+
+    // return 24 bit bus identifier
+    virtual uint32_t get_bus_id(void) const = 0;
 };
 
 class AP_HMC5843_BusDriver_HALDevice : public AP_HMC5843_BusDriver
@@ -98,6 +103,16 @@ public:
 
     AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
 
+    // set device type within a device class
+    void set_device_type(uint8_t devtype) override {
+        _dev->set_device_type(devtype);
+    }
+
+    // return 24 bit bus identifier
+    uint32_t get_bus_id(void) const override {
+        return _dev->get_bus_id();
+    }
+    
 private:
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;
 };
@@ -120,6 +135,12 @@ public:
 
     AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
 
+    // set device type within a device class
+    void set_device_type(uint8_t devtype) override;
+
+    // return 24 bit bus identifier
+    uint32_t get_bus_id(void) const override;
+    
 private:
     AuxiliaryBus *_bus;
     AuxiliaryBusSlave *_slave;

libraries/AP_Compass/AP_Compass_LSM303D.cpp

@@ -268,7 +268,9 @@ bool AP_Compass_LSM303D::init()
 
     /* register the compass instance in the frontend */
     _compass_instance = register_compass();
-    set_dev_id(_compass_instance, AP_COMPASS_TYPE_LSM303D);
+
+    _dev->set_device_type(AP_COMPASS_TYPE_LSM303D);
+    set_dev_id(_compass_instance, _dev->get_bus_id());
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
     // FIXME: wrong way to force internal compass

libraries/AP_Compass/AP_Compass_LSM9DS1.cpp

@@ -60,7 +60,7 @@ extern const AP_HAL::HAL &hal;
 AP_Compass_Backend *AP_Compass_LSM9DS1::probe(Compass &compass,
                                               AP_HAL::OwnPtr<AP_HAL::Device> dev)
 {
-    AP_Compass_LSM9DS1 *sensor = new AP_Compass_LSM9DS1(compass, std::move(dev), AP_COMPASS_TYPE_LSM9DS1);
+    AP_Compass_LSM9DS1 *sensor = new AP_Compass_LSM9DS1(compass, std::move(dev));
     if (!sensor || !sensor->init()) {
         delete sensor;
         return nullptr;
@@ -94,7 +94,9 @@ bool AP_Compass_LSM9DS1::init()
     }
 
     _compass_instance = register_compass();
-    set_dev_id(_compass_instance, _dev_id);
+
+    _dev->set_device_type(AP_COMPASS_TYPE_LSM9DS1);
+    set_dev_id(_compass_instance, _dev->get_bus_id());
 
     _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM9DS1::_update, bool));
 
@@ -230,10 +232,9 @@ bool AP_Compass_LSM9DS1::_set_scale(void)
     return true;
 }
 
-AP_Compass_LSM9DS1::AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev, uint32_t dev_id)
+AP_Compass_LSM9DS1::AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev)
     : AP_Compass_Backend(compass)
     , _dev(std::move(dev))
-    , _dev_id(dev_id)
 {
 }
 

libraries/AP_Compass/AP_Compass_LSM9DS1.h

@@ -22,7 +22,7 @@ public:
     virtual ~AP_Compass_LSM9DS1() {}
 
 private:
-    AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev, uint32_t dev_id);
+    AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev);
     bool _check_id(void);
     bool _configure(void);
     bool _set_scale(void);
@@ -35,7 +35,6 @@ private:
     void _dump_registers();
 
     AP_HAL::OwnPtr<AP_HAL::Device> _dev;
-    uint32_t _dev_id;
     uint8_t _compass_instance;
     float _scaling;
     float _mag_x_accum;