AP_Compass: LSM303D: cleanup driver

Make it similar to other compass drivers, including method names.

libraries/AP_Compass/AP_Compass.cpp

@@ -441,7 +441,7 @@ void Compass::_detect_backends(void)
     _add_backend(AP_Compass_QURT::detect(*this));
 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_RASPILOT
     _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)));
-    _add_backend(AP_Compass_LSM303D::detect_spi(*this));
+    _add_backend(AP_Compass_LSM303D::probe(*this));
 #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH
     // detect_mpu9250() failed will cause panic if no actual mpu9250 backend,
     // in BH, only one compass should be detected

libraries/AP_Compass/AP_Compass_LSM303D.cpp

@@ -14,8 +14,8 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <AP_Math/AP_Math.h>
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
 
 #include "AP_Compass_LSM303D.h"
 
@@ -147,47 +147,25 @@ extern const AP_HAL::HAL& hal;
 
 #define REG7_CONT_MODE_M        ((0<<1) | (0<<0))
 
-
 #define INT_CTRL_M              0x12
 #define INT_SRC_M               0x13
 
-/* default values for this device */
-#define LSM303D_ACCEL_DEFAULT_RANGE_G           8
-#define LSM303D_ACCEL_DEFAULT_RATE          800
-#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ    50
-#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ    30
-
 #define LSM303D_MAG_DEFAULT_RANGE_GA          2
 #define LSM303D_MAG_DEFAULT_RATE            100
 
-#define LSM303D_DEBUG 0
+AP_Compass_LSM303D::AP_Compass_LSM303D(Compass &compass)
-#if LSM303D_DEBUG
+    : AP_Compass_Backend(compass)
-#include <stdio.h>
+{
-#define error(...) fprintf(stderr, __VA_ARGS__)
+}
-#define debug(...) hal.console->printf(__VA_ARGS__)
-#define ASSERT(x) assert(x)
-#else
-#define error(...)
-#define debug(...)
-#define ASSERT(x)
-#endif
 
-// constructor
+AP_Compass_Backend *AP_Compass_LSM303D::probe(Compass &compass)
-AP_Compass_LSM303D::AP_Compass_LSM303D(Compass &compass):
-    AP_Compass_Backend(compass)
-{}
-
-// detect the sensor
-AP_Compass_Backend *AP_Compass_LSM303D::detect_spi(Compass &compass)
 {
     AP_Compass_LSM303D *sensor = new AP_Compass_LSM303D(compass);
-    if (sensor == NULL) {
+    if (!sensor || !sensor->init()) {
-        return NULL;
-    }
-    if (!sensor->init()) {
         delete sensor;
-        return NULL;
+        return nullptr;
     }
+
     return sensor;
 }
 
@@ -239,9 +217,7 @@ bool AP_Compass_LSM303D::_data_ready()
 bool AP_Compass_LSM303D::_read_raw()
 {
     if (_register_read(ADDR_CTRL_REG7) != _reg7_expected) {
-        hal.console->println(
+        hal.console->println("LSM303D _read_data_transaction_accel: _reg7_expected unexpected");
-                               "LSM303D _read_data_transaction_accel: _reg7_expected unexpected");
-        // reset();
         return false;
     }
 
@@ -282,13 +258,12 @@ bool AP_Compass_LSM303D::_read_raw()
     return true;
 }
 
-// Public Methods //////////////////////////////////////////////////////////////
+bool AP_Compass_LSM303D::init()
-bool
-AP_Compass_LSM303D::init()
 {
     // TODO: support users without data ready pin
-    if (LSM303D_DRDY_M_PIN < 0)
+    if (LSM303D_DRDY_M_PIN < 0) {
         return false;
+    }
 
     hal.scheduler->suspend_timer_procs();
 
@@ -325,13 +300,12 @@ AP_Compass_LSM303D::init()
         }
     } while (1);
 
-    _scaling[0] = 1.0;
+    _initialised = true;
-    _scaling[1] = 1.0;
-    _scaling[2] = 1.0;
 
     /* register the compass instance in the frontend */
     _compass_instance = register_compass();
-    set_dev_id(_compass_instance, get_dev_id());
+    set_dev_id(_compass_instance, AP_COMPASS_TYPE_LSM303D);
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
     set_external(_compass_instance, false);
 #endif
@@ -340,19 +314,13 @@ AP_Compass_LSM303D::init()
 
     _spi_sem->give();
     hal.scheduler->resume_timer_procs();
-    _initialised = true;
 
-    return _initialised;
+    return true;
 }
 
-uint32_t AP_Compass_LSM303D::get_dev_id()
+bool AP_Compass_LSM303D::_hardware_init()
 {
-    return AP_COMPASS_TYPE_LSM303D;
+    if (!_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-}
-
-bool AP_Compass_LSM303D::_hardware_init(void)
-{
-    if (!_spi_sem->take(100)) {
         AP_HAL::panic("LSM303D: Unable to get semaphore");
     }
 
@@ -371,8 +339,6 @@ bool AP_Compass_LSM303D::_hardware_init(void)
     _mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA);
     _mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE);
 
-    // TODO: Software filtering
-
     // now that we have initialised, we set the SPI bus speed to high
     _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
     _spi_sem->give();
@@ -382,6 +348,9 @@ bool AP_Compass_LSM303D::_hardware_init(void)
 
 void AP_Compass_LSM303D::_update()
 {
+    uint32_t time_us = AP_HAL::micros();
+    Vector3f raw_field;
+
     if (AP_HAL::micros() - _last_update_timestamp < 10000) {
         return;
     }
@@ -390,28 +359,16 @@ void AP_Compass_LSM303D::_update()
         return;
     }
 
-    _collect_samples();
-
-    _last_update_timestamp = AP_HAL::micros();
-    _spi_sem->give();
-}
-
-void AP_Compass_LSM303D::_collect_samples()
-{
-    if (!_initialised) {
-        return;
-    }
-
     if (!_read_raw()) {
-        error("_read_raw() failed\n");
+        goto fail;
-    } else {
+    }
-        Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale;
+
-        uint32_t time_us = AP_HAL::micros();
+    raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale;
 
     // rotate raw_field from sensor frame to body frame
     rotate_field(raw_field, _compass_instance);
 
-        // publish raw_field (uncorrected point sample) for _scaling use
+    // publish raw_field (uncorrected point sample) for calibration use
     publish_raw_field(raw_field, time_us, _compass_instance);
 
     // correct raw_field for known errors
@@ -427,16 +384,17 @@ void AP_Compass_LSM303D::_collect_samples()
         _mag_z_accum /= 2;
         _accum_count = 5;
     }
-    }
+
+    _last_update_timestamp = AP_HAL::micros();
+
+fail:
+    _spi_sem->give();
 }
 
 // Read Sensor data
 void AP_Compass_LSM303D::read()
 {
     if (!_initialised) {
-        // someone has tried to enable a compass for the first time
-        // mid-flight .... we can't do that yet (especially as we won't
-        // have the right orientation!)
         return;
     }
 
@@ -446,9 +404,7 @@ void AP_Compass_LSM303D::read()
     }
 
     hal.scheduler->suspend_timer_procs();
-    Vector3f field(_mag_x_accum * _scaling[0],
+    Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
-                   _mag_y_accum * _scaling[1],
-                   _mag_z_accum * _scaling[2]);
     field /= _accum_count;
 
     _accum_count = 0;
@@ -458,7 +414,7 @@ void AP_Compass_LSM303D::read()
     publish_filtered_field(field, _compass_instance);
 }
 
-void AP_Compass_LSM303D::_disable_i2c(void)
+void AP_Compass_LSM303D::_disable_i2c()
 {
     // TODO: use the register names
     uint8_t a = _register_read(0x02);
@@ -471,68 +427,65 @@ void AP_Compass_LSM303D::_disable_i2c(void)
     _register_write(0x02, (0xE7 & a));
 }
 
-uint8_t AP_Compass_LSM303D::_mag_set_range(uint8_t max_ga)
+bool AP_Compass_LSM303D::_mag_set_range(uint8_t max_ga)
 {
     uint8_t setbits = 0;
     uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
     float new_scale_ga_digit = 0.0f;
 
-    if (max_ga == 0)
+    if (max_ga == 0) {
         max_ga = 12;
+    }
 
     if (max_ga <= 2) {
         _mag_range_ga = 2;
         setbits |= REG6_FULL_SCALE_2GA_M;
         new_scale_ga_digit = 0.080f;
-
     } else if (max_ga <= 4) {
         _mag_range_ga = 4;
         setbits |= REG6_FULL_SCALE_4GA_M;
         new_scale_ga_digit = 0.160f;
-
     } else if (max_ga <= 8) {
         _mag_range_ga = 8;
         setbits |= REG6_FULL_SCALE_8GA_M;
         new_scale_ga_digit = 0.320f;
-
     } else if (max_ga <= 12) {
         _mag_range_ga = 12;
         setbits |= REG6_FULL_SCALE_12GA_M;
         new_scale_ga_digit = 0.479f;
-
     } else {
-        return -1;
+        return false;
     }
 
     _mag_range_scale = new_scale_ga_digit;
     _register_modify(ADDR_CTRL_REG6, clearbits, setbits);
-    return 0;
+
+    return true;
 }
 
-uint8_t AP_Compass_LSM303D::_mag_set_samplerate(uint16_t frequency)
+bool AP_Compass_LSM303D::_mag_set_samplerate(uint16_t frequency)
 {
     uint8_t setbits = 0;
     uint8_t clearbits = REG5_RATE_BITS_M;
 
-    if (frequency == 0)
+    if (frequency == 0) {
         frequency = 100;
+    }
 
     if (frequency <= 25) {
         setbits |= REG5_RATE_25HZ_M;
         _mag_samplerate = 25;
-
     } else if (frequency <= 50) {
         setbits |= REG5_RATE_50HZ_M;
         _mag_samplerate = 50;
-
     } else if (frequency <= 100) {
         setbits |= REG5_RATE_100HZ_M;
         _mag_samplerate = 100;
-
     } else {
-        return -1;
+        return false;
     }
 
     _register_modify(ADDR_CTRL_REG5, clearbits, setbits);
-    return 0;
+
+    return true;
 }

libraries/AP_Compass/AP_Compass_LSM303D.h

@@ -10,49 +10,44 @@
 
 class AP_Compass_LSM303D : public AP_Compass_Backend
 {
+public:
+    static AP_Compass_Backend *probe(Compass &compass);
+
+    bool init() override;
+    void read() override;
+
 private:
-    bool    _read_raw(void);
+    AP_Compass_LSM303D(Compass &compass);
+
+    bool _read_raw();
     uint8_t _register_read(uint8_t reg);
     void _register_write(uint8_t reg, uint8_t val);
     void _register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits);
     bool _data_ready();
-    bool    _hardware_init(void);
+    bool _hardware_init();
-    void    _collect_samples();
     void _update();
-    void    _disable_i2c(void);
+    void _disable_i2c();
-    uint8_t _mag_set_range(uint8_t max_ga);
+    bool _mag_set_range(uint8_t max_ga);
-    uint8_t _mag_set_samplerate(uint16_t frequency);
+    bool _mag_set_samplerate(uint16_t frequency);
 
-    AP_HAL::SPIDeviceDriver *_spi = nullptr;
+    AP_HAL::SPIDeviceDriver *_spi;
-    AP_HAL::Semaphore *_spi_sem = nullptr;
+    AP_HAL::Semaphore *_spi_sem;
-    AP_HAL::DigitalSource *_drdy_pin_m = nullptr;
+    AP_HAL::DigitalSource *_drdy_pin_m;
-    float _scaling[3] = { 0 };
-    bool _initialised = false;
 
-    int16_t _mag_x = 0;
+    float _mag_range_scale;
-    int16_t _mag_y = 0;
+    float _mag_x_accum;
-    int16_t _mag_z = 0;
+    float _mag_y_accum;
-    float _mag_x_accum = 0.0f;
+    float _mag_z_accum;
-    float _mag_y_accum = 0.0f;
+    uint32_t _last_update_timestamp;
-    float _mag_z_accum = 0.0f;
+    int16_t _mag_x;
-    uint32_t _last_accum_time = 0;
+    int16_t _mag_y;
-    uint32_t _last_update_timestamp = 0;
+    int16_t _mag_z;
-    uint8_t _accum_count = 0;
+    uint8_t _accum_count;
 
-    uint8_t _compass_instance = 0;
+    uint8_t _compass_instance;
-    uint8_t _product_id = 0;
+    bool _initialised;
 
-    uint8_t _mag_range_ga = 0;
+    uint8_t _mag_range_ga;
-    uint8_t _mag_samplerate = 0;
+    uint8_t _mag_samplerate;
-    uint8_t _reg7_expected = 0;
+    uint8_t _reg7_expected;
-    float _mag_range_scale = 0.0f;
-
-public:
-    AP_Compass_LSM303D(Compass &compass);
-    bool        init(void);
-    void        read(void);
-    uint32_t    get_dev_id();
-
-    // detect the sensor
-    static AP_Compass_Backend *detect_spi(Compass &compass);
 };