/*
   
*/

// ---------------------------------------------------------------------------
#include <Wire.h>
// ------------------- Define some constants for convenience -----------------
#define CHANNEL1 0
#define CHANNEL2 1
#define CHANNEL3 2
#define CHANNEL4 3

#define YAW      0
#define PITCH    1
#define ROLL     2
#define THROTTLE 3

#define X           0     
#define Y           1     
#define Z           2     
#define MPU_ADDRESS 0x68  
#define FREQ        250   
#define SSF_GYRO    65.5  

#define STOPPED  0
#define STARTING 1
#define STARTED  2

#define servoMin  450  
#define servoMax 2500  
#define servoMid 1025 
#define pidRange  400  


volatile byte previous_state[4];


volatile unsigned int pulse_length[4] = {servoMid, servoMid, servoMid, servoMid};


volatile unsigned long current_time;
volatile unsigned long timer[4]; // Timer of each channel

// Used to configure which control (yaw, pitch, roll, throttle) is on which channel
int mode_mapping[4];
// ----------------------- MPU variables -------------------------------------
// The RAW values got from gyro (in °/sec) in that order: X, Y, Z
int gyro_raw[3] = {0, 0, 0};

// Average gyro offsets of each axis in that order: X, Y, Z
long gyro_offset[3] = {0, 0, 0};

// Calculated angles from gyro's values in that order: X, Y, Z
float gyro_angle[3]  = {0, 0, 0};

// The RAW values got from accelerometer (in m/sec²) in that order: X, Y, Z
int acc_raw[3] = {0, 0, 0};

// Calculated angles from accelerometer's values in that order: X, Y, Z
float acc_angle[3] = {0, 0, 0};

// Total 3D acceleration vector in m/s²
long acc_total_vector;

// Calculated angular motion on each axis: Yaw, Pitch, Roll
float angular_motions[3] = {0, 0, 0};

/**
   
*/
float measures[3] = {0, 0, 0};

// MPU's temperature
int temperature;

// Init flag set to TRUE after first loop
boolean initialized;
// debbuging 
boolean DEBUG_pulse = true;
boolean DEBUG_measures = true;
boolean DEBUG_angular_motions = true;
boolean DEBUG_RC = false; 

// ----------------------- Variables for servo signal generation -------------
unsigned int  period; // Sampling period
unsigned long loop_timer;
unsigned long now, difference;

unsigned long pulse_length_esc1 = servoMin,
              pulse_length_esc2 = servoMin,
              pulse_length_esc3 = servoMin,
              pulse_length_esc4 = servoMin;

// ------------- Global variables used for PID controller --------------------
float pid_set_points[3] = {0, 0, 0}; // Yaw, Pitch, Roll
// Errors
float errors[3];                     // Measured errors (compared to instructions) : [Yaw, Pitch, Roll]
float delta_err[3]      = {0, 0, 0}; // Error deltas in that order   : Yaw, Pitch, Roll
float error_sum[3]      = {0, 0, 0}; // Error sums (used for integral component) : [Yaw, Pitch, Roll]
float previous_error[3] = {0, 0, 0}; // Last errors (used for derivative component) : [Yaw, Pitch, Roll]
// PID coefficients
float Kp[3] = {4.0, 1.3, 1.3};    // P coefficients in that order : Yaw, Pitch, Roll
float Ki[3] = {0.02, 0.04, 0.04}; // I coefficients in that order : Yaw, Pitch, Roll
float Kd[3] = {0, 18, 18};        // D coefficients in that order : Yaw, Pitch, Roll
// ---------------------------------------------------------------------------
/**
   Status of the quadcopter:
     - 0 : stopped
     - 1 : starting
     - 2 : started

   @var int
*/
int status = STOPPED;
// ---------------------------------------------------------------------------
int battery_voltage;
// ---------------------------------------------------------------------------

/**
   Setup configuration
*/
void setup() {
  // Start I2C communication
  Wire.begin();
  TWBR = 12; // Set the I2C clock speed to 400kHz.

  // DEBUG
  if (DEBUG_pulse || DEBUG_measures || DEBUG_angular_motions || DEBUG_RC) {
    Serial.begin(57600);
  }
  // Turn LED on during setup
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);

  // Set pins #4 #5 #6 #7 as outputs
  DDRD |= B11110000;

  setupMpu6050Registers();

  calibrateMpu6050();

  configureChannelMapping();
  //Serial.begin(9600);

  
     // Configure interrupts for receiver
     PCICR  |= (1 << PCIE0);  // Set PCIE0 to enable PCMSK0 scan
     PCMSK0 |= (1 << PCINT0); // Set PCINT0 (digital input 8) to trigger an interrupt on state change
     PCMSK0 |= (1 << PCINT1); // Set PCINT1 (digital input 9) to trigger an interrupt on state change
     PCMSK0 |= (1 << PCINT2); // Set PCINT2 (digital input 10)to trigger an interrupt on state change
     PCMSK0 |= (1 << PCINT3); // Set PCINT3 (digital input 11)to trigger an interrupt on state change
  
  period = (1000000 / FREQ) ; // Sampling period in µs

  // Initialize loop_timer
  loop_timer = micros();

  // Turn LED off now setup is done
  digitalWrite(13, LOW);
  stopAll();
}

/**
   Main program loop
*/
void loop() {
  // 1. First, read raw values from MPU-6050

  readSensor();

  // 2. Calculate angles from gyro & accelerometer's values
  calculateAngles();
  readAnalogue();
  // 3. Calculate set points of PID controller
  calculateSetPoints();

  // 4. Calculate errors comparing angular motions to set points
  calculateErrors();

  //if (isStarted()) {
  // 5. Calculate motors speed with PID controller
  pidController();

  //compensateBatteryDrop();
  //  }


  // 6. Apply motors speed

  //Serial.println(pulse_length[CHANNEL3]);
  if (pulse_length[CHANNEL3] <  (servoMin+18)) {
    stopAll();
  } else {
    pidController();
  }
  applyMotorSpeed();

  if (DEBUG_pulse) {
    Serial.print(pulse_length_esc1);
    Serial.print(",");
    Serial.print(pulse_length_esc2);
    Serial.print(",");
    Serial.print(pulse_length_esc3);
    Serial.print(",");
    Serial.print(pulse_length_esc4);
    Serial.print(","); 
  }

  if (DEBUG_RC) {
    Serial.print(pulse_length[CHANNEL1]);
    Serial.print(",");
    Serial.print(pulse_length[CHANNEL2]);
    Serial.print(",");
    Serial.print(pulse_length[CHANNEL3]);
    Serial.print(",");
    Serial.print(pulse_length[CHANNEL4]);
    Serial.print(","); 
  }
  
  if (DEBUG_measures) {
    Serial.print(measures[ROLL]);
    Serial.print(",");
    Serial.print(measures[PITCH]);
    Serial.print(",");
    Serial.print(measures[YAW]);
    Serial.print(",");
  }

  if (DEBUG_angular_motions) {
    Serial.print(angular_motions[ROLL]);
    Serial.print(",");
    Serial.print(angular_motions[PITCH]);
    Serial.print(",");
    Serial.print(angular_motions[YAW]);
    Serial.print(",");
  }
  if (DEBUG_pulse || DEBUG_measures || DEBUG_angular_motions || DEBUG_RC) {
      Serial.print("\n");
  }

}


   Generate servo-signal on d
void applyMotorSpeed() {
  // Refresh rate is 250Hz: send ESC pulses every 4000µs
  while ((now = micros()) - loop_timer < period);

  // Update loop timer
  loop_timer = now;

  // Set pins #4 #5 #6 #7 HIGH
  PORTD |= B11110000;

  // Wait until all pins #4 #5 #6 #7 are LOW
  while (PORTD >= 16) {
    now        = micros();
    difference = now - loop_timer;

    if (difference >= pulse_length_esc1) PORTD &= B11101111; // Set pin #4 LOW
    if (difference >= pulse_length_esc2) PORTD &= B11011111; // Set pin #5 LOW
    if (difference >= pulse_length_esc3) PORTD &= B10111111; // Set pin #6 LOW
    if (difference >= pulse_length_esc4) PORTD &= B01111111; // Set pin #7 LOW
  }
}

/**
   Request raw values from MPU6050.
*/
void readSensor() {
  Wire.beginTransmission(MPU_ADDRESS); // Start communicating with the MPU-6050
  Wire.write(0x3B);                    // Send the requested starting register
  Wire.endTransmission();              // End the transmission
  Wire.requestFrom(MPU_ADDRESS, 14);   // Request 14 bytes from the MPU-6050

  // Wait until all the bytes are received
  while (Wire.available() < 14);

  acc_raw[X]  = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[X] variable
  acc_raw[Y]  = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[Y] variable
  acc_raw[Z]  = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the acc_raw[Z] variable
  temperature = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the temperature variable
  gyro_raw[X] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[X] variable
  gyro_raw[Y] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[Y] variable
  gyro_raw[Z] = Wire.read() << 8 | Wire.read(); // Add the low and high byte to the gyro_raw[Z] variable
}

/**
   Calculate real angles from gyro and accelerometer's values
*/
void calculateAngles() {
  calculateGyroAngles();
  calculateAccelerometerAngles();

  if (initialized) {
    // Correct the drift of the gyro with the accelerometer
    gyro_angle[X] = gyro_angle[X] * 0.9996 + acc_angle[X] * 0.0004;
    gyro_angle[Y] = gyro_angle[Y] * 0.9996 + acc_angle[Y] * 0.0004;
  } else {
    // At very first start, init gyro angles with accelerometer angles
    resetGyroAngles();

    initialized = true;
  }

  // To dampen the pitch and roll angles a complementary filter is used
  measures[ROLL]  = measures[ROLL]  * 0.9 + gyro_angle[X] * 0.1;
  measures[PITCH] = measures[PITCH] * 0.9 + gyro_angle[Y] * 0.1;
  measures[YAW]   = -gyro_raw[Z] / SSF_GYRO; // Store the angular motion for this axis

  // Apply low-pass filter (10Hz cutoff frequency)
  angular_motions[ROLL]  = 0.7 * angular_motions[ROLL]  + 0.3 * gyro_raw[X] / SSF_GYRO;
  angular_motions[PITCH] = 0.7 * angular_motions[PITCH] + 0.3 * gyro_raw[Y] / SSF_GYRO;
  angular_motions[YAW]   = 0.7 * angular_motions[YAW]   + 0.3 * gyro_raw[Z] / SSF_GYRO;
}

/**
   Calculate pitch & roll angles using only the gyro.
*/
void calculateGyroAngles() {
  // Subtract offsets
  gyro_raw[X] -= gyro_offset[X];
  gyro_raw[Y] -= gyro_offset[Y];
  gyro_raw[Z] -= gyro_offset[Z];

  // Angle calculation using integration
  gyro_angle[X] += (gyro_raw[X] / (FREQ * SSF_GYRO));
  gyro_angle[Y] += (-gyro_raw[Y] / (FREQ * SSF_GYRO)); // Change sign to match the accelerometer's one

  // Transfer roll to pitch if IMU has yawed
  gyro_angle[Y] += gyro_angle[X] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
  gyro_angle[X] -= gyro_angle[Y] * sin(gyro_raw[Z] * (PI / (FREQ * SSF_GYRO * 180)));
}

/**
   Calculate pitch & roll angles using only the accelerometer.
*/
void calculateAccelerometerAngles() {
  // Calculate total 3D acceleration vector : √(X² + Y² + Z²)
  acc_total_vector = sqrt(pow(acc_raw[X], 2) + pow(acc_raw[Y], 2) + pow(acc_raw[Z], 2));

  // To prevent asin to produce a NaN, make sure the input value is within [-1;+1]
  if (abs(acc_raw[X]) < acc_total_vector) {
    acc_angle[X] = asin((float)acc_raw[Y] / acc_total_vector) * (180 / PI); // asin gives angle in radian. Convert to degree multiplying by 180/pi
  }

  if (abs(acc_raw[Y]) < acc_total_vector) {
    acc_angle[Y] = asin((float)acc_raw[X] / acc_total_vector) * (180 / PI);
  }
}

/**
   Calculate motor speed for each motor of an X quadcopter depending on received instructions and measures from sensor
   by applying PID control.

   (A) (B)     x
     \ /     z ↑
      X       \|
     / \       +----→ y
   (C) (D)

   Motors A & D run clockwise.
   Motors B & C run counter-clockwise.

   Each motor output is considered as a servomotor. As a result, value range is about 1000µs to 2000µs
*/
void pidController() {
  float yaw_pid      = 0;
  float pitch_pid    = 0;
  float roll_pid     = 0;
  int   throttle     = pulse_length[mode_mapping[THROTTLE]];

  // Initialize motor commands with throttle
  pulse_length_esc1 = throttle;
  pulse_length_esc2 = throttle;
  pulse_length_esc3 = throttle;
  pulse_length_esc4 = throttle;

  // Do not calculate anything if throttle is 0
  if (throttle >= 450) {
    // PID = e.Kp + ∫e.Ki + Δe.Kd
    yaw_pid   = (errors[YAW]   * Kp[YAW])   + (error_sum[YAW]   * Ki[YAW])   + (delta_err[YAW]   * Kd[YAW]);
    pitch_pid = (errors[PITCH] * Kp[PITCH]) + (error_sum[PITCH] * Ki[PITCH]) + (delta_err[PITCH] * Kd[PITCH]);
    roll_pid  = (errors[ROLL]  * Kp[ROLL])  + (error_sum[ROLL]  * Ki[ROLL])  + (delta_err[ROLL]  * Kd[ROLL]);

    // Keep values within acceptable range. TODO export hard-coded values in variables/const
    yaw_pid   = minMax(yaw_pid, -pidRange, pidRange);
    pitch_pid = minMax(pitch_pid, -pidRange, pidRange);
    roll_pid  = minMax(roll_pid, -pidRange, pidRange);

    // Calculate pulse duration for each ESC
    pulse_length_esc1 = throttle - roll_pid - pitch_pid + yaw_pid;
    pulse_length_esc2 = throttle + roll_pid - pitch_pid - yaw_pid;
    pulse_length_esc3 = throttle - roll_pid + pitch_pid - yaw_pid;
    pulse_length_esc4 = throttle + roll_pid + pitch_pid + yaw_pid;
  }

  // Prevent out-of-range-values
  pulse_length_esc1 = minMax(pulse_length_esc1, servoMin, servoMax);
  pulse_length_esc2 = minMax(pulse_length_esc2, servoMin, servoMax);
  pulse_length_esc3 = minMax(pulse_length_esc3, servoMin, servoMax);
  pulse_length_esc4 = minMax(pulse_length_esc4, servoMin, servoMax);
}

/**
   Calculate errors used by PID controller
*/
void calculateErrors() {
  // Calculate current errors
  errors[YAW]   = angular_motions[YAW]   - pid_set_points[YAW];
  errors[PITCH] = angular_motions[PITCH] - pid_set_points[PITCH];
  errors[ROLL]  = angular_motions[ROLL]  - pid_set_points[ROLL];

  // Calculate sum of errors : Integral coefficients
  error_sum[YAW]   += errors[YAW];
  error_sum[PITCH] += errors[PITCH];
  error_sum[ROLL]  += errors[ROLL];

  // Keep values in acceptable range
  error_sum[YAW]   = minMax(error_sum[YAW],   -pidRange / Ki[YAW],   pidRange / Ki[YAW]);
  error_sum[PITCH] = minMax(error_sum[PITCH], -pidRange / Ki[PITCH], pidRange / Ki[PITCH]);
  error_sum[ROLL]  = minMax(error_sum[ROLL],  -pidRange / Ki[ROLL],  pidRange / Ki[ROLL]);

  // Calculate error delta : Derivative coefficients
  delta_err[YAW]   = errors[YAW]   - previous_error[YAW];
  delta_err[PITCH] = errors[PITCH] - previous_error[PITCH];
  delta_err[ROLL]  = errors[ROLL]  - previous_error[ROLL];

  // Save current error as previous_error for next time
  previous_error[YAW]   = errors[YAW];
  previous_error[PITCH] = errors[PITCH];
  previous_error[ROLL]  = errors[ROLL];
}

/**
  
*/
void configureChannelMapping() {
  mode_mapping[YAW]      = CHANNEL4;
  mode_mapping[PITCH]    = CHANNEL2;
  mode_mapping[ROLL]     = CHANNEL1;
  mode_mapping[THROTTLE] = CHANNEL3;
}

/**
*/
void setupMpu6050Registers() {
  // Configure power management
  Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
  Wire.write(0x6B);                    // Request the PWR_MGMT_1 register
  Wire.write(0x00);                    // Apply the desired configuration to the register
  Wire.endTransmission();              // End the transmission

  // Configure the gyro's sensitivity
  Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
  Wire.write(0x1B);                    // Request the GYRO_CONFIG register
  Wire.write(0x08);                    // Apply the desired configuration to the register : ±500°/s
  Wire.endTransmission();              // End the transmission

  // Configure the acceleromter's sensitivity
  Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
  Wire.write(0x1C);                    // Request the ACCEL_CONFIG register
  Wire.write(0x10);                    // Apply the desired configuration to the register : ±8g
  Wire.endTransmission();              // End the transmission

  // Configure low pass filter
  Wire.beginTransmission(MPU_ADDRESS); // Start communication with MPU
  Wire.write(0x1A);                    // Request the CONFIG register
  Wire.write(0x03);                    // Set Digital Low Pass Filter about ~43Hz
  Wire.endTransmission();              // End the transmission
}

/**
   
void calibrateMpu6050() {
  int max_samples = 10;

  for (int i = 0; i < max_samples; i++) {
    readSensor();

    gyro_offset[X] += gyro_raw[X];
    gyro_offset[Y] += gyro_raw[Y];
    gyro_offset[Z] += gyro_raw[Z];

    // Generate low throttle pulse to init ESC and prevent them beeping
    PORTD |= B11110000;      // Set pins #4 #5 #6 #7 HIGH
    delayMicroseconds(600); // Wait 1000µs
    PORTD &= B00001111;      // Then set LOW

    // Just wait a bit before next loop
    delay(3);
  }

  // Calculate average offsets
  gyro_offset[X] /= max_samples;
  gyro_offset[Y] /= max_samples;
  gyro_offset[Z] /= max_samples;
}

/**
   Make sure that given value is not over min_value/max_value range.

   @param float value     : The value to convert
   @param float min_value : The min value
   @param float max_value : The max value

   @return float
*/
float minMax(float value, float min_value, float max_value) {
  if (value > max_value) {
    value = max_value;
  } else if (value < min_value) {
    value = min_value;
  }

  return value;
}

/**
   
   @return bool
*/
bool isStarted() {

  // When left stick is moved in the bottom left corner
  if (status == STOPPED && pulse_length[mode_mapping[YAW]] <= 462 && pulse_length[mode_mapping[THROTTLE]] <= 462) {
    status = STARTING;
  }

  // When left stick is moved back in the center position
  if (status == STARTING && pulse_length[mode_mapping[YAW]] == 1025 && pulse_length[mode_mapping[THROTTLE]] <= 462) {
    status = STARTED;

    // Reset PID controller's variables to prevent bump start
    resetPidController();

    resetGyroAngles();
  }

  // When left stick is moved in the bottom right corner
  if (status == STARTED && pulse_length[mode_mapping[YAW]] >= 2480 && pulse_length[mode_mapping[THROTTLE]] <= 462) {
    status = STOPPED;
    // Make sure to always stop motors when status is STOPPED
    stopAll();
  }

  resetPidController();
  resetGyroAngles();
  return status == STARTED;
}

/**
   Reset gyro's angles with accelerometer's angles.
*/
void resetGyroAngles() {
  gyro_angle[X] = acc_angle[X];
  gyro_angle[Y] = acc_angle[Y];
}

/**
   Reset motors' pulse length to 1000µs to totally stop them.
*/
void stopAll() {
  pulse_length_esc1 = servoMin;
  pulse_length_esc2 = servoMin;
  pulse_length_esc3 = servoMin;
  pulse_length_esc4 = servoMin;
}

/**
   Reset all PID controller's variables.
*/
void resetPidController() {
  errors[YAW]   = 0;
  errors[PITCH] = 0;
  errors[ROLL]  = 0;

  error_sum[YAW]   = 0;
  error_sum[PITCH] = 0;
  error_sum[ROLL]  = 0;

  previous_error[YAW]   = 0;
  previous_error[PITCH] = 0;
  previous_error[ROLL]  = 0;
}

/**
   Calculate PID set points on axis YAW, PITCH, ROLL
*/
void calculateSetPoints() {
  pid_set_points[YAW]   = calculateYawSetPoint(pulse_length[mode_mapping[YAW]], pulse_length[mode_mapping[THROTTLE]]);
  pid_set_points[PITCH] = calculateSetPoint(measures[PITCH], pulse_length[mode_mapping[PITCH]]);
  pid_set_points[ROLL]  = calculateSetPoint(measures[ROLL], pulse_length[mode_mapping[ROLL]]);
}

/**
   
   @return float
*/
float calculateSetPoint(float angle, int channel_pulse) {
  float level_adjust = angle * 15; // Value 15 limits maximum angle value to ±32.8°
  float set_point    = 0;

  // Need a dead band of 16µs for better result
  if (channel_pulse > servoMid + 8 ) {
    set_point = channel_pulse - servoMid + 8;
  } else if (channel_pulse <  servoMid - 8) {
    set_point = channel_pulse - servoMid -8;
  }

  set_point -= level_adjust;
  set_point /= 3;

  return set_point;
}

/**
   Calculate the PID set point of YAW axis in °/s

   @param int yaw_pulse      Receiver pulse length of yaw's channel
   @param int throttle_pulse Receiver pulse length of throttle's channel
   @return float
*/
float calculateYawSetPoint(int yaw_pulse, int throttle_pulse) {
  float set_point = 0;

  // Do not yaw when turning off the motors
  if (throttle_pulse > servoMin) {
    // There is no notion of angle on this axis as the quadcopter can turn on itself
    set_point = calculateSetPoint(0, yaw_pulse);
  }

  return set_point;
}

/**
   Compensate battery drop applying a coefficient on output values
*/
void compensateBatteryDrop() {
  if (isBatteryConnected()) {
    pulse_length_esc1 += pulse_length_esc1 * ((1240 - battery_voltage) / (float) 3500);
    pulse_length_esc2 += pulse_length_esc2 * ((1240 - battery_voltage) / (float) 3500);
    pulse_length_esc3 += pulse_length_esc3 * ((1240 - battery_voltage) / (float) 3500);
    pulse_length_esc4 += pulse_length_esc4 * ((1240 - battery_voltage) / (float) 3500);
  }
}

/**
   Read battery voltage & return whether the battery seems connected

   @return boolean
*/
bool isBatteryConnected() {
  // Reduce noise with a low-pass filter (10Hz cutoff frequency)
  battery_voltage = battery_voltage * 0.92 + (analogRead(0) + 65) * 0.09853;

  return battery_voltage < 1240 && battery_voltage > 800;
}

/**
*/

  ISR(PCINT0_vect) {
       current_time = micros();

       // Channel 1 -------------------------------------------------
       if (PINB & B00000001) {                                        
           if (previous_state[CHANNEL1] == LOW) {                     
               previous_state[CHANNEL1] = HIGH;                      
               timer[CHANNEL1] = current_time;                        
           }
       } else if (previous_state[CHANNEL1] == HIGH) {                
           previous_state[CHANNEL1] = LOW;                            
           pulse_length[CHANNEL1] = current_time - timer[CHANNEL1];   
       }

       // Channel 2 -------------------------------------------------
       if (PINB & B00000010) {                                        
           if (previous_state[CHANNEL2] == LOW) {                     
               previous_state[CHANNEL2] = HIGH;                       
               timer[CHANNEL2] = current_time;                        
       } else if (previous_state[CHANNEL2] == HIGH) {                 
           previous_state[CHANNEL2] = LOW;                            
           pulse_length[CHANNEL2] = current_time - timer[CHANNEL2];  

       // Channel 3 -------------------------------------------------
       if (PINB & B00000100) {                                        // Is input 10 high ?
           if (previous_state[CHANNEL3] == LOW) {                     // Input 10 changed from 0 to 1 (rising edge)
               previous_state[CHANNEL3] = HIGH;                       // Save current state
               timer[CHANNEL3] = current_time;                        // Save current time
           }
       } else if (previous_state[CHANNEL3] == HIGH) {                 // Input 10 changed from 1 to 0 (falling edge)
           previous_state[CHANNEL3] = LOW;                            // Save current state
           pulse_length[CHANNEL3] = current_time - timer[CHANNEL3];   // Calculate pulse duration & save it
       }

       // Channel 4 -------------------------------------------------
       if (PINB & B00001000) {                                        // Is input 11 high ?
           if (previous_state[CHANNEL4] == LOW) {                     // Input 11 changed from 0 to 1 (rising edge)
               previous_state[CHANNEL4] = HIGH;                       // Save current state
               timer[CHANNEL4] = current_time;                        // Save current time
           }
       } else if (previous_state[CHANNEL4] == HIGH) {                 // Input 11 changed from 1 to 0 (falling edge)
           previous_state[CHANNEL4] = LOW;                            // Save current state
           pulse_length[CHANNEL4] = current_time - timer[CHANNEL4];   // Calculate pulse duration & save it
       }
  }

void readAnalogue() {
  pulse_length[CHANNEL1] = servoMid; //map(analogRead(A0),0,1023,450,2500); // ROLL
  pulse_length[CHANNEL2] = servoMid; //map(analogRead(A1),0,1023,450,2500); // PITCH
  pulse_length[CHANNEL3] = map(analogRead(A3), 0, 1023, 450, 2500); // THROTTLE
  pulse_length[CHANNEL4] = servoMid; //map(analogRead(A2),0,1023,450,2500); // YAW

}