Специальные цены   новые товары
+ Ответить в теме
Страница 32 из 165 ПерваяПервая ... 22 30 31 32 33 34 42 ... ПоследняяПоследняя
Показано с 1,241 по 1,280 из 6569

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

Тема раздела Квадрокоптеры. Общие вопросы в категории Квадрокоптеры и мультироторы; Сообщение от SergDoc Вот интересно, а почему правда в NAZE32 4-я ревизия два акселя, а никто не фильтрует их вместе? ...

  1. #1241

    Регистрация
    16.12.2005
    Адрес
    Москва
    Возраст
    34
    Сообщений
    4,452
    Записей в дневнике
    13
    Цитата Сообщение от SergDoc Посмотреть сообщение
    Вот интересно, а почему правда в NAZE32 4-я ревизия два акселя, а никто не фильтрует их вместе?
    Я тут для побочного проекта считал "векторы" вибраций отдельно акселей и отдельно гироскопов (датчик - пресловутый 6050). Дак несовпадение полное получил, что расстроило, понятное дело.
    Причем вибрация - явно ниже 100 Гц, однако суммы квадратов (x^2+y^2) вращались относительно друг друга с изменяющейся частотой! То ли фазовая разница, то ли сам дурак..

    Я к чему это все - насколько можно доверять данным с двух разных датчиков?

  2.  
  3. #1242

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

    Цитата Сообщение от leprud Посмотреть сообщение
    Я к чему это все - насколько можно доверять данным с двух разных датчиков?
    да доверять то нечему и так, а если векторы вибраций разные то в резонанс не должны по идее попасть?

  4. #1243

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,472
    Цитата Сообщение от leprud Посмотреть сообщение
    Причем вибрация - явно ниже 100 Гц, однако суммы квадратов (x^2+y^2) вращались относительно друг друга с изменяющейся частотой! То ли фазовая разница, то ли сам дурак..
    Аксель и ДУС мериют разные физвеличины. Как они могли одинаковые (симфазные) данные показать?

    Цитата Сообщение от leprud Посмотреть сообщение
    Я к чему это все - насколько можно доверять данным с двух разных датчиков?
    Если датчики мериют одно и тоже, либо их приводят к одним величинам, но сами дачики разные ( и исправные), то можно повысить отношение сигнал/шум.

  5. #1244

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Копаюсь в акселе дальше - второй регистр:
    Код:
    #define LSM330_A_HPCF1		0x10	/* High-pass filter cutoff frequency selection	*/
    #define LSM330_A_HPCF2		0x20	/* High-pass filter cutoff frequency selection	*/
    #define LSM330_A_HPCF3		0x30	/* High-pass filter cutoff frequency selection	*/
    выбор частоты среза, но нигде не написано какой?

  6.  
  7. #1245

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

    см. стр. 39, там правда для гиры, но вроде как одинаково с акселем.

  8. #1246

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    у гиры 3 бита у акселя 2?
    Вечером попробовать надо....

  9. #1247

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

    Частота среза и там и там 2 бита, частота дескретизации у акселя 4 бита.

  10.  
  11. #1248

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Не на 39 странице для гиры задаётся и частота дискретизации и частота среза, у акселя в этом регистре только частота дискретизации 29стр. меня же интересует ВЧ фильтр акселя 30стр. у гиры 40 стр. у гиры я не использую хватает первого регистра 39стр. а у акселя такого нет...
    Я сейчас могу задействовать лапу прерывания готовности, BMP выкинул и входы (аппаратные) прерываний свободны, к тому же дорожки около ЛСМ-ки проходят, было бы классно задействовать, хотя тоже самое и программным сделается...
    Последний раз редактировалось SergDoc; 14.02.2013 в 17:42.

  12. #1249
    DVE
    DVE вне форума

    Регистрация
    16.06.2008
    Адрес
    EU
    Возраст
    37
    Сообщений
    4,386
    А Вы не пробовали разобраться, что за данные умеет выдавать MPU6000/6050? Там же на борту какой-то onboard Digital Motion Processor заявлен, судя по описанию.

    onboard Digital Motion Processor™ (DMP™) capable of processing complex 9-axis MotionFusion algorithms. The parts’ integrated 9-axis MotionFusion algorithms access external magnetometers or other sensors through an auxiliary master I²C bus, allowing the devices to gather a full set of sensor data without intervention from the system processor

  13. #1250

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    А я разобрался куда деваются ДУСы при ФС - на этой платке PPM все каналы уходят при выключении передатчика в 1000, что как раз соответствует калибровке их любимых, в Wii по идее тоже самое будет происходить, значит либо переделывать ПО платки, а лучше всё-таки купить нормальные передатчик с приёмником....

  14. #1251

    Регистрация
    17.06.2011
    Адрес
    Минск
    Возраст
    39
    Сообщений
    1,941
    Цитата Сообщение от DVE Посмотреть сообщение
    А Вы не пробовали разобраться, что за данные умеет выдавать MPU6000/6050? Там же на борту какой-то onboard Digital Motion Processor заявлен, судя по описанию.
    Там 6axis а не 9. Кроме того, вун Дидроновцы уже давно пытаются заюзать DMP, но пока не видно результата. Хотя как вторичный AHRS его можно включить в Ардукоптере. В принципе и код можно оттуда передрать если очень надо, то разобраться там без описания будет сложновато.

  15. #1252

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Потанцевал с бубном вокруг акселя: перевёл на 400Гц, 1g=1024, acc_lpf_factor = 80. Дёрганий резких нет, имеется увод на 1.5-2 градуса. АХ (ещё не настраивал) имеют место прыжки 0.5м, при использовании акселя имеются небольшие осцилляции на снижении (с магнитометром ведёт себя чуть получше), в целом - лучше чем было но доверять ещё не стоит (акселю)...
    а на зависть товарищам из ветки Трикоптер. серва быстрая цифровая Operating Speed: 0.07sec.60º/ 0.06sec.60º ведёт себя прилично без осцилляций (стоит как вкопаная в нужном положении)
    Последний раз редактировалось SergDoc; 17.02.2013 в 15:59.

  16. #1253

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,472
    Сергей, несколько впросов. Это ты про ЛСМ330ю? acc_lpf_factor - праметр фильтра ЛСМ или проги? Из 400сот отсчётов все обрабатываешь?
    Я без поднятия частоты отсчетов усреднил вектор от акселя и получил довольно стабильный горизонт, единственный недостаток при нескльких кувырках накаливается статистическая ошибка из-за замеделенной реакции полученного вектора,на самоль не пойдёт, но зато при плавныхполётах никаких завалов, а для самоля не так важна виброустойчивость. Так шта можно игратца коэффициентом усреднения.

  17. #1254

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Да про ЛСМ на 400 наблюдаю меньший шум, что в принципе странно, acc_lpf_factor - простое усреднение (80 - как раз получается как-бы 50 раз в секунду я получаю данные), надо всё-таки пробовать выводить ногу на прерывание и задействовать внутренний фильтр, иначе он не работает...
    сегодня с акселем полетал почти как в акро, так что улучшения явно есть...

    Я тут подумал, гиры я срезал и 25Гц - это когда с сервой боролся - ничего существенно не менялось (в стабилизации), так что можно усреднение на акселе увеличить - попробую.....
    Последний раз редактировалось SergDoc; 17.02.2013 в 16:15.

  18. #1255

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

  19. #1256

    Регистрация
    27.12.2000
    Адрес
    С-Петербург
    Возраст
    41
    Сообщений
    1,747
    А вот кстати, какие скорости снижения коптера являются критическими? Я слышал что на многих контроллерах наблюдаются осцилляции при быстром снижении.. Сегодня пробовал "падать" на трикоптере с сотни метров м, быстрее 10-12 м/с страшно, но осцилляций нет. Ручка газа где-то на 20% газа была. Небольшие осцилляции (градусов 5..10 на глаз, не более) происходят если дать полный газ с целью быстро остановится, осцилляции происходят в момент гашения скорости. Если давать газ плавно, то осцилляций на глаз нет. торможу я метров с 25..

  20. #1257
    DVE
    DVE вне форума

    Регистрация
    16.06.2008
    Адрес
    EU
    Возраст
    37
    Сообщений
    4,386
    Главное кстати при таком "падении" не останавливать винты полностью, а то из-за мешающего потока воздуха они уже не раскрутятся, был опыт, обошлось сломанным лучом.

  21. #1258

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,472
    Цитата Сообщение от serj Посмотреть сообщение
    какие скорости снижения коптера являются критическими? Я слышал что на многих контроллерах наблюдаются осцилляции при быстром снижении..
    Тут как бы много причин можно надумать, но как человек, которого учили (безуспешно) ТАУ 3 года, относительно своего контроллера

    предполагаю выход системы (обЪект управления+система управления) из линейного режима и\или из оптимума для установленных коэффициентов ПИДа. А конкретней: когда многолёт поднимается либо висит ВМГ работают в линейной части свей характиристики, т.е. соблюдается пропорция между процентом управляющего сигнала и тягой, кроме того коптер находится в ламинарном (невозмущённом) потоке воздуха, что тоже поддерживает линейность всей системы. При сниженнии же ВМГ работают в минимальном режиме и чтоб получить адекватную управляющему сигналу (УС) отдачу по тяге необходимо несколько большее время (за которое накапливается большая ошибка в интегральной части ПИД), соответсвенно когда ВМГ выходит на необходмый режим УС уже превышает величину необходимую для компенсации - происходит проскок заданного положения и перерегулирование, кроме того происходит нагнетание воздушной подушки под винтами и рамой, и коптер стремится сползти с неё кренясь то в одну то в другую сторону.
    Если же праметры всей системы не входят в зону колебаний, то явление не наблюдается, и дело здесь не столько в контроллере сколько в ВМГ, весе, геометрии, плотности воздуха и пр.
    Кроме этого можно предпложить что ВМГ упирается в ограничение снизу минимального газа (для слабонагруженных ВМГ).

  22. #1259

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Перечитывал сейчас польский форум http://rc-fpv.pl/viewtopic.php?t=778...er=asc&start=0 тоже на 400 Гц аксель запущен, но по SPI, на сколько сумел разобратся, то прерывания не выведены, блин может у меня просто микруха кривая, ДУСы - то отлично работают...

  23. #1260

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,472
    Цитата Сообщение от SergDoc Посмотреть сообщение
    блин может у меня просто микруха кривая, ДУСы - то отлично работают...

    Что тебя не устраивает? Ты проверял горизонт?

  24. #1261

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    мне датчик выдаёт в спокойном состоянии +-5-7попугаев (датчик откалиброван), а в полёте заваливает на 2-5градусов, если из-за вибраций то я уже всю салатницу развязывал демпферами с рамой (движки и те отбалансированы)....
    на старой железяке гиры были не к чёрту (не удобно с ними работать на F103, 0.67мВ/гр./сек.) и то никаких проблем не было, а тут валится и всё, в акро стики отпускаю - дрейфует по ветру, а как только подключаю аксель, так норовит свалить куда-нибудь.... сейчас правда не так рьяно как раньше - можно обдумать спокойно дальнейшие действия отпустив стики...

  25. #1262

    Регистрация
    22.08.2011
    Адрес
    Калининград
    Возраст
    35
    Сообщений
    947
    Записей в дневнике
    2
    Цитата Сообщение от SergDoc Посмотреть сообщение
    мне датчик выдаёт в спокойном состоянии +-5-7попугаев (датчик откалиброван), а в полёте заваливает на 2-5градусов, если
    А попробуй на тележке покатать мозги дома по полу, туд сюда и по кругу глянь заваливатеся горизонт?

    Цитата Сообщение от serj Посмотреть сообщение
    Небольшие осцилляции (градусов 5..10 на глаз, не более) происходят если дать полный газ с целью быстро остановится
    А если пулей в небо на макс газу с земли такие же осцилляции?
    Скорей всего вибрации просачиваются к мозгам, или их устранять или занижать P завышать D.

  26. #1263

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

  27. #1264

    Регистрация
    27.12.2000
    Адрес
    С-Петербург
    Возраст
    41
    Сообщений
    1,747
    Цитата Сообщение от Razek Посмотреть сообщение

    А если пулей в небо на макс газу с земли такие же осцилляции?
    Скорей всего вибрации просачиваются к мозгам, или их устранять или занижать P завышать D.
    Нет, только небольшой (градуса 2..3) наклон вперед. Газовал неоднократно, запас по тяге двойной, очень весело

    Если спускаться 4..5м/с то осцилляций нет и очень резво останавливается. имеется выраженная нелинейность остановки от скорости (хотя ожидаемо, кинетическая энергия -то в квадрате от скорости, а тяга постоянна)

  28. #1265

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Я совсем запутался, АХ РАБОТАЕТ!!! значит по крайней мере Z акселя работает адекватно и вибрации в пределах нормы, так почему же тянет вперёд и вправо - аксель специально перекалибровывал с наклоном и триммировал, картина такая - подключаю аксель - полетел вперёд и вправо, отключаю встал в горизонт, да блин как буд-то режимы перепутаны, бррр...

  29. #1266

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,472
    Цитата Сообщение от SergDoc Посмотреть сообщение
    же тянет вперёд и вправо
    У меня так же, покажи код ИМУ

  30. #1267

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    ИМУ отсюда: http://code.google.com/p/afrodevices...rc/imu.c?r=265
    Код:
    #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  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;
    
    
    // **************
    // 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;
    
    
    #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
    void rotateV(struct fp_vector *v, float *delta)
    {
        struct fp_vector v_tmp = *v;
    
    
    #if INACCURATE
        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;
    #else
        // This does a  "proper" matrix rotation using gyro deltas without small-angle approximation
        float mat[3][3];
        float cosx, sinx, cosy, siny, cosz, sinz;
        float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
    
    
        cosx = cosf(-delta[PITCH]);
        sinx = sinf(-delta[PITCH]);
        cosy = cosf(delta[ROLL]);
        siny = sinf(delta[ROLL]);
        cosz = cosf(delta[YAW]);
        sinz = sinf(delta[YAW]);
    
    
        coszcosx = cosz * cosx;
        coszcosy = cosz * cosy;
        sinzcosx = sinz * cosx;
        coszsinx = sinx * cosz;
        sinzsinx = sinx * sinz;
    
    
        mat[0][0] = coszcosy;
        mat[0][1] = sinz * cosy;
        mat[0][2] = -siny;
        mat[1][0] = (coszsinx * siny) - sinzcosx;
        mat[1][1] = (sinzsinx * siny) + (coszcosx);
        mat[1][2] = cosy * sinx;
        mat[2][0] = (coszcosx * siny) + (sinzsinx);
        mat[2][1] = (sinzcosx * siny) - (coszsinx);
        mat[2][2] = cosy * cosx;
    
    
        v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
        v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
        v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
    #endif
    }
    
    
    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 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);
            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
    #if INACCURATE
        angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
        angle[PITCH] = _atan2f(EstG.V.Y, EstG.V.Z);
    #else
        // This hack removes gimbal lock (sorta) on pitch, so rolling around doesn't make pitch jump when roll reaches 90deg
        angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
        angle[PITCH] = -asinf(EstG.V.Y / -sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (180.0f / M_PI * 10.0f);
    #endif
        
    #ifdef MAG
        if (sensors(SENSOR_MAG)) {
            // Attitude of the cross product vector GxM
    #if INACCURATE
            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);
    #else
            float rollRAD = (float)angle[ROLL] * RADX10;
            float pitchRAD = -(float)angle[PITCH] * RADX10;
            float magX = EstM.A[1];                         // Swap X/Y
            float magY = EstM.A[0];                         // Swap X/Y
            float magZ = EstM.A[2];
            float cr = cosf(rollRAD);
            float sr = sinf(rollRAD);
            float cp = cosf(pitchRAD);
            float sp = sinf(pitchRAD);
            float Xh = magX * cp + magY * sr * sp + magZ * cr * sp;
            float Yh = magY * cr - magZ * sr;
            float hd = (atan2f(-Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
            heading = hd;                      // magnetic heading * 10
    #endif
            heading = heading + magneticDeclination;
            heading = heading / 10;
    
    
            if (heading > 180)
                heading = heading - 360;
            else if (heading < -180)
                heading = heading + 360;
        }
    #endif
    }
    
    
    #ifdef BARO
    #define UPDATE_INTERVAL 25000   // 40hz update rate (20hz LPF on acc)
    #define INIT_DELAY      4000000 // 4 sec initialization delay
    
    
    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)
    {
        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;
        int16_t accZ;
        static float vel = 0.0f;
        static int32_t lastBaroAlt;
        float baroVel;
        float rpy[3];
        t_fp_vector accel_ned;
    
    
    
    
        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 = 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
        //EstAlt = BaroAlt;
        // 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
    
    
        // the accel values have to be rotated into the earth frame
        rpy[0] = angle[ROLL] * DEG2RAD / 10.0;
        rpy[1] = angle[PITCH] * DEG2RAD / 10.0;
        rpy[2] = heading * DEG2RAD / 10.0;
    
    
        accel_ned.A[0] = EstG.A[0];
        accel_ned.A[1] = EstG.A[1];
        accel_ned.A[2] = EstG.A[2];
        rotateV(&accel_ned.V, rpy);
        accZ = accel_ned.A[2] - acc_1G;
    //    invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
    //    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;
    
    
        if ( 0 == accZ) {
            vel = 0.0;
        }
        else {
            // Integrator - velocity, cm/sec
            vel += accZ * accVelScale * dTime;
        }
    
    
        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;
    
    
        // 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 = 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 */
    магнитометр отключил для чистоты эксперимента, но такое чувство что 0 смещён по осям X и Y потому что влево не досчитывает 1g =950 примерно, вправо пересчитывает 1050, тоже самое и по тангажу, как буд-то плата криво стоит....
    надо наверно отдохнуть немного, уже скоро закажу большие платы, а ещё леопардов зелёных развелось дома (дети ветрянку подхватили) и доче сегодня уже год!!!

    я тут подумал, надо оси переназначит и плату развернуть, посмотреть что будет.....

  31. #1268

    Регистрация
    07.04.2012
    Адрес
    Брянск
    Возраст
    29
    Сообщений
    1,675
    Записей в дневнике
    6
    Цитата Сообщение от SergDoc Посмотреть сообщение
    ИМУ отсюда
    хм.... а если в порядке чистоты эксперимента попробовать чисто виевское ИМУ? (без оптимизированных флоат вычислений и прочих доработок что вижу тут присутствуют)
    т.к. у меня все вери гуд пашет.

    П.С. дефайн INACCURATE включен?

  32. #1269

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Цитата Сообщение от mataor Посмотреть сообщение
    дефайн INACCURATE включен?
    выключен... но надо перепроверить...

  33. #1270

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,472
    Цитата Сообщение от SergDoc Посмотреть сообщение
    доче сегодня уже год!!!
    С имениннецей!)
    Цитата Сообщение от SergDoc Посмотреть сообщение
    а ещё леопардов зелёных развелось дома (дети ветрянку подхватили)
    Это не страшно, чем раньше переболеют тем лучше.
    Цитата Сообщение от SergDoc Посмотреть сообщение
    уже скоро закажу большие платы,
    Это под МПУ6000 с СПИ?

    А в коде ничё не понял

  34. #1271

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Цитата Сообщение от rual Посмотреть сообщение
    Это под МПУ6000 с СПИ?
    ну да, я тут чё подумал - откалибровать платку а потом наклонить слегка - у меня это легко сделать...

    Цитата Сообщение от rual Посмотреть сообщение
    А в коде ничё не понял
    ну да я тоже на чём летаю?

  35. #1272

    Регистрация
    22.08.2011
    Адрес
    Калининград
    Возраст
    35
    Сообщений
    947
    Записей в дневнике
    2
    Цитата Сообщение от SergDoc Посмотреть сообщение
    как буд-то плата криво стоит....
    Может в процедуре калибровки, какой косяк? Биасы неправильно подсчитываются. Есть возможность видео записать с гуев с симптомами?

  36. #1273

    Регистрация
    07.04.2012
    Адрес
    Брянск
    Возраст
    29
    Сообщений
    1,675
    Записей в дневнике
    6
    Цитата Сообщение от SergDoc Посмотреть сообщение
    выключен...
    при включенном проверьте - тогда код макс приближен к виевскому

  37. #1274

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Выкинул в дебаг сырые данные с акселя по осям, и что 1g 3950 в среднем(за 4000 не заходит), а не 4096 как по правильному Wii-ному да у меня дома аномалия

  38. #1275

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Не могу разобраться:
    Код:
    void baroKalmanfilterStepUnstable(int32_t *baro)
    {
            static int32_t _lastTime = 0;
            float pressure = *baro; // float приравниваем к int ?
            uint32_t currentTime = micros();
            float dt = (currentTime - _lastTime) * 1e-6;  // тут тоже всегда целочисленное получится
            _lastTime = currentTime;
    
            static int _init = 0;
            if (!_init)
            {
                    _init = 1;
                    est[0] = pressure;
    
                    return pressure; // вернуть float в int ?
            }
    компилятор ругается что не может вернуть значение?

  39. #1276

    Регистрация
    07.04.2012
    Адрес
    Брянск
    Возраст
    29
    Сообщений
    1,675
    Записей в дневнике
    6
    хм... насколько помню ф-я в С должна быть того же типа, что и возвращаемое значение, а тут у вас функция неопределенного типа void

  40. #1277

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    у чувака компилятор какой-то интересный
    Цитата Сообщение от SergDoc Посмотреть сообщение
    static int _init = 0;
    в цыкле переваривает, и по ходу всё остальное тоже....

    Цитата Сообщение от SergDoc Посмотреть сообщение
    float dt = (currentTime - _lastTime) * 1e-6;
    а тут всегда ноль (ну если значение currentTime - _lastTime не с 6-ю нолями в конце получается) если моим компилятором пользоваться ибо обе переменные целочисленные, и таких бяк очень много, (и с акселем) по этому и не летит нормально наверно, но ДУСы то работают отменно....

    ага
    eclipse

    таак надо как-то всё переносить?
    Последний раз редактировалось SergDoc; 20.02.2013 в 11:49.

  41. #1278

    Регистрация
    01.11.2010
    Адрес
    Belarus Slonim
    Возраст
    36
    Сообщений
    4,462
    Записей в дневнике
    8
    Может правда обозвать эти все файлы (с Калманом которые) .срр ?

  42. #1279

    Регистрация
    19.04.2010
    Адрес
    Ханты
    Возраст
    40
    Сообщений
    1,472
    Цитата Сообщение от SergDoc Посмотреть сообщение
    Может правда обозвать эти все файлы (с Калманом которые) .срр ?
    Обзови, проблема с внутренним объявлением переменных решится. А насчёт функции в void bla_bla(){
    //должон быть просто
    return;
    // без переменной
    }

  43. #1280

    Регистрация
    22.08.2011
    Адрес
    Калининград
    Возраст
    35
    Сообщений
    947
    Записей в дневнике
    2
    А у первоисточника, под какой компилятор? Сталкивался, что из-за того, что не аккуратно используют типы в коде, разные версии одного и того же компилятора приводят к непредсказуемым результатам.

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

Похожие темы

  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

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

Ваши права

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