Hassan Elahi
reading from 6 sensors and packaging the data
Diff: LSM303D.cpp
- Revision:
- 0:f186dd92c836
- Child:
- 1:3948b63ff4bd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM303D.cpp Tue Aug 26 13:20:01 2014 +0000
@@ -0,0 +1,297 @@
+/** Tilt-compensated compass interface Library for the STMicro LSM303DLH 3-axis magnetometer, 3-axis acceleromter
+ *
+ * Written by Eric Coyle
+ *
+ * Based on Michael Shimniok's LSM303DLH library which is based on
+ * 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 "LSM303D.h"
+#include "mbed.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+LSM303D::LSM303D(SPI &spi, PinName CS) : cs(CS), mspi(spi) {
+ cs = 1;
+}
+
+int LSM303D::whoami() {
+ int me;
+ cs=0; //talk to compass
+
+ //Send who am I (first bit must be 1 for read)
+ mspi.write(0x8F);
+
+ //Get who am I response
+ me = mspi.write(0x00);
+ cs=1; //done talking
+
+ return me;
+}
+
+int LSM303D::initialize() {
+ int iam;
+
+ //Check device
+ iam=whoami();
+
+ if (iam==73) {
+
+ //set up accelerometer
+ //CTRL1
+ cs=0; //talk to compass
+ mspi.write(0x20);
+ mspi.write(0x57); //50Hz, continuous update, all axes enabled
+ cs=1; //done talking
+
+
+ //CTRL2
+ cs=0; //talk to compass
+ mspi.write(0x21);
+ mspi.write(0x00); // 773Hz anti-alias filter, +/- 2g scale, self-test disabled, 4-wire SPI
+ cs=1; //done talking
+
+ //CTRL3
+
+ //set up magnetometer
+ //CTRL5
+ cs=0; //talk to compass
+ mspi.write(0x24);
+ mspi.write(0x70); //temperature disabled, high resolution, 50 Hz, int2 disable, int1 disabled
+ cs=1; //done talking
+
+
+ //CTRL6 (Magnetic Scale)
+ cs=0; //talk to compass
+ mspi.write(0x25);
+ mspi.write(0x20); //+/-4g
+ cs=1; //done talking
+
+ //CTRL7 (filtering settings, and other)
+ cs=0; //talk to compass
+ mspi.write(0x26);
+ mspi.write(0x00); //normal mode, keep other on
+ cs=1; //done talking
+
+ return 1;
+ }
+ else {
+ //Not talking to right device
+ return 0;
+ }
+}
+
+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;
+}
+
+int LSM303D::magnitometer(int axis) {
+ if (axis==0) {
+ cs=0; //lower cs to talk
+ mspi.write(0x88);
+ compass.b[0] = mspi.write(0x00);
+
+ cs=1; //done talking
+ cs=0; //lower cs to talk
+
+ mspi.write(0x89);
+ compass.b[1] = mspi.write(0x00);
+
+ cs=1; //done talking
+ }
+ else if (axis==1) {
+ //Y-Axis
+ cs=0;
+ mspi.write(0x8A);
+ compass.b[0] = mspi.write(0x00);
+
+ cs=1; //done talking
+ cs=0; //lower cs to talk
+
+ mspi.write(0x8B);
+ compass.b[1] = mspi.write(0x00);
+
+ cs=1; //done talking
+ }
+ else if (axis==2) {
+ //Z-Axis
+ cs=0;
+ mspi.write(0x8C);
+ compass.b[0] = mspi.write(0x00);
+
+ cs=1; //done talking
+ cs=0; //lower cs to talk
+
+ mspi.write(0x8D);
+ compass.b[1] = mspi.write(0x00);
+
+ cs=1; //done talking
+ }
+ return compass.raw;
+}
+
+int LSM303D::accelerometer(int axis) {
+ if (axis==0) {
+ cs=0; //lower cs to talk
+ mspi.write(0xA8);
+ compass.b[0] = mspi.write(0x00);
+
+ cs=1; //done talking
+ cs=0; //lower cs to talk
+
+ mspi.write(0xA9);
+ compass.b[1] = mspi.write(0x00);
+
+ cs=1; //done talking
+ }
+ else if (axis==1) {
+ //Y-Axis
+ cs=0;
+ mspi.write(0xAA);
+ compass.b[0] = mspi.write(0x00);
+
+ cs=1; //done talking
+ cs=0; //lower cs to talk
+
+ mspi.write(0xAB);
+ compass.b[1] = mspi.write(0x00);
+
+ cs=1; //done talking
+ }
+ else if (axis==2) {
+ //Z-Axis
+ cs=0;
+ mspi.write(0xAC);
+ compass.b[0] = mspi.write(0x00);
+
+ cs=1; //done talking
+ cs=0; //lower cs to talk
+
+ mspi.write(0xAD);
+ compass.b[1] = mspi.write(0x00);
+
+ cs=1; //done talking
+ }
+ return compass.raw;
+}
+
+void LSM303D::read(vector &a, vector &m)
+{
+ short a_x, a_y, a_z;
+ short m_x, m_y, m_z;
+ //Timer t;
+ //int usec1, usec2;
+
+ //t.reset();
+ //t.start();
+
+ //usec1 = t.read_us();
+ /*read_reg_short(addr_acc, OUT_X_A, &a_x);
+ read_reg_short(addr_acc, OUT_Y_A, &a_y);
+ read_reg_short(addr_acc, OUT_Z_A, &a_z);
+ read_reg_short(addr_mag, OUT_X_M, &m_x);
+ read_reg_short(addr_mag, OUT_Y_M, &m_y);
+ read_reg_short(addr_mag, OUT_Z_M, &m_z);*/
+
+ a_x=accelerometer(XAXIS);
+ a_y=accelerometer(YAXIS);
+ a_z=accelerometer(ZAXIS);
+ m_x=magnitometer(XAXIS);
+ m_y=magnitometer(YAXIS);
+ m_z=magnitometer(ZAXIS);
+
+ //usec2 = t.read_us();
+
+ //if (debug) debug->printf("%d %d %d\n", usec1, usec2, usec2-usec1);
+
+ // Perform simple lowpass filtering
+ // Intended to stabilize heading despite
+ // device vibration such as on a UGV
+ _filt_ax += a_x - (_filt_ax >> FILTER_SHIFT);
+ _filt_ay += a_y - (_filt_ay >> FILTER_SHIFT);
+ _filt_az += a_z - (_filt_az >> FILTER_SHIFT);
+
+ a.x = (float) (_filt_ax >> FILTER_SHIFT);
+ a.y = (float) (_filt_ay >> FILTER_SHIFT);
+ a.z = (float) (_filt_az >> FILTER_SHIFT);
+
+ // offset and scale
+ m.x = (m_x + _offset_x) * _scale_x;
+ m.y = (m_y + _offset_y) * _scale_y;
+ m.z = (m_z + _offset_z) * _scale_z;
+}
+
+/// Returns the number of degrees from the -Y axis that it
+// is pointing.
+float LSM303D::heading()
+{
+ return heading((vector){0,-1,0});
+}
+
+float LSM303D::heading(vector from)
+{
+ vector a, m;
+
+ this->read(a, m);
+
+ ////////////////////////////////////////////////
+ // compute heading
+ ////////////////////////////////////////////////
+
+ vector temp_a = a;
+ // normalize
+ vector_normalize(&temp_a);
+ //vector_normalize(&m);
+
+ // compute E and N
+ vector E;
+ vector N;
+ vector_cross(&m,&temp_a,&E);
+ vector_normalize(&E);
+ vector_cross(&temp_a,&E,&N);
+
+ // compute heading
+ float heading = atan2(vector_dot(&E,&from), vector_dot(&N,&from)) * 180/M_PI;
+ if (heading < 0) heading += 360;
+
+ return heading;
+}
\ No newline at end of file