Diff: SPI_9dSensor.cpp

Revision:

0:830ddddc129f

Child:

2:ec30613f2b2b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SPI_9dSensor.cpp	Thu Jun 09 09:46:38 2016 +0000
@@ -0,0 +1,284 @@
+#include "SPI_9dSensor.h"
+#include "SensorFusion.h"
+
+#include <stdio.h>  // printf()
+#include <signal.h> // signal()
+#include <errno.h> // strerro
+#include <string.h>
+
+SPI spi2(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];
+
+short int Gyro_axis_data[3]; // X, Y, Z axis
+short int Gyro_axis_zero[3] = {GX_offset, GY_offset, GZ_offset};///new{-57,-10,34};///{32, -28, 138};///{25, -25, 120};
+float Gyro_scale[3];
+
+short int Acce_axis_data[3]; // X, Y, Z axis
+short int Acce_axis_zero[3] = {AX_offset, AY_offset, AZ_offset};///new{-847,-420,186};///{-855, -590, 186};///{-855, -504, 186};///{-855, -454, 186};///
+float Acce_scale[3];
+
+short int Magn_axis_data[3]; //X,Y,Z axis
+short int Magn_axis_zero[3] = {MX_offset, MY_offset, MZ_offset};///{-55, -21, 77};//{-32,-25,80};
+float Magn_scale[3];
+
+float B_x = 1015.0f*Magn_gain;
+float B_y = 0.0f*Magn_gain;
+float B_z = -580.0f*Magn_gain;
+float B_total = 0.0f;
+
+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)
+{
+    if(signo == SIGINT)interrupt = 1;
+}
+
+void setup_spi(void)
+{
+//    signal(SIGINT, sig_handler);
+//    wiringPiSetupGpio () ;
+    sensor_CSG = 1;
+    sensor_CSXM = 1;
+    spi2.format(8, 3);
+    pc.printf("SPI ready.\n");
+    wait_ms(50);
+}
+
+void init_Sensors(void)
+{
+    Gyro_axis_data[0] = 0; // X
+    Gyro_axis_data[1] = 0; // Y
+    Gyro_axis_data[2] = 0; // Z
+    Acce_axis_data[0] = 0;
+    Acce_axis_data[1] = 0;
+    Acce_axis_data[2] = 0;
+    Magn_axis_data[0] = 0;
+    Magn_axis_data[1] = 0;
+    Magn_axis_data[2] = 0;
+    sensorG_setup();
+    sensorXM_setup();
+}
+
+void sensorG_setup(void)
+{
+    sensorG_CTRL_REG[1] = 0b11111111; // x, y, z enabled
+    sensorG_CTRL_REG[2] = 0b00001001; // ODR = 800Hz, LPfc = 110Hz
+    sensorG_CTRL_REG[3] = 0b00000000; // output circuit: push- pull
+    sensorG_CTRL_REG[4] = 0b00110000; // 2000dps
+    sensorG_CTRL_REG[5] = 0b00000000; // HPen = 0, LPFen = 0
+
+    // write mode (R/W = 0)
+    // Auto increase address (M/S = 1)
+    //sensorG_data_write = 0x20;
+    sensorG_CTRL_REG[0] = (sensorG_CTRL_REG1_address & 0b00111111) | 0b01000000; // mask first two bits, write first two bits
+
+    // 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_CSG = 1;
+    // end
+}
+
+void sensorXM_setup(void)
+{
+    sensorXM_CTRL_REG[1] = 0b00000000;
+    sensorXM_CTRL_REG[2] = 0b10010111; // ODR = 800Hz, x, y, z enabled
+    sensorXM_CTRL_REG[3] = 0b11011000; // LPfc = 110Hz, +-8g
+    sensorXM_CTRL_REG[4] = 0b00000000; // output circuit: push- pull
+    sensorXM_CTRL_REG[5] = 0b00000000;
+    sensorXM_CTRL_REG[6] = 0b01110100; //Magnetic data rate 100Hz
+    sensorXM_CTRL_REG[7] = 0b01000000; //mag full scale +-8 gauss
+    sensorXM_CTRL_REG[8] = 0b00000000; // HPen = 0, LPFen = 0
+    // write mode (R/W = 0)
+    // Auto increase address (M/S = 1)
+    //sensorXM_data_write = sensorXM_CTRL_REG0_address;
+    sensorXM_CTRL_REG[0] = (sensorXM_CTRL_REG0_address & 0b00111111) | 0b01000000; // mask first two bits, write first two bits
+
+    // 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_CSXM = 1;
+    // end
+}
+
+void sensorG_read_3axis(void)
+{
+    static unsigned char sensorG_READ_REG[7];
+    // read mode (R/W = 1)
+    // Auto increase address (M/S = 1)
+    //sensorG_data_write = sensorG_OUT_X_L_address;
+    sensorG_READ_REG[0] = 0x28;
+    sensorG_READ_REG[0] = (sensorG_READ_REG[0] & 0b00111111) | 0b11000000; // mask first two bits, write first two bits
+    sensorG_READ_REG[1] = 0x00;
+    sensorG_READ_REG[2] = 0x00;
+    sensorG_READ_REG[3] = 0x00;
+    sensorG_READ_REG[4] = 0x00;
+    sensorG_READ_REG[5] = 0x00;
+    sensorG_READ_REG[6] = 0x00;
+
+    // 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_CSG = 1;
+    // end
+
+    // Data reconstruction
+    Gyro_axis_data[0] = (short int)((sensorG_READ_REG[2]<<8) | sensorG_READ_REG[1]);
+    Gyro_axis_data[1] = (short int)((sensorG_READ_REG[4]<<8) | sensorG_READ_REG[3]);
+    Gyro_axis_data[2] = (short int)((sensorG_READ_REG[6]<<8) | sensorG_READ_REG[5]);
+}
+
+void sensorX_read_3axis(void)
+{
+    static unsigned char sensorX_READ_REG[7];
+
+    // read mode (R/W = 1)
+    // Auto increase address (M/S = 1)
+    sensorX_READ_REG[0] = 0x28;//accelerator's first address
+    sensorX_READ_REG[0] = (sensorX_READ_REG[0] & 0b00111111) | 0b11000000; // mask first two bits, write first two bits
+    sensorX_READ_REG[1] = 0x00;
+    sensorX_READ_REG[2] = 0x00;
+    sensorX_READ_REG[3] = 0x00;
+    sensorX_READ_REG[4] = 0x00;
+    sensorX_READ_REG[5] = 0x00;
+    sensorX_READ_REG[6] = 0x00;
+    // 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_CSXM = 1;
+    // end
+
+    // Data reconstruction
+    Acce_axis_data[0] = (short int)((sensorX_READ_REG[2]<<8) | sensorX_READ_REG[1]); // X
+    Acce_axis_data[1] = (short int)((sensorX_READ_REG[4]<<8) | sensorX_READ_REG[3]); // Y
+    Acce_axis_data[2] = (short int)((sensorX_READ_REG[6]<<8) | sensorX_READ_REG[5]); // Z
+}
+
+void sensorM_read_3axis(void)
+{
+    static unsigned char sensorM_READ_REG[7];
+
+    // read mode (R/W = 1)
+    // Auto increase address (M/S = 1)
+    sensorM_READ_REG[0] = 0x08;//magnetometer's first address
+    sensorM_READ_REG[0] = (sensorM_READ_REG[0] & 0b00111111) | 0b11000000; // mask first two bits, write first two bits
+    sensorM_READ_REG[1] = 0x00;
+    sensorM_READ_REG[2] = 0x00;
+    sensorM_READ_REG[3] = 0x00;
+    sensorM_READ_REG[4] = 0x00;
+    sensorM_READ_REG[5] = 0x00;
+    sensorM_READ_REG[6] = 0x00;
+
+    // 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_CSXM = 1;
+    // end
+
+    // Data reconstruction
+    Magn_axis_data[0] = (short int)((sensorM_READ_REG[2]<<8) | sensorM_READ_REG[1]); // X
+    Magn_axis_data[1] = (short int)((sensorM_READ_REG[4]<<8) | sensorM_READ_REG[3]); // Y
+    Magn_axis_data[2] = (short int)((sensorM_READ_REG[6]<<8) | sensorM_READ_REG[5]); // Z
+}
+
+short int filted_sensor_data(unsigned char axis_index, float freq)
+{
+    float data_to_filt = 0.0;
+
+    switch(axis_index)
+    {
+        case 1: data_to_filt = (float)Gyro_axis_data[0]; break;
+        case 2: data_to_filt = (float)Gyro_axis_data[1]; break;
+        case 3: data_to_filt = (float)Gyro_axis_data[2]; break;
+        case 4: data_to_filt = (float)Acce_axis_data[0]; break;
+        case 5: data_to_filt = (float)Acce_axis_data[1]; break;
+        case 6: data_to_filt = (float)Acce_axis_data[2]; break;
+        case 7: data_to_filt = (float)Magn_axis_data[0]; break;
+        case 8: data_to_filt = (float)Magn_axis_data[1]; break;
+        case 9: data_to_filt = (float)Magn_axis_data[2]; break;
+        default: break;
+    }
+
+    u_cali[axis_index - 1] = lpf(data_to_filt, u_old[axis_index - 1], freq);
+    u_old[axis_index - 1] = u_cali[axis_index - 1];
+    return (short int)u_cali[axis_index - 1];
+}
+
+void reset_gyro_offset(void)
+{
+    sensorG_read_3axis();
+    Gyro_axis_zero[0] = filted_sensor_data(INDEX_GYRO_X, 1.8);
+    Gyro_axis_zero[1] = filted_sensor_data(INDEX_GYRO_Y, 1.8);
+    Gyro_axis_zero[2] = filted_sensor_data(INDEX_GYRO_Z, 1.8);
+}
+
+void reset_acceX_offset(void)
+{
+    sensorX_read_3axis();
+    Acce_axis_zero[0] = filted_sensor_data(INDEX_ACCE_X, 1.8);
+}
+
+void get_9axis_scale(void)
+{
+    Gyro_scale[0] = ((float)Gyro_axis_data[0]-Gyro_axis_zero[0])*Gyro_gainx;
+    Gyro_scale[1] = ((float)Gyro_axis_data[1]-Gyro_axis_zero[1])*Gyro_gainy;
+    Gyro_scale[2] = ((float)Gyro_axis_data[2]-Gyro_axis_zero[2])*Gyro_gainz;
+    Acce_scale[0] = ((float)(Acce_axis_data[0]-Acce_axis_zero[0]))*Acce_gainx;
+    Acce_scale[1] = ((float)(Acce_axis_data[1]-Acce_axis_zero[1]))*Acce_gainy;
+    Acce_scale[2] = ((float)(Acce_axis_data[2]-Acce_axis_zero[2]))*Acce_gainz;
+    Magn_scale[0] = ((float)(Magn_axis_data[0]-Magn_axis_zero[0]))*Magn_gain;
+    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;
+}