Специальные цены   новые товары
+ Ответить в теме
Страница 19 из 165 ПерваяПервая ... 9 17 18 19 20 21 29 ... ПоследняяПоследняя
Показано с 721 по 760 из 6569

Создание собственной системы стабилизации

Тема раздела Квадрокоптеры. Общие вопросы в категории Квадрокоптеры и мультироторы; лучше мотор с PB8 на PA11 - разъёмы хорошо встают на этих портах не надо шаманств с резюками... А вообще ...

  1. #721

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    лучше мотор с PB8 на PA11 - разъёмы хорошо встают

    на этих портах не надо шаманств с резюками...

    А вообще можно echo оставить на pb9, а trigger повесить на PA13 или 14 там штырьки есть....
    Последний раз редактировалось SergDoc; 21.09.2012 в 14:29.

  2.  
  3. #722

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Вот нашел:
    sensors.c
    Код:
     
    void Sonar_init(void) 
    {
    -    hcsr04_init(sonar_rc78);
    + hcsr04_init(sonar_pwm56);
        sensorsSet(SENSOR_SONAR);
        sonarAlt = 0; 
    }
    В общем, поковырялся - сонар в GUI засветился, но на портах PB8, PB9 шимы не выключились, надо вообще их обрубить...
    Последний раз редактировалось SergDoc; 21.09.2012 в 16:42.

  4. #723

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    может я ошибаюсь но скорей всего нужно закоментировать всё относящееся к портам PB8 PB9 в драйвере drv_pvm.c , а так же убрать из того же драйвера
    Код:
    if (init->useServos && !init->airplane) {
                // remap PWM9+10 as servos (but not in airplane mode LOL)
                if (port == PWM9 || port == PWM10)
                    mask = TYPE_S;
            }
    port == PWM10 , тогда по идее освободятся порты для сонара и моторы сдвинутся на 1, конечно можно будет использовать только квадрик без подвеса и трикоптер, ну мне большего пока и ненадо от этой платы...

  5. #724

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    ну мне большего пока и ненадо от этой платы...
    Это для маленькой с 103м?
    Цитата Сообщение от SergDoc Посмотреть сообщение
    но на портах PB8, PB9 шимы не выключились
    надо найти место где инициализация ШИМ выполняется...

  6.  
  7. #725

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    да на 103-м

    Цитата Сообщение от rual Посмотреть сообщение
    надо найти место где инициализация ШИМ выполняется...
    drv_pwm.c - буду ковырять...

  8. #726

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    drv_pwm.c - буду ковырять...
    Найди вызов в майне pwmInit, найди в параметре ссылку на одну из структур
    и убери из неё PWM5, PWM6
    Код:
    static const uint8_t multiPWM[] = { 
        PWM1 | TYPE_IW,     // input #1 
        PWM2 | TYPE_IW, 
        PWM3 | TYPE_IW, 
        PWM4 | TYPE_IW, 
       // PWM5 | TYPE_IW, /// !!!!!!!!!!!!!!!!!!    
        //PWM6 | TYPE_IW, /// !!!!!!!!!!!!!!!!!!
        PWM7 | TYPE_IW, 
        PWM8 | TYPE_IW,     // input #8 
        PWM9 | TYPE_M,      // motor #1 or servo #1 (swap to servo if needed) 
        PWM10 | TYPE_M,     // motor #2 or servo #2 (swap to servo if needed) 
        PWM11 | TYPE_M,     // motor #1 or #3 
        PWM12 | TYPE_M, 
        PWM13 | TYPE_M, 
        PWM14 | TYPE_M,     // motor #4 or #6 
        0xFF 
    };
    или просто закоментируй, ломать код не надо, всё структурами настраивается.

  9. #727

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Вот здесь
    Код:
        // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ] 
        if (init->airplane) 
            i = 2; // switch to air hardware config 
        if (init->usePPM) 
            i++; // next index is for PPM 
     
        setup = hardwareMaps[i];
    выбирается одна из структур
    Код:
    static const uint8_t *hardwareMaps[] = { 
        multiPWM, 
        multiPPM, 
        airPWM, 
        airPPM, 
    };
    которые описаны выше,
    Код:
     static const uint8_t multiPPM[] = { 
        PWM1 | TYPE_IP,     // PPM input 
        PWM9 | TYPE_M,      // Swap to servo if needed 
        PWM10 | TYPE_M,     // Swap to servo if needed 
        PWM11 | TYPE_M, 
        PWM12 | TYPE_M, 
        PWM13 | TYPE_M, 
        PWM14 | TYPE_M, 
        PWM5 | TYPE_M,      // Swap to servo if needed 
        PWM6 | TYPE_M,      // Swap to servo if needed 
        PWM7 | TYPE_M,      // Swap to servo if needed 
        PWM8 | TYPE_M,      // Swap to servo if needed 
        0xFF 
    }; 
     
    static const uint8_t multiPWM[] = { 
        PWM1 | TYPE_IW,     // input #1 
        PWM2 | TYPE_IW, 
        PWM3 | TYPE_IW, 
        PWM4 | TYPE_IW, 
        PWM5 | TYPE_IW, 
        PWM6 | TYPE_IW, 
        PWM7 | TYPE_IW, 
        PWM8 | TYPE_IW,     // input #8 
        PWM9 | TYPE_M,      // motor #1 or servo #1 (swap to servo if needed) 
        PWM10 | TYPE_M,     // motor #2 or servo #2 (swap to servo if needed) 
        PWM11 | TYPE_M,     // motor #1 or #3 
        PWM12 | TYPE_M, 
        PWM13 | TYPE_M, 
        PWM14 | TYPE_M,     // motor #4 or #6 
        0xFF 
    }; 
     
    static const uint8_t airPPM[] = { 
        PWM1 | TYPE_IP,     // PPM input 
        PWM9 | TYPE_M,      // motor #1 
        PWM10 | TYPE_M,     // motor #2 
        PWM11 | TYPE_S,     // servo #1 
        PWM12 | TYPE_S, 
        PWM13 | TYPE_S, 
        PWM14 | TYPE_S,     // servo #4 
        PWM5 | TYPE_S,      // servo #5 
        PWM6 | TYPE_S, 
        PWM7 | TYPE_S, 
        PWM8 | TYPE_S,      // servo #8 
        0xFF 
    }; 
     
    static const uint8_t airPWM[] = { 
        PWM1 | TYPE_IW,     // input #1 
        PWM2 | TYPE_IW, 
        PWM3 | TYPE_IW, 
        PWM4 | TYPE_IW, 
        PWM5 | TYPE_IW, 
        PWM6 | TYPE_IW, 
        PWM7 | TYPE_IW, 
        PWM8 | TYPE_IW,     // input #8 
        PWM9 | TYPE_M,      // motor #1 
        PWM10 | TYPE_M,     // motor #2 
        PWM11 | TYPE_S,     // servo #1 
        PWM12 | TYPE_S, 
        PWM13 | TYPE_S, 
        PWM14 | TYPE_S,     // servo #4 
        0xFF 
    };
    можешь свою добавить и включить в hardwareMaps[]

  10.  
  11. #728

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Запустил на портах PB8, PB9 проверяю работоспособность остальных шимов...

    серва точно работает моторы не стартонули не один
    Последний раз редактировалось SergDoc; 21.09.2012 в 20:55.

  12. #729

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Вобщем надо, свистелку тобиш триггер пересадить на порт pa11

  13. #730

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    моторы не стартонули не один
    ШИМа нет?

  14. #731

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    как раз то шим есть похоже моторы не назначаются...

    Короче я сам дурак - последние прошивки Таймкопа ( там где он якобы отладил алт холд) НЕРАБОЧИЕ - армятся, но шимы не меняются...
    Последний раз редактировалось SergDoc; 21.09.2012 в 23:19.

  15. #732

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    откатил прошивку, движки сместились без проблем, но сонар не заработал

    Фух всё, работают и движки, и серва, и сонар PB8 - PB9 , а вот по ходу альт холд придётся самому присобачивать, да и всё равно сонар в иму вклинивать надо.... пошел я спать...
    на картинке я специально сонар вверх вниз дёргал, где скачёк - пальцем закрыл...
    Миниатюры Миниатюры Нажмите на изображение для увеличения
Название: Sonar.jpg‎
Просмотров: 43
Размер:	48.0 Кб
ID:	694702  
    Последний раз редактировалось SergDoc; 22.09.2012 в 01:08.

  16. #733

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Вот только интересный момент, у меня получается на одном таймере теперь висит и 50Гц шим и 400Гц - как-то нехорошо получаеццо....

  17. #734

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    у меня получается на одном таймере теперь висит и 50Гц шим и 400Гц - как-то нехорошо получаеццо....
    Не очень, у всех каналов таймера будет герцовка как у последнего в структуре по ссылке в hardwareMaps[]

  18. #735

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    так вот я и хочу повесить trigger сонара на порт A-какой нибудь PA13 или PA14 - как правильно в драйвере записать GPIO ?

    серва то работает, а она бы на 400Гц не потянула...

    вот где переключается канал в 50Гц режим
    Код:
    if (init->useServos) {
                // remap PWM9+10 as servos
                if (port == PWM9 )//|| port == PWM10)
                    mask = TYPE_S;
            }
    и отсчёт моторов сдвигается - я закоментировал PWM 10 -порт PA11, отсчёт пошол с него и мотор на нём завёлся, возможно что теперь он не на 400Гц а на 50Гц.

    На сколько начитался, можно на каналах использовать разную частоту, лиш бы основная частота таймера не менялась, а она постоянна независимо на какой частоте шим...
    Последний раз редактировалось SergDoc; 22.09.2012 в 21:25.

  19. #736

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    вот статейка шимы на одном таймере но с разной частотой по каналам.... это я себя успокаиваю - будет погода подлетну посмотрю

  20. #737

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    так вот я и хочу повесить trigger сонара на порт A-какой нибудь PA13 или PA14 - как правильно в драйвере записать GPIO ?
    вылож файлы сонара .c и .h поправлю

  21. #738

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    я впринципе попробовал перекидывать - не вышло
    Код:
    case sonar_pwm56:
            trigger_pin = GPIO_Pin_8;   // здесь номер порта
            echo_pin = GPIO_Pin_9;      // PWM6 (PB9) - 5v tolerant
            exti_line = EXTI_Line9;
            exti_pin_source = GPIO_PinSource9;
            exti_irqn = EXTI9_5_IRQn;
            break;
    -------------------------------------------------------------------------
    // tp - trigger pin 
        GPIO_InitStructure.GPIO_Pin = trigger_pin;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
        GPIO_Init(GPIOB, &GPIO_InitStructure); // сам порт

  22. #739

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    я впринципе попробовал перекидывать - не вышло
    Код:
    // tp - trigger pin 
        GPIO_InitStructure.GPIO_Pin = trigger_pin;
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
        GPIO_Init(GPIOA, &GPIO_InitStructure); // сам порт
    Порт забыл поменять...
    Вложения

  23. #740

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Нет менял, это просто для примера, возможно он где-то задействован в программном (возможно для переключения в cli как в арду или переключение каких-то режимов), но я не нашел и даже упоминаний....

    ааа вот оно чё
    Код:
     last_measurement = current_time;
        distance_ptr = distance;
    
        GPIO_SetBits(GPIOB, trigger_pin);
        //  The width of trig signal must be greater than 10us
        delayMicroseconds(11);
        GPIO_ResetBits(GPIOB, trigger_pin);
    завтра перепишу, а то свой ноут выключил - взял сына (одолжили ноут попользоватся, так теперь сынишка с ним балуется) фильм посмотреть...

    замерял линейкой сонар врёт на сантиметр...

  24. #741

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    На портах А не пошло...

    это порты для отладки через St-Link , их надо где-то переназначить....

    не надо голову дурить, у меня ещё порт B не кончился 12, 13, 14, 15 - тобиш SPI но я его как порядочная св не развёл придётся припаятся пямо к лапе...
    Последний раз редактировалось SergDoc; 23.09.2012 в 02:53.

  25. #742

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    В общем trigger переехал на PA11, моторы на 4-й таймер, серва на 1-м, всё завелось

  26. #743

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от SergDoc Посмотреть сообщение
    всё завелось
    отлично, удержание высоты проверял?

  27. #744

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Я ещё только пытаюсь алт холд Александра (Mahovik) воткнуть, т.к. не пошли прошивки последние, а потом уже вклинивать сонар в IMU буду, он сейчас только показывает высоту и всё, в расчётах не учавствует...

    Ещё идея лезет, от прошлого проекта осталась платка под GPS (можно крепить поверх мелкой на стойках), пожертвовать двумя AUXами - на первом сделать LEVEL, LEVEL+Alt Hold, LEVEL+Alt Hold+Headfree - трёхпозиционник, на втором переключение GPS - удержание позиции и возврат домой, под GPS освободить второй UART...
    Последний раз редактировалось SergDoc; 23.09.2012 в 15:10.

  28. #745

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Новый Алт Холд вклинил, в отличии от Таймкопа - моторы работают, набираюсь смелости и вперёд за сонаром...

  29. #746

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Не знаю, толи мои кривые руки, толи особенности программирования ARM, алгоритм я полностью не осилил, сделал проверки на адекватность сонара непосредственнов IMU, так же Alt Hold теперь работает если активирован сонар (не важно есть он или нет его физически - главное чтоб задефайнен был - надеюсь пока), вобщем всё размазано по всему проекту, даже страшно показывать что есть на данный момент, сейчас разбираюсь с фильтрами на сонаре - надо они, ненадо? С ошибками тоже лопухнулся немного по сей причине пока переключение с барометра на сонар затягивается, ну это поправлю, по крайней мере в GUI уже что-то вменяемое показывает, беспокоит аксель, как бы фильтронуть его по ВЧ - желательно прямо сырые данные, а может и не надо. Вывел в Cli параметры для фильтров сонара - можно баловатся не перешивая (но пока я вообще фильтр отключил чтобы в гуи посмотреть что реально творится) главное сейчас не заленится и продолжить испытания, хотя для меня сейчас уже радость что во первых компилируется, а во вторых довольно вменяемые данные
    ну если добью огрехи, выложу этот страх божий на обозрение

  30. #747

    Регистрация
    22.08.2011
    Адрес
    Калининград
    Возраст
    35
    Сообщений
    945
    Записей в дневнике
    2
    Цитата Сообщение от SergDoc Посмотреть сообщение
    сейчас разбираюсь с фильтрами на сонаре - надо они, ненадо?

    А сонар какой HC-SR04? Все зависит от сонара и его монтажа. Если данные скачут то фильтр нужен хотя он и будет снижать быстродействие, ессено на столовых тестах они могут и не скакать. Товарищ Алексмос добивался хороших результатов без фильтра на вие

  31. #748

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Да HC-SR04 сначала попробую без фильтра, показания похожи на правду, главное над тряпками не летать - даже рубашка глушит сонар полностью
    О, надо над ковром попробовать...

  32. #749

    Регистрация
    11.05.2008
    Адрес
    Великий Новгород
    Возраст
    38
    Сообщений
    3,939
    Записей в дневнике
    22
    при приеме данных сонара кто-нибудь учитывает крен/тангаж?

  33. #750

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,471
    Цитата Сообщение от HikeR Посмотреть сообщение
    при приеме данных сонара кто-нибудь учитывает крен/тангаж?
    А смысл? Сонар не имеет четкой направленности в пределах рабочего сектора. Если только сфокусировать его...

  34. #751

    Регистрация
    11.05.2008
    Адрес
    Великий Новгород
    Возраст
    38
    Сообщений
    3,939
    Записей в дневнике
    22
    и что он покажет при наклоне аппарата больше "рабочего сектора"? дунул ветер при посадке (или висении), аппарат наклонился для компенсации, высота резко "увеличилась", софт обязан сбросить газ.
    в симуляторе (эмуляция сонара с 10° сектором и идеально ровной земли) это приводит к такой колбасне, что после 2-3 раза обязательно последует падение.

  35. #752

    Регистрация
    23.01.2011
    Адрес
    Минск
    Возраст
    46
    Сообщений
    1,345
    Цитата Сообщение от HikeR Посмотреть сообщение
    кто-нибудь учитывает крен/тангаж?
    Что-то, в свое время на мегапирате пробовали

  36. #753

    Регистрация
    17.06.2011
    Адрес
    Минск
    Возраст
    39
    Сообщений
    1,941
    Цитата Сообщение от HikeR Посмотреть сообщение
    и что он покажет при наклоне аппарата больше "рабочего сектора"?
    Реально поможет только повесить сонар на подвес, смотрящий всегда вниз. При большем угле чем 10-15гр - показания будут равны нулю.
    В Ардукоптере (и в мегапирате) есть такая коррекция, но отключена всегда:
    Код:
                #if SONAR_TILT_CORRECTION == 1
                    // correct alt for angle of the sonar
                    float temp = cos_pitch_x * cos_roll_x;
                    temp = max(temp, 0.707);
                    sonar_alt = (float)sonar_alt * temp;
                #endif

  37. #754

    Регистрация
    22.08.2011
    Адрес
    Калининград
    Возраст
    35
    Сообщений
    945
    Записей в дневнике
    2
    Я использую MB1000 у него хорошая площадь пятна наклоны до 15 градусов нормально отрабатываются при половине дальности сонара, больше наклоны не проверял, думаю больше и не нужно. Над ковром работает отлично, над травой фигово.

  38. #755

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Ну вот, что-то похожее на иму
    Код:
    #include "board.h"
    #include "mw.h"
    
    int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
    float accLPFVel[3];
    int16_t acc_25deg = 0;
    int32_t  BaroAlt;
    int16_t  sonarAlt;//to think about the unit
    int32_t  EstSAlt;
    int32_t  EstAlt;             // in cm
    int16_t  BaroPID = 0;
    int32_t  AltHold;
    int16_t  errorAltitudeI = 0;
    float magneticDeclination = 0.0f; // calculated at startup from config
    float accVelScale; //Dobavil
    // **************
    // gyro+acc IMU
    // **************
    int16_t gyroData[3] = { 0, 0, 0 };
    int16_t gyroZero[3] = { 0, 0, 0 };
    int16_t angle[2] = { 0, 0 };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
    
    static void getEstimatedAttitude(void);
    
    void imuInit(void)
    {
        acc_25deg = acc_1G * 0.423f;
        accVelScale = 9.80665f / acc_1G / 10000.0f; //Dobabil
    #ifdef MAG
        // if mag sensor is enabled, use it
        if (sensors(SENSOR_MAG))
            Mag_init();
    #endif
    }
    
    
    void computeIMU(void)
    {
        uint32_t axis;
        static int16_t gyroADCprevious[3] = { 0, 0, 0 };
        int16_t gyroADCp[3];
        int16_t gyroADCinter[3];
        static uint32_t timeInterleave = 0;
        static int16_t gyroYawSmooth = 0;
    
    #define GYRO_INTERLEAVE
    
        if (sensors(SENSOR_ACC)) {
            ACC_getADC();
            getEstimatedAttitude();
        }
    
        Gyro_getADC();
    
        for (axis = 0; axis < 3; axis++) {
    #ifdef GYRO_INTERLEAVE
            gyroADCp[axis] = gyroADC[axis];
    #else
            gyroData[axis] = gyroADC[axis];
    #endif
            if (!sensors(SENSOR_ACC))
                accADC[axis] = 0;
        }
        timeInterleave = micros();
        annexCode();
    #ifdef GYRO_INTERLEAVE
        if ((micros() - timeInterleave) > 650) {
            annex650_overrun_count++;
        } else {
            while ((micros() - timeInterleave) < 650);  // empirical, interleaving delay between 2 consecutive reads
        }
    
        Gyro_getADC();
        for (axis = 0; axis < 3; axis++) {
            gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
            // empirical, we take a weighted value of the current and the previous values
            gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
            gyroADCprevious[axis] = gyroADCinter[axis] / 2;
            if (!sensors(SENSOR_ACC))
                accADC[axis] = 0;
        }
    #endif
    
        if (feature(FEATURE_GYRO_SMOOTHING)) {
            static uint8_t Smoothing[3] = { 0, 0, 0 };
            static int16_t gyroSmooth[3] = { 0, 0, 0 };
            if (Smoothing[0] == 0) {
                // initialize
                Smoothing[ROLL] = (cfg.gyro_smoothing_factor >> 16) & 0xff;
                Smoothing[PITCH] = (cfg.gyro_smoothing_factor >> 8) & 0xff;
                Smoothing[YAW] = (cfg.gyro_smoothing_factor) & 0xff;
            }
            for (axis = 0; axis < 3; axis++) {
                gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1 ) / Smoothing[axis]);
                gyroSmooth[axis] = gyroData[axis];
            }
        } else if (cfg.mixerConfiguration == MULTITYPE_TRI) {
            gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3;
            gyroYawSmooth = gyroData[YAW];
        }
    }
    
    // **************************************************
    // Simplified IMU based on "Complementary Filter"
    // Inspired by http://starlino.com/imu_guide.html
    //
    // adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
    //
    // The following ideas was used in this project:
    // 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
    // 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
    // 3) C. Hastings approximation for atan2()
    // 4) Optimization tricks: http://www.hackersdelight.org/
    //
    // Currently Magnetometer uses separate CF which is used only
    // for heading approximation.
    //
    // Modified: 19/04/2011  by ziss_dm
    // Version: V1.1
    //
    // code size deduction and tmp vector intermediate step for vector rotation computation: October 2011 by Alex
    // **************************************************
    
    //******  advanced users settings *******************
    
    /* Set the Low Pass Filter factor for Magnetometer */
    /* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
    /* Comment this if  you do not want filter at all.*/
    /* Default WMC value: n/a*/
    //#define MG_LPF_FACTOR 4
    
    /* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
    /* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
    /* Default WMC value: n/a*/
    #define GYR_CMPFM_FACTOR 200.0f
    
    //****** end of advanced users settings *************
    
    #define INV_GYR_CMPF_FACTOR   (1.0f / ((float)cfg.gyro_cmpf_factor + 1.0f))
    #define INV_GYR_CMPFM_FACTOR  (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
    
    #define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f))     // 32767 / 16.4lsb/dps for MPU3000
    
    // #define GYRO_SCALE ((2380 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f))     //should be 2279.44 but 2380 gives better result (ITG-3200)
      // +-2000/sec deg scale
      //#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)     
      // +- 200/sec deg scale
      // 1.5 is emperical, not sure what it means
      // should be in rad/sec
    
    typedef struct fp_vector {
        float X;
        float Y;
        float Z;
    } t_fp_vector_def;
    
    typedef union {
        float A[3];
        t_fp_vector_def V;
    } t_fp_vector;
    	t_fp_vector EstG;
    // Rotate Estimated vector(s) with small angle approximation, according to the gyro data
    // Rotate Estimated vector(s) with small angle approximation, according to the gyro data
    void rotateV(struct fp_vector *v, float *delta)
    {
        struct fp_vector v_tmp = *v;
        v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y;
        v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y;
        v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
    }
    
    static int16_t _atan2f(float y, float x)
    {
        // no need for aidsy inaccurate shortcuts on a proper platform
        return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
    }
    
    static void getEstimatedAttitude(void)
    {
        uint32_t axis;
        int32_t accMag = 0;
    	
    	//static t_fp_vector EstG;
    //	static float accLPFVel[3]; // Dobavil
    	
        //static t_fp_vector EstG; // Ubral
        static t_fp_vector EstM;
    #if defined(MG_LPF_FACTOR)
        static int16_t mgSmooth[3];
    #endif
        static float accLPF[3];
        static uint32_t previousT;
        uint32_t currentT = micros();
        float scale, deltaGyroAngle[3];
    
        scale = (currentT - previousT) * GYRO_SCALE;
        previousT = currentT;
    
        // Initialization
        for (axis = 0; axis < 3; axis++) {
            deltaGyroAngle[axis] = gyroADC[axis] * scale;
            if (cfg.acc_lpf_factor > 0) {
                accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor);
                accSmooth[axis] = accLPF[axis];
            } else {
                accSmooth[axis] = accADC[axis];
            }
    				accLPFVel[axis] = accLPFVel[axis] * (1.0f - (1.0f / cfg.acc_lpf_for_velocity)) + accADC[axis] * (1.0f / cfg.acc_lpf_for_velocity); // Dobavil
            accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
    
            if (sensors(SENSOR_MAG)) {
    #if defined(MG_LPF_FACTOR)
                mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
    #define MAG_VALUE mgSmooth[axis]
    #else
    #define MAG_VALUE magADC[axis]
    #endif
            }
        }
        accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
    
        rotateV(&EstG.V, deltaGyroAngle);
        if (sensors(SENSOR_MAG))
            rotateV(&EstM.V, deltaGyroAngle);
    
        if (abs(accSmooth[ROLL]) < acc_25deg && abs(accSmooth[PITCH]) < acc_25deg && accSmooth[YAW] > 0)
            f.SMALL_ANGLES_25 = 1;
        else
            f.SMALL_ANGLES_25 = 0;
    
        // Apply complimentary filter (Gyro drift correction)
        // If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
        // To do that, we just skip filter, as EstV already rotated by Gyro
        if ((36 < accMag && accMag < 196) || f.SMALL_ANGLES_25) {
            for (axis = 0; axis < 3; axis++)
                EstG.A[axis] = (EstG.A[axis] * (float)cfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
        }
    
        if (sensors(SENSOR_MAG)) {
            for (axis = 0; axis < 3; axis++)
                EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
        }
    
        // Attitude of the estimated vector
        angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
        angle[PITCH] = _atan2f(EstG.V.Y, EstG.V.Z);
    
    #ifdef MAG
        if (sensors(SENSOR_MAG)) {
            // Attitude of the cross product vector GxM
            heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z);
            heading = heading + magneticDeclination;
            heading = heading / 10;
    
            if (heading > 180)
                heading = heading - 360;
            else if (heading < -180)
                heading = heading + 360;
        }
    #endif
    }
    
    #ifdef BARO //*BARO+Sonar*//
    // #define NEW_ACCZ_HOLD // Dobavil ne aktiviroval да потому что Альт Нолд только Mahovik, а остальное пристрелил :)
    #define UPDATE_INTERVAL 25000   // 40hz update rate (20hz LPF on acc)
    #define INIT_DELAY      4000000 // 4 sec initialization delay
    //#define BARO_TAB_SIZE   40
    //#define BARO_TAB_SIZE_MAX   48
    //#define ACC_Z_DEADBAND (acc_1G / 50)
    /////////////////////////////////////////////////////////////////
    	//#ifdef NEW_ACCZ_HOLD
    		int16_t applyDeadband16(int16_t value, int16_t deadband)
    		{
    		    if (abs(value) < deadband) {
    	        value = 0;
    		    } else if (value > 0) {
    		        value -= deadband;
    		    } else if (value < 0) {
    	        value += deadband;
    		    }
    		    return value;
    		}
    		
    		float applyDeadbandFloat(float value, int16_t deadband)
    		{
    		    if (abs(value) < deadband) {
    		        value = 0;
    		    } else if (value > 0) {
    		        value -= deadband;
    		    } else if (value < 0) {
    		        value += deadband;
    		    }
    		    return value;
    		}
    		
    		float InvSqrt(float x)
    	{
    		    union {
    		        int32_t i;
    		        float f;
    		    } conv;
    		    conv.f = x;
    		    conv.i = 0x5f3759df - (conv.i >> 1);
    		    return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
    	}
    		
    		int32_t isq(int32_t x)
    		{
    		    return x * x;
    		}
    		/////////////////////////////////////////////////////////////////////////////////
    		
    		
    void getEstimatedAltitude(void)
    		{
    		    //uint8_t index;
    		    static uint32_t deadLine = INIT_DELAY;
    		    static int16_t baroHistTab[BARO_TAB_SIZE_MAX];
    		    static int8_t baroHistIdx;
    		    static int32_t baroHigh;
    		    uint32_t dTime;
    		    int16_t error;
    		    float invG;
    		    int16_t accZ;
    		    static float vel = 0.0f;
    		    static int32_t lastBaroAlt;
    		    float baroVel;
    		    static int16_t SonarHistTab[SONAR_TAB_SIZE_MAX];
    		    static int8_t SonarHistIdx;
    		    static int32_t SonarHigh;
    				static int32_t lastSonarAlt;
    		    float SonarVel;
    			  uint8_t limit = 0.0f;
            static uint8_t sonarerrors = 0;
    			
    		    if ((int32_t)(currentTime - deadLine) < UPDATE_INTERVAL)
    	        return;
    		    dTime = currentTime - deadLine;
    		    deadLine = currentTime;
    		
    		    //**** Alt. Set Point stabilization PID ****
    		     baroHistTab[baroHistIdx] = BaroAlt / 10;
    	       baroHigh += baroHistTab[baroHistIdx];
    	 	     baroHigh -= baroHistTab[(baroHistIdx + 1) % cfg.baro_tab_size];
    		
    		    baroHistIdx++;
    		    if (baroHistIdx == cfg.baro_tab_size)
    		        baroHistIdx = 0;
    		
    		    // EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
    		     EstAlt = EstAlt * cfg.baro_noise_lpf + (baroHigh * 10.0f / (cfg.baro_tab_size - 1)) * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
    		
    		    // P
    		    error = constrain(AltHold - EstAlt, -300, 300);
    	      error = applyDeadband16(error, 10); // remove small P parametr to reduce noise near zero position
    		    BaroPID = constrain((cfg.P8[PIDALT] * error / 100), -150, +150);
    		
    		    // I
    		    errorAltitudeI += error * cfg.I8[PIDALT] / 50;
    		    errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
    		    BaroPID += (errorAltitudeI / 500); // I in range +/-60
    		
    		    // projection of ACC vector to global Z, with 1G subtructed
    		    // Math: accZ = A * G / |G| - 1G
    		    invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
    		    // int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - acc_1G;
    		    // int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - 1/invG;
    		    accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
    		    accZ = applyDeadband16(accZ, acc_1G / cfg.accz_deadband);
    		    debug[0] = accZ;
    		
    		    // Integrator - velocity, cm/sec
    		    vel += accZ * accVelScale * dTime;
    		
    		    //lastBaroAlt = EstSAlt;
    		    baroVel = (EstAlt - lastBaroAlt) / (dTime / 1000000.0f);
    		    baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
    		    baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero
    		    lastBaroAlt = EstAlt;
    		    debug[1] = baroVel;
    				
    		//#ifdef SONAR
         if ((sonarAlt < SONAR_MAX_DISTANCE)&(sonarAlt > SONAR_MIN_DISTANCE)) {
            if(sonarerrors > limit) sonarerrors--;
            else sonarerrors = limit;
           } 
    		 else {
          if(sonarerrors < SONAR_ERROR_MAX)
    				sonarerrors++;;
           } 
    				
    		     SonarHistTab[SonarHistIdx] = sonarAlt;// / 10;
    	       SonarHigh += SonarHistTab[SonarHistIdx];
    	 	     SonarHigh -= SonarHistTab[(SonarHistIdx + 1) % cfg.sonar_tab_size];
    		
    		    SonarHistIdx++;
    		    if (SonarHistIdx == cfg.sonar_tab_size){
    		        SonarHistIdx = 0;
    				}
    		    // EstAlt 
    		     EstSAlt = SonarHigh; //EstSAlt * cfg.sonar_noise_lpf + (SonarHigh * 10.0f / (cfg.sonar_tab_size - 1)) * (1.0f - cfg.sonar_noise_lpf); // additional LPF 
    		
    			   error = constrain(AltHold - EstSAlt, -300, 300);
    			
    				SonarVel = (EstSAlt - lastSonarAlt) / (dTime / 1000000.0f);
    		    SonarVel = constrain(SonarVel, -300, 300); // constrain baro velocity +/- 300cm/s
    		    SonarVel = applyDeadbandFloat(SonarVel, 2); // to reduce noise near zero
    		    lastSonarAlt = EstSAlt;
    		    debug[3] = SonarVel;
    			if(sonarerrors == 0) {	
    				 vel = vel * cfg.sonar_cf + SonarVel * (1.0f - cfg.sonar_cf);
    		  }
    		  else if(sonarerrors> 0 & sonarerrors < SONAR_ERROR_MAX) {
    				  vel = (vel * (SONAR_ERROR_MAX - sonarerrors) + SonarVel * sonarerrors)/SONAR_ERROR_MAX;
    			}
    			else if (sonarerrors >=SONAR_ERROR_MAX) {
    			// Sonar is crasy, 
    			    vel = vel * cfg.baro_cf + baroVel * (1.0f - cfg.baro_cf);
    		  } 
    		//#endif 
    		    // apply Complimentary Filter to keep near zero caluculated velocity based on baro velocity
    		  //  vel = vel * cfg.baro_cf + baroVel * (1.0f - cfg.baro_cf);//vel = vel * 0.985f + baroVel * 0.015f;
    		    // vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
    		   
    			 debug[2] = vel;
    		    // debug[3] = applyDeadbandFloat(vel, 5);
    		
    		    // D
    		    BaroPID -= constrain(cfg.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
    		    //debug[3] = BaroPID;
    		}
    	
    #endif /* BARO+Sonar */
    сильно не пинайте...
    Последний раз редактировалось SergDoc; 26.09.2012 в 16:05.

  39. #756

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Рано выложил - нашел ещё косяков, исправил, теперь по моему будет без разницы - включен сонар, не включен, окончательную версию выложу после испытаний...

  40. #757

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Барометр в Россию уехал

  41. #758

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Ну вот добился чего-то похожего на правду, сонар висит на 2-м(триггер) и 6-м(эхо) моторах имеется ввиду NAZE32, в моей платке расположение моторов другое, код портирован из мультивия http://www.multiwii.com/forum/viewto...1033&start=110, естественно с AltHold от Mahowik.
    Паршивка подходит к NAZE32 без MPU3050 можно использовать только на квадрике без подвеса и трикоптере, ну Y4 должен работать, для подключения квадрика пропускать второй мотор, трикоптер без изменений.
    через Cli включить сонар...
    Последний раз редактировалось SergDoc; 28.09.2012 в 16:25.

  42. #759
    Забанен
    Регистрация
    30.11.2011
    Адрес
    Киев
    Возраст
    36
    Сообщений
    373
    Парочка функций для реализации задержки и отсчета времени в микросекундах. Может пригодится.
    Вложения
    • Тип файла: rar dwt.rar‎ (749 байт, Просмотров: 12)

  43. #760

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,459
    Записей в дневнике
    8
    Цитата Сообщение от SergDoc Посмотреть сообщение
    Паршивка подходит к NAZE32 без MPU3050
    Перед прошивкой ОБЯЗАТЕЛЬНО произвести полное стирание!!!

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

Похожие темы

  1. Система стабилизации гиро+акселерометр
    от Фантомас в разделе Полеты по камере, телеметрия
    Ответов: 32
    Последнее сообщение: 25.01.2011, 14:47
  2. Продам Продам Клона Trex 450SEV2 + Аппаратура + Запчасти+ система стабилизации RTF
    от omegapraim в разделе Барахолка. Вертолеты
    Ответов: 1
    Последнее сообщение: 12.01.2011, 18:16
  3. Продам Трёхосевую систему стабилизации Turnigy V-Bar 600
    от avi@tor в разделе Барахолка. Аппаратура
    Ответов: 1
    Последнее сообщение: 08.11.2010, 13:02
  4. Продам Gaui система стабилизации GU365, дёшево.
    от avi@tor в разделе Барахолка. Вертолеты
    Ответов: 3
    Последнее сообщение: 03.08.2010, 11:13
  5. Системы стабилизации
    от max815 в разделе Фото и видеосъемка, системы стабилизации
    Ответов: 16
    Последнее сообщение: 11.03.2010, 03:14

Метки этой темы

Ваши права

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