Roger Weng
pure sensor fusion
Diff: main.cpp
- Revision:
- 0:56bfa7bd6f71
- Child:
- 1:81a146dffd7a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Oct 13 05:36:58 2016 +0000
@@ -0,0 +1,329 @@
+#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 real_sensor_value(void);
+void sensor_fusion(void);
+
+LSM9DS0 sensor(SPI_MODE, D9, D6); // SPI_CS1 : D7 , SPI_CS2 : D6
+
+int sensor_flag = 0; // sensor initial flag
+int sensor_count = 0;
+
+// sensor gain
+#define Gyro_gain_x 0.00113356568421052631578947368421
+#define Gyro_gain_y 0.00113356568421052631578947368421
+#define Gyro_gain_z 0.00113356568421052631578947368421
+#define Acce_gain_x -0.0019768904308522832538352805428 //-9.81/(max-zero)
+#define Acce_gain_y -0.00210816564158896459269414388347
+#define Acce_gain_z -0.00237364674122154818035038507811
+
+// 宣告從sensor讀到的值要存入處理的變數
+float Gyro_axis_data[3]; // X, Y, Z axis
+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] = {-3.7153254648333,-8.013402897667,-57.26524894};
+
+float Acce_axis_data[3]; // X, Y, Z axis
+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] = {-802.8320503,-460.282926875,4.717775229};
+
+// final sensor output value
+float axm = 0;
+float aym = 0;
+float azm = 0;
+float u1 = 0;
+float u2 = 0;
+float u3 = 0;
+
+// new defined direction
+float ax = 0;
+float ay = 0;
+float az = 0;
+float w1 = 0;
+float w2 = 0;
+float w3 = 0;
+
+// estimated state variables
+float gs1_hat = 0;
+float gs1_hat_old = 0;
+float gs2_hat = 0;
+float gs2_hat_old = 0;
+float gs3_hat = 0;
+float gs3_hat_old = 0;
+// normalized variables
+float n = 0;
+float gs1_hat_n = 0;
+float gs2_hat_n = 0;
+float gs3_hat_n = 0;
+// bandwidth
+float alpha = 6.28; // 1Hz
+
+/********************************************************************************/
+//Variable(s) state
+float gama = 0.0; //Roll Angle
+float gama_old = 0.0;
+float gama_f = 0.0;
+float gama_f_old = 0.0;
+float Igama = 0.0; //
+float Igama_f = 0.0;
+float Igama_f_old = 0.0;
+float dgama = 0.0; //Roll Angle
+float dgama_old = 0.0;
+float dgama_f = 0.0;
+float dgama_f_old = 0.0;
+
+float roll = 0.0; //Roll Angle
+float roll_old = 0.0;
+float roll_f = 0.0;
+float roll_f_old = 0.0;
+float Iroll = 0.0; //
+float Iroll_f = 0.0;
+float Iroll_f_old = 0.0;
+float droll = 0.0; //Roll Angle
+float droll_old = 0.0;
+float droll_f = 0.0;
+float droll_f_old = 0.0;
+
+float pitch = 0.0; //Roll Angle
+float pitch_old = 0.0;
+float pitch_f = 0.0;
+float pitch_f_old = 0.0;
+float Ipitch = 0.0; //
+float Ipitch_f = 0.0;
+float Ipitch_f_old = 0.0;
+float dpitch = 0.0; //Roll Angle
+float dpitch_old = 0.0;
+float dpitch_f = 0.0;
+float dpitch_f_old = 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
+ {
+ real_sensor_value();
+ sensor_fusion();
+ uart_send();
+ }
+ }
+ } // while(1) end
+}
+
+int i = 0;
+void uart_send(void)
+{
+ // uart send data
+ display[0] = 100*pitch*180/3.1415926;
+ display[1] = 100*roll*180/3.1415926;
+ display[2] = 100*w3;
+ display[3] = 100*gs1_hat_n;
+ display[4] = 100*gs2_hat_n;
+ display[5] = 100*gs3_hat_n;
+
+ 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 real_sensor_value(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();
+
+ // gyro filter
+ Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);
+ Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
+ Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);
+ Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
+ Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);
+ Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
+
+ // acce filter
+ Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);
+ Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
+ Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);
+ Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
+ Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);
+ Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
+
+ Gyro_scale[0] = (Gyro_axis_data_f[0]-Gyro_axis_zero[0])*Gyro_gain_x;
+ Gyro_scale[1] = (Gyro_axis_data_f[1]-Gyro_axis_zero[1])*Gyro_gain_y;
+ Gyro_scale[2] = (Gyro_axis_data_f[2]-Gyro_axis_zero[2])*Gyro_gain_z;
+
+ Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x;
+ Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y;
+ Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z;
+
+ // final 6 axis data
+ axm = Acce_scale[0];
+ aym = Acce_scale[1];
+ azm = Acce_scale[2];
+
+ u1 = Gyro_scale[0];
+ u2 = Gyro_scale[1];
+ u3 = Gyro_scale[2];
+
+ ax = axm;
+ ay = aym;
+ az = azm;
+
+ w1 = u1;
+ w2 = u2;
+ w3 = u3;
+}
+
+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);
+}
+
+void sensor_fusion(void){
+ // sensor fusion
+ gs1_hat = lpf(ax + w3*ay/alpha - w2*az/alpha , gs1_hat_old , alpha);
+ gs1_hat_old = gs1_hat;
+ gs2_hat = lpf(-w3*ax/alpha + ay + w1*az/alpha , gs2_hat_old , alpha);
+ gs2_hat_old = gs2_hat;
+ gs3_hat = lpf(w2*ax/alpha - w1*ay/alpha + az , gs3_hat_old , alpha);
+ gs3_hat_old = gs3_hat;
+
+ n = sqrt(gs1_hat*gs1_hat + gs2_hat*gs2_hat + gs3_hat*gs3_hat);
+ gs1_hat_n = (gs1_hat / n) * 9.81;
+ gs2_hat_n = (gs2_hat / n) * 9.81;
+ gs3_hat_n = (gs3_hat / n) * 9.81;
+
+ pitch = asin(gs1_hat_n/9.81);
+ roll = atan(gs2_hat_n / gs3_hat_n);
+
+ pitch_f = lpf(pitch, pitch_f_old, 1.0);
+ pitch_f_old = pitch_f;
+ Ipitch = Ipitch + pitch_f*0.01;
+ Ipitch_f = lpf(Ipitch, Ipitch_f_old, 18.0);
+ Ipitch_f_old = Ipitch_f;
+ dpitch = (pitch - dpitch_old)/0.01;
+ dpitch_old = pitch;
+
+ dpitch_f = lpf(dpitch, dpitch_f_old, 1.0);
+ dpitch_f_old = dpitch_f;
+
+ roll_f = lpf(roll, roll_f_old, 1.0);
+ roll_f_old = roll_f;
+ Iroll = Iroll + roll_f*0.01;
+ Iroll_f = lpf(Iroll, Iroll_f_old, 18.0);
+ Iroll_f_old = Iroll_f;
+ droll = (roll - droll_old)/0.01;
+ droll_old = roll;
+
+ droll_f = lpf(droll, droll_f_old, 1.0);
+ droll_f_old = droll_f;
+
+
+}
+
+float lpf(float input, float output_old, float frequency)
+{
+ float output = 0;
+
+ output = (output_old + frequency*T*input) / (1 + frequency*T);
+
+ return output;
+}