File content as of revision 3:78402ec2d9c0:

#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);
}