Alt Hold for CC3D Flight Controller at Baro

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