~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();

    }

  }

}