/* ------------------------------------------------------------------------ */
/* Flight code for the Quad Wanze with DX7 satellite                        */
/* DX7 transmitter modification to acro and aux2 to rocker                  */
/* use in arduino nano board with 328                                       */
/* last modification: 19 October 2013                                       */
/* k.-p.neitzke                                                             */
/* ------------------------------------------------------------------------ */
/* ------------------------------------------------------------------------ */
/* define global values                                                     */
/* ------------------------------------------------------------------------ */
int rc_lift, rc_pitch, rc_yaw, rc_roll, rc_s1, rc_s2;
int m1, m2, m3, m4;
unsigned long uart_time_old;
unsigned long imu_time_old;
unsigned int i, z[16]; 

double Pi, rx; 
double pitch_g_sens, roll_g_sens, yaw_g_sens, acc_x, acc_y, acc_z, acc_r;
double pitch_g_sens_old, roll_g_sens_old;
double nx_tp, ny_tp, nz_tp, nx_hp, ny_hp, nz_hp, old_nx, old_ny, old_nz;
double nx, ny, nz, nr;

/* ------------------------------------------------------------------------ */
/* setup procedure                                                          */
/* ------------------------------------------------------------------------ */
void setup()
 { 
   uart_time_old= 0;
   imu_time_old= 0;
   Pi= 3.14159;
   rx= 0.001;

   for (i= 1; i<=16; i= i+1) z[i]= 0;
   i= 1;
   rc_pitch= 0; rc_yaw= 0; rc_roll= 0; rc_lift= 0; rc_s1= 0; rc_s2= 0;
   
   Serial.begin(115200);  /* DX7 satellite */
   
   old_nx= 0; nx_tp= 0; nx_hp= 0;
   old_ny= 0; ny_tp= 0; ny_hp= 0;
   old_nz= 1; nz_tp= 1; nz_hp= 0;
 }

/* ------------------------------------------------------------------------ */
/* read the DX7 satellite procedure                                         */
/* ------------------------------------------------------------------------ */
void read_rc()
 { 
  unsigned long uart_time;
  unsigned int uart_dt; 
   
   uart_time= micros();
   uart_dt= int(uart_time - uart_time_old);
   uart_time_old= uart_time;   
  
   if (uart_dt > 8000) i= 1; 
   else i= i+1;
   z[i]= Serial.read();
   
   if (i == 16)
    {
      rc_pitch= int(256 * z[ 7] + z[ 8]) - 2560;
      rc_roll=  1536 - int(256 * z[ 3] + z[ 4]);
      rc_yaw=   3582 - int(256 * z[ 9] + z[10]);
      rc_lift=   929 - int(256 * z[11] + z[12]);
      
      /* trim values - 1 trim bit = 4 steps in the value */
      rc_pitch= rc_pitch + 4* 0;
      rc_roll=  rc_roll  + 4* 0;
      rc_yaw=   rc_yaw   + 4* 0;
      
      rc_s1= 0;
      if (int(256 * z[ 5] + z[ 6]) < 5700) rc_s1= 1;   
      if (int(256 * z[ 5] + z[ 6]) < 5500) rc_s1= 2;  
      rc_s2= 0;
      if (int(256 * z[13] + z[14]) < 4600) rc_s2= 1;
      
      i= 1;
    }
 }

/* ------------------------------------------------------------------------ */
/* read the imu                                                             */
/* ------------------------------------------------------------------------ */
void read_imu()
 {
   pitch_g_sens= (double(analogRead(0)) - 507) / 1.23; /* deg/sec */
   roll_g_sens=  (double(analogRead(1)) - 507) / 1.23; /* deg/sec */
   yaw_g_sens=   (515 - double(analogRead(5))) / 1.23; /* deg/sec */
   acc_x=        (double(analogRead(4)) - 506) / 155;        /* g */
   acc_y=        (515 - double(analogRead(3))) / 155;        /* g */
   acc_z=        (double(analogRead(2)) - 506) / 155;        /* g */
 }

/* ------------------------------------------------------------------------ */
/* calculate sqr                                                            */
/* ------------------------------------------------------------------------ */
double sqr(double val)
 {
   return val*val;
 }

/* ------------------------------------------------------------------------ */
/* calculate the complementary filter                                       */
/* ------------------------------------------------------------------------ */
void calc_imu()
 {
  unsigned long imu_time; 
  double imu_dt;
  double pitch, roll, yaw; 
   
   imu_time= micros();
   imu_dt= double(imu_time - imu_time_old) / 1e6 * Pi/180;
   imu_time_old= imu_time;
   
   if (imu_dt > 0)
    {
      pitch=   pitch_g_sens * imu_dt;
      roll = - roll_g_sens  * imu_dt;
      yaw  =   yaw_g_sens   * imu_dt;

      nx= old_nx +  roll * old_nz -   yaw *old_ny;
      ny= old_ny +   yaw * old_nx - pitch *old_nz;
      nz= old_nz + pitch * old_ny -  roll *old_nx;

      nr= sqrt(sqr(nx) + sqr(ny) + sqr(nz));
      if (nr > 0.01)
       {
         nx= nx / nr;
         ny= ny / nr;
         nz= nz / nr;
       }

      acc_r= sqrt(sqr(acc_x) + sqr(acc_y) + sqr(acc_z));
      if (acc_r > 0.01)
       {
         acc_x= acc_x / acc_r;
         acc_y= acc_y / acc_r;
         acc_z= acc_z / acc_r;
       }
         
      nx_tp= (1-rx) * nx_tp + rx * acc_x;
      ny_tp= (1-rx) * ny_tp + rx * acc_y;
      nz_tp= (1-rx) * nz_tp + rx * acc_z;

      nx_hp= (1-rx) * nx_hp + nx - old_nx;
      ny_hp= (1-rx) * ny_hp + ny - old_ny;
      nz_hp= (1-rx) * nz_hp + nz - old_nz;

      nx= nx_tp + nx_hp;
      ny= ny_tp + ny_hp;
      nz= nz_tp + nz_hp;
      
      old_nx= nx;
      old_ny= ny;
      old_nz= nz;
   }
 }

/* ------------------------------------------------------------------------ */
/* calculate the motor values                                               */
/* ------------------------------------------------------------------------ */
void calc_mot()
 {
  int L, P, R, Y;
   
   L= 4 * rc_lift;    
   P= 2 * rc_pitch - 3 * pitch_g_sens + 400 * ny; 
   R= 2 *  rc_roll - 3 *  roll_g_sens + 400 * nx;
   Y= 3 *   rc_yaw - 4 *   yaw_g_sens;
    
   m1= (L -P -R -Y) / 40;
   m2= (L +P -R +Y) / 40;
   m3= (L +P +R -Y) / 40;
   m4= (L -P +R +Y) / 40;

  }

/* ------------------------------------------------------------------------ */
/* drive the motor 1 2 3 4                                                  */
/* ------------------------------------------------------------------------ */
void run_mot()
 {
   if (rc_s2 == 0)
    {
      m1= constrain(140 + m1, 140, 235);
      m2= constrain(140 + m2, 140, 235);
      m3= constrain(140 + m3, 140, 235);
      m4= constrain(140 + m4, 140, 235);

      analogWrite(11,m1);
      analogWrite(3, m2);
      analogWrite(9, m3);
      analogWrite(10,m4);
    }
   else
    {
      analogWrite(11,120);
      analogWrite(3, 120);
      analogWrite(9, 120);
      analogWrite(10,120);     
    } 
 }

/* ------------------------------------------------------------------------ */
/* global loop procedure                                                    */
/* ------------------------------------------------------------------------ */
void loop()
 { 
   while (Serial.available() == 0) 
    {
      read_imu();
      calc_imu();
      calc_mot();
      run_mot();
    }
   read_rc();
 }
 
/* ------------------------------------------------------------------------ */
/* end of program                                                           */
/* ------------------------------------------------------------------------ */