[Из песочницы] 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< 1 )
{
TWCR = (1<> (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)<>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;
}
}
