mynewt-commits mailing list archives

Site index · List index
Message view « Date » · « Thread »
Top « Date » · « Thread »
From vipulrah...@apache.org
Subject [20/50] incubator-mynewt-core git commit: Added magnetometer to driver
Date Fri, 24 Feb 2017 19:38:15 GMT
Added magnetometer to driver


Project: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/repo
Commit: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/commit/b04fe061
Tree: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/tree/b04fe061
Diff: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/diff/b04fe061

Branch: refs/heads/develop
Commit: b04fe06170d78cf21d2cc4b039ab94522772143b
Parents: 616f752
Author: microbuilder <contact@microbuilder.eu>
Authored: Thu Dec 29 02:24:20 2016 +0100
Committer: microbuilder <contact@microbuilder.eu>
Committed: Thu Dec 29 02:24:20 2016 +0100

----------------------------------------------------------------------
 .../lsm303dlhc/include/lsm303dlhc/lsm303dlhc.h  |   3 +-
 hw/drivers/sensors/lsm303dlhc/src/lsm303dlhc.c  | 236 +++++++++++++++----
 2 files changed, 189 insertions(+), 50 deletions(-)
----------------------------------------------------------------------


http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/b04fe061/hw/drivers/sensors/lsm303dlhc/include/lsm303dlhc/lsm303dlhc.h
----------------------------------------------------------------------
diff --git a/hw/drivers/sensors/lsm303dlhc/include/lsm303dlhc/lsm303dlhc.h b/hw/drivers/sensors/lsm303dlhc/include/lsm303dlhc/lsm303dlhc.h
index c5e9601..8bc3307 100644
--- a/hw/drivers/sensors/lsm303dlhc/include/lsm303dlhc/lsm303dlhc.h
+++ b/hw/drivers/sensors/lsm303dlhc/include/lsm303dlhc/lsm303dlhc.h
@@ -70,7 +70,8 @@ enum lsm303dlhc_mag_rate {
 struct lsm303dlhc_cfg {
     enum lsm303dlhc_accel_range accel_range;
     enum lsm303dlhc_accel_rate accel_rate;
-    uint16_t sample_itvl;
+    enum lsm303dlhc_mag_gain mag_gain;
+    enum lsm303dlhc_mag_rate mag_rate;
 };
 
 struct lsm303dlhc {

http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/b04fe061/hw/drivers/sensors/lsm303dlhc/src/lsm303dlhc.c
----------------------------------------------------------------------
diff --git a/hw/drivers/sensors/lsm303dlhc/src/lsm303dlhc.c b/hw/drivers/sensors/lsm303dlhc/src/lsm303dlhc.c
index f9c2dee..b3821e1 100644
--- a/hw/drivers/sensors/lsm303dlhc/src/lsm303dlhc.c
+++ b/hw/drivers/sensors/lsm303dlhc/src/lsm303dlhc.c
@@ -27,6 +27,7 @@
 #include "hal/hal_i2c.h"
 #include "sensor/sensor.h"
 #include "sensor/accel.h"
+#include "sensor/mag.h"
 #include "lsm303dlhc/lsm303dlhc.h"
 #include "lsm303dlhc_priv.h"
 
@@ -45,6 +46,13 @@ STATS_SECT_START(lsm303dlhc_stat_section)
     STATS_SECT_ENTRY(samples_acc_4g)
     STATS_SECT_ENTRY(samples_acc_8g)
     STATS_SECT_ENTRY(samples_acc_16g)
+    STATS_SECT_ENTRY(samples_mag_1_3g)
+    STATS_SECT_ENTRY(samples_mag_1_9g)
+    STATS_SECT_ENTRY(samples_mag_2_5g)
+    STATS_SECT_ENTRY(samples_mag_4_0g)
+    STATS_SECT_ENTRY(samples_mag_4_7g)
+    STATS_SECT_ENTRY(samples_mag_5_6g)
+    STATS_SECT_ENTRY(samples_mag_8_1g)
     STATS_SECT_ENTRY(errors)
 STATS_SECT_END
 
@@ -54,6 +62,13 @@ STATS_NAME_START(lsm303dlhc_stat_section)
     STATS_NAME(lsm303dlhc_stat_section, samples_acc_4g)
     STATS_NAME(lsm303dlhc_stat_section, samples_acc_8g)
     STATS_NAME(lsm303dlhc_stat_section, samples_acc_16g)
+    STATS_NAME(lsm303dlhc_stat_section, samples_mag_1_3g)
+    STATS_NAME(lsm303dlhc_stat_section, samples_mag_1_9g)
+    STATS_NAME(lsm303dlhc_stat_section, samples_mag_2_5g)
+    STATS_NAME(lsm303dlhc_stat_section, samples_mag_4_0g)
+    STATS_NAME(lsm303dlhc_stat_section, samples_mag_4_7g)
+    STATS_NAME(lsm303dlhc_stat_section, samples_mag_5_6g)
+    STATS_NAME(lsm303dlhc_stat_section, samples_mag_8_1g)
     STATS_NAME(lsm303dlhc_stat_section, errors)
 STATS_NAME_END(lsm303dlhc_stat_section)
 
@@ -172,7 +187,7 @@ int
 lsm303dlhc_read48(uint8_t addr, uint8_t reg, uint8_t *buffer)
 {
     int rc;
-    uint8_t payload[7] = { reg | 0x80, 0, 0, 0, 0, 0, 0 };
+    uint8_t payload[7] = { reg, 0, 0, 0, 0, 0, 0 };
 
     struct hal_i2c_master_data data_struct = {
         .address = addr,
@@ -258,12 +273,20 @@ lsm303dlhc_init(struct os_dev *dev, void *arg)
         goto err;
     }
 
+    /* Add the accelerometer */
     rc = sensor_set_driver(sensor, SENSOR_TYPE_ACCELEROMETER,
             (struct sensor_driver *) &g_lsm303dlhc_sensor_driver);
     if (rc != 0) {
         goto err;
     }
 
+    /* Add the magnetometer */
+    rc = sensor_set_driver(sensor, SENSOR_TYPE_MAGNETIC_FIELD,
+            (struct sensor_driver *) &g_lsm303dlhc_sensor_driver);
+    if (rc != 0) {
+        goto err;
+    }
+
     rc = sensor_mgr_register(sensor);
     if (rc != 0) {
         goto err;
@@ -279,10 +302,10 @@ lsm303dlhc_config(struct lsm303dlhc *lsm, struct lsm303dlhc_cfg *cfg)
 {
     int rc;
 
-    /* Overwrite the configuration associated with this generic accelleromter. */
+    /* Overwrite the configuration data. */
     memcpy(&lsm->cfg, cfg, sizeof(*cfg));
 
-    /* Set data rate (or power down) and enable XYZ output */
+    /* Set accel data rate (or power down) and enable XYZ output */
     rc = lsm303dlhc_write8(LSM303DLHC_ADDR_ACCEL,
         LSM303DLHC_REGISTER_ACCEL_CTRL_REG1_A,
         lsm->cfg.accel_rate | 0x07);
@@ -290,7 +313,7 @@ lsm303dlhc_config(struct lsm303dlhc *lsm, struct lsm303dlhc_cfg *cfg)
         goto err;
     }
 
-    /* Set scale */
+    /* Set accel scale */
     rc = lsm303dlhc_write8(LSM303DLHC_ADDR_ACCEL,
         LSM303DLHC_REGISTER_ACCEL_CTRL_REG4_A,
         lsm->cfg.accel_range);
@@ -298,6 +321,30 @@ lsm303dlhc_config(struct lsm303dlhc *lsm, struct lsm303dlhc_cfg *cfg)
         goto err;
     }
 
+    /* Enable the magnetomer (set to continuous conversion mode) */
+    rc = lsm303dlhc_write8(LSM303DLHC_ADDR_MAG,
+        LSM303DLHC_REGISTER_MAG_MR_REG_M,
+        0x00);
+    if (rc != 0) {
+        goto err;
+    }
+
+    /* Set mag rate */
+    rc = lsm303dlhc_write8(LSM303DLHC_ADDR_MAG,
+        LSM303DLHC_REGISTER_MAG_CRA_REG_M,
+        lsm->cfg.mag_rate);
+    if (rc != 0) {
+        goto err;
+    }
+
+    /* Set mag gain */
+    rc = lsm303dlhc_write8(LSM303DLHC_ADDR_MAG,
+        LSM303DLHC_REGISTER_MAG_CRB_REG_M,
+        lsm->cfg.mag_gain);
+    if (rc != 0) {
+        goto err;
+    }
+
 err:
     return (rc);
 }
@@ -314,77 +361,167 @@ lsm303dlhc_sensor_read(struct sensor *sensor, sensor_type_t type,
 {
     struct lsm303dlhc *lsm;
     struct sensor_accel_data sad;
+    struct sensor_mag_data smd;
     int rc;
     int16_t x, y, z;
     float mg_lsb;
+    int16_t gauss_lsb_xy;
+    int16_t gauss_lsb_z;
     uint8_t payload[6];
 
-    /* If the read isn't looking for accel data, then don't do anything. */
-    if (!(type & SENSOR_TYPE_ACCELEROMETER)) {
+    /* If the read isn't looking for accel or mag data, don't do anything. */
+    if (!(type & SENSOR_TYPE_ACCELEROMETER) &&
+       (!(type & SENSOR_TYPE_MAGNETIC_FIELD))) {
         rc = SYS_EINVAL;
         goto err;
     }
 
     lsm = (struct lsm303dlhc *) SENSOR_GET_DEVICE(sensor);
 
-    x = y = z = 0;
-    rc = lsm303dlhc_read48(LSM303DLHC_ADDR_ACCEL,
-                          LSM303DLHC_REGISTER_ACCEL_OUT_X_L_A,
-                          payload);
-    if (rc != 0) {
-        goto err;
-    }
-
-    /* Shift raw values based on sensor type */
+    /* Get a new accelerometer sample */
     if (type & SENSOR_TYPE_ACCELEROMETER) {
+        x = y = z = 0;
+        rc = lsm303dlhc_read48(LSM303DLHC_ADDR_ACCEL,
+                              LSM303DLHC_REGISTER_ACCEL_OUT_X_L_A | 0x80,
+                              payload);
+        if (rc != 0) {
+            goto err;
+        }
+
         /* Shift 12-bit left-aligned accel values into 16-bit int */
         x = ((int16_t)(payload[0] | (payload[1] << 8))) >> 4;
         y = ((int16_t)(payload[2] | (payload[3] << 8))) >> 4;
         z = ((int16_t)(payload[4] | (payload[5] << 8))) >> 4;
-    }
 
-    /* Determine mg per lsb based on range */
-    switch(lsm->cfg.accel_range) {
-        case LSM303DLHC_ACCEL_RANGE_2:
+        /* Determine mg per lsb based on range */
+        switch(lsm->cfg.accel_range) {
+            case LSM303DLHC_ACCEL_RANGE_2:
 #if MYNEWT_VAL(LSM303DLHC_STATS)
-            STATS_INC(g_lsm303dlhcstats, samples_acc_2g);
+                STATS_INC(g_lsm303dlhcstats, samples_acc_2g);
 #endif
-            mg_lsb = 0.001F;
-            break;
-        case LSM303DLHC_ACCEL_RANGE_4:
+                mg_lsb = 0.001F;
+                break;
+            case LSM303DLHC_ACCEL_RANGE_4:
 #if MYNEWT_VAL(LSM303DLHC_STATS)
-            STATS_INC(g_lsm303dlhcstats, samples_acc_4g);
+                STATS_INC(g_lsm303dlhcstats, samples_acc_4g);
 #endif
-            mg_lsb = 0.002F;
-            break;
-        case LSM303DLHC_ACCEL_RANGE_8:
+                mg_lsb = 0.002F;
+                break;
+            case LSM303DLHC_ACCEL_RANGE_8:
 #if MYNEWT_VAL(LSM303DLHC_STATS)
-            STATS_INC(g_lsm303dlhcstats, samples_acc_8g);
+                STATS_INC(g_lsm303dlhcstats, samples_acc_8g);
 #endif
-            mg_lsb = 0.004F;
-            break;
-        case LSM303DLHC_ACCEL_RANGE_16:
+                mg_lsb = 0.004F;
+                break;
+            case LSM303DLHC_ACCEL_RANGE_16:
 #if MYNEWT_VAL(LSM303DLHC_STATS)
-            STATS_INC(g_lsm303dlhcstats, samples_acc_16g);
+                STATS_INC(g_lsm303dlhcstats, samples_acc_16g);
 #endif
-            mg_lsb = 0.012F;
-            break;
-        default:
-            LSM303DLHC_ERR("Unknown accel range: 0x%02X. Assuming +/-2G.\n",
-                lsm->cfg.accel_range);
-            mg_lsb = 0.001F;
-            break;
+                mg_lsb = 0.012F;
+                break;
+            default:
+                LSM303DLHC_ERR("Unknown accel range: 0x%02X. Assuming +/-2G.\n",
+                    lsm->cfg.accel_range);
+                mg_lsb = 0.001F;
+                break;
+        }
+
+        /* Convert from mg to Earth gravity in m/s^2 */
+        sad.sad_x = (float)x * mg_lsb * 9.80665F;
+        sad.sad_y = (float)y * mg_lsb * 9.80665F;
+        sad.sad_z = (float)z * mg_lsb * 9.80665F;
+
+        /* Call data function */
+        rc = data_func(sensor, data_arg, &sad);
+        if (rc != 0) {
+            goto err;
+        }
     }
 
-    /* Convert from mg to Earth gravity in m/s^2 */
-    sad.sad_x = (float)x * mg_lsb * 9.80665F;
-    sad.sad_y = (float)y * mg_lsb * 9.80665F;
-    sad.sad_z = (float)z * mg_lsb * 9.80665F;
-
-    /* Call data function */
-    rc = data_func(sensor, data_arg, &sad);
-    if (rc != 0) {
-        goto err;
+    /* Get a new magnetometer sample */
+    if (type & SENSOR_TYPE_MAGNETIC_FIELD) {
+        x = y = z = 0;
+        rc = lsm303dlhc_read48(LSM303DLHC_ADDR_MAG,
+                              LSM303DLHC_REGISTER_MAG_OUT_X_H_M,
+                              payload);
+        if (rc != 0) {
+            goto err;
+        }
+
+        /* Shift mag values into 16-bit int */
+        x = (int16_t)(payload[1] | ((int16_t)payload[0] << 8));
+        y = (int16_t)(payload[3] | ((int16_t)payload[2] << 8));
+        z = (int16_t)(payload[5] | ((int16_t)payload[4] << 8));
+
+        /* Determine gauss per lsb based on gain */
+        switch (lsm->cfg.mag_gain) {
+            case LSM303DLHC_MAG_GAIN_1_3:
+#if MYNEWT_VAL(LSM303DLHC_STATS)
+                STATS_INC(g_lsm303dlhcstats, samples_mag_1_3g);
+#endif
+                gauss_lsb_xy = 1100;
+                gauss_lsb_z = 980;
+                break;
+            case LSM303DLHC_MAG_GAIN_1_9:
+#if MYNEWT_VAL(LSM303DLHC_STATS)
+                STATS_INC(g_lsm303dlhcstats, samples_mag_1_9g);
+#endif
+                gauss_lsb_xy = 855;
+                gauss_lsb_z = 760;
+                break;
+            case LSM303DLHC_MAG_GAIN_2_5:
+#if MYNEWT_VAL(LSM303DLHC_STATS)
+                STATS_INC(g_lsm303dlhcstats, samples_mag_2_5g);
+#endif
+                gauss_lsb_xy = 670;
+                gauss_lsb_z = 600;
+                break;
+            case LSM303DLHC_MAG_GAIN_4_0:
+#if MYNEWT_VAL(LSM303DLHC_STATS)
+                STATS_INC(g_lsm303dlhcstats, samples_mag_4_0g);
+#endif
+                gauss_lsb_xy = 450;
+                gauss_lsb_z = 400;
+                break;
+            case LSM303DLHC_MAG_GAIN_4_7:
+#if MYNEWT_VAL(LSM303DLHC_STATS)
+                STATS_INC(g_lsm303dlhcstats, samples_mag_4_7g);
+#endif
+                gauss_lsb_xy = 400;
+                gauss_lsb_z = 355;
+                break;
+            case LSM303DLHC_MAG_GAIN_5_6:
+#if MYNEWT_VAL(LSM303DLHC_STATS)
+                STATS_INC(g_lsm303dlhcstats, samples_mag_5_6g);
+#endif
+                gauss_lsb_xy = 330;
+                gauss_lsb_z = 295;
+                break;
+            case LSM303DLHC_MAG_GAIN_8_1:
+#if MYNEWT_VAL(LSM303DLHC_STATS)
+                STATS_INC(g_lsm303dlhcstats, samples_mag_8_1g);
+#endif
+                gauss_lsb_xy = 230;
+                gauss_lsb_z = 205;
+                break;
+            default:
+                LSM303DLHC_ERR("Unknown mag gain: 0x%02X. Assuming +/-1.3g.\n",
+                    lsm->cfg.mag_gain);
+                gauss_lsb_xy = 1100;
+                gauss_lsb_z = 980;
+                break;
+        }
+
+        /* Convert from gauss to micro Tesla */
+        smd.smd_x = (float)x / gauss_lsb_xy * 100.0F;
+        smd.smd_y = (float)y / gauss_lsb_xy * 100.0F;
+        smd.smd_z = (float)z / gauss_lsb_z * 100.0F;
+
+        /* Call data function */
+        rc = data_func(sensor, data_arg, &smd);
+        if (rc != 0) {
+            goto err;
+        }
     }
 
     return (0);
@@ -398,7 +535,8 @@ lsm303dlhc_sensor_get_config(struct sensor *sensor, sensor_type_t type,
 {
     int rc;
 
-    if (type != SENSOR_TYPE_ACCELEROMETER) {
+    if ((type != SENSOR_TYPE_ACCELEROMETER) &&
+        (type != SENSOR_TYPE_MAGNETIC_FIELD)) {
         rc = SYS_EINVAL;
         goto err;
     }


Mime
View raw message