Diff: SPI_9dSensor.cpp

Revision:

10:3b952ea7fad4

Parent:

8:79ca11e0129d

Child:

11:45a641da473d

--- a/SPI_9dSensor.cpp	Mon Jul 18 09:36:16 2016 +0000
+++ b/SPI_9dSensor.cpp	Mon Jul 18 12:12:50 2016 +0000
@@ -1,21 +1,10 @@
 #include "SPI_9dSensor.h"
 #include "SensorFusion.h"
 
-#include <stdio.h>  // printf()
-#include <signal.h> // signal()
-#include <errno.h> // strerro
-#include <string.h>
-
 SPI sensor_LSM9D(PB_15,PB_14,PB_13);
 DigitalOut sensor_CSG(PB_2);
 DigitalOut sensor_CSXM(PB_1);
 
-Serial pc(D1,D0);
-
-int volatile interrupt = 0;
-
-const int speed = 1875000;
-
 unsigned char sensorG_CTRL_REG[6];
 unsigned char sensorXM_CTRL_REG[9];
 
@@ -39,21 +28,15 @@
 float u_cali[9] = {0,0,0,0,0,0,0,0,0};
 float u_old[9] = {GX_offset, GY_offset, GZ_offset, AX_offset, AY_offset, AZ_offset, MX_offset, MY_offset, MZ_offset};
 
-void sig_handler(int signo)
+bool setup_spi_sensor(void)
 {
-    if(signo == SIGINT)interrupt = 1;
-}
-
-void setup_spi_sensor(void)
-{
-//    signal(SIGINT, sig_handler);
-//    wiringPiSetupGpio () ;
     sensor_CSG = 1;
     sensor_CSXM = 1;
     sensor_LSM9D.frequency(10500000);
     sensor_LSM9D.format(8, 3);
-    pc.printf("SPI sensor ready.\r\n");
+//    pc.printf("SPI sensor ready.\r\n");
     wait_ms(50);
+    return 1;
 }
 
 void init_Sensors(void)
@@ -284,22 +267,22 @@
     Magn_scale[2] = ((float)(Magn_axis_data[2]-Magn_axis_zero[2]))*Magn_gain;
 }
 
-void print_WhoAmI_G(void)
+unsigned char get_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);
+    return WhoAmI_G;
 }
 
-void print_WhoAmI_XM(void)
+unsigned char get_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);
+    return WhoAmI_XM;
 }