~lenzgr/+junk/QuaduIMU

/* *********************************************************************** */
/*                    QuaduIMU Quadcopter                                  */
/*                                                                         */
/* Code based on ArduIMU DCM code from Diydrones.com  (r20)                */
/* and Jose Julio's JJ ArduIMU Quadcopter (Ver. 1.15)                      */
/* Author:  Lenz Grimmer                                                   */
#define SOFTWARE_VER "0.4"
/* This version supports the HMC5843 Triple Axis Magnetometer (Optional)   */
/* Merged and adapted from Jose's v1.27 (mini) code                        */
/* Merged Motor and RX control from Paul René Jørgensen's Quaduino Code    */
/* Merged ADC improvements and loop control from Loris Rion's EasyIMUPilot */
/* *********************************************************************** */

#include <inttypes.h>
#include <math.h>
#include <Wire.h>   // For magnetometer readings
#include <ServoDecode.h>
#include <ServoTimer2.h> 

/* ***************************************************************************** */
/*  CONFIGURATION PART                                                           */
/* ***************************************************************************** */

#define HAVE_MAGNETOMETER 1  // 0 : No magnetometer    1: Magnetometer

// QuadCopter Attitude control PID GAINS
#define KP_QUAD_ROLL 2.0
#define KD_QUAD_ROLL 0.4
#define KI_QUAD_ROLL 0.5 
#define KP_QUAD_PITCH 2.0 //1.75 //1.6 // 2.2   //1.75
#define KD_QUAD_PITCH 0.4 //0.4 //0.42 // 0.54  //0.45
#define KI_QUAD_PITCH 0.5 //0.42 // 0.4  //0.5
#define KP_QUAD_YAW 3.5 // 4.6  //3.2 //2.6
#define KD_QUAD_YAW 1.0 // 0.7  //0.8 //0.4
#define KI_QUAD_YAW 0.15 // 0.2  //0.15

#define KD_QUAD_COMMAND_PART 13.0   // for special KD implementation (in two parts). Higher values makes the quadcopter more responsive to user inputs

// Maximum Roll and Pitch angles (in °)
#define MAX_ROLL 45
#define MAX_PITCH 45
// Maximum absolute yaw speed (in °/s)
#define MAX_YAW_SPEED 8

// The IMU should be correctly adjusted : Gyro Gains and also initial IMU offsets:
// We have to take this values with the IMU flat (0º roll, 0ºpitch)
#define acc_offset_x 508 
#define acc_offset_y 504
#define acc_offset_z 501       // We need to rotate the IMU exactly 90º to take this value  
#define gyro_offset_roll 365
#define gyro_offset_pitch 370
#define gyro_offset_yaw 380

// RX pulse range (in ms)
#define MIN_CHANN 1160      // RX Throttle pulse width at minimum...
#define MAX_CHANN 1830
#define CHANN_CENTER 1500
#define MINCHECK MIN_CHANN+40
#define MAXCHECK MAX_CHANN-40

//Loop speed - AHRS_LOOP must be a multiple of all other loop rates
#define AHRS_LOOP 100           // IMU main loop (in Hz)
#define MAGNETOMETER_LOOP 10    // IMU magnetometer loop (in Hz)
#define TELEMETRY_LOOP 20       // Telemetry loop (in Hz)
#define RADIO_LOOP 50           // Radio RX loop (in Hz)
#define CONTROL_LOOP 100        // Control (PID + servos update) loop (in Hz)

#define ARM_DELAY 2 // Motor arming delay (in seconds)

/* *************************************************** */
/*           END OF CONFIGURATION PART                 */
/* *************************************************** */

// ADC : Voltage reference 3.3v / 10bits(1024 steps) => 3.22mV/ADC step
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 3.22mV/ADC step => 330/3.22 = 102.48
// Tested value : 101
#define GRAVITY 101 //this equivalent to 1G in the raw data coming from the accelerometer 
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square

#define ToRad(x) (x*0.01745329252)  // *pi/180
#define ToDeg(x) (x*57.2957795131)  // *180/pi

// LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, 3.22mV/ADC step => 1.03
// Tested values : 0.96,0.96,0.94
#define Gyro_Gain_X 0.92 //X axis Gyro gain
#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second

#define Kp_ROLLPITCH 0.0125 //0.008  //0.0125 //0.010 // Pitch&Roll Proportional Gain
#define Ki_ROLLPITCH 0.000010 // Pitch&Roll Integrator Gain
#define Kp_YAW 1.0 // Yaw Porportional Gain  
#define Ki_YAW 0.00005 // Yaw Integrator Gain

//Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
int SENSOR_SIGN[]={ 1, -1, -1, 1, -1, 1, -1, -1, -1};
// Sensor pin order (for ArduIMU v2 flat)
uint8_t sensors[6] = {6, 7, 3, 0, 1, 2};

volatile unsigned int AN_raw[8]; //store the ADC raw data in 12 bits
unsigned int AN_OFFSET[6]; //Array that stores the Offset
float AN[6]; //array that store the 6 ADC filtered data

float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
float Accel_Vector_unfiltered[3]= {0,0,0}; //Store the acceleration in a vector
float Accel_magnitude;
float Accel_weight;
float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn 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 roll=0;
float pitch=0;
float yaw=0;

//Magnetometer variables
boolean magnetom_enabled=true;
int magnetom_x;
int magnetom_y;
int magnetom_z;
float MAG_Heading;

float DCM_Matrix[3][3]= {
  {
    1,0,0  }
  ,{
    0,1,0  }
  ,{
    0,0,1  }
}; 

//  RX PPM channel order for DX7 and Spektrum2PPM
// ===============================================
//  1: Throttle (left stick down->up)
//  2: Roll (right stick right->left)
//  3: Pitch/Nick (right stick down->up)
//  4: Yaw (left stick right->left)
//  5: Gear (Switch 1->0)
//  6: Flt Mode (Switch 2->1->N)
//  7: Aux2 (Switch 1->0)

#define RX_THROTTLE 1
#define RX_ROLL 2
#define RX_PITCH 3
#define RX_YAW 4
#define RX_GEAR 5
#define RX_FLTMODE 6
#define RX_AUX 7

// Configure stick behaviour
#define REVERSE_ROLL -          // Reverse roll  (+ or -)
#define REVERSE_PITCH -         // Reverse pitch (+ or -)
#define REVERSE_YAW -           // Reverse yaw (+ or -)
#define REVERSE_THROTTLE +      // Reverse throttle (+ or -)

#define THROTTLE RX_THROTTLE-1
#define ROLL RX_ROLL-1
#define PITCH RX_PITCH-1
#define YAW RX_YAW-1
#define GEAR RX_GEAR-1
#define FLTMODE RX_FLTMODE-1
#define AUX RX_AUX-1

// PPM Rx signal read (ICP) constants and variables
// RX command states
#define RX_NOT_SYNCHED 0
#define RX_ACQUIRING 1
#define RX_READY 2
#define RX_IN_FAILSAFE 3

// Define LEDs
// Red indicates motors are armed
#define REDLEDPIN 5
#define REDLEDON digitalWrite(REDLEDPIN, HIGH)
#define REDLEDOFF digitalWrite(REDLEDPIN, LOW)
// Blue indicates RX readiness
#define BLUELEDPIN 6
#define BLUELEDON digitalWrite(BLUELEDPIN, HIGH)
#define BLUELEDOFF digitalWrite(BLUELEDPIN, LOW)
// Yellow indicates IMU activity and commands being sent to the motors
#define YELLOWLEDPIN 7
#define YELLOWLEDON digitalWrite(YELLOWLEDPIN, HIGH)
#define YELLOWLEDOFF digitalWrite(YELLOWLEDPIN, LOW)

#define MOTOR_FRONT 0
#define MOTOR_REAR 1
#define MOTOR_LEFT 2
#define MOTOR_RIGHT 3

struct RX {
  int raw[7];        // Raw 
  int value[7];      // Smoothed
  float command[7];  // Processed
  float diff[7];     // Delta between two samples
  int state;         // RX command state (e.g. RX_NOT_SYNCHED or RX_READY
} rx = {
  { 0, 0, 0, 0, 0, 0, 0 },
  { 0, 0, 0, 0, 0, 0, 0 },
  { 0, 0, 0, 0, 0, 0, 0 },
  { 0, 0, 0, 0, 0, 0, 0 },
  RX_NOT_SYNCHED
};

struct Motor {
  int command[4];
  boolean armed;
} motor = {
  { MIN_CHANN, MIN_CHANN, MIN_CHANN, MIN_CHANN },
  false,
};

// ADC variables
volatile uint8_t MuxSel=0;
volatile uint8_t analog_reference = DEFAULT;
volatile uint16_t analog_buffer[8];
volatile uint8_t analog_count[8];
int an_count;

// Attitude control variables
int control_roll;           // Control results
int control_pitch;
int control_yaw;

// Attitude control
float roll_I=0;
float roll_D;
float err_roll;
float pitch_I=0;
float pitch_D;
float err_pitch;
float yaw_I=0;
float yaw_D;
float err_yaw;

// timer variables
unsigned int counter = 0;
float G_Dt;    // Integration time for the gyros (DCM algorithm)
long timer = 0; //general purpose timer 
long timer_old = 0;
unsigned int arming_counter = 0;

void setup(){
  
  Serial.begin(38400);
  Serial.println();
  Serial.print("QuaduIMU v");
  Serial.println(SOFTWARE_VER);

  pinMode(BLUELEDPIN, OUTPUT); // Blue LED
  pinMode(REDLEDPIN, OUTPUT);  // Red LED
  pinMode(YELLOWLEDPIN, OUTPUT); // Yellow LED
  BLUELEDON;
  REDLEDON;
  YELLOWLEDON;

  // Attach ESCs and set to minimum throttle. Motors are disarmed.
  setupMotors();
 
  Analog_Reference(EXTERNAL);
  Analog_Init();
 
#if (HAVE_MAGNETOMETER==1)
  // Magnetometer initialization
  I2C_Init();
  Compass_Init();
#endif

  Read_adc_raw();
  delay(20);

  // Offset values for accels and gyros...
  AN_OFFSET[0] = gyro_offset_roll;
  AN_OFFSET[1] = gyro_offset_pitch;
  AN_OFFSET[2] = gyro_offset_yaw;
  AN_OFFSET[3] = acc_offset_x;
  AN_OFFSET[4] = acc_offset_y;
  AN_OFFSET[5] = acc_offset_z;
 
  // Take the gyro offset values
  for(int i=0;i<300;i++)
  {
    Read_adc_raw();
    for(int y=0; y<=2; y++)   // Read initial ADC values for offset.
      AN_OFFSET[y]=AN_OFFSET[y]*0.8 + AN[y]*0.2;
    delay(20);
  }
 
  Serial.print("AN:");
  for (int i=0; i<6; i++)
  {
    Serial.print(",");
    Serial.println(AN_OFFSET[i]);
  }
  YELLOWLEDOFF;

  setupRX();

  Read_adc_raw();   // Start ADC readings...
  timer = millis();
  delay(20);
  BLUELEDOFF;
}

/* MAIN LOOP */
void loop() {
  
  if((DIYmillis()-timer) >= (1000/AHRS_LOOP)) //AHRS LOOP
  {
    counter++;
    timer_old = timer;
    timer=millis();
    G_Dt = (timer-timer_old)/1000.0;      // Real time of loop run 
	
#if (HAVE_MAGNETOMETER==1)
    if (counter % (AHRS_LOOP/MAGNETOMETER_LOOP) == 0 && magnetom_enabled) //MAGNETOMETER LOOP
    {
      Read_Compass();    // Read I2C magnetometer
      Compass_Heading(); // Calculate magnetic heading  
    }
#endif
    
    //Read ADC (accelero & gyros)
    Read_adc_raw();

    // IMU DCM Algorithm
    DCM(); 
   
    if (counter % (AHRS_LOOP/RADIO_LOOP) == 0) //CONTROL LOOP
    {
       updateRX();
    }

    if (counter % (AHRS_LOOP/CONTROL_LOOP) == 0) //CONTROL LOOP
    {
      if (rx.state==RX_READY) {
        Attitude_control();
      
        // Quadcopter mix
        if (rx.raw[THROTTLE] > (MINCHECK) && motor.armed)  // Minimum throttle to start control
        {
          YELLOWLEDON; 
          motor.command[MOTOR_RIGHT]=rx.raw[THROTTLE] - control_roll + control_yaw;    // Right motor
          motor.command[MOTOR_LEFT]=rx.raw[THROTTLE] + control_roll + control_yaw;    // Left motor
          motor.command[MOTOR_FRONT]=rx.raw[THROTTLE] + control_pitch - control_yaw;   // Front motor
          motor.command[MOTOR_REAR]=rx.raw[THROTTLE] - control_pitch - control_yaw;   // rear motor
          updateMotors();
        } else {
          YELLOWLEDOFF; 
          rx.command[YAW] = ToDeg(yaw);
          roll_I = 0;  // reset I terms...
          pitch_I = 0;
          yaw_I = 0; 
          // Motors stopped
          setAllMotors(MIN_CHANN);
          updateMotors();
        }
      } else {  // rx.state!=RX_READY => Lost radio signal => Descend slowly...
        // Descend  (Reduce throttle)
        if (rx.raw[THROTTLE]>MIN_CHANN)
          rx.raw[THROTTLE]--;
        rx.command[ROLL] = 0;     // Stabilize to roll=0, pitch=0, yaw not important
        rx.command[PITCH] = 0;
        Attitude_control();
        // Quadcopter mix, no yaw
        motor.command[MOTOR_RIGHT]=rx.raw[THROTTLE] - control_roll;    // Right motor
        motor.command[MOTOR_LEFT]=rx.raw[THROTTLE] + control_roll;    // Left motor
        motor.command[MOTOR_FRONT]=rx.raw[THROTTLE] + control_pitch;   // Front motor
        motor.command[MOTOR_REAR]=rx.raw[THROTTLE] - control_pitch;   // rear motor
        updateMotors();
      }
    }

    if (counter % (AHRS_LOOP/TELEMETRY_LOOP) == 0) //TELEMETRY LOOP
    {
      // Telemetry data...
      printdata();
    }
  }
}