LDSC_Robotics_TAs
read LSM9DS0
Revision 0:d94aee54fde4, committed 2016-08-30
- Comitter:
- vmjack
- Date:
- Tue Aug 30 08:07:42 2016 +0000
- Commit message:
- read LSM9DS0
Changed in this revision
diff -r 000000000000 -r d94aee54fde4 LSM9DS0.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM9DS0.lib Tue Aug 30 08:07:42 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/LSM9DS0/#e79b970b0258
diff -r 000000000000 -r d94aee54fde4 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Aug 30 08:07:42 2016 +0000
@@ -0,0 +1,188 @@
+#include "mbed.h"
+#include "LSM9DS0.h"
+#include <math.h>
+
+// timer 1
+#define LOOPTIME 1000 // 1 ms
+unsigned long lastMilli = 0;
+// sampling time
+float T = 0.001;
+
+Timer t;
+
+Serial uart_1(D10,D2); // TX : D10 RX : D2 // serial 1
+
+void init_uart(void);
+void separate(void);
+void uart_send(void);
+
+float lpf(float input, float output_old, float frequency);
+
+// uart send data
+int display[6] = {0,0,0,0,0,0};
+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
+
+void init_sensor(void);
+void read_sensor(void);
+
+LSM9DS0 sensor(SPI_MODE, D7, D6); // SPI_CS1 : D7 , SPI_CS2 : D6
+
+int sensor_flag = 0; // sensor initial flag
+int sensor_count = 0;
+
+// sensor data
+int16_t Gyro_axis_data[3] = {0}; // X, Y, Z axis
+int16_t Acce_axis_data[3] = {0}; // X, Y, Z axis
+int16_t Magn_axis_data[3] = {0}; // X, Y, Z axis
+
+// sensor gain
+#define Gyro_gain_x 0
+#define Gyro_gain_y 0
+#define Gyro_gain_z 0.00122173 // datasheet : 70 mdeg/digit
+#define Acce_gain_x 0
+#define Acce_gain_y 0
+#define Acce_gain_z 0
+#define Magn_gain 0
+
+// sensor filter correction data
+float Gyro_axis_data_f[3];
+float Gyro_axis_data_f_old[3];
+float Gyro_scale[3]; // scale = (data - zero) * gain
+float Gyro_axis_zero[3] = {0,0,0};
+
+float Acce_axis_data_f[3];
+float Acce_axis_data_f_old[3];
+float Acce_scale[3]; // scale = (data - zero) * gain
+float Acce_axis_zero[3] = {0,0,0};
+
+float Magn_axis_data_f[3];
+float Magn_axis_data_f_old[3];
+float Magn_scale[3]; // scale = (data - zero) * gain
+float Magn_axis_zero[3] = {0,0,0};
+
+int main()
+{
+ t.start();
+
+ init_uart();
+ init_sensor();
+
+ while(1)
+ {
+ // timer 1
+ if((t.read_us() - lastMilli) >= LOOPTIME) // 2000 us = 2 ms
+ {
+ lastMilli = t.read_us();
+
+ // sensor initial start
+ if(sensor_flag == 0)
+ {
+ sensor_count++;
+
+ if(sensor_count >= 50)
+ {
+ sensor_flag = 1;
+ sensor_count = 0;
+ }
+ }
+ else
+ {
+ read_sensor();
+ uart_send();
+ }
+ }
+ } // while(1) end
+}
+
+int i = 0;
+void uart_send(void)
+{
+ // uart send data
+ display[0] = Gyro_axis_data[0];
+ display[1] = Gyro_axis_data[1];
+ display[2] = Gyro_axis_data[2];
+ display[3] = Acce_axis_data[0];
+ display[4] = Acce_axis_data[1];
+ display[5] = Acce_axis_data[2];
+
+ separate();
+
+ int j = 0;
+ int k = 1;
+ while(k)
+ {
+ if(uart_1.writeable())
+ {
+ uart_1.putc(num[i]);
+ i++;
+ j++;
+ }
+
+ if(j>1) // send 2 bytes
+ {
+ k = 0;
+ j = 0;
+ }
+ }
+
+ if(i>13)
+ i = 0;
+}
+
+void read_sensor(void)
+{
+ // sensor raw data
+ Gyro_axis_data[0] = sensor.readRawGyroX();
+ Gyro_axis_data[1] = sensor.readRawGyroY();
+ Gyro_axis_data[2] = sensor.readRawGyroZ();
+
+ Acce_axis_data[0] = sensor.readRawAccelX();
+ Acce_axis_data[1] = sensor.readRawAccelY();
+ Acce_axis_data[2] = sensor.readRawAccelZ();
+
+ Magn_axis_data[0] = sensor.readRawMagX();
+ Magn_axis_data[1] = sensor.readRawMagY();
+ Magn_axis_data[2] = sensor.readRawMagZ();
+}
+
+void init_uart()
+{
+ uart_1.baud(115200); // 設定baudrate
+}
+
+void separate(void)
+{
+ num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列
+ num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列
+ num[4] = display[1] >> 8;
+ num[5] = display[1] & 0x00ff;
+ num[6] = display[2] >> 8;
+ num[7] = display[2] & 0x00ff;
+ num[8] = display[3] >> 8;
+ num[9] = display[3] & 0x00ff;
+ num[10] = display[4] >> 8;
+ num[11] = display[4] & 0x00ff;
+ num[12] = display[5] >> 8;
+ num[13] = display[5] & 0x00ff;
+}
+
+void init_sensor(void)
+{
+ sensor.begin();
+ // sensor.begin() setting :
+ // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS,
+ // accel_scale aScl = A_SCALE_8G,
+ // mag_scale mScl = M_SCALE_8GS,
+ // gyro_odr gODR = G_ODR_760_BW_100,
+ // accel_odr aODR = A_ODR_800,
+ // mag_odr mODR = M_ODR_100);
+}
+
+float lpf(float input, float output_old, float frequency)
+{
+ float output = 0;
+
+ output = (output_old + frequency*T*input) / (1 + frequency*T);
+
+ return output;
+}
diff -r 000000000000 -r d94aee54fde4 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Aug 30 08:07:42 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/2241e3a39974 \ No newline at end of file