Hochschule München / LSM303D_imu

imu01c

Diff: LSM303D.cpp

Revision:
0:456611adedf8
Child:
1:5d0acf280330
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303D.cpp	Sat Oct 31 16:14:13 2015 +0000
@@ -0,0 +1,228 @@
+/** Tilt-compensated compass interface Library for the STMicro LSM303D 3-axis magnetometer, 3-axis acceleromter
+ *
+ * Based on
+ * 
+ * Michael Shimniok http://bot-thoughts.com
+ *
+ * test program by tosihisa and 
+ *
+ * Pololu sample library for LSM303DLH breakout by ryantm:
+ *
+ * Copyright (c) 2011 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com/
+ * http://forum.pololu.com/
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+ 
+#include "mbed.h"
+#include "LSM303D.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+#define FILTER_SHIFT 2      // used in filtering acceleromter readings
+
+const int addr_ls303d = 0x3A;
+
+enum REG_ADDRS {
+    /* --- Mag --- */
+    OUT_TEMP    = 0x05,
+    OUT_X_M     = 0x08,
+    OUT_Y_M     = 0x0A,
+    OUT_Z_M     = 0x0C,
+    /* --- Acc --- */
+    CTRL_REG1_A = 0x20,
+    CTRL_REG2_A = 0x21,
+    CTRL_REG5_A = 0x24,
+    CTRL_REG6_A = 0x25,
+    CTRL_REG7_A = 0x26,
+    OUT_X_A     = 0x28,
+    OUT_Y_A     = 0x2A,
+    OUT_Z_A     = 0x2C,
+};
+
+bool LSM303D::write_reg(int addr_i2c,int addr_reg, char v)
+{
+    char data[2] = {addr_reg, v}; 
+    return LSM303D::_compass.write(addr_i2c, data, 2) == 0;
+}
+
+bool LSM303D::read_reg(int addr_i2c,int addr_reg, char *v)
+{
+    char data = addr_reg; 
+    bool result = false;
+    
+    __disable_irq();
+    if ((_compass.write(addr_i2c, &data, 1) == 0) && (_compass.read(addr_i2c, &data, 1) == 0)){
+        *v = data;
+        result = true;
+    }
+    __enable_irq();
+    return result;
+}
+
+
+
+LSM303D::LSM303D(PinName sda, PinName scl):
+    _compass(sda, scl), _offset_x(0), _offset_y(0), _offset_z(0), _scale_x(1), _scale_y(1), _scale_z(1), _filt_ax(0), _filt_ay(0), _filt_az(9000)
+{
+    char reg_v;
+    //_compass.frequency(100000);
+    
+    //Chip Setup
+    
+    //CTRL 0 (1Fh)all Default
+    
+    //CTRL 1 (20h)   
+    reg_v = 0;
+    reg_v |= 0x04 << 4;     /* ACC 25Hz Autosample  */
+    reg_v |= 0x07;          /* X/Y/Z axis enable. */
+    write_reg(addr_ls303d,CTRL_REG1_A,reg_v); //
+    reg_v = 0;
+    read_reg(addr_ls303d,CTRL_REG1_A,&reg_v);
+    
+    //CTRL 2 (21h)
+    reg_v = 0x00;          
+    reg_v |= 0x03 << 6;     /* 50Hz Antialias Filter */
+    reg_v |= 0x00 << 3;     /* +/- 2g */
+    write_reg(addr_ls303d,CTRL_REG2_A,reg_v);
+    
+    //CTRL 3 (22h) all Default
+    
+    //CTRL 4 (23h) all Default
+    
+    //CTRL 5 (24h)
+    reg_v = 0x00;          
+    reg_v |= 0x01 << 7;     /* Temp Sensor Enable 1*/
+    reg_v |= 0x03 << 5;     /* Mag high Res  3*/
+    reg_v |= 0x03 << 2;     /* Mag 25Hz Autosample*/
+    write_reg(addr_ls303d,CTRL_REG5_A,reg_v);
+    
+    //CTRL 6 (25h)
+    reg_v = 0x00;          
+    reg_v |= 0x00 << 5;     /* +-2 Gauss*/
+    write_reg(addr_ls303d,CTRL_REG6_A,reg_v); //25h
+
+    //CTRL 7 (26h)
+    reg_v = 0x00;          
+    reg_v |= 0x00 << 0;     /* Mag Continuous Conversation*/
+    write_reg(addr_ls303d,CTRL_REG7_A,reg_v); //26h
+}
+
+
+void LSM303D::setOffset(float x, float y, float z)
+{
+    _offset.x = x;
+    _offset.y = y;
+    _offset.z = z;   
+}
+
+void LSM303D::setScale(float x, float y, float z)
+{
+    _scale.x = x;
+    _scale.y = y;
+    _scale.z = z;
+}
+
+void LSM303D::set_vectors()
+{
+
+    // Perform simple lowpass filtering
+    // Intended to stabilize heading despite
+    // device vibration such as on a UGV
+    _filt_ax += acc_raw.x - (_filt_ax >> FILTER_SHIFT);
+    _filt_ay += acc_raw.y - (_filt_ay >> FILTER_SHIFT);
+    _filt_az += acc_raw.z - (_filt_az >> FILTER_SHIFT);
+
+    acc.x = (float) (_filt_ax >> FILTER_SHIFT);
+    acc.y = (float) (_filt_ay >> FILTER_SHIFT);
+    acc.z = (float) (_filt_az >> FILTER_SHIFT);
+    
+    // offset and scale
+    mag.x = (3 * mag.x + ((mag_raw.x + _offset.x) * _scale.x))/4;
+    mag.y = (3 * mag.y + ((mag_raw.y + _offset.y) * _scale.y))/4;
+    mag.z = (3 * mag.z + ((mag_raw.z + _offset.z) * _scale.z))/4; 
+    
+    vector_normalize(&mag);  
+}
+
+void LSM303D::read()
+{
+    char data[1] = { OUT_X_A | (1<<7)}; //Page 23 In order to read multiple bytes, it is necessary to assert the most significant bit of the subaddress field. In other words, SUB(7) must be equal to ‘1’ while SUB(6-0) represents the address of the first register to be read.
+    char out[6] = {0,0,0,0,0,0};
+    _compass.write( addr_ls303d, data,1);
+    _compass.read( addr_ls303d, out, 6);
+
+    acc_raw.x= short( (((short)out[1]) << 8) | out[0] );
+    acc_raw.y= short( (((short)out[3]) << 8) | out[2] );
+    acc_raw.z= short( (((short)out[5]) << 8) | out[4] );   
+
+    data[0] =  (OUT_X_M | (1<<7));
+
+    _compass.write( addr_ls303d, data, 1 );
+    _compass.read ( addr_ls303d, out, 6 );
+    
+    mag_raw.x= short( (((short)out[1]) << 8)| out[0]);
+    mag_raw.y= short( (((short)out[3]) << 8)| out[2]);
+    mag_raw.z= short( (((short)out[5]) << 8)| out[4]); 
+    
+    set_vectors();
+    
+    calc_pos();
+}
+
+
+
+void LSM303D::calc_pos(void)
+{   
+    ////////////////////////////////////////////////
+    // compute heading       
+    ////////////////////////////////////////////////
+    vector from = {1,0,0};
+    vector temp_a = acc;
+    
+    // normalize
+    vector_normalize(&temp_a);
+
+    // compute E and N
+    vector E;
+    vector N;
+    vector_cross(&mag,&temp_a,&E);
+    vector_normalize(&E);
+    vector_cross(&temp_a,&E,&N);
+    
+    // compute heading
+    hdg = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
+    if (hdg < 0) hdg += 360;
+    
+    vector_norm_xz(&temp_a);
+    pitch = asin(-temp_a.x) * 180/M_PI + 30;
+    
+}
+
+void LSM303D::frequency(int hz)
+{
+    _compass.frequency(hz);
+}
\ No newline at end of file