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;   } }