File content as of revision 1:ea4efc600a1e:

#include "mbed.h"
#include <stdio.h>
#include "BufferSerial.h"
#include "BMP085.h"
//#include "MODSERIAL.h"

Serial pc(USBTX,USBRX);
I2C i2c(p28,p27);
BMP085 alt_sensor(i2c);
BufferSerial kirim(USBTX,USBRX,1);
//BufferSerial kirim(p9, p10, 1);
PwmOut Servo_1(p21);
PwmOut Servo_2(p22);

//const int SlaveAddressI2C = 0xEE;


#define ACCEL 0xA6
#define MAGNET 0x3C
#define ID_GATHOTKACA 106

//initialize accel
#define ADXL345_AXIS_X_SCALE 4

#define adxl345_address (0xA6>>1)
#define adxl345_reg_data_format (0x31)
#define adxl345_reg_pwr_ctl (0x2D)
#define adxl345_reg_xlsb (0x32)

//initialize gyro
#define ITG3200_R 0x69
#define ITG3200_W 0x68
#define WHO 0x00
#define SMPL 0x15
#define INT_C 0x17
#define TMP_H 0x1B
#define GX_H 0x1D
#define GY_H 0x1F
#define GZ_H 0x21
#define PWR_M 0x3E
#define DLPF 0x16
#define INT_S 0x1A
#define TMP_L 0x1C
#define GX_L 0x1E
#define GY_L 0x20
#define GZ_L 0x22

#define itg3200_address (0xD0)
#define itg3200_reg_xmsb (0x1D)
#define itg3200_reg_who_am_I (0x00)
#define itg3200_reg_smplrt_div (0x15)
#define itg3200_reg_dlpf_fs (0x16)
#define DLPF_CFG_0 (1<<0)
#define DLPF_CFG_1 (1<<1)
#define DLPF_CFG_2 (1<<2)
#define DLPF_FS_SEL_0 (1<<3)
#define DLPF_FS_SEL_1 (1<<4)


//initialize magneto
#define HMC5843_W 0x3C
#define HMC5843_R 0x3D
#define PI 3.14159265

#define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration

    #define MAGN_X_MAX 465
    #define MAGN_Y_MAX 475
    #define MAGN_Z_MAX 600

    #define MAGN_X_MIN -561
    #define MAGN_Y_MIN -547
    #define MAGN_Z_MIN -379
    #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
    #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
    #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)

    #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
    #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
    #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
    
    #define DEBUG__NO_DRIFT_CORRECTION true

    //Change this parameter if you wanna use another gyroscope.
    //by default it is ITG3200 gain.
    #define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second
    #define GYRO_GAIN (0.06956521739130434782608695652174)
    // DCM parameters
    #define Kp_ROLLPITCH 0.02f
    #define Ki_ROLLPITCH 0.00002f
    #define Kp_YAW 1.2f
    #define Ki_YAW 0.00002f

    #define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration
    #define TO_RAD(x) (x * 0.01745329252)  // *pi/180
    #define TO_DEG(x) (x * 57.2957795131)
        #define GYRO_BIAS_X -74.49
    #define GYRO_BIAS_Y -49.43
    #define GYRO_BIAS_Z -17.06

    #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
    #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
    #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
    #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
    #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
    #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
    #define ACCEL_X_MIN ((float) -35)
    #define ACCEL_X_MAX ((float) 33)

    #define ACCEL_Y_MIN ((float) -34)//267
    #define ACCEL_Y_MAX ((float) 34)

    #define ACCEL_Z_MIN ((float) -36)
    #define ACCEL_Z_MAX ((float) 33)
    
#define goal_ang_1 75
#define goal_ang_2 -105
#define Kp 15
#define Ki 0
#define Kd 0
#define base_speed 50
#define Kp_ang 15
#define Ki_ang 0
#define Kd_ang 0

short rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ;
unsigned short sendAccX, sendAccY, sendAccZ, sendGyroX, sendGyroY, sendGyroZ, sendMagX, sendMagY, sendMagZ;
unsigned char baca=0;

//variabel homing
signed short angle;
signed short GoalAngle;
signed short delta_angle_1, delta_angle_2;
signed char delta;
int servo_left, servo_right;
int jump;
float jump_error;
float e, ea, ea_sum, ea_old;
long int delta_lat, delta_long;
float error, error_sum, error_old;
////=====================================

//variabel DCM
float MAG_Heading;
float Accel_Vector[3];//= {0, 0, 0}; // Store the acceleration in a vector
float Gyro_Vector[3];//= {0, 0, 0}; // Store the gyros turn rate in a vector
float Omega_Vector[3];//= {0, 0, 0}; // Corrected Gyro_Vector data
float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction
float Omega_I[3];//= {0, 0, 0}; // Omega Integrator
float Omega[3];//= {0, 0, 0};
float errorRollPitch[3];// = {0, 0, 0};
float errorYaw[3];// = {0, 0, 0};
float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
float G_Dt;
int rawMagnet[3];
int gyroBias[3];
float accel[3];  // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
int accelRaw[3];
float accel_min[3];
float accel_max[3];

float magnet[3];
int magnetRaw[3];
float magnetom_min[3];
float magnetom_max[3];

int gyro[3];
int gyroRaw[3];
float gyro_average[3];

int gyro_num_samples;
//euler
float yaw;
float pitch;
float roll;
//=======
//==================================================================

void bin_dec_conv(unsigned short data)
{
    unsigned short hasil;
//    unsigned char kirim[16];

    hasil = data%100;
    pc.printf("%d%d%d",(data/100),(hasil/10),(hasil%10));

}

void tulis_i2c(unsigned char devadd, unsigned char regadd, unsigned char data)
{
    i2c.start();
    i2c.write(devadd);
    i2c.write(regadd);
    i2c.write(data);
    i2c.stop();
    
}
unsigned char baca_i2c(unsigned char devadd, unsigned char regadd)
{
    unsigned char data;
    i2c.start();
    i2c.write(devadd);
    i2c.write(regadd);
    i2c.start();
    i2c.write(devadd|1);
    data=i2c.read(0);
    i2c.stop();
    return data;
}
int baca_adxl()
{
    unsigned char acc_x_msb, acc_x_lsb;
    unsigned char acc_y_msb, acc_y_lsb;
    unsigned char acc_z_msb, acc_z_lsb;
    
    tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 0);
    tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 16);
    tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 8);
    
    tulis_i2c(ACCEL, adxl345_reg_data_format, 0x03);
    
     acc_x_lsb = baca_i2c(ACCEL,0x32);
     acc_x_msb = baca_i2c(ACCEL,0x33);
     acc_y_lsb = baca_i2c(ACCEL,0x34);
     acc_y_msb = baca_i2c(ACCEL,0x35);
     acc_z_lsb = baca_i2c(ACCEL,0x36);
     acc_z_msb = baca_i2c(ACCEL,0x37);
    
        float acc_x = (signed short)((signed short)(acc_x_msb<<8) | acc_x_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f;
        float acc_y = 1*(signed short)((signed short)(acc_y_msb<<8) | acc_y_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f;
        float acc_z = (signed short)((signed short)(acc_z_msb<<8) | acc_z_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f
        
//        rawAccX=-1*acc_y;
//        rawAccY=acc_x;
//        rawAccZ=acc_z;
        rawAccX=acc_x;
        rawAccY=acc_y;
        rawAccZ=acc_z;

}



void baca_itg()
{
    tulis_i2c(itg3200_address, itg3200_reg_dlpf_fs, (DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0));
    tulis_i2c(itg3200_address, itg3200_reg_smplrt_div, 9);
    char xh,xl,yh,yl,zh,zl;
    short x,y,z;
        xl = baca_i2c(itg3200_address,0x1E);
        xh = baca_i2c(itg3200_address,0x1D);
        yh = baca_i2c(itg3200_address,0x1F);
        yl = baca_i2c(itg3200_address,0x20);
        zh = baca_i2c(itg3200_address,0x21);
        zl = baca_i2c(itg3200_address,0x22);
        
        x=1*(signed short)((signed short)(xl | (xh<<8)));//*0.0695652174;
        y=1*(signed short)((signed short)(yl | (yh<<8)));//*0.0695652174;
        z=1*(signed short)((signed short)(zl | (zh<<8)));
        //
//        rawGyroX=-1*y;
//        rawGyroY=x;
//        rawGyroZ=z;

        rawGyroX=x;
        rawGyroY=y;
        rawGyroZ=z;
}

void i2c_tulis_hmc(unsigned char address, unsigned char data)
{
i2c.start();
i2c.write(HMC5843_W); // write 0x
i2c.write(address); // write register address
i2c.write(data); // write register address
i2c.stop();
//__delay_ms(10);
}

unsigned char i2c_baca_hmc(unsigned char address)
{
unsigned char data;
i2c.start();
i2c.write(HMC5843_W); // write 0x
i2c.write(address); // write register address
i2c.start();
i2c.write(HMC5843_R); // write 0x
data = i2c.read(0); // Get MSB result
i2c.stop();
//__delay_ms(10);
return data;
}

int baca_hmc() // baca_itg(raw) untuk ambil data raw : baca_itg(scaled) untuk ambil data terskala
{
//    char xh, xl, yh, yl, zh, zl;
//    short x, y, z;
  int xh, xl, yh, yl, zh, zl;
    float x, y, z;
    
      i2c_tulis_hmc(0x02,0x01);
        xh=i2c_baca_hmc(0x03);
        xl=i2c_baca_hmc(0x04);
        zh=i2c_baca_hmc(0x05);
        zl=i2c_baca_hmc(0x06);
        yh=i2c_baca_hmc(0x07);
        yl=i2c_baca_hmc(0x08);
        x=(signed short)((signed short)(xl | (xh<<8)));
        y=(signed short)((signed short)(yl | (yh<<8)));
        z=(signed short)((signed short)(zl | (zh<<8)));
        rawMagX=x;
        rawMagY=y;
        rawMagZ=z;
//        rawMagX=-1*x;
//        rawMagY=1*y;
//        rawMagZ=1*z;
}

void setAccelRaw(int accelRawX,int accelRawY,int accelRawZ)
{
    accelRaw[0] = (uint16_t)accelRawX;
    accelRaw[1] = (uint16_t)accelRawY;
    accelRaw[2] = (uint16_t)accelRawZ;
}
void setGyroRaw(int gyroRawX,int gyroRawY,int gyroRawZ)
{
    gyroRaw[0] = gyroRawX;
    gyroRaw[1] = gyroRawY;
    gyroRaw[2] = gyroRawZ;
}
void setMagnetRaw(int magnetRawX,int magnetRawY,int magnetRawZ)
{
    magnetRaw[0] = (uint16_t)magnetRawX;
    magnetRaw[1] = (uint16_t)magnetRawY;
    magnetRaw[2] = (uint16_t)magnetRawZ;
}

void ReadMagnetometer()
{
    rawMagnet[0] = ((int16_t)magnetRaw[0]);
    rawMagnet[1] = ((int16_t)magnetRaw[1]);
    rawMagnet[2] = ((int16_t)magnetRaw[2]);

    magnet[0] = ((float)(rawMagnet[0] - MAGN_X_OFFSET) * MAGN_X_SCALE);
    magnet[1] = ((float)(rawMagnet[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE);
    magnet[2] = ((float)(rawMagnet[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE);
}
void ReadAccelerometer()
{
    accel[0] = (((int16_t)accelRaw[0]) - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
    accel[1] = (((int16_t)accelRaw[1]) - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
    accel[2] = (((int16_t)accelRaw[2]) - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
}
void ReadGyroscope()
{
    gyro[0] = (gyroRaw[0] - GYRO_BIAS_X);
    gyro[1] = (gyroRaw[1] - GYRO_BIAS_Y);
    gyro[2] = (gyroRaw[2] - GYRO_BIAS_Z);
}

void Compass_Heading()
{
  float mag_x;
  float mag_y;
  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;

  cos_roll = cos(roll);
  sin_roll = sin(roll);
  cos_pitch = cos(pitch);
  sin_pitch = sin(pitch);

  // Tilt compensated magnetic field X
  mag_x = magnet[0]*cos_pitch + magnet[1]*sin_roll*sin_pitch + magnet[2]*cos_roll*sin_pitch;
  // Tilt compensated magnetic field Y
  mag_y = magnet[1]*cos_roll - magnet[2]*sin_roll;
  // Magnetic Heading
  MAG_Heading = atan2(-mag_y, mag_x);
}
//=========fungsi matrix======
void Vector_Cross_Product(float C[3], float A[3], float B[3])
{
    C[0] = (A[1] * B[2]) - (A[2] * B[1]);
    C[1] = (A[2] * B[0]) - (A[0] * B[2]);
    C[2] = (A[0] * B[1]) - (A[1] * B[0]);

    return;
}

void Vector_Scale(float C[3], float A[3], float b)
{
    int m;
    for (m = 0; m < 3; m++)
        C[m] = A[m] * b;

    return;
}

float Vector_Dot_Product(float A[3], float B[3])
{
    float result = 0.0;

    int i;
    for (i = 0; i < 3; i++) {
        result += A[i] * B[i];
    }

    return result;
}

void Vector_Add1(float C[3], float A[3], float B[3])
{
    int m;
    for (m = 0; m < 3; m++)
        C[m] = A[m] + B[m];
    return;
}

void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
{
    int m,n;
    for (m = 0; m < 3; m++)
        for (n = 0; n < 3; n++)
            C[m][n] = A[m][n] + B[m][n];
}

void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
{
    int i,j,k;
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++) {
            C[i][j] = 0;
            for (k = 0; k < 3; k++) {
               C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

//=========end matrix=========

//=====mulai DCM======
//dari objek terhadap tanah
void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll)
{
  float c1 = cos(roll);
  float s1 = sin(roll);
  float c2 = cos(pitch);
  float s2 = sin(pitch);
  float c3 = cos(yaw);
  float s3 = sin(yaw);

  // Euler angles, right-handed, intrinsic, XYZ convention
  // (which means: rotate around body axes Z, Y', X'')
  m[0][0] = c2 * c3;
  m[0][1] = c3 * s1 * s2 - c1 * s3;
  m[0][2] = s1 * s3 + c1 * c3 * s2;

  m[1][0] = c2 * s3;
  m[1][1] = c1 * c3 + s1 * s2 * s3;
  m[1][2] = c1 * s2 * s3 - c3 * s1;

  m[2][0] = -s2;
  m[2][1] = c2 * s1;
  m[2][2] = c1 * c2;
}

float constrain(float in, float min, float max)
{
    in = in > max ? max : in;
    in = in < min ? min : in;
    return in;
}

void Normalize()
{
  float error=0;
  float temporary[3][3];
  float renorm=0;

  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19

  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
  Vector_Add1(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
  Vector_Add1(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19

  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20

  renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);

  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);

  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}

void Drift_correction()
{
  float mag_heading_x;
  float mag_heading_y;
  float errorCourse;
  //Compensation the Roll, Pitch and Yaw drift.
  static float Scaled_Omega_P[3];
  static float Scaled_Omega_I[3];
  float Accel_magnitude;
  float Accel_weight;
    //*****Roll and Pitch***************

  // Calculate the magnitude of the accelerometer vector
  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
  // Dynamic weighting of accelerometer info (reliability filter)
  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //

  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
   Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
  Vector_Add1(Omega_I,Omega_I,Scaled_Omega_I);
    //*****YAW***************
  // We make the gyro YAW drift correction based on compass magnetic heading

  mag_heading_x = cos(MAG_Heading);
  mag_heading_y = sin(MAG_Heading);
  errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.

  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
  Vector_Add1(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.

  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
  Vector_Add1(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
}
void Matrix_update()
{
  Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
  Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
  Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw

  Accel_Vector[0]=accel[0];
  Accel_Vector[1]=accel[1];
  Accel_Vector[2]=accel[2];

  Vector_Add1(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
  Vector_Add1(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term

  Update_Matrix[0][0]=0;
  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
  Update_Matrix[1][1]=0;
  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];
  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];
  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];
  Update_Matrix[2][2]=0;

  Matrix_Multiply(Temporary_Matrix,DCM_Matrix,Update_Matrix); //a*b=c

  int x,y;
  for(x=0; x<3; x++) //Matrix Addition (update)
  {
    for(y=0; y<3; y++)
    {
      DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
    }
  }
}
void Euler_angles()
{
  pitch = -asin(DCM_Matrix[2][0]);
  roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
  yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
}
void Sensors()
{
    ReadAccelerometer();
    ReadGyroscope();
    ReadMagnetometer();
    Compass_Heading();
}
void UpdateFilter()
{
    Sensors();
    Matrix_update();
    Normalize();
    Drift_correction();
    Euler_angles();
}
void UpdateFilter2()
{
    baca_itg();
    baca_hmc();
    baca_adxl();

    setAccelRaw(rawAccX,rawAccY,rawAccZ);

    setGyroRaw(rawGyroX,rawGyroY,rawGyroZ);

    setMagnetRaw(rawMagX,rawMagY,rawMagZ);
    UpdateFilter();
}
void reset_sensor_fusion() {
  float temp1[3];
  float temp2[3];
  float xAxis[] = {1.0f, 0.0f, 0.0f};

  Sensors();

  // GET PITCH
  // Using y-z-plane-component/x-component of gravity vector
  pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2]));

  Vector_Cross_Product(temp1, accel, xAxis);
  Vector_Cross_Product(temp2, xAxis, temp1);
  roll = atan2(temp2[1], temp2[2]);

  // GET YAW
  Compass_Heading();
  yaw = MAG_Heading;

  // Init rotation matrix
    init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
}
float getYaw()
{
    return TO_DEG(yaw);
}
float getPitch()
{
    return TO_DEG(pitch);
}
float getRoll()
{
    return TO_DEG(roll);
}
void data_DCM ()
{
    UpdateFilter2();
    yaw = getYaw();
    pitch = getPitch();
    roll = getRoll();
}

void DCM(float dt)
{
    int i,l,j;
    for(i = 0; i < 3; i++)
    {
        Accel_Vector[i] = 0; // Store the acceleration in a vector
        Gyro_Vector[i] = 0; // Store the gyros turn rate in a vector
        Omega_Vector[i] = 0; // Corrected Gyro_Vector data
        Omega_P[i] = 0; // Omega Proportional correction
        Omega_I[i] = 0; // Omega Integrator
        Omega[i]= 0;
        errorRollPitch[i] = 0;
        errorYaw[i] = 0;
    }

    int k = 1;

    for(l = 0; l < 3; l++)
    {
        for(j = 0; j < 3; j++)
        {
            DCM_Matrix[l][j] = 0;
            Update_Matrix[l][j] = k;
            Temporary_Matrix[l][j] = 0;
            k++;
        }
        k++;
    }

    G_Dt = dt;

    reset_sensor_fusion();
}
void dataoutdcm2()
{
//    unsigned char i;
    pc.printf("#YPR=%.2f,%.2f,%.2f\n",yaw,pitch,roll);
//    for(i=0;i<3;i++)
//    {
//        sprintf(kirim,"Acc=%d,%d,%d\n",roll,pitch,yaw);

//    }

}
void FilterInit(int rawAccX, int rawAccY, int rawAccZ, int rawGyroX, int rawGyroY, int rawGyroZ, int rawMagX, int rawMagY, int rawMagZ)
{
    baca_itg();
    baca_hmc();
    baca_adxl();

   setAccelRaw(rawAccX,rawAccY,rawAccZ);

    setGyroRaw(rawGyroX,rawGyroY,rawGyroZ);

    setMagnetRaw(rawMagX,rawMagY,rawMagZ);

    reset_sensor_fusion();
}
//=====end DCM========


//=====baca baro
void baca_baro()
{
   unsigned char tekanan;
    while(1)
    {
        tekanan=alt_sensor.get_pressure();
        pc.printf("Altitude: %f\r\n", alt_sensor.get_altitude_m());
    }
}

void Servo()
{
       jump = base_speed - delta;
    if(jump>100) servo_left = 100;
    else if(jump<0) servo_left = 0;
    else servo_left = (unsigned char)(jump);

    jump = base_speed + delta;
    if(jump>100) servo_right = 100;
    else if(jump<0) servo_right = 0;
    else servo_right = (unsigned char)(jump);
    
    servo_left = 1000 + servo_left*10;
    servo_right = 1000 + servo_right*10;
    Servo_1.pulsewidth_us(servo_left);
    Servo_2.pulsewidth_us(servo_right);
    pc.printf("%d || %d\n",servo_left,servo_right);
}

void PID()
{
     FilterInit(rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ);
     data_DCM ();
     angle = (signed short)(yaw);
    delta_angle_1 = abs(angle-goal_ang_1);
    if(delta_angle_1>180) delta_angle_1 = 360 - delta_angle_1;
    delta_angle_2 = abs(angle-goal_ang_2);
    if(delta_angle_2>180) delta_angle_2 = 360 - delta_angle_2;
    if(delta_angle_1<delta_angle_2) GoalAngle = goal_ang_1;
    else GoalAngle = goal_ang_2;
        jump = (angle - GoalAngle);
    error = atan2(sin(TO_RAD(jump)),cos(TO_RAD(jump)));
        error_sum = error_sum + error;
         jump_error = error_old;
    error_old=error;
    delta = (signed char) (Kp_ang*error + Ki_ang*error_sum + Kd_ang*(error-error_old));
    

     
}


void raw_to_send()
{
    sendAccX = (unsigned short) (rawAccX + 512);//if(sendAccX>999) sendAccX=999;
    sendAccY = (unsigned short) (rawAccY + 512);//if(sendAccY>999) sendAccY=999;
    sendAccZ = (unsigned short) (rawAccZ + 512);//if(sendAccZ>999) sendAccZ=999;
    sendGyroX = (unsigned short)((rawGyroX*0.06956+2400)*0.2083);//if(sendGyroX>999) sendGyroX=999;
    sendGyroY = (unsigned short)((rawGyroY*0.06956+2400)*0.2083);//if(sendGyroY>999) sendGyroY=999;
    sendGyroZ = (unsigned short)((rawGyroZ*0.06956+2400)*0.2083);//if(sendGyroZ>999) sendGyroZ=999;
    sendMagX = (unsigned short)((rawMagX+2048)*0.25);//if(sendMagX>999) sendMagX=999;
    sendMagY = (unsigned short)((rawMagY+2048)*0.25);//if(sendMagY>999) sendMagY=999;
    sendMagZ = (unsigned short)((rawMagZ+2048)*0.25);//if(sendMagZ>999) sendMagZ=999;
}

int main()
{
    //baca = 1;
    int count = 0;
    pc.baud(57600);
    Servo_1.period_ms(20);
    Servo_2.period_ms(20);
    while(1)
    {
        if(kirim.readable())
          {
              baca=kirim.getc();
              if(baca=='s')
              {
                  baca='0';
                  while(1)
                  {
                      
                      count=count++;
                  baca_adxl();
                  baca_itg();
                  baca_hmc();
                  raw_to_send();

                  pc.putc(0x0D);
                  bin_dec_conv(ID_GATHOTKACA);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendAccX);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendAccY);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendAccZ);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendGyroX);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendGyroY);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendGyroZ);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendMagX);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendMagY);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendMagZ);
                  
                  wait_ms(75); 
                  
                  baca=kirim.getc();
                  
                  //if(count ==4)
//                  {
//                    Servo_1.pulsewidth_us(1000);
//                    Servo_2.pulsewidth_us(2000);  
//                  }
//                  if(count ==8)
//                  {
//                     Servo_1.pulsewidth_us(2000); 
//                     Servo_2.pulsewidth_us(1000); 
//                     count = 0;
//                  }
                  
                  if(baca=='x')
                  {
                      baca='0';
                      break;
                  }
                  }
              }
              //baca=kirim.getc();
              if(baca=='d')
              {
                  baca='0';
                  while(1)
                  {
                    FilterInit(rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ);
                   data_DCM ();

                      dataoutdcm2();
                      PID();
                      Servo();
                      wait_ms(10);
                      baca=kirim.getc();
                  if(baca=='x')
                  {
                     baca='0';
                     break; 
                  }
                  }
              }
              if(baca=='b')
              {
                  baca='0';
                  while(1)
                  {
                      PID();
                      baca=kirim.getc();
                  if(baca=='x')
                  {
                     baca='0';
                     break; 
                  }
                  }  
              }
              if(baca=='p')
              {
                    baca='0';
                    while(1)
                    {
                         pc.printf("IVAN JELEK IVAN KELEKEJFKDFJKJ \n");
                         baca=kirim.getc();
                         if(baca=='x')
                         {
                             baca='0';
                             break;
                         }
                    }    
              }
              
              
          }        
    }
}