LDSC_Robotics_TAs / Mbed 2 deprecated read_sensor_data

read LSM9DS0

Dependencies:   LSM9DS0 mbed

main.cpp@0:d94aee54fde4, 2016-08-30 (annotated)

Committer:
vmjack
Date:
Tue Aug 30 08:07:42 2016 +0000
Revision:
0:d94aee54fde4
read LSM9DS0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vmjack 0:d94aee54fde4 1 #include "mbed.h"
vmjack 0:d94aee54fde4 2 #include "LSM9DS0.h"
vmjack 0:d94aee54fde4 3 #include <math.h>
vmjack 0:d94aee54fde4 4
vmjack 0:d94aee54fde4 5 // timer 1
vmjack 0:d94aee54fde4 6 #define LOOPTIME 1000 // 1 ms
vmjack 0:d94aee54fde4 7 unsigned long lastMilli = 0;
vmjack 0:d94aee54fde4 8 // sampling time
vmjack 0:d94aee54fde4 9 float T = 0.001;
vmjack 0:d94aee54fde4 10
vmjack 0:d94aee54fde4 11 Timer t;
vmjack 0:d94aee54fde4 12
vmjack 0:d94aee54fde4 13 Serial uart_1(D10,D2); // TX : D10 RX : D2 // serial 1
vmjack 0:d94aee54fde4 14
vmjack 0:d94aee54fde4 15 void init_uart(void);
vmjack 0:d94aee54fde4 16 void separate(void);
vmjack 0:d94aee54fde4 17 void uart_send(void);
vmjack 0:d94aee54fde4 18
vmjack 0:d94aee54fde4 19 float lpf(float input, float output_old, float frequency);
vmjack 0:d94aee54fde4 20
vmjack 0:d94aee54fde4 21 // uart send data
vmjack 0:d94aee54fde4 22 int display[6] = {0,0,0,0,0,0};
vmjack 0:d94aee54fde4 23 char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0}; // char num[0] , num[1] : 2 start byte
vmjack 0:d94aee54fde4 24
vmjack 0:d94aee54fde4 25 void init_sensor(void);
vmjack 0:d94aee54fde4 26 void read_sensor(void);
vmjack 0:d94aee54fde4 27
vmjack 0:d94aee54fde4 28 LSM9DS0 sensor(SPI_MODE, D7, D6); // SPI_CS1 : D7 , SPI_CS2 : D6
vmjack 0:d94aee54fde4 29
vmjack 0:d94aee54fde4 30 int sensor_flag = 0; // sensor initial flag
vmjack 0:d94aee54fde4 31 int sensor_count = 0;
vmjack 0:d94aee54fde4 32
vmjack 0:d94aee54fde4 33 // sensor data
vmjack 0:d94aee54fde4 34 int16_t Gyro_axis_data[3] = {0}; // X, Y, Z axis
vmjack 0:d94aee54fde4 35 int16_t Acce_axis_data[3] = {0}; // X, Y, Z axis
vmjack 0:d94aee54fde4 36 int16_t Magn_axis_data[3] = {0}; // X, Y, Z axis
vmjack 0:d94aee54fde4 37
vmjack 0:d94aee54fde4 38 // sensor gain
vmjack 0:d94aee54fde4 39 #define Gyro_gain_x 0
vmjack 0:d94aee54fde4 40 #define Gyro_gain_y 0
vmjack 0:d94aee54fde4 41 #define Gyro_gain_z 0.00122173 // datasheet : 70 mdeg/digit
vmjack 0:d94aee54fde4 42 #define Acce_gain_x 0
vmjack 0:d94aee54fde4 43 #define Acce_gain_y 0
vmjack 0:d94aee54fde4 44 #define Acce_gain_z 0
vmjack 0:d94aee54fde4 45 #define Magn_gain 0
vmjack 0:d94aee54fde4 46
vmjack 0:d94aee54fde4 47 // sensor filter correction data
vmjack 0:d94aee54fde4 48 float Gyro_axis_data_f[3];
vmjack 0:d94aee54fde4 49 float Gyro_axis_data_f_old[3];
vmjack 0:d94aee54fde4 50 float Gyro_scale[3]; // scale = (data - zero) * gain
vmjack 0:d94aee54fde4 51 float Gyro_axis_zero[3] = {0,0,0};
vmjack 0:d94aee54fde4 52
vmjack 0:d94aee54fde4 53 float Acce_axis_data_f[3];
vmjack 0:d94aee54fde4 54 float Acce_axis_data_f_old[3];
vmjack 0:d94aee54fde4 55 float Acce_scale[3]; // scale = (data - zero) * gain
vmjack 0:d94aee54fde4 56 float Acce_axis_zero[3] = {0,0,0};
vmjack 0:d94aee54fde4 57
vmjack 0:d94aee54fde4 58 float Magn_axis_data_f[3];
vmjack 0:d94aee54fde4 59 float Magn_axis_data_f_old[3];
vmjack 0:d94aee54fde4 60 float Magn_scale[3]; // scale = (data - zero) * gain
vmjack 0:d94aee54fde4 61 float Magn_axis_zero[3] = {0,0,0};
vmjack 0:d94aee54fde4 62
vmjack 0:d94aee54fde4 63 int main()
vmjack 0:d94aee54fde4 64 {
vmjack 0:d94aee54fde4 65 t.start();
vmjack 0:d94aee54fde4 66
vmjack 0:d94aee54fde4 67 init_uart();
vmjack 0:d94aee54fde4 68 init_sensor();
vmjack 0:d94aee54fde4 69
vmjack 0:d94aee54fde4 70 while(1)
vmjack 0:d94aee54fde4 71 {
vmjack 0:d94aee54fde4 72 // timer 1
vmjack 0:d94aee54fde4 73 if((t.read_us() - lastMilli) >= LOOPTIME) // 2000 us = 2 ms
vmjack 0:d94aee54fde4 74 {
vmjack 0:d94aee54fde4 75 lastMilli = t.read_us();
vmjack 0:d94aee54fde4 76
vmjack 0:d94aee54fde4 77 // sensor initial start
vmjack 0:d94aee54fde4 78 if(sensor_flag == 0)
vmjack 0:d94aee54fde4 79 {
vmjack 0:d94aee54fde4 80 sensor_count++;
vmjack 0:d94aee54fde4 81
vmjack 0:d94aee54fde4 82 if(sensor_count >= 50)
vmjack 0:d94aee54fde4 83 {
vmjack 0:d94aee54fde4 84 sensor_flag = 1;
vmjack 0:d94aee54fde4 85 sensor_count = 0;
vmjack 0:d94aee54fde4 86 }
vmjack 0:d94aee54fde4 87 }
vmjack 0:d94aee54fde4 88 else
vmjack 0:d94aee54fde4 89 {
vmjack 0:d94aee54fde4 90 read_sensor();
vmjack 0:d94aee54fde4 91 uart_send();
vmjack 0:d94aee54fde4 92 }
vmjack 0:d94aee54fde4 93 }
vmjack 0:d94aee54fde4 94 } // while(1) end
vmjack 0:d94aee54fde4 95 }
vmjack 0:d94aee54fde4 96
vmjack 0:d94aee54fde4 97 int i = 0;
vmjack 0:d94aee54fde4 98 void uart_send(void)
vmjack 0:d94aee54fde4 99 {
vmjack 0:d94aee54fde4 100 // uart send data
vmjack 0:d94aee54fde4 101 display[0] = Gyro_axis_data[0];
vmjack 0:d94aee54fde4 102 display[1] = Gyro_axis_data[1];
vmjack 0:d94aee54fde4 103 display[2] = Gyro_axis_data[2];
vmjack 0:d94aee54fde4 104 display[3] = Acce_axis_data[0];
vmjack 0:d94aee54fde4 105 display[4] = Acce_axis_data[1];
vmjack 0:d94aee54fde4 106 display[5] = Acce_axis_data[2];
vmjack 0:d94aee54fde4 107
vmjack 0:d94aee54fde4 108 separate();
vmjack 0:d94aee54fde4 109
vmjack 0:d94aee54fde4 110 int j = 0;
vmjack 0:d94aee54fde4 111 int k = 1;
vmjack 0:d94aee54fde4 112 while(k)
vmjack 0:d94aee54fde4 113 {
vmjack 0:d94aee54fde4 114 if(uart_1.writeable())
vmjack 0:d94aee54fde4 115 {
vmjack 0:d94aee54fde4 116 uart_1.putc(num[i]);
vmjack 0:d94aee54fde4 117 i++;
vmjack 0:d94aee54fde4 118 j++;
vmjack 0:d94aee54fde4 119 }
vmjack 0:d94aee54fde4 120
vmjack 0:d94aee54fde4 121 if(j>1) // send 2 bytes
vmjack 0:d94aee54fde4 122 {
vmjack 0:d94aee54fde4 123 k = 0;
vmjack 0:d94aee54fde4 124 j = 0;
vmjack 0:d94aee54fde4 125 }
vmjack 0:d94aee54fde4 126 }
vmjack 0:d94aee54fde4 127
vmjack 0:d94aee54fde4 128 if(i>13)
vmjack 0:d94aee54fde4 129 i = 0;
vmjack 0:d94aee54fde4 130 }
vmjack 0:d94aee54fde4 131
vmjack 0:d94aee54fde4 132 void read_sensor(void)
vmjack 0:d94aee54fde4 133 {
vmjack 0:d94aee54fde4 134 // sensor raw data
vmjack 0:d94aee54fde4 135 Gyro_axis_data[0] = sensor.readRawGyroX();
vmjack 0:d94aee54fde4 136 Gyro_axis_data[1] = sensor.readRawGyroY();
vmjack 0:d94aee54fde4 137 Gyro_axis_data[2] = sensor.readRawGyroZ();
vmjack 0:d94aee54fde4 138
vmjack 0:d94aee54fde4 139 Acce_axis_data[0] = sensor.readRawAccelX();
vmjack 0:d94aee54fde4 140 Acce_axis_data[1] = sensor.readRawAccelY();
vmjack 0:d94aee54fde4 141 Acce_axis_data[2] = sensor.readRawAccelZ();
vmjack 0:d94aee54fde4 142
vmjack 0:d94aee54fde4 143 Magn_axis_data[0] = sensor.readRawMagX();
vmjack 0:d94aee54fde4 144 Magn_axis_data[1] = sensor.readRawMagY();
vmjack 0:d94aee54fde4 145 Magn_axis_data[2] = sensor.readRawMagZ();
vmjack 0:d94aee54fde4 146 }
vmjack 0:d94aee54fde4 147
vmjack 0:d94aee54fde4 148 void init_uart()
vmjack 0:d94aee54fde4 149 {
vmjack 0:d94aee54fde4 150 uart_1.baud(115200); // 設定baudrate
vmjack 0:d94aee54fde4 151 }
vmjack 0:d94aee54fde4 152
vmjack 0:d94aee54fde4 153 void separate(void)
vmjack 0:d94aee54fde4 154 {
vmjack 0:d94aee54fde4 155 num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列
vmjack 0:d94aee54fde4 156 num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列
vmjack 0:d94aee54fde4 157 num[4] = display[1] >> 8;
vmjack 0:d94aee54fde4 158 num[5] = display[1] & 0x00ff;
vmjack 0:d94aee54fde4 159 num[6] = display[2] >> 8;
vmjack 0:d94aee54fde4 160 num[7] = display[2] & 0x00ff;
vmjack 0:d94aee54fde4 161 num[8] = display[3] >> 8;
vmjack 0:d94aee54fde4 162 num[9] = display[3] & 0x00ff;
vmjack 0:d94aee54fde4 163 num[10] = display[4] >> 8;
vmjack 0:d94aee54fde4 164 num[11] = display[4] & 0x00ff;
vmjack 0:d94aee54fde4 165 num[12] = display[5] >> 8;
vmjack 0:d94aee54fde4 166 num[13] = display[5] & 0x00ff;
vmjack 0:d94aee54fde4 167 }
vmjack 0:d94aee54fde4 168
vmjack 0:d94aee54fde4 169 void init_sensor(void)
vmjack 0:d94aee54fde4 170 {
vmjack 0:d94aee54fde4 171 sensor.begin();
vmjack 0:d94aee54fde4 172 // sensor.begin() setting :
vmjack 0:d94aee54fde4 173 // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS,
vmjack 0:d94aee54fde4 174 // accel_scale aScl = A_SCALE_8G,
vmjack 0:d94aee54fde4 175 // mag_scale mScl = M_SCALE_8GS,
vmjack 0:d94aee54fde4 176 // gyro_odr gODR = G_ODR_760_BW_100,
vmjack 0:d94aee54fde4 177 // accel_odr aODR = A_ODR_800,
vmjack 0:d94aee54fde4 178 // mag_odr mODR = M_ODR_100);
vmjack 0:d94aee54fde4 179 }
vmjack 0:d94aee54fde4 180
vmjack 0:d94aee54fde4 181 float lpf(float input, float output_old, float frequency)
vmjack 0:d94aee54fde4 182 {
vmjack 0:d94aee54fde4 183 float output = 0;
vmjack 0:d94aee54fde4 184
vmjack 0:d94aee54fde4 185 output = (output_old + frequency*T*input) / (1 + frequency*T);
vmjack 0:d94aee54fde4 186
vmjack 0:d94aee54fde4 187 return output;
vmjack 0:d94aee54fde4 188 }