File content as of revision 6:94c4a0e7bf48:

#include "mbed.h"
#include "MPU6050.h"
#include "madgwickfilter.h"
MPU6050 mpu6050(D5,D7);
Madgwickfilter filter;
const double PI = 3.14159265358979323846f;
const double kRad2Deg = 180.0/PI;
Serial pc(USBTX,USBRX);
PwmOut LED_f(LED1);

int main()
{
    mpu6050.init();
    while(1) {
        //ジャイロから値取得
        mpu6050.CalMPU6050();
        double accel_x = mpu6050.GetAccelX();
        double accel_y = mpu6050.GetAccelY();
        double accel_z = mpu6050.GetAccelZ();
        double omega_x = mpu6050.GetXRadPerSec();
        double omega_y = mpu6050.GetYRadPerSec();
        double omega_z = mpu6050.GetZRadPerSec();
        //madgwickフィルターをかけ、yaw, pitch, rollを計算
        filter.Update(accel_x, accel_y, accel_z, omega_x, omega_y, omega_z);
        LED_f=filter.getYaw()*kRad2Deg/180;
        if(filter.getYaw()*kRad2Deg/180<=0.1){
            LED_f=0.1;
            }
        else if(filter.getYaw()*kRad2Deg/180>=0.9){
            LED_f=0.9;
            }
        printf("Yaw, Pitch, Roll[deg]: %9.4f %9.4f %9.4f\r\n",
               filter.getYaw()*kRad2Deg, filter.getPitch()*kRad2Deg, filter.getRoll()*kRad2Deg);
               
    }
}

/*
#include "mbed.h"
#include "madgwickfilter.h"   //ライブラリをインクルード


Madgwickfilter filter();
Serial pc(USBTX, USBRX); // tx, rx

int main()
{
   double x_acc,y_acc,z_acc;
   double x_rate,y_rate,z_rate;
   double roll,pitch,yaw; 
   while(1){
       filter.Update(x_acc,y_acc,z_acc,x_rate,y_rate,z_rate);
       roll=filter.getRoll();
       pitch=filter.getPitch();
       yaw=filter.getYaw();
       pc.printf("roll:%8.4f,pitch:%8.4f,yaw:%8.4f\r\n",roll,pitch,yaw);
   }
}
*/