Search in topic

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

Кста, Александр, покажи скрины с идикатора, хочу посмотреть какой вектор кажет магнитометр.

отлично компас отрабатывает! никуда не скачет при наклонах… и это без калибровки? точненькА юг север запад восток показывает!
я так понимаю у тебя автокалибровка компаса сделана? т.к. на команду “c” по идее только гира и аксель калибруется…

только один баг странный, иногда гуй подвисает на 1-3 сек, потом оклемается и дальше пашет…

Ты получается ближе меня к “магнитному пупу” Земли, судя по вектору )))

отлично компас отрабатывает! никуда не скачет при наклонах… и это без калибровки?

Да, это без калибровки. Особо не вижу в этом смысла, по кругу расхождение в максимумах единицы градусов.

я так понимаю у тебя автокалибровка компаса сделана? т.к. на команду “c” по идее только гира и аксель калибруется…

нет, автокалибровки нет. Пробовал делать, но мне не понравилась, т.к. случайные выбросы смещают центры и масштаб осей. Клибровку компаса надо делать в услових близким к идельным, но как писал выше смысла не вижу… По команде “с” делается клибровка ДУС в части постоянных дрейфа осей, калибровка акселя тут не правильная (это не страшно если оси пропорциональны и отцентрованы), выполняется только замер средней магнитуды вектора G. Нужна была для экспериментов с измерением акселем линейных перемещений.

только один баг странный, иногда гуй подвисает на 1-3 сек, потом оклемается и дальше пашет…

Проблема в функциях millis() и micros(), надо заменить их на это hw_config.c

__IO uint16_t Sys_time_ms;
__IO uint8_t SysTime_256;
uint16_t millis(void)
{
 Sys_time_ms = (((uint32_t)~SysTick->VAL>>16)&0x0ff)|((uint16_t)SysTime_256)<<8;
 return Sys_time_ms;
}

hw_config.h

#define SysTick_Reload (uint32_t)0x00ffffff
#define micros() ((uint16_t)~SysTick->VAL>>3)
extern __IO uint16_t Sys_time_ms;
extern __IO uint8_t SysTime_256;
uint16_t millis(void);

stm32f30x_it.c

void SysTick_Handler(void)
{
 if (inLoop){
  /* àâàðèéíûé çàïðîñ ÄÓÑà */
  if (GPIO_ReadInputDataBit(L3GD20_SPI_INT2_GPIO_PORT,L3GD20_SPI_INT2_PIN)==Bit_SET)
   if(EXTI_GetITStatus(L3GD20_SPI_INT2_EXTI_LINE)==RESET)
    if (cntDUSRdy--==0){
       cntDUSRdy=5;
       DUS_Zapros();
    }
  /* àâàðèéíûé çàïðîñ àêñåëÿ */
  if (GPIO_ReadInputDataBit(LSM303DLHC_I2C_INT1_GPIO_PORT,LSM303DLHC_I2C_INT1_PIN)==Bit_SET)
   if(EXTI_GetITStatus(LSM303DLHC_I2C_INT1_EXTI_LINE)==RESET)
    if (cntAccRdy--==0){
     cntAccRdy=20;
     Acc_Zapros();
    }
 }
 /* ñèñòåìíîå âðåìÿ */
 SysTime_256 ++;
 Sys_time_ms = (((uint32_t)~SysTick->VAL>>16)&0xff)|((uint16_t)SysTime_256)<<8;
}

Кто нибудь знает, какие есть критерии при выборе частоты LPF фильтра гиры и акселя?
Я пока только одно упоминание увидел более-менее вразумительное, в исходниках ArduCopter’a: “Частота LPF выбирается в два раза меньше SAMPLE Rate”

“Частота LPF выбирается в два раза меньше SAMPLE Rate”

Это классика, следствие из теоремы Котельникова.
Вполне справедливо, нужно чтобы отсчёты не “пролетели” мимо обработки.

Это классика, следствие из теоремы Котельникова. Вполне справедливо, нужно чтобы отсчёты не “пролетели” мимо обработки.

Как теорему Котельникова применить к обработке Аналогово сигнала я знаю.
Sample Rate тут по всей видимости никаким боком (Это частота на которой обновляются регистры с выходными данными). Потому что гира на самом деле оцифровывается с частотой 1KHz или 8KHz (для MPU6000). Если я правильно понимаю, LPF отсеивает вибрации ниже определенной частоты. Вопрос только в том, какие частоты в виде помех обычно бывают на коптере. Отсюда видимо и надо плясать при выборе LPF (и возможно HPF).

Надо попробовать написать тулзу, для вывода графика FFT и подбора значений фильтра по нему. Причем при работающих моторах…

при работающих моторах…

моторы обычно не вращаются с одной и той же скоростью, особенно когда их несколько ;)

а где вы видели озвученный коментарий про частоту, не ткнете?

а где вы видели озвученный коментарий про частоту, не ткнете?

AP_InertialSensor_MPU6000.cpp:

    // sample rate and filtering
    // to minimise the effects of aliasing we choose a filter
    // that is less than half of the sample rate

моторы обычно не вращаются с одной и той же скоростью, особенно когда их несколько 😉

Логично, поэтому надо строить график при Throttle от нуля до 100%.

AP_InertialSensor_MPU6000.cpp:

о, эту штуку недавно добавили только в ArduPlane, но это скорее обычное усреднение, чем какой-то специальный ФНЧ или ФВЧ, суммируем N значений и делим на N. для самолетов, мол, хватает.

строить график при Throttle от нуля до 100%.

это если мотор один, а как складывать гармоники будете от четырех-восьми работающих вразнобой?

p.s.
все-таки почитал вдумчивее, это действительно LPF, который имется в MPU6000 (цифровой встроенный), invensense.com/mems/gyro/…/RM-MPU-6000A.pdf страница 13-14. судя по временной задержке ничего сложнее усреднения там нет.

это если мотор один, а как складывать гармоники будете от четырех-восьми работающих вразнобой?

Вразнобой как раз и будет интересно анализировать, т.к. они будут резонировать на каких то частотах, где один работающий мотор не даст вообще никаких помех.
Т.е. в теории будет так: “плавно поднимаем газ от 0 до 100, при этом на графике FFT отмечаются максимумы, после этого меняем частоты LPF и HPF и повторяем тест до тех пор, пока не избавимся от помех от моторов”.

Кто нибудь знает, какие есть критерии при выборе частоты LPF фильтра гиры и акселя?

в вие я на этом собаку сьел! 😃

внутренние LPF:

1. гира 42гц - если для умеренных и быстрых полетов
   гира 20гц - для авто полетов, фпв и начнающих летунов
   Не смотря на довольно низкие значения скорости гир вполне хватает для четкой стабилизации и отброса вибраций…
2. аксель 5-10гц - при условии что внутреннее усреднение сделано по нормальному в чипе, т.е. при понижении LPF не уменьшается самплиннг… т.к. к примеру в какашечном adxl345 сделано именно уменьшением самплинга…

внешние/алгоритмические LPF:

1. гира - не трогаем 😃
2. аксель - душим на 0.1-0.5гц, т.е. оставляем его скорость сходной со скоростью “корректора”… в вие это компл. фильтр, в арду dcm, но сути не меняет…

1. гира 42гц - если для умеренных и быстрых полетов гира 20гц - для авто полетов, фпв и начнающих летунов Не смотря на довольно низкие значения скорости гир вполне хватает для четкой стабилизации и отброса вибраций… 2) аксель 5-10гц - при условии что внутреннее усреднение сделано по нормальному в чипе, т.е. при понижении LPF не уменьшается самплиннг… т.к. к примеру в какашечном adxl345 сделано именно уменьшением самплинга…

Эммм… так вроде в MPU, LPF выставляется одновременно для Гиры и Акселя.
Посмотрим какие будут отзывы о мерапирате 2,9 - там можно выбрать частоту LPF…

еще оч. важный параметр акселя это внутр. диапазон акселя… нужно брать 8-16g, иначе от вибраций и внутренних резонансов на этих самых вибрациях может происходить перенасыщение внутреннего АЦП акселя и тогда уже никакие внутр/внешн. фильтры и матеметика не помогут… типа как синусоида режется в меандр с диким расширением спектра гармоник…
в вие пока не доперли до этого, мучались с дрейфами в став/левел моде ибо “на столе” все пахало, а полете всплывало…

Эммм… так вроде в MPU, LPF выставляется одновременно для Гиры и Акселя.

ну да, я тоже задавался этом вопросом, но в даташит так и не залез… оставил на 42-20гц + внешний НЧ для акселя…

при понижении LPF не уменьшается самплиннг

неужто есть такие волшебные датчики??

нужно брать 8-16g

вот никогда не понимал этого. если аксель показывает цифры больше 2g, то его показания вобще можно не учитывать, а увеличение диапазона автоматом уменьшает разрешение. ну и если на аппарате идут вибрации больше 2g, то тут уже надо напильник брать, а не фильтровать.

вот никогда не понимал этого. если аксель показывает цифры больше 2g, то его показания вобще можно не учитывать

при переполнении плюс на минус поменяется 😦

вот никогда не понимал этого. если аксель показывает цифры больше 2g, то его показания вобще можно не учитывать, а увеличение диапазона автоматом уменьшает разрешение. ну и если на аппарате идут вибрации больше 2g, то тут уже надо напильник брать, а не фильтровать.

я не зря написал про внутренние резонансы самого акселя… дело скорее всего именно в них, т.к. тоже недоумевали откуда вибрации могут быть в таких кол-вах G…
но это уж факт, проводили тесты, спектр. состав смотрели и т.д… я делился подобными мыслями с vis.asta, так он подвердил тоже самое, что допер до этого уже год назад и у него 8g… а как у него летает, думаю все видели 😃
что до падения разрешения, то в норм. акселях даже при 8-16g это от 512 и выше, т.е. с головой т.к. нелинейность самих осей даст больше погрешности в разы…

плюс на минус поменяется

выкопал валяющийся CC3D, как мог колотил по столу, но увидел только “полочку” упирающуюся в дефолтные 8G. может все-таки предусмотрена хоть какая-то защита? или надо перешить на менее низкую чувствительность?

а как у него летает, думаю все видели

вот честно скажу, “как” - неинтересно, интересно “почему”.

Посмотрим какие будут отзывы о мерапирате 2,9 - там можно выбрать частоту LPF…

раньше кстати в арду 3g для bma180 выбрано было… посмотри что сейчас…

выкопал валяющийся CC3D, как мог колотил по столу, но увидел только “полочку” упирающуюся в дефолтные 8G.

калотить не надо 😃, это не то… отруби внутренний НЧ акселя, выставь самплинг на макс., вруби моторы и скорми данные FFT… увидишь много нового 😉, на некоторых полках газа от 30% и выше всплывают резонансы до 4g и это без пропов! видимо ВЧ вибрации моторов оч. хорошо совпадают с размерами и конструктивными особенностями мемс акселей в плане резонансов…

так может просто не нужно отключать фильтры и мучаться с максимальной частотой, если и так все работает?

да, мотор у меня один, вертолетный 😉 только “трансмиссия” вносит кучу бякости, сам мотор ~40K, основной ротор ~3К плюс хвостовой ротор ~14К (К = 1000об/мин). то есть получается связка ~660/50/230 Гц в всех 3-х плоскостях, цеплял когда-то пару мемс-микрофонов и писал с них на обычный диктофон, там такая каша, что просто удивительно как вобще может что-то работать.

так может просто не нужно отключать фильтры и мучаться с максимальной частотой

так фильтры никто не отключает… отключать, это если полный спектр гармоник надо глянуть…

если и так все работает?

не работает в том и дело… тут, как писал выше, именно в искажении исходного сигнала внутренним АЦП, когда синусоуда режется ограничением диапазона акселя…
т.е. смотри, внутри мемс цифр. акселя: аналог. аксель => АЦП => цифровой НЧ фильтр… так искажение происходит уже после АЦП и далее данные уже не верные как не фильтруй (по осям где переполнение произошло)…

я пользовал VibAnalyser… плюс в том, что вибрации меряем самим полетным контроллером, т.е. по сути тем же сенсором на котором летаем…
если есть ардуина или контроллер на меге то вот:
rcopen.com/forum/f134/topic224135/199
rcopen.com/forum/f134/topic224135/218

им же и моторы пробовал балансить… все отлично видно…

upd: вот кстати нашел всю историю как это было… читать отсюда + много страниц 😃

я пользовал VibAnalyser… плюс в том, что вибрации меряем самим полетным контроллером, т.е. по сути тем же сенсором на котором летаем…
если есть ардуина или контроллер на меге то вот:
Виброизоляция, виброразвязка и демпфирование. Балансировка.
Виброизоляция, виброразвязка и демпфирование. Балансировка.

им же и моторы пробовал балансить… все отлично видно…

О… когда-то видел… потом доолго искал но найти не мог… Опять возродилась идея добавить ее в виде модуля к коду вия (как уже добавил модуль прошивки регулей ESC_USBLinker)

Ну понаписали )))

Как теорему Котельникова применить к обработке Аналогово сигнала я знаю. Sample Rate тут по всей видимости никаким боком (Это частота на которой обновляются регистры с выходными данными)

На самом деле всё связано со всем:) Ща попробую объяснить, берём данные гиры, которая формирует их с частотой 200Гц, а мы получаем и обрабатываем их с частотой 50Гц. Получается мы получаем только каждый 4й отсчёт. Что мы получим при интегрировании? Мы каждый раз пропускаем 3и отсчёта в течении которых может быть “горбик”, либо “провал”. Соответственно в первом случае при интегрировании мы получим “недокрут”, в последнем “перекрут”. А может быть ещё хуже, допустим гира дает гармонический сигнал кратный частоте обработки, и мы можем в данном случае получить попадание либо в максимумы, либо в минимумы. В первом случае будет постояное вращение в одну сторону, в последнем в другую, а на самом деле имеется гармоническая вибрация со средней линией в нуле. Более сложные ситуации сами себе нафантазируйте (например частоты биений). Т.е. чтобы правильно интегрировать сигнал, нам нужна его полная форма. Увидели Котельникова?
Для “абсолютных” датчиков это не так важно, т.е компас будет показывать примерно в одном направлении, ну немножко будет уплывать мат.ожидание (тоже самое с ГПС). А вот если аксель используем для быстрого получения верт. скорости, то этот момент тоже крайне важен, а для горизонта не очень:)