File content as of revision 42:24c93e42c3f4:

#include "mbed.h"
#include "PIDcontroller.h"
#include "SBUS.hpp"
#include "LoopTicker.hpp"
#include "MPU6050.h"
#include "MAG3110.h"
#include "I2Cdev.h"
#include "FastPWM.h"
#include "Matrix.h"
#include "MatrixMath.h"
#include "math.h"

#define MPU6050_PWR_MGMT_1   0x6B
#define MPU_ADDRESS  0x68
#define pi 3.141562
#define ACCEL_FSR MPU6050_ACCEL_FS_8
#define ACCEL_SSF 4096.0
#define GYRO_FSR MPU6050_GYRO_FS_250
#define GYRO_SSF 131.0
#define MPU6050_LPF MPU6050_DLPF_BW_256
#define gyro_th 20.0
#define PID_dt 0.015
#define servoPwmMax  1800.0
#define servoPwmMin  1200.0
#define motorPwmMax  2000.0
#define motorPwmMin  1100.0
#define pitch_align 8.0*3.141562/180.0
#define roll_align -2.8*3.141562/180.0

MPU6050 accelgyro;
MAG3110 mag(PB_9,PB_8);
SBUS sbus(PD_5, PD_6);
Serial pc(USBTX, USBRX);
Serial sd(PG_14,PG_9);
DigitalIn userButton(USER_BUTTON);
FastPWM servoRight(PE_9);
FastPWM servoLeft(PE_11);
FastPWM servoThrust(PE_13);
PID pitchPID(5.0, 0,0,PID_dt); //rad
PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
PID rollPID(5.0,0.0,0.0,PID_dt);
PID rollratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
Timer t;

Matrix qhat(4,1);
Matrix Phat(4,4);
Matrix Qgyro(3,3);
Matrix Racc(3,3);
Matrix Rmag(3,3);
Matrix D(3,3);

int loop_count = 0;
int obs_count = 0;
float att_dt = 0.01;
float rc[16];
float pitch = 0.0;
float roll = 0.0;
float yaw = 0.0;
int16_t ax, ay, az;
int16_t gx, gy, gz;
MotionSensorDataUnits mdata;
float magval[3] = {1.0,0.0,0.0};
float acc_x,acc_y,acc_z;
float dynacc_x,dynacc_y,dynacc_z;
float gyro_x,gyro_y,gyro_z;
float mag_x,mag_y,mag_z;

float LPacc_x=0.0;
float LPacc_y=0.0;
float LPacc_z=0.0;
float LPmag_x = 0.0;
float LPmag_y = 0.0;
float LPmag_z = 0.0;;

int out1, out2;
float scaledServoOut[3]= {0,0};
float scaledMotorOut[1]= {-1};
float servoOut[3] = {1500.0,1500.0};
float motorOut[1] = {1100.0};
 
float accnorm;
float magnorm;
float LPaccnorm;
float LPmagnorm;
float accrefnorm;
float magrefnorm;


float val_thmg = 0.0;
float th_mg = 0.0;
float dynaccnorm = 0.0;
float accnormerr = 0.0;
float accref[3] = {0, 0, -0.98};
float magref[3] = {0.65, 0, 0.75};
 
int agoffset[6] = {0, 0, 0, -123, -575, -1};
float magbias[4] = {-143.593872, 125.311264, -161.226898, 27.998077};

// eepromのread writeのためのunionを定義
int address = 0xAE; // EEPROMの3つの入力が全て+より
int pointeraddress = 0;

I2C i2c(PB_9,PB_8);  // sda, scl
union U{
 int i[8]; // 0: flag(>1で正常に書き込まれた状態) 1 ~ 3: 磁場の中心座標, 4: 磁場の半径、 5~7; mag, 8~10: acc bias
 char c[32]; // floatはcharの4倍
};
int gxs = 0;
int gys = 0;
int gzs = 0;



// eeprom書き込み・読み込みに必要な関数
void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
{
    char i2cBuffer[size + 2];
    i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
    i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
 
    for (int i = 0; i < size; i++) {
        i2cBuffer[i + 2] = data[i];
    }
 
    int result = i2c.write(address, i2cBuffer, size + 2, false);
    //sleep_ms(500);
}
 
// this function has no read limit
void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
{
    char i2cBuffer[2];
    i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
    i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
 
    // Reset eeprom pointer address
    int result = i2c.write(address, i2cBuffer, 2, false);
    
    //sleep_ms(500);
 
    // Read eeprom
    i2c.read(address, data, size);
    //sleep_ms(500);
}

void writeSdcard()
{
    //magcal.getExtremes(&magmin[0],&magmax[0]);
    //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]);
    //pc.printf("%f %f %f %f %f %f %f %f %f %f %f %f \r\n",acc_x,acc_y,acc_z,mag_x,mag_y,mag_z,accref[0],accref[1],accref[2],magref[0],magref[1],magref[2]);
    //pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
    pc.printf("%f %f %f %f : %f %f %f\r\n",float(obs_count)/100.0,th_mg,dynaccnorm,accnormerr,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
    //pc.printf("%d \r\n",userButton.read());
    //pc.printf("%f %f %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
    //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
    //pc.printf("%f %f %f %f %f %f\r\n",dynacc_x,dynacc_y,dynacc_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
    //pc.printf("%f %f %f %f %f : %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
    //pc.printf("%f %f %f %f %f : %f %f %f %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],magval[0],magval[1],magval[2],mdata.x,mdata.y,mdata.z);
}

float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void calcMagRef(float mx, float my, float mz){
    float q0 = qhat.getNumber( 1, 1 );
    float q1 = qhat.getNumber( 2, 1 ); 
    float q2 = qhat.getNumber( 3, 1 ); 
    float q3 = qhat.getNumber( 4, 1 ); 
    float hx = 2.0f * (mx * (0.5f - q2*q2 - q3*q3) + my * (q1*q2 - q0*q3) + mz * (q1*q3 + q0*q2));
    float hy = 2.0f * (mx * (q1*q2 + q0*q3) + my * (0.5f - q1*q1 - q3*q3) + mz * (q2*q3 - q0*q1));
    float bx = sqrt(hx * hx + hy * hy);
    float bz = 2.0f * (mx * (q1*q3 - q0*q2) + my * (q2*q3 + q0*q1) + mz * (0.5f - q1*q1 - q2*q2));
    magref[0] = bx;
    magref[1] = 0.0;
    magref[2] = bz;
}


void getIMUval(){
    // gx gy gz ax ay az
    accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
    ax =  ax - agoffset[0];
    ay =  ay - agoffset[1];
    az =  -az - agoffset[2];
    gx = gx - agoffset[3];
    gy = gy - agoffset[4];
    gz = -gz - agoffset[5];
    // 加速度値を分解能で割って加速度(G)に変換する
    acc_x = float(ax) / ACCEL_SSF;  //FS_SEL_0 16,384 LSB / g
    acc_y = float(ay) / ACCEL_SSF;
    acc_z = float(az) / ACCEL_SSF;
    // 角速度値を分解能で割って角速度(rad per sec)に変換する
    gyro_x = float(gx) / GYRO_SSF * 0.0174533f;  // (rad/s)
    gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
    gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
    mag.getAxis(mdata); // flush the magnetmeter
    magval[0] = (mdata.x - magbias[0]);
    magval[1] = (mdata.y - magbias[1]);
    magval[2] = (mdata.z - magbias[2]);
    mag_x = -magval[0]/magbias[3];
    mag_y = -magval[1]/magbias[3];
    mag_z = -magval[2]/magbias[3];
    
    float lpc_acc = 0.15;
    LPacc_x = lpc_acc*acc_x+(1.0-lpc_acc)*LPacc_x;
    LPacc_y = lpc_acc*acc_y+(1.0-lpc_acc)*LPacc_y;
    LPacc_z = lpc_acc*acc_z+(1.0-lpc_acc)*LPacc_z;

    float lpc_mag = 0.15;
    LPmag_x = lpc_mag*mag_x+(1.0-lpc_mag)*LPmag_x;
    LPmag_y = lpc_mag*mag_y+(1.0-lpc_mag)*LPmag_y;
    LPmag_z = lpc_mag*mag_z+(1.0-lpc_mag)*LPmag_z;
    
    accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
    magnorm = sqrt(mag_x*mag_x+mag_y*mag_y+mag_z*mag_z);
    LPaccnorm = sqrt(LPacc_x*LPacc_x+LPacc_y*LPacc_y+LPacc_z*LPacc_z);
    LPmagnorm = sqrt(LPmag_x*LPmag_x+LPmag_y*LPmag_y+LPmag_z*LPmag_z);
    accrefnorm = sqrt(accref[0]*accref[0]+accref[1]*accref[1]+accref[2]*accref[2]);
    magrefnorm = sqrt(magref[0]*magref[0]+magref[1]*magref[1]+magref[2]*magref[2]);
    calcMagRef(LPmag_x/LPmagnorm, LPmag_y/LPmagnorm, LPmag_z/LPmagnorm);
    
    
}

void updateBetweenMeasures(){
    float q0 = qhat.getNumber( 1, 1 );
    float q1 = qhat.getNumber( 2, 1 ); 
    float q2 = qhat.getNumber( 3, 1 ); 
    float q3 = qhat.getNumber( 4, 1 );
    
    Matrix B(4,3);
    B   << q1  << q2 << q3
        <<-q0  << q3 <<-q2
        <<-q3  <<-q0 << q1             
        << q2  <<-q1 <<-q0;
    B *= 0.5f;
     
    Matrix phi(4,4);
    phi <<  1.0               << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
        <<  gyro_x*0.5*att_dt << 1.0                << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
        <<  gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0               << gyro_x*0.5*att_dt
        <<  gyro_z*0.5*att_dt <<  gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
    qhat = phi*qhat;
    
    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
    qhat *= (1.0f/ qnorm);
    
    Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
    
    q0 = qhat.getNumber( 1, 1 );
    q1 = qhat.getNumber( 2, 1 ); 
    q2 = qhat.getNumber( 3, 1 ); 
    q3 = qhat.getNumber( 4, 1 );
    
    D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
    D.add(1,2,2*(q1*q2 + q0*q3));
    D.add(1,3,2*(q1*q3 - q0*q2));
    D.add(2,1,2*(q1*q2 - q0*q3));
    D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
    D.add(2,3,2*(q2*q3 + q0*q1));
    D.add(3,1,2*(q1*q3 + q0*q2));
    D.add(3,2,2*(q2*q3 - q0*q1));
    D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
    
}

void updateAcrossMeasures(float v1,float v2, float v3, float u1, float u2, float u3, Matrix R){
        
    Matrix u(3,1);
    Matrix v(3,1);
    
    u << u1 << u2 <<u3;
    v << v1 << v2 <<v3;
    
    float q0 = qhat.getNumber( 1, 1 );
    float q1 = qhat.getNumber( 2, 1 ); 
    float q2 = qhat.getNumber( 3, 1 ); 
    float q3 = qhat.getNumber( 4, 1 ); 
    
    Matrix A1(3,3);
    A1 << q0 << q3 << -q2
       <<-q3 << q0 << q1
       <<q2  <<-q1 <<q0;
    A1 *= 2.0f;
    
    Matrix A2(3,3);   
    A2 << q1 << q2 << q3
       << q2 <<-q1 << q0
       << q3 <<-q0 <<-q1;
    A2 *= 2.0f;
    
    Matrix A3(3,3);
    A3 <<-q2 << q1 <<-q0
       << q1 << q2 << q3
       << q0 << q3 <<-q2;
    A3 *= 2.0f;
    
    Matrix A4(3,3);
    A4 <<-q3 << q0 << q1
       <<-q0 <<-q3 << q2
       << q1 << q2 << q3;
    A4 *= 2.0f;
    
    Matrix H(3,4);

    Matrix ab1(A1*u);
    Matrix ab2(A2*u);
    Matrix ab3(A3*u);
    Matrix ab4(A4*u);

    H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
      << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
      << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
    
    
    Matrix K(4,3);
    K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
    
    Matrix dq(4,1);
    dq = K*(v-D*u);
    qhat = qhat+dq;
    
    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
    qhat *= (1.0f/ qnorm);
    
    Matrix eye4(4,4);
    eye4 << 1 << 0 << 0 << 0
         << 0 << 1 << 0 << 0
         << 0 << 0 << 1 << 0
         << 0 << 0 << 0 << 1;
    Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K);
}

void computeAngles()
{
    float q0 = qhat.getNumber( 1, 1 );
    float q1 = qhat.getNumber( 2, 1 ); 
    float q2 = qhat.getNumber( 3, 1 ); 
    float q3 = qhat.getNumber( 4, 1 ); 
    roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
    pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
    yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
}

void triad(float fb1,float fb2, float fb3, float fn1, float fn2, float fn3,float mb1,float mb2, float mb3, float mn1, float mn2, float mn3){
    Matrix W1(3,1);
    W1 << fb1 << fb2<< fb3;
    Matrix W2(3,1);
    W2 << mb1 << mb2<< mb3;
    
    Matrix V1(3,1);
    V1 << fn1 << fn2<< fn3;
    Matrix V2(3,1);
    V2 << mn1 << mn2<< mn3;
    

    Matrix Ou2(3,1);
    Ou2 << W1.getNumber( 2, 1 )*W2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*W2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*W2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*W2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*W2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*W2.getNumber( 1, 1 );
    Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2));
    Matrix Ou3(3,1);
    Ou3 << W1.getNumber( 2, 1 )*Ou2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*Ou2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*Ou2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*Ou2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*Ou2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*Ou2.getNumber( 1, 1 );
    Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3));
    Matrix R2(3,1);
    R2  << V1.getNumber( 2, 1 )*V2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*V2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*V2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*V2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*V2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*V2.getNumber( 1, 1 );
    R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2));
    Matrix R3(3,1);
    R3  << V1.getNumber( 2, 1 )*R2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*R2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*R2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*R2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*R2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*R2.getNumber( 1, 1 );
    R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3));

    Matrix Mou(3,3);
    Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 )
        << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 )
        << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 );
    Matrix Mr(3,3);
    Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 )
       << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 )
       << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 );
       
    Matrix Cbn = Mr*MatrixMath::Transpose(Mou);

    float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 ));

    qhat.add(1,1,0.5*sqtrp1);
    qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1);
    qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1);
    qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1);
   
    float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
    qhat *= (1.0f/ qnorm);
}

void calcDynAcc(){
     dynacc_x = LPacc_x-(D.getNumber( 1, 1 )*accref[0]+D.getNumber( 1, 2 )*accref[1]+D.getNumber( 1, 3 )*accref[2]);
     dynacc_y = LPacc_y-(D.getNumber( 2, 1 )*accref[0]+D.getNumber( 2, 2 )*accref[1]+D.getNumber( 2, 3 )*accref[2]);
     dynacc_z = LPacc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]);
}

void execCalibration() {
  pc.printf("\r\nEnter to Calibration Mode\r\n");
  wait(5);
  pc.printf("\r\n Acc and Gyro Calibration Start\r\n");
  int axs = 0;
  int ays = 0;
  int azs = 0;
  gxs = 0;
  gys = 0;
  gzs = 0;

  int iter_n = 5000;
  for (int i = 0; i < iter_n; i++) {
    accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
    axs += ax;
    ays += ay;
    azs -= az;
    gxs += gx;
    gys += gy;
    gzs -= gz;
    wait(0.001);
  }
  axs = axs / iter_n;
  ays = ays / iter_n;
  azs = azs / iter_n;
  gxs = gxs / iter_n;
  gys = gys / iter_n;
  gzs = gzs / iter_n;
  pc.printf("\r\nagoffset : 0, 0, 0, %d, %d, %d \r\n", gxs, gys, gzs);

  pc.printf("\r\n Mag Calibration Start\r\n");
  float f = 0;
  while (1) {
    mag.getAxis(mdata);  // flush the magnetmeter
    magval[0] = (mdata.x - magbias[0]);
    magval[1] = (mdata.y - magbias[1]);
    magval[2] = (mdata.z - magbias[2]);
    float mag_r =
        magval[0] * magval[0] + magval[1] * magval[1] + magval[2] * magval[2];
    float lr = 0.00001f;
    f = mag_r - magbias[3] * magbias[3];
    magbias[0] = magbias[0] + 4 * lr * f * magval[0];
    magbias[1] = magbias[1] + 4 * lr * f * magval[1];
    magbias[2] = magbias[2] + 4 * lr * f * magval[2];
    magbias[3] = magbias[3] + 4 * lr * f * magbias[3];

    if (userButton.read() == 1) {
      break;
    }
    wait(0.001);
  }
  pc.printf("magbias : %f, %f, %f, %f\r\n", magbias[0], magbias[1], magbias[2],
            magbias[3]);
  pc.printf("\r\n Refvec Calibration waiting\r\n");
  wait(5);
  pc.printf("\r\n Calibration Start\r\n");
  float arefs[3] = {0.0, 0.0, 0.0};
  float mrefs[3] = {0.0, 0.0, 0.0};
  for (int i = 0; i < iter_n; i++) {
    getIMUval();
    arefs[0] += acc_x;
    arefs[1] += acc_y;
    arefs[2] += acc_z;
    mrefs[0] += mag_x;
    mrefs[1] += mag_y;
    mrefs[2] += mag_z;
    wait(0.001);
  }
  arefs[0] /= iter_n;
  arefs[1] /= iter_n;
  arefs[2] /= iter_n;
  mrefs[0] /= iter_n;
  mrefs[1] /= iter_n;
  mrefs[2] /= iter_n;
  pc.printf("\r\naccreg : %f, %f, %f\r\n", arefs[0], arefs[1], arefs[2]);
  pc.printf("\r\nmagreg : %f, %f, %f\r\n", mrefs[0], mrefs[1], mrefs[2]);
  // while(1) {wait(1000);}
  pc.printf("\r Writing to EEPROM...\r\n");
  U transfer_data;
  transfer_data.i[0] = 2;
  for (int i = 1; i < 5; i++) {
    if (!isnan(magbias[i - 1]))
      transfer_data.i[i] = (int)magbias[i - 1];  // intに丸めた値を送る。
    else {
      pc.printf("mag bias is NOT transferred\n");
      transfer_data.i[i] = 100;
    }
  }
  // gxs,gys,gzsを送る
  int gxyzs[3] = {gxs, gys, gzs};
  for (int i = 5; i < 8; i++) {
    if (!isnan(gxyzs[i - 5]))
      transfer_data.i[i] = gxyzs[i - 5];
    else {
      pc.printf("gxyzs is NOT transferred\n");
      transfer_data.i[i] = 0;
    }
  }
   
  for (int i = 0; i < 8; i++) {
    pc.printf("transfer_data[%d]: %\d\n", i, transfer_data.i[i]);
  }
  writeEEPROM(address, pointeraddress, transfer_data.c, 32);
  wait(3);
  U read_test;
  readEEPROM(address, pointeraddress, read_test.c, 32);
  wait(3);
  for (int i = 0 ; i < 8; i ++){
      pc.printf("transfer_data[%d]: %d\n",i, read_test.i[i]); 
  }
}


// 割り込まれた時点での出力(computeの結果）を返す関数
void calcServoOut(){
    if(sbus.failSafe == false) {
        // sbusデータの読み込み
        for (int i =0 ; i < 16;i ++){
            rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
        }
    }
    
    pitchPID.setProcessValue(pitch);
    pitchratePID.setProcessValue(gyro_y);
    rollPID.setProcessValue(roll);
    rollratePID.setProcessValue(gyro_x);
    float de = pitchPID.compute()+pitchratePID.compute()-rc[1]+rc[0];
    float da = rollPID.compute()+rollratePID.compute()+rc[0]+rc[1];
    float dT = rc[2];
    
    scaledServoOut[0]=de+da;
    scaledServoOut[1]=-de+da;
    scaledMotorOut[0]= dT;
    
    float LP_servo = 0.2;
    float LP_motor = 0.2;
    for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){
        servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
        if(servoOut[i]<servoPwmMin) {
            servoOut[i] = servoPwmMin;
        };
        if(servoOut[i]>servoPwmMax) {
            servoOut[i] = servoPwmMax;
        };
    }
    
    for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
        motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
        if(motorOut[i]<motorPwmMin) {
            motorOut[i] = motorPwmMin;
        };
        if(motorOut[i]>motorPwmMax) {
            motorOut[i] = motorPwmMax;
        };
    }
    servoRight.pulsewidth_us(servoOut[0]);
    servoLeft.pulsewidth_us(servoOut[1]); 
    servoThrust.pulsewidth_us(motorOut[0]);
     
    if(loop_count > 10){
        writeSdcard();
        loop_count = 1;
        obs_count = 0;
    }else{
        loop_count +=1;
    }
}

int main()
{
    pc.baud(57600);
    U read_data; // eepromからの読み込み
    readEEPROM(address, pointeraddress, read_data.c, 32);
    pc.printf("%d",read_data.i[0]); // for debug
    if(read_data.i[0] < 0){
        pc.printf("No Data Stored in EEPROM!!!");
        execCalibration();
    }
    for (int i = 1 ; i < 5; i ++) magbias[i] = read_data.i[i];
    gxs = read_data.i[5];
    gys = read_data.i[6];
    gzs = read_data.i[7];
    pitchPID.setSetPoint(0.0);
    pitchratePID.setSetPoint(0.0); 
    rollPID.setSetPoint(0.0); 
    rollratePID.setSetPoint(0.0); 
    pitchPID.setBias(0.0);
    pitchratePID.setBias(0.0);
    rollPID.setBias(0.0); 
    rollratePID.setBias(0.0); 
    pitchPID.setOutputLimits(-1.0,1.0);
    pitchratePID.setOutputLimits(-1.0,1.0);
    rollPID.setOutputLimits(-1.0,1.0); 
    rollratePID.setOutputLimits(-1.0,1.0);
    pitchPID.setInputLimits(-pi,pi);
    pitchratePID.setInputLimits(-pi,pi);
    rollPID.setInputLimits(-pi,pi); 
    rollratePID.setInputLimits(-pi,pi);
    
    servoRight.period_us(15000.0);
    servoLeft.period_us(15000.0);
    servoThrust.period_us(15000.0);
    servoRight.pulsewidth_us(1500.0);
    servoLeft.pulsewidth_us(1500.0); 
    servoThrust.pulsewidth_us(1100.0);
    
    sd.baud(57600);
    sd.printf("\r\n Program Start \r\n");
    accelgyro.initialize();
    //加速度計のフルスケールレンジを設定
    accelgyro.setFullScaleAccelRange(ACCEL_FSR);
    //角速度計のフルスケールレンジを設定
    accelgyro.setFullScaleGyroRange(GYRO_FSR);
    //MPU6050のLPFを設定
    accelgyro.setDLPFMode(MPU6050_LPF);
    //地磁気
    mag.enable();
    
    if(userButton.read() == 0){
        qhat << 1 << 0 << 0 << 0;
    
        Phat << 0.01 << 0 <<0 <<0
             << 0 << 0.01 <<0 <<0
             << 0 << 0 <<0.001 <<0
             << 0 << 0 << 0 << 0.001;
        
        Qgyro << 5.4732e-04 << 0     <<0
              << 0     <<5.4732e-04 <<0
              << 0     << 0     <<5.4732e-04;  
        
        Racc.add(1,1,1.0);
        Racc.add(2,2,1.0);
        Racc.add(3,3,1.0);
             
        Rmag << 1   << 0   <<0
             << 0   << 1   <<0
             << 0   << 0   <<1;   
        getIMUval();
        triad(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm, accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,mag_x/magnorm,mag_y/magnorm,mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm);
        for(int i = 0; i<1000 ;i++){
            getIMUval();
            val_thmg += acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm);
        }
        val_thmg /= 1000;
        LoopTicker PIDtick;
        PIDtick.attach(calcServoOut,PID_dt);
    
        t.start();
        
        while(1) {
            float tstart = t.read();
            //姿勢角を更新
            getIMUval();
            updateBetweenMeasures();
            calcDynAcc();
            th_mg = abs(acos((LPmag_x*LPacc_x+LPmag_y*LPacc_y+LPmag_z*LPacc_z)/LPmagnorm/LPaccnorm)-val_thmg);
            dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z);
            accnormerr = abs(LPaccnorm-accrefnorm);
            //pc.printf("%f %f : %f \r\n",th_mg,th_mgref,abs(th_mg-th_mgref));
            int ang_th  = th_mg<0.01;
            int dyn_th = dynaccnorm < 0.01;
            int norm_th =  accnormerr< 0.01;
            //pc.printf("%d %d %d %f %f %f\r\n",ang_th,dyn_th,norm_th,abs(th_mg-val_thmg),dynaccnorm ,abs(accnorm-accrefnorm));
            if(dyn_th+ang_th+norm_th>0){
                    obs_count += 1;
                    updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
                    updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
            }
            computeAngles();
            PIDtick.loop(); 
            
            float tend = t.read();
            att_dt = (tend-tstart);
        }
    }else{
        execCalibration();
    }
}