Alt Hold для CC3D полетного контроллера на Баро

/* Alt Holt with BMP095
 * by Aleksandr Stroganov 16.01.2016
 * 
 * In from receiver
 * PD2 - AUX1 in
 * PD3 - Trl in
 * 
 * Out to CC3D
 * PB1 Aux1 out
 * PB2 Trl out
 */
#ifndef F_CPU 
   #define F_CPU 16000000UL
#endif

//I2C and BMP085
#define F_I2C          400000UL                 //Частота шины I2C
#define TWBR_VALUE    (((F_CPU)/(F_I2C)-16)/2)  //Расчет значения для желаемой чистоты
#define BMP085_ADDRESS 0x77                     // I2C address of BMP085
const unsigned char OSS = 3;                    // Oversampling Setting

// BMP085 Calibration values
int ac1;
int ac2;
int ac3;
unsigned int ac4;
unsigned int ac5;
unsigned int ac6;
int b1;
int b2;
int mb;
int mc;
int md;
// b5 is calculated in bmp085GetTemperature(...), this variable is also used in bmp085GetPressure(...)
// so ...Temperature(...) must be called before ...Pressure(...).
long b5;

// Loop Flag
#define TRUE  1
#define FALSE 0

// Flight Mode
#define Manual_Mode  0
#define Alt_Hold     1
 
volatile unsigned int cnt_rising = 0;
volatile unsigned int cnt_falling = 0;
volatile unsigned int cnt_thro = 0;
volatile unsigned int cnt_FMD = 0;
volatile unsigned int t_scale = 0; //Множитель чтоб расчитать 40Гц

// LPF
#define SamplingTime    0.025     // Control Loop Period 40Hz
float CutOffFrequency = 3.0;      // Hz
float LPF_error       = 0.0;
float LPF_ee          = 0.0;
float LPF_ee1         = 0.0;
float LPF_ww          = 0.0;

// PID
int Thro_Neutral       = 3000;      // Pulse width when throttle is neutral
int Thro_Deadband      = 30;        // throttle zero when it's in between -30~30
float A_outer_Pgain    = 0.5;
float A_inner_Pgain    = 1.5;
float A_inner_Igain    = 0.25;
float A_inner_Dgain    = 0.0;
float Hov_Thro         = 0.0;
int THRO_CMD_MAX       = 200;
int ALT_RATE_ERR_MAX   = 200;
int ALT_RATE_I_MAX     = 200;
int ALT_PID_MAX        = 400;

byte T_flag = 0;
byte FMD_flag = 0;

//Расчет по высоте
short T;                        //Температура
long  P;                        //Давление
float H_alt            = 0.0;
float H_temp           = 0.0;
float Altitude         = 0.0;
float Altitude_cm      = 0.0;
float Altitude_LPF     = 0.0;
float ClimbRate        = 0.0;
float pre_Altitude_LPF = 0.0;


int Thro_cmd           = 0; 
long Thro_in           = 0;
float ALT_ERR          = 0.0;
float ALT_RATE_ERR     = 0.0;
float ALT_RATE_P       = 0.0;
float ALT_RATE_I       = 0.0;
float ALT_RATE_D       = 0.0;
float pre_ALT_RATE_ERR = 0.0;
float ALT_PID          = 0.0;
float ALT_cont         = 0.0;
 
void Port_init()
{
    //DDRD |= 0x00;         // INT 0,1 настраиваем на вход
    //PORTD = 0b00001100;   // INT 0,1 подключаем подтягивающие резисторы
        
    DDRD = (0 << DDD2)|(0 << DDD3);       // INT 0,1 настраиваем на вход
    PORTD = (1 << PORTD2)|(1 << PORTD3);  // INT 0,1 подключаем подтягивающие резисторы
    EIMSK = 0b00000011;                   // INT 0,1 Внешнее прерывание индивидуальное разрешение
    EICRA = 0b00001111;                   // INT 0,1 - rising edge настройки триггеров
}
 
void Timer1_init()
{
    DDRB |= 0b00000110;		  // OC1A, OC1B Установите выходной направление выводов
    TCCR1A = 0b10100010;	  // Fast PWM Установить режим работы
    TCCR1B = 0b00011010;	  // Fast PWM 14 mode, 8 sclaer.
    ICR1 = 40000;           // 20ms, 50Hz period
}

void Timer2_init()
{
    TCCR2A = 0b00000011;    // Fast PWM, No OC output, Установить режим работы
    TCCR2B = 0b00001100;    // Fast PWM 7 mode, 64 sclaer.
    OCR2A = 250;            // 1ms, 1000Hz period
    TIMSK2 = 0b00000001;
}

 
ISR(INT0_vect)				// Flight Mode вход
{
    if(EICRA == 0b00001111){
        cnt_rising = TCNT1;
        EICRA = 0b00001110;
    }
    else{
        cnt_falling = TCNT1;
        cnt_FMD = (40001 - cnt_rising + cnt_falling) % 40001;
        EICRA = 0b00001111;
	}
}

ISR(INT1_vect)				// Throttle вход
{
    if(EICRA == 0b00001111){
        cnt_rising = TCNT1;
        EICRA = 0b00001011;
    }
    else{
        cnt_falling = TCNT1;
        cnt_thro = (40001 - cnt_rising + cnt_falling) % 40001;
        EICRA = 0b00001111;
    }
}

ISR(TIMER2_OVF_vect)        // 1000Гц Превращаем в 40Гц и ставим флаг расчета параметров
{
    t_scale ++;
    if(t_scale == 25){
        t_scale = 0;
        T_flag = TRUE;
    }
}
 
byte FMD_check(unsigned int FMD_in) //Функция проверки режима полета
{
    byte FMD_out;
    if(FMD_in > 2150 && FMD_in < 2900)
    {
        FMD_out = 1;
    }
    else if(FMD_in > 2900 && FMD_in < 3700)
    {
        FMD_out = 1;        
    }
    else
    {
        FMD_out = 0;        // Ручной режим.
    }
    return FMD_out;
}

void Limit_cut(float *ff, int MIN_LIMIT, int MAX_LIMIT)
{
    if(*ff < MIN_LIMIT)
    {
        *ff = MIN_LIMIT;
    }
    else if(*ff > MAX_LIMIT)
    {
        *ff = MAX_LIMIT;
    }
}

void Limit_cut_int(int *ff, int MIN_LIMIT, int MAX_LIMIT)
{
    if(*ff < MIN_LIMIT)
    {
        *ff = MIN_LIMIT;
    }
    else if(*ff > MAX_LIMIT)
    {
        *ff = MAX_LIMIT;
    }
}

void Limit_cut_long(long *ff, long int MIN_LIMIT, long int MAX_LIMIT)
{
    if(*ff < MIN_LIMIT)
    {
        *ff = MIN_LIMIT;
    }
    else if(*ff > MAX_LIMIT)
    {
        *ff = MAX_LIMIT;
    }
}

void Stick_Deadband(int *Stick, int MIN_RANGE, int MAX_RANGE)
{
    if(*Stick > MIN_RANGE && *Stick < MAX_RANGE)
    {
        *Stick = 0;
    }
}

//Инициализация шины I2C 
void TWI_Init(void)
{
  TWBR = TWBR_VALUE;
  TWSR = 0x00;
}

// Stores all of the bmp085's calibration values into global variables
// Calibration values are required to calculate temp and pressure
// This function should be called at the beginning of the program
void bmp085Calibration()
{
  ac1 = bmp085ReadInt(0xAA);
  ac2 = bmp085ReadInt(0xAC);
  ac3 = bmp085ReadInt(0xAE);
  ac4 = bmp085ReadInt(0xB0);
  ac5 = bmp085ReadInt(0xB2);
  ac6 = bmp085ReadInt(0xB4);
  b1 =  bmp085ReadInt(0xB6);
  b2 =  bmp085ReadInt(0xB8);
  mb =  bmp085ReadInt(0xBA);
  mc =  bmp085ReadInt(0xBC);
  md =  bmp085ReadInt(0xBE);
}

// Read 2 bytes from the BMP085
// First byte will be from 'address'
// Second byte will be from 'address'+1
int bmp085ReadInt(unsigned char address)
{
  unsigned char msb, lsb;
 
  TWI_Start();
  TWI_Start_SLA_W( BMP085_ADDRESS, address, 0 );
  TWI_Stop();

  TWI_Start();
  TWI_Start_SLA_R( BMP085_ADDRESS );

  msb = TWI_Read_Byte( 1 );
  lsb = TWI_Read_Byte( 2 );

  TWI_Stop();

  return (int) msb<<8 | lsb;
}

void TWI_Start()
{
  /*формируем состояние СТАРТ*/ 
  TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
  while(!(TWCR & (1<<TWINT)));
}

char TWI_Start_SLA_R( unsigned char address_i2c )
{
  /*выдаемна шину пакет SLA-R*/
  TWDR = (address_i2c<<1)|1;
  TWCR = (1<<TWINT)|(1<<TWEN); 
  while(!(TWCR & (1<<TWINT)));
}

char TWI_Start_SLA_W( unsigned char address_i2c, unsigned char adr, unsigned char data )
{
  //выдаемна шину пакет SLA-W
  TWDR = (address_i2c<<1)|0;
  TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA); 
  while(!(TWCR & (1<<TWINT)));

  //передаем адрес регистра 
  TWDR = adr;
  TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA); 
  while(!(TWCR & (1<<TWINT)));

  if( data )
  {
    TWDR = data;
    TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA); 
    while(!(TWCR & (1<<TWINT)));
  }

}

char TWI_Read_Byte(char n)
{
  char data;

  /*считываем данные*/
  if( n > 1 )
  {
    TWCR = (1<<TWINT)|(1<<TWEN)|(0<<TWEA);    
  }
  else
  {
    TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA);    
  }
  
  while(!(TWCR & (1<<TWINT)));
  data = TWDR;

  return data; 
}

void TWI_Stop()
{
  /*формируем состояние СТОП*/
  TWCR = (1<<TWINT)|(1<<TWSTO)|(1<<TWEN);
}

// Read the uncompensated temperature value
unsigned int bmp085ReadUT()
{
  unsigned int ut;
 
  // Write 0x2E into Register 0xF4
  // This requests a temperature reading

  TWI_Start();
  TWI_Start_SLA_W( BMP085_ADDRESS, 0xF4, 0x2E );
  TWI_Stop();

 
  // Wait at least 4.5ms
  delay(5);

  // Read two bytes from registers 0xF6 and 0xF7
  ut = bmp085ReadInt(0xF6);

  return ut;
}

// Read the uncompensated pressure value
unsigned long bmp085ReadUP()
{
  unsigned char msb, lsb, xlsb;
  unsigned long up = 0;
 
  // Write 0x34+(OSS<<6) into register 0xF4
  // Request a pressure reading w/ oversampling setting
 
  TWI_Start();
  TWI_Start_SLA_W( BMP085_ADDRESS, 0xF4, 0x34 + (OSS<<6) );
  TWI_Stop();

  // Wait for conversion, delay time dependent on OSS
  delay(2 + (3<<OSS));
 
  // Read register 0xF6 (MSB), 0xF7 (LSB), and 0xF8 (XLSB)

  TWI_Start();
  TWI_Start_SLA_W( BMP085_ADDRESS, 0xF6, 0 );
  TWI_Stop();

  TWI_Start();
  TWI_Start_SLA_R( BMP085_ADDRESS );

  msb = TWI_Read_Byte( 1 );
  lsb = TWI_Read_Byte( 1 );
  xlsb = TWI_Read_Byte( 2 );

  TWI_Stop();
 
  up = (((unsigned long) msb << 16) | ((unsigned long) lsb << 8) | (unsigned long) xlsb) >> (8-OSS);
 
  return up;
}

// Calculate temperature given ut.
// Value returned will be in units of 0.1 deg C
short bmp085GetTemperature(unsigned int ut)
{
  long x1, x2;
 
  x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;
  x2 = ((long)mc << 11)/(x1 + md);
  b5 = x1 + x2;

  return ((b5 + 8)>>4);  
//  return ((b5 + 8)/pow(2,4));  

}

// Calculate pressure given up
// calibration values must be known
// b5 is also required so bmp085GetTemperature(...) must be called first.
// Value returned will be pressure in units of Pa.
long bmp085GetPressure(unsigned long up)
{
  long x1, x2, x3, b3, b6, p;
  unsigned long b4, b7;
 
  b6 = b5 - 4000;
  // Calculate B3
  x1 = (b2 * (b6 * b6)>>12)>>11;
  x2 = (ac2 * b6)>>11;
  x3 = x1 + x2;
  b3 = (((((long)ac1)*4 + x3)<<OSS) + 2)>>2;
 
  // Calculate B4
  x1 = (ac3 * b6)>>13;
  x2 = (b1 * ((b6 * b6)>>12))>>16;
  x3 = ((x1 + x2) + 2)>>2;
  b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;
 
  b7 = ((unsigned long)(up - b3) * (50000>>OSS));
  if (b7 < 0x80000000)
    p = (b7<<1)/b4;
  else
    p = (b7/b4)<<1;
    
  x1 = (p>>8) * (p>>8);
  x1 = (x1 * 3038)>>16;
  x2 = (-7357 * p)>>16;
  p += (x1 + x2 + 3791)>>4;
 
  return p;
}

float LPF(float input, float CutOffFrequency)
{
    float output;
 
    LPF_error = input - LPF_ww;
    LPF_ee = LPF_error * CutOffFrequency;
    LPF_ww = LPF_ww + (LPF_ee+LPF_ee1)*SamplingTime*0.5;
    LPF_ee1 = LPF_ee;
    output = LPF_ww;
    
    return output;
}

void setup() {
    TWI_Init();
    bmp085Calibration();   

    SREG |= 0x80;
    Port_init();
    Timer1_init();
    Timer2_init();      
}

void loop() {
  if(T_flag == TRUE) //Срабатывание каждые 40Гц
  {

    T = bmp085GetTemperature( bmp085ReadUT() );
    P = bmp085GetPressure( bmp085ReadUP() );

    H_temp = (P/100.0f)/1013.25;
    H_alt = (1-pow(H_temp,0.190284)) * 145366.45;
    Altitude = 0.3048*H_alt;

    Altitude_cm = (float)((long)(Altitude*100));  // 1cm
    Altitude_LPF = LPF(Altitude_cm, CutOffFrequency);      // LPF // Altitude_feedback
    ClimbRate = (Altitude_LPF - pre_Altitude_LPF)/SamplingTime;  // ClimbRate_feedback
    pre_Altitude_LPF = Altitude_LPF;    
  	
  	FMD_flag = FMD_check(cnt_FMD);     // FMD Проверяем режим
  
    OCR1A = cnt_FMD;                    //Передаем в CC3D режим полета

  	switch(FMD_flag){
  		case Manual_Mode:                     // При ручном режиме

        OCR1B = cnt_thro;                   //Передаем в CC3D положение газа
        Hov_Thro = cnt_thro-2000;
        Limit_cut(&Hov_Thro, 800, 1200);
        Thro_in = Altitude_LPF;
        ALT_RATE_I = 0.0;

  		break;
  		case Alt_Hold:                              // Режим управления Контроль Высоты
     
        Thro_cmd = 0.25 * ((int)cnt_thro-Thro_Neutral);  // -250~250 means -2.5~2.5
        Stick_Deadband(&Thro_cmd, -Thro_Deadband, Thro_Deadband);
        Limit_cut_int(&Thro_cmd, -THRO_CMD_MAX, THRO_CMD_MAX);
        Thro_in = Thro_in + Thro_cmd*SamplingTime;
        Limit_cut_long(&Thro_in, -200000000, 200000000);  
    
        ALT_ERR = ((float)Thro_in) - Altitude_LPF;
        ALT_RATE_ERR = ALT_ERR * A_outer_Pgain - ClimbRate;
        Limit_cut(&ALT_RATE_ERR, -ALT_RATE_ERR_MAX, ALT_RATE_ERR_MAX);
        ALT_RATE_P = ALT_RATE_ERR * A_inner_Pgain;
        ALT_RATE_I = ALT_RATE_I + (ALT_RATE_ERR * A_inner_Igain) * SamplingTime;
        Limit_cut(&ALT_RATE_I, -ALT_RATE_I_MAX, ALT_RATE_I_MAX);
        ALT_RATE_D = (ALT_RATE_ERR - pre_ALT_RATE_ERR)/SamplingTime * A_inner_Dgain;
        pre_ALT_RATE_ERR = ALT_RATE_ERR;
    
        ALT_PID = ALT_RATE_P + ALT_RATE_I + ALT_RATE_D;
        Limit_cut(&ALT_PID, -ALT_PID_MAX, ALT_PID_MAX);
        ALT_cont = ALT_PID + Hov_Thro;
    
        OCR1B = 2000 + (int)(ALT_cont);  
        
  			break;
  		default: 
  			break;
  	}
    T_flag = FALSE;
  }
}