Diff: SPI_9dSensor.cpp

Revision:

4:b0967990e390

Parent:

2:ec30613f2b2b

Child:

8:79ca11e0129d

--- a/SPI_9dSensor.cpp	Wed Jun 22 04:35:18 2016 +0000
+++ b/SPI_9dSensor.cpp	Wed Jul 06 06:54:15 2016 +0000
@@ -6,7 +6,7 @@
 #include <errno.h> // strerro
 #include <string.h>
 
-SPI spi2(PB_15,PB_14,PB_13);
+SPI sensor_LSM9D(PB_15,PB_14,PB_13);
 DigitalOut sensor_CSG(PB_2);
 DigitalOut sensor_CSXM(PB_1);
 
@@ -44,14 +44,14 @@
     if(signo == SIGINT)interrupt = 1;
 }
 
-void setup_spi(void)
+void setup_spi_sensor(void)
 {
 //    signal(SIGINT, sig_handler);
 //    wiringPiSetupGpio () ;
     sensor_CSG = 1;
     sensor_CSXM = 1;
-    spi2.format(8, 3);
-    pc.printf("SPI ready.\r\n");
+    sensor_LSM9D.format(8, 3);
+    pc.printf("SPI sensor ready.\r\n");
     wait_ms(50);
 }
 
@@ -86,12 +86,12 @@
     // start
     sensor_CSG = 0;
     
-    spi2.write(sensorG_CTRL_REG[0]);
-    spi2.write(sensorG_CTRL_REG[1]);
-    spi2.write(sensorG_CTRL_REG[2]);
-    spi2.write(sensorG_CTRL_REG[3]);
-    spi2.write(sensorG_CTRL_REG[4]);
-    spi2.write(sensorG_CTRL_REG[5]);
+    sensor_LSM9D.write(sensorG_CTRL_REG[0]);
+    sensor_LSM9D.write(sensorG_CTRL_REG[1]);
+    sensor_LSM9D.write(sensorG_CTRL_REG[2]);
+    sensor_LSM9D.write(sensorG_CTRL_REG[3]);
+    sensor_LSM9D.write(sensorG_CTRL_REG[4]);
+    sensor_LSM9D.write(sensorG_CTRL_REG[5]);
         
     sensor_CSG = 1;
     // end
@@ -115,15 +115,15 @@
     // start
     sensor_CSXM = 0;
     
-    spi2.write(sensorXM_CTRL_REG[0]);
-    spi2.write(sensorXM_CTRL_REG[1]);
-    spi2.write(sensorXM_CTRL_REG[2]);
-    spi2.write(sensorXM_CTRL_REG[3]);
-    spi2.write(sensorXM_CTRL_REG[4]);
-    spi2.write(sensorXM_CTRL_REG[5]);
-    spi2.write(sensorXM_CTRL_REG[6]);
-    spi2.write(sensorXM_CTRL_REG[7]);
-    spi2.write(sensorXM_CTRL_REG[8]);
+    sensor_LSM9D.write(sensorXM_CTRL_REG[0]);
+    sensor_LSM9D.write(sensorXM_CTRL_REG[1]);
+    sensor_LSM9D.write(sensorXM_CTRL_REG[2]);
+    sensor_LSM9D.write(sensorXM_CTRL_REG[3]);
+    sensor_LSM9D.write(sensorXM_CTRL_REG[4]);
+    sensor_LSM9D.write(sensorXM_CTRL_REG[5]);
+    sensor_LSM9D.write(sensorXM_CTRL_REG[6]);
+    sensor_LSM9D.write(sensorXM_CTRL_REG[7]);
+    sensor_LSM9D.write(sensorXM_CTRL_REG[8]);
         
     sensor_CSXM = 1;
     // end
@@ -147,13 +147,13 @@
     // start
     sensor_CSG = 0;
     
-    spi2.write(sensorG_READ_REG[0]);
-    sensorG_READ_REG[1] = spi2.write(0x00); // XL
-    sensorG_READ_REG[2] = spi2.write(0x00); // XH
-    sensorG_READ_REG[3] = spi2.write(0x00); // YL
-    sensorG_READ_REG[4] = spi2.write(0x00); // YH
-    sensorG_READ_REG[5] = spi2.write(0x00); // ZL
-    sensorG_READ_REG[6] = spi2.write(0x00); // YH
+    sensor_LSM9D.write(sensorG_READ_REG[0]);
+    sensorG_READ_REG[1] = sensor_LSM9D.write(0x00); // XL
+    sensorG_READ_REG[2] = sensor_LSM9D.write(0x00); // XH
+    sensorG_READ_REG[3] = sensor_LSM9D.write(0x00); // YL
+    sensorG_READ_REG[4] = sensor_LSM9D.write(0x00); // YH
+    sensorG_READ_REG[5] = sensor_LSM9D.write(0x00); // ZL
+    sensorG_READ_REG[6] = sensor_LSM9D.write(0x00); // YH
     
     sensor_CSG = 1;
     // end
@@ -181,13 +181,13 @@
     // start
     sensor_CSXM = 0;
     
-    spi2.write(sensorX_READ_REG[0]);
-    sensorX_READ_REG[1] = spi2.write(0x00); // XL
-    sensorX_READ_REG[2] = spi2.write(0x00); // XH
-    sensorX_READ_REG[3] = spi2.write(0x00); // YL
-    sensorX_READ_REG[4] = spi2.write(0x00); // YH
-    sensorX_READ_REG[5] = spi2.write(0x00); // ZL
-    sensorX_READ_REG[6] = spi2.write(0x00); // YH
+    sensor_LSM9D.write(sensorX_READ_REG[0]);
+    sensorX_READ_REG[1] = sensor_LSM9D.write(0x00); // XL
+    sensorX_READ_REG[2] = sensor_LSM9D.write(0x00); // XH
+    sensorX_READ_REG[3] = sensor_LSM9D.write(0x00); // YL
+    sensorX_READ_REG[4] = sensor_LSM9D.write(0x00); // YH
+    sensorX_READ_REG[5] = sensor_LSM9D.write(0x00); // ZL
+    sensorX_READ_REG[6] = sensor_LSM9D.write(0x00); // YH
     
     sensor_CSXM = 1;
     // end
@@ -216,13 +216,13 @@
     // start
     sensor_CSXM = 0;
     
-    spi2.write(sensorM_READ_REG[0]);
-    sensorM_READ_REG[1] = spi2.write(0x00); // XL
-    sensorM_READ_REG[2] = spi2.write(0x00); // XH
-    sensorM_READ_REG[3] = spi2.write(0x00); // YL
-    sensorM_READ_REG[4] = spi2.write(0x00); // YH
-    sensorM_READ_REG[5] = spi2.write(0x00); // ZL
-    sensorM_READ_REG[6] = spi2.write(0x00); // YH
+    sensor_LSM9D.write(sensorM_READ_REG[0]);
+    sensorM_READ_REG[1] = sensor_LSM9D.write(0x00); // XL
+    sensorM_READ_REG[2] = sensor_LSM9D.write(0x00); // XH
+    sensorM_READ_REG[3] = sensor_LSM9D.write(0x00); // YL
+    sensorM_READ_REG[4] = sensor_LSM9D.write(0x00); // YH
+    sensorM_READ_REG[5] = sensor_LSM9D.write(0x00); // ZL
+    sensorM_READ_REG[6] = sensor_LSM9D.write(0x00); // YH
     
     sensor_CSXM = 1;
     // end
@@ -282,3 +282,23 @@
     Magn_scale[1] = ((float)(Magn_axis_data[1]-Magn_axis_zero[1]))*Magn_gain;
     Magn_scale[2] = ((float)(Magn_axis_data[2]-Magn_axis_zero[2]))*Magn_gain;
 }
+
+void print_WhoAmI_G(void)
+{
+    static unsigned char WhoAmI_G = 0;
+    sensor_CSG = 0;
+    sensor_LSM9D.write((0x0F & 0x3F) | 0x80);
+    WhoAmI_G = sensor_LSM9D.write(0x00);
+    sensor_CSG = 1;
+    pc.printf("%X\r\n", WhoAmI_G);
+}
+
+void print_WhoAmI_XM(void)
+{
+    static unsigned char WhoAmI_XM = 0;
+    sensor_CSXM = 0;
+    sensor_LSM9D.write((0x0F & 0x3F) | 0x80);
+    WhoAmI_XM = sensor_LSM9D.write(0x00);
+    sensor_CSXM = 1;
+    pc.printf("%X\r\n", WhoAmI_XM);
+}