Diff: main.cpp

Revision:

0:cd56e3a52a36

Child:

2:2dfe6101b283

diff -r 000000000000 -r cd56e3a52a36 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Sep 29 19:13:42 2018 +0000
@@ -0,0 +1,137 @@
+#include "stm32f103c8t6.h"
+#include "Kalman.h"
+#include "mbed.h"
+#include "LSM6DS3.h"
+//#include "FXOS8700CQ.h"
+#define RESTRICT_PITCH
+#define RAD_TO_DEG 180/3.14
+Kalman kalmanX; // Create the Kalman instances
+Kalman kalmanY;
+Serial pc(PA_9, PA_10); // tx, rx
+LSM6DS3 LSM6DS3(PB_7, PB_6);
+//FXOS8700CQ fxos(PTE25,PTE24);
+//Data fxos_acc;
+Timer t;
+//AnalogIn pot0(A0), pot1(A1), pot2(A2), pot3(A3);
+
+unsigned long int start_time = 0, end_time = 0, t1;
+uint16_t adc_value;
+
+double gyroXangle, gyroYangle; // Angle calculate using the gyro only
+double compAngleX, compAngleY; // Calculated angle using a complementary filter
+double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
+
+void gyro_getdata();
+
+int main() {
+    t.start();
+    pc.baud(9600);
+ //   pc.printf("before\n");
+    LSM6DS3.begin(LSM6DS3.G_SCALE_245DPS, LSM6DS3.A_SCALE_2G, LSM6DS3.G_ODR_208, LSM6DS3.A_ODR_208);
+//    pc.printf("after\n");
+//    LSM6DS3.begin();
+//    fxos.init();
+        LSM6DS3.readAccel();
+        LSM6DS3.readGyro();
+    #ifdef RESTRICT_PITCH
+            double roll  = atan2(LSM6DS3.ay, LSM6DS3.az) * RAD_TO_DEG;
+            double pitch = atan(-LSM6DS3.ax / sqrt(LSM6DS3.ay * LSM6DS3.ay + LSM6DS3.az * LSM6DS3.az)) * RAD_TO_DEG;
+            #else // Eq. 28 and 29
+            double roll  = atan(LSM6DS3.ay / sqrt(LSM6DS3.ax * LSM6DS3.ax + LSM6DS3.ax * LSM6DS3.ax)) * RAD_TO_DEG;
+            double pitch = atan2(-LSM6DS3.ax, LSM6DS3.ax) * RAD_TO_DEG;
+            #endif
+            kalmanX.setAngle(roll); // Set starting angle
+            kalmanY.setAngle(pitch);
+            gyroXangle = roll;
+            gyroYangle = pitch;
+            compAngleX = roll;
+            compAngleY = pitch;
+            t1 = t.read_us();
+    while(1)
+    {
+        start_time = t.read_us();
+        //read Accel & Gyro
+//        fxos_acc = fxos.get_values();
+        LSM6DS3.readAccel();
+        LSM6DS3.readGyro();
+        end_time = t.read_us();
+    //    pc.printf("\nPass time: %d\n", end_time - start_time);
+        //serial send Accel (board)
+//        pc.printf("BoardAccelerX[%f]\n",fxos_acc.ax);
+//        pc.printf("BoardAccelerY[%f]\n",fxos_acc.ay);
+//        pc.printf("BoardAccelerZ[%f]\n",fxos_acc.az);
+        //serial send Accel (lsm6ds33)
+      //  pc.printf("AccelerX[%f]\n",LSM6DS3.ax);
+      //  pc.printf("AccelerY[%f]\n",LSM6DS3.ay);
+      //  pc.printf("AccelerZ[%f]\n",LSM6DS3.az);
+        //serial send Gyro
+      //  pc.printf("GyroX[%f]\n",LSM6DS3.gx);
+      //  pc.printf("GyroY[%f]\n",LSM6DS3.gy);
+      //  pc.printf("GyroZ[%f]\n",LSM6DS3.gz);
+        wait(1.0f);
+        gyro_getdata();
+        pc.printf("\n");
+    }}
+
+void gyro_getdata(){
+        LSM6DS3.readAccel();
+        LSM6DS3.readGyro();
+        double dt = (double)(t.read_us() - t1) / 1000000; // Calculate delta time
+        t1 = t.read_us();
+        #ifdef RESTRICT_PITCH
+            double roll  = atan2(LSM6DS3.ay, LSM6DS3.az) * RAD_TO_DEG;
+            double pitch = atan(-LSM6DS3.ax / sqrt(LSM6DS3.ay * LSM6DS3.ay + LSM6DS3.az * LSM6DS3.az)) * RAD_TO_DEG;
+            #else // Eq. 28 and 29
+            double roll  = atan(LSM6DS3.ay / sqrt(LSM6DS3.ax * LSM6DS3.ax + LSM6DS3.ax * LSM6DS3.ax)) * RAD_TO_DEG;
+            double pitch = atan2(-LSM6DS3.ax, LSM6DS3.ax) * RAD_TO_DEG;
+            #endif
+            double gyroXrate = LSM6DS3.gx / 131.0; // Convert to deg/s
+            double gyroYrate = LSM6DS3.gy / 131.0; // Convert to deg/s
+
+        #ifdef RESTRICT_PITCH
+  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
+  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
+    kalmanX.setAngle(roll);
+    compAngleX = roll;
+    kalAngleX = roll;
+    gyroXangle = roll;
+  } else
+    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
+
+  if (abs(kalAngleX) > 90)
+    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
+  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
+#else
+  // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
+  if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
+    kalmanY.setAngle(pitch);
+    compAngleY = pitch;
+    kalAngleY = pitch;
+    gyroYangle = pitch;
+  } else
+    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
+
+  if (abs(kalAngleY) > 90)
+    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
+  kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
+#endif
+
+  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
+  gyroYangle += gyroYrate * dt;
+  gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
+  gyroYangle += kalmanY.getRate() * dt;
+
+  compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
+  compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
+
+  // Reset the gyro angle when it has drifted too much
+  if (gyroXangle < -180 || gyroXangle > 180)
+    gyroXangle = kalAngleX;
+  if (gyroYangle < -180 || gyroYangle > 180)
+    gyroYangle = kalAngleY;
+    pc.printf("%.2f",roll);
+    pc.printf("\n");
+    pc.printf("%.2f",pitch);
+    pc.printf("\n");
+    wait_ms(500);
+}
\ No newline at end of file