FC_EASY

#include <Wire.h>
#include <Servo.h>
#include "RadioController.h"

RadioController rc;

// Bardzo dziwne zachowanie PITCH - wysoki wzrost

#define GSCALE ((70.f / 1000.f))
#define ASCALE ((0.244f / 1000.f))

#define ACC_SCALE(x) ((float)x * ASCALE)
#define GYRO_SCALE(x) ((float)x * GSCALE)

unsigned long integration = 0, sensor = 0, update_start = 0;

long gx, gy, gz, ax, ay, az;

float gxc, gyc, gzc;

double pitch, roll, yaw,
       angle_pitch, angle_roll, angle_yaw;

bool cal = false;

const float P_ROLL = 1.3f, I_ROLL = 0.03f, D_ROLL = 15.f;
float pid_roll, last_pid_roll_error, i_pid_roll;

const float P_PITCH = P_ROLL, I_PITCH = I_ROLL, D_PITCH = D_ROLL;
float pid_pitch, last_pid_pitch_error, i_pid_pitch;

const float P_YAW = 4.f, I_YAW = 0.02f, D_YAW = 0.f;
float pid_yaw, last_pid_yaw_error, i_pid_yaw;

const short PID_MAX = 400,  // +-
            ANGLE_CORRECTION_SENS = 15;


float cal_pitch = 0.0f, cal_roll = 0.0f;

Servo NCCW, NCW, SCW, SCCW;

void setup()
{
  Serial.begin(9600);
  while(Serial.read() != 'g');

  NCCW.attach(2);
  NCCW.writeMicroseconds(1000);

  NCW.attach(3);
  NCW.writeMicroseconds(1000);

  // SCW.attach(4);
  // SCW.writeMicroseconds(1000);

  SCCW.attach(5);
  SCCW.writeMicroseconds(1000);


  rc.Setup();

  Wire.begin();
  Wire.setClock(400000L);

  WriteReg(0x10, 0b01101100); // 208Hz refresh rate, +-8g anti-aliasing 200Hz
  WriteReg(0x11, 0b01101100); // 208Hz refresh rate, +- 2k dps, full-scale at 125dps off
  WriteReg(0x12, 0b00000100);

  Serial.println("Calibration: ");
  delay(4);

  for(int i = 0; i < 1000; i++)
  {
    Read();

    if(i % 50 == 0) Serial.print(".");
    gxc += gx;
    gyc += gy;
    gzc += gz;

    delay(4);
  }

  gxc /= 1000;
  gyc /= 1000;
  gzc /= 1000;

  cal = true;
  Serial.println("Done!");


  // while(rc.

  integration = update_start = millis();
}

ISR(PCINT0_vect)
{
  rc.HandleInterrupt();
}


void WriteReg(uint8_t reg, uint8_t val)
{
  Wire.beginTransmission(0b1101011);
  Wire.write(reg);
  Wire.write(val);
  Wire.endTransmission();
}
bool Read()
{
  unsigned long current_timer = millis();

  if(current_timer - sensor < 4)
    return false;

  sensor = current_timer;

  Wire.beginTransmission(0b1101011);
  Wire.write(0x28); // OUTX_L_XL
  Wire.endTransmission();
  Wire.requestFrom(0b1101011, (uint8_t) 6);

  uint8_t axl = Wire.read(), axh = Wire.read(),
          ayl = Wire.read(), ayh = Wire.read(),
          azl = Wire.read(), azh = Wire.read();

  ax = -(long)(axh << 8 | axl); // mg
  ay = -(long)(ayh << 8 | ayl); // mg
  az = -(long)(azh << 8 | azl); // mg

  Wire.beginTransmission(0b1101011);
  Wire.write(0x22); // OUTX_L_G
  Wire.endTransmission();
  Wire.requestFrom(0b1101011, (uint8_t) 6);

  uint8_t gxl = Wire.read(), gxh = Wire.read(),
          gyl = Wire.read(), gyh = Wire.read(),
          gzl = Wire.read(), gzh = Wire.read();

  gx = (long)(gxh << 8 | gxl);
  gy = (long)(gyh << 8 | gyl);
  gz = (long)(gzh << 8 | gzl);

  if(cal)
  {
    gx -= gxc;
    gy -= gyc;
    gz -= gzc;
  }

  return true;
}

bool startup = false;

void loop()
{

  rc.Normalize();

  // rc.DEBUG_PrintAngles();
  //if(!rc.CheckConnection())
 // {
  //  Serial.print("RC ERROR");
  //  rc.DEBUG_PrintValues();
 //   return;
 // }
  // while(abs(rc.GetChannel(1)) > 500) { Serial.println("NO RADIO SIGNAL"); }

 // unsigned long start = micros();
  Read();

  pitch = pitch * 0.75 + (gy * GSCALE) * 0.25;
  roll = roll * 0.75 + (gx * GSCALE) * 0.25;
  yaw = yaw * 0.75 + (gz * GSCALE) * 0.25;

  float dt = (millis() - update_start) / 1000.f;

  angle_pitch += gy * GSCALE * dt;
  angle_roll += gx * GSCALE * dt;
  float sin_yaw = sin(gz * GSCALE * dt * DEG_TO_RAD);

  update_start = millis();

  angle_pitch -= angle_roll * sin_yaw;
  angle_roll += angle_pitch * sin_yaw;

  float acc_vector = sqrt(ax * ax + ay * ay + az * az),
        a_pitch = 0.0, a_roll = 0.0;

  if(abs(ay) < acc_vector)
    a_pitch = asin((float)ay / acc_vector) * RAD_TO_DEG;

  if(abs(ax) < acc_vector)
    a_roll = asin((float)ax / acc_vector) * -RAD_TO_DEG;

  a_pitch -= -4.10;
  a_roll -=  -2.30;

  angle_pitch = angle_pitch * 0.9996 + 0.0004 * a_pitch;
  angle_roll = angle_roll * 0.9996 + 0.0004 * a_roll;

  if(!startup)
  {
    angle_pitch = a_pitch;
    angle_roll = a_roll;
    startup = true;

    i_pid_roll = 0;
    last_pid_roll_error = 0;
    i_pid_pitch = 0;
    last_pid_pitch_error = 0;
    i_pid_yaw = 0;
    last_pid_yaw_error = 0;
  }


  calculate_pid();

  // rc.DEBUG_PrintValues();
  // Serial.println(micros() - start);
  Serial.print(angle_pitch);// Serial.print(" "); Serial.print(pitch); Serial.print(" "); Serial.println(a_pitch);
  Serial.print(" "); Serial.println(angle_roll);// Serial.print(" "); Serial.print(roll); Serial.print(" "); Serial.println(a_roll);
  // Serial.print(ax); Serial.print(" "); Serial.print(ay); Serial.print(" "); Serial.println(az);
  //Serial.print(angle_pitch); Serial.print(" "); Serial.println(angle_roll);
  // delay(100);


  // CommandMotors
  long throttle = (rc.GetChannel(3) > 1800) ? 1800 : rc.GetChannel(3),
       esc_nccw = throttle - pid_pitch + pid_roll - pid_yaw,
       esc_ncw = throttle + pid_pitch + pid_roll + pid_yaw,
       esc_sccw = throttle + pid_pitch - pid_roll - pid_yaw,
       esc_scw = throttle - pid_pitch - pid_roll + pid_yaw;

  if(esc_nccw < 1000) esc_nccw = 1000;
  else if(esc_nccw > 2000) esc_nccw = 2000;

  if(esc_ncw < 1000) esc_ncw = 1000;
  else if(esc_ncw > 2000) esc_ncw = 2000;

  if(esc_sccw < 1000) esc_sccw = 1000;
  else if(esc_sccw > 2000) esc_sccw = 2000;

  if(esc_scw < 1000) esc_scw = 1000;
  else if(esc_scw > 2000) esc_scw = 2000;

  Serial.print("THR: "); Serial.print(throttle); Serial.print(" PIT: "); Serial.print(pid_pitch); Serial.print(" ROL: "); Serial.print(pid_roll); Serial.print(" YAW: "); Serial.println(pid_yaw);
  // Serial.print("ESC_NCCW: "); Serial.print(esc_nccw); Serial.print(" ESC_NCW: "); Serial.print(esc_ncw); Serial.print(" ESC_SCW: "); Serial.print(esc_scw); Serial.print(" ESC_SCCW: "); Serial.println(esc_sccw);

  NCCW.writeMicroseconds(esc_nccw);
  NCW.writeMicroseconds(esc_ncw);
  SCCW.writeMicroseconds(esc_sccw);
  // SCW.writeMicroseconds(esc_scw);

  while(millis() - integration < 4);
  integration = millis();
}

void calculate_pid()
{
  float error_temp = roll - (rc.GetChannelDeg(1) - angle_roll * ANGLE_CORRECTION_SENS);

 // Serial.print("ROLL: "); Serial.print(rc.GetChannelDeg(1)); Serial.print(angle_roll * ANGLE_CORRECTION_SENS); Serial.print( " " ); Serial.println(roll);
  i_pid_roll += I_ROLL * error_temp;

  if(i_pid_roll > PID_MAX) i_pid_roll = PID_MAX;
  else if(i_pid_roll < -PID_MAX) i_pid_roll = -PID_MAX;

  pid_roll = P_ROLL * error_temp + i_pid_roll + D_ROLL * (error_temp - last_pid_roll_error);
  if(pid_roll > PID_MAX) pid_roll = PID_MAX;
  else if(pid_roll < -PID_MAX) pid_roll = -PID_MAX;

  last_pid_roll_error = error_temp;

  error_temp = pitch - (rc.GetChannelDeg(2) - angle_pitch * ANGLE_CORRECTION_SENS);
 //  Serial.print("PITCH: "); Serial.print(rc.GetChannelDeg(2)); Serial.print(angle_pitch * ANGLE_CORRECTION_SENS); Serial.print( " " ); Serial.println(pitch);
  i_pid_pitch += I_PITCH * error_temp;

  if(i_pid_pitch > PID_MAX) i_pid_pitch = PID_MAX;
  else if(i_pid_pitch < -PID_MAX) i_pid_pitch = -PID_MAX;

  pid_pitch = P_PITCH * error_temp + i_pid_pitch + D_PITCH * (error_temp - last_pid_pitch_error);
  if(pid_pitch > PID_MAX) pid_pitch = PID_MAX;
  else if(pid_pitch < -PID_MAX) pid_pitch = -PID_MAX;

  last_pid_pitch_error = error_temp;


  error_temp = yaw - rc.GetChannelDeg(4);
  i_pid_yaw += I_YAW * error_temp;

  if(i_pid_yaw > PID_MAX) i_pid_yaw = PID_MAX;
  else if(i_pid_yaw < -PID_MAX) i_pid_yaw = -PID_MAX;

  pid_yaw = P_YAW * error_temp + i_pid_yaw + D_YAW * (error_temp - last_pid_yaw_error);
  if(pid_yaw > PID_MAX) pid_yaw = PID_MAX;
  else if(pid_yaw < -PID_MAX) pid_yaw = -PID_MAX;

  last_pid_yaw_error = error_temp;
}