
float rp_rctrl_Fa[] = RP_RCTRL_FA;
float rp_rctrl_Ga[] = RP_RCTRL_GA;
float rp_rctrl_Ha[] = RP_RCTRL_HA;
float rp_rctrl_Aod[] = RP_RCTRL_AOD;
float rp_rctrl_Bod[] = RP_RCTRL_BOD;
float rp_rctrl_Cod[] = RP_RCTRL_COD;
float egx_integ = 0;	// gx̒Ǐ]΍̐ϕl
float egy_integ = 0;	// gy̒Ǐ]΍̐ϕl
float dmxe	= 0;	// MX̐l
float dmye	= 0;	// MY̐l

float u1_F, u2_F;
float u1_G, u2_G;
float u1_H, u2_H;
float mx;			// [gMXl
float my;			// [gMYl
float egx;		// gx̒Ǐ]΍
float egy;		// gy̒Ǐ]΍
float x_s2_l, y_s2_l;	// x_s2, y_s2̒liԐpj
float dmxe_next;	// dmxe1TvXV̒l
float dmye_next;	// dmye1TvXV̒l



205	/*
206	  //X Axis
207	  error = pid->x_s1 - gyro_rad->gx;


235	if(pid->y_s2 > MAX_ADJ_AMOUNT)  pid->y_s2 = MAX_ADJ_AMOUNT;
236	  if(pid->y_s2 < -MAX_ADJ_AMOUNT)  pid->y_s2 = -MAX_ADJ_AMOUNT;
237	*/


  if(gTHR<MIN_THR)
  {
    egx_integ = 0; 
    egy_integ = 0;
    dmxe = 0;
    dmye = 0;
  }

  //XY Axis
  mx = rp_rctrl_Cod[0] * dmxe + rp_rctrl_Cod[1] * dmye;
  my = rp_rctrl_Cod[2] * dmxe + rp_rctrl_Cod[3] * dmye;
  u1_F = rp_rctrl_Fa[0] * mx + rp_rctrl_Fa[1] * my +
 rp_rctrl_Fa[2] * gyro_rad->gx + rp_rctrl_Fa[3] * gyro_rad->gy;
  u2_F = rp_rctrl_Fa[4] * mx + rp_rctrl_Fa[5] * my +
rp_rctrl_Fa[6] * gyro_rad->gx + rp_rctrl_Fa[7] * gyro_rad->gy;
  egx = pid->x_s1 - gyro_rad->gx;
  egy = pid->y_s1 - gyro_rad->gy;
  egx_integ += egx * pid->ts;
  if(egx_integ > EGX_I_LIMIT)
    egx_integ = EGX_I_LIMIT;
  else if(egx_integ < -EGX_I_LIMIT)
    egx_integ = -EGX_I_LIMIT;
  egy_integ += egy * pid->ts;
  if(egy_integ > EGY_I_LIMIT)
    egy_integ = EGY_I_LIMIT;
  else if(egy_integ < -EGY_I_LIMIT)
    egy_integ = -EGY_I_LIMIT;
  u1_G = rp_rctrl_Ga[0] * egx_integ + rp_rctrl_Ga[1] * egy_integ;
  u2_G = rp_rctrl_Ga[2] * egx_integ + rp_rctrl_Ga[3] * egy_integ;
  u1_H = rp_rctrl_Ha[0] * pid->x_s1 + rp_rctrl_Ha[1] * pid->y_s1;
  u2_H = rp_rctrl_Ha[2] * pid->x_s1 + rp_rctrl_Ha[3] * pid->y_s1;
  pid->x_s2 = u1_F + u1_G + u1_H;
  pid->y_s2 = u2_F + u2_G + u2_H;

  if(pid->x_s2 > MAX_ADJ_AMOUNT)  pid->x_s2 = MAX_ADJ_AMOUNT;
  if(pid->x_s2 < -MAX_ADJ_AMOUNT)  pid->x_s2 = -MAX_ADJ_AMOUNT;

  if(pid->y_s2 > MAX_ADJ_AMOUNT)  pid->y_s2 = MAX_ADJ_AMOUNT;
  if(pid->y_s2 < -MAX_ADJ_AMOUNT)  pid->y_s2 = -MAX_ADJ_AMOUNT;

  x_s2_l = pid->x_s2;
  if (x_s2_l > X_S2_LIMIT_O)
    x_s2_l = X_S2_LIMIT_O;
  else if (x_s2_l < -X_S2_LIMIT_O)
    x_s2_l = -X_S2_LIMIT_O;
  y_s2_l = pid->y_s2;
  if (y_s2_l > Y_S2_LIMIT_O)
    y_s2_l = Y_S2_LIMIT_O;
  else if (y_s2_l < -Y_S2_LIMIT_O)
    y_s2_l = -Y_S2_LIMIT_O;
  dmxe_next = rp_rctrl_Aod[0] * dmxe + rp_rctrl_Aod[1] * dmye +
    rp_rctrl_Bod[0] * x_s2_l + rp_rctrl_Bod[1] * y_s2_l;
  dmye_next = rp_rctrl_Aod[2] * dmxe + rp_rctrl_Aod[3] * dmye +
    rp_rctrl_Bod[2] * x_s2_l + rp_rctrl_Bod[3] * y_s2_l;
  dmxe = dmxe_next;
  dmye = dmye_next;




list2
