Специальные цены   новые товары
+ Ответить в теме
Показано с 1 по 5 из 5

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

Тема раздела Коптеры. Комплектующие, сборка, настройка. в категории Квадрокоптеры и мультироторы; Добрый день много уважаемая публика Думаю, я не открою америки. Но когда у меня возникла необходимость, сделать стабилизацию по высоте ...

  1. #1

    Регистрация
    29.03.2011
    Адрес
    Пермь
    Возраст
    35
    Сообщений
    22

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

    Добрый день много уважаемая публика
    Думаю, я не открою америки. Но когда у меня возникла необходимость, сделать стабилизацию по высоте с баро для CC3D решение не нашлось. Были варианты перепрошить его с OpenPilot на что то другое и там есть такие вещи. Но я решил пойти не самым стандартным и легким способом. Мое решение это контроллер между приемником и CC3D (или любым другим) который меряет высоту и вносит корректировку в канал газа, а затем передает ее в полетный контроллер. таким образом держит высоту.
    Нажмите на изображение для увеличения
Название: 111.JPG
Просмотров: 65
Размер:	9.4 Кб
ID:	1160856

    Как это все собирается.
    Я использовал датчик давления BMP085 не самый точный. Но зато был в наличии. Поэтому на видео видно значительные колебания. Тем не менее в разбросе 1 метр высота держится.

    BMP085 подключен по I2C к Arduino
    входы А4 и А5 и питание

    Приемник цепляем ко входам 2 и 3

    Выход на CC3D 9 и 10
    9 это канал Aux1 если он не нужен то на полетный контроллер его можно не передавать.

    В моем решении это двух позиционный переключатель для активации режима удержания. Лично у меня далее в коптере этот переключатель не используется.

    скеч ардуино:
    Код:
    /* 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;
      }
    }
    Готовое решение выглядит так
    Нажмите на изображение для увеличения
Название: Фото 16.01.16, 19 21 33.jpg
Просмотров: 32
Размер:	34.4 Кб
ID:	1160857Нажмите на изображение для увеличения
Название: Фото 16.01.16, 17 47 09.jpg
Просмотров: 25
Размер:	41.3 Кб
ID:	1160858Нажмите на изображение для увеличения
Название: Фото 16.01.16, 17 47 13.jpg
Просмотров: 32
Размер:	44.7 Кб
ID:	1160859Нажмите на изображение для увеличения
Название: Фото 16.01.16, 17 47 17.jpg
Просмотров: 30
Размер:	53.2 Кб
ID:	1160860

    Тестовый полет (использовался Arduino Uno). Включен режим удержания высоты. я корректирую только положение. Прошу прощения у публики пилот из меня не очень поэтому коптер изрядно мотает из стороны в сторону.


    Вопросы, пожелания и предложения с радостью выслушаю.

  2.  
  3. #2

    Регистрация
    29.03.2011
    Адрес
    Пермь
    Возраст
    35
    Сообщений
    22
    Нажмите на изображение для увеличения
Название: My_CC3D.jpg
Просмотров: 64
Размер:	39.9 Кб
ID:	1160910
    Не нашел как отредактировать. Немного по красивей нарисовал схему включения

  4. #3

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,491
    Записей в дневнике
    9
    Собственно удержания высоты-то и не видно (
    1) барометр, не айс
    2) на одном барометре высоту не удержишь - нужен ещё датчик (аксел)
    3) CC3D правильно настроенный неплохо держит высоту (там есть ПИД по газу и экспоненты по газу)
    могу дать подсказку:
    1)прицепить к ардуине MPU6050 и MS5611
    2) выдрать альтхолд и калибровку датчиков из мультивий...

  5. #4

    Регистрация
    09.10.2010
    Адрес
    Протвино, Серпухов
    Возраст
    44
    Сообщений
    4,819
    Записей в дневнике
    25
    Развитие темы есть?

  6.  
  7. #5

    Регистрация
    29.03.2011
    Адрес
    Пермь
    Возраст
    35
    Сообщений
    22
    Цитата Сообщение от shura2000 Посмотреть сообщение
    Развитие темы есть?
    Не особо, но при желании можно и развить. Общественность не поддержала. я забил

+ Ответить в теме

Похожие темы

  1. Полетный контроллер DJI Naza M v1
    от Sci_Rosso в разделе Коптеры. Комплектующие, сборка, настройка.
    Ответов: 20002
    Последнее сообщение: 16.05.2017, 10:01
  2. Выбор современного полётного контроллера с GPS и прочими фишками.
    от ARXITEKTOR в разделе Коптеры. Комплектующие, сборка, настройка.
    Ответов: 14
    Последнее сообщение: 08.02.2017, 12:29
  3. Продам Полетный контроллер DJI NAZA M-V2
    от Genry в разделе Барахолка. Квадрокоптеры и другие многороторные системы
    Ответов: 10
    Последнее сообщение: 22.04.2016, 16:48
  4. Продам 3 полетных контроллера
    от raefa в разделе Барахолка. Квадрокоптеры и другие многороторные системы
    Ответов: 0
    Последнее сообщение: 16.06.2015, 23:56
  5. Хороший модуль питания полетных контроллеров.
    от Shuricus в разделе Полеты по камере, телеметрия
    Ответов: 45
    Последнее сообщение: 14.03.2015, 21:56

Ваши права

  • Вы не можете создавать новые темы
  • Вы не можете отвечать в темах
  • Вы не можете прикреплять вложения
  • Вы не можете редактировать свои сообщения