Создание собственной системы стабилизации
А можно ли перед #ifdef SONAR вставить
EstAlt = EstAlt*cfg.baro_noise_lpf + (baroHigh*10.0f/(cfg.baro_tab_size - 1))*(1-cfg.baro_noise_lpf);
ну если нет сонара?
так кажется придумал корректировку баро оффсета в полете 😃
int32_t tempAlt = baroHigh*10.0f/(BARO_TAB_SIZE - 1);
if(!f.ARMED) { //init offset till motors not armed
EstAltStart = EstAltStart*0.8f + tempAlt*0.2f;
}
debug[0] = EstAltStart;
#if defined(SONAR) && defined(SONAR_BARO_FUSION)
//mix baro/sonar
debug[1] = sonarAlt;
if(EstAltStart != 0) {
if(sonarAlt < SONAR_BARO_FUSION_LC) {
tempAlt = EstAltStart + sonarAlt;
} else if(sonarAlt < SONAR_BARO_FUSION_HC) {
// make smooth EstAltStart correction because baro have drift
EstAltStart = EstAltStart*0.95f + (tempAlt-sonarAlt)*0.05f;
float fade = SONAR_BARO_FUSION_RATIO;
if(fade == 0.0) {
fade = (SONAR_BARO_FUSION_HC-sonarAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
}
fade = constrain(fade, 0.0, 1.0);
debug[2] = fade;
tempAlt = (EstAltStart + sonarAlt)*fade + tempAlt*(1 - fade);
}
}
#endif
EstAlt = EstAlt*0.6f + tempAlt*0.4f; // additional LPF to reduce baro noise
debug[3] = EstAlt;
у меня SONAR_BARO_FUSION_LC=1.5м и SONAR_BARO_FUSION_HC=2.5м
т.е. корректировать можно находясь в этом диапзоне, т.к. баро до 1…1.5м врет, а выше 2.5…3.5м уже не видит сонар, т.е. корректировку делаем через “мягкий” фильтр:
// make smooth EstAltStart correction because baro have drift
EstAltStart = EstAltStart*0.95f + (tempAlt-sonarAlt)*0.05f;
Блин вчера дочка проснулась расплакалась всех разбудила, пришлось бежать, а не будет он постоянно считать отсюда?
else if(sonarAlt < SONAR_BARO_FUSION_HC) {
может всётаки лучше как раньше?
else if(sonarAlt>=SONAR_BARO_FUSION_LC && sonarAlt <SONAR_BARO_FUSION_HC) {
AVR, по моему, всё равно проверит все условия, если даже первое выполнилось?
так кажется придумал корректировку баро оффсета в полете
попробовал вроде работает 😃 только погоды нет 😦
// **** 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;
tempAlt = baroHigh*10.0f/(cfg.baro_tab_size - 1);
if(!f.ARMED) { //init offset till motors not armed
BaroHome = BaroHome*0.9f + tempAlt*0.1f; // play with optimal coef. here
debug[0] = BaroHome;
}
#if defined(SONAR) && defined(SONAR_BARO_FUSION)
//mix baro/sonar
debug[1] = sonarAlt;
if(BaroHome != 0) {
if(sonarAlt>=SONAR_MIN && sonarAlt<SONAR_MAX){
if(sonarAlt < SONAR_BARO_FUSION_LC) {
tempAlt = BaroHome + sonarAlt;
} else if(sonarAlt>=SONAR_BARO_FUSION_LC && sonarAlt<SONAR_BARO_FUSION_HC) {
// make smooth EstAltStart correction because baro have drift
BaroHome = BaroHome*0.95f + (tempAlt-sonarAlt)*0.05f;
fade = SONAR_BARO_FUSION_RATIO;
if(fade == 0.0) {
fade = (SONAR_BARO_FUSION_HC-sonarAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
}
fade = constrain(fade, 0.0, 1.0);
debug[2] = fade;
tempAlt = (BaroHome + sonarAlt)*fade + tempAlt*(1 - fade);
}
}
}
#endif
EstAlt = EstAlt*cfg.baro_noise_lpf + tempAlt*(1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
Ну что, оставить или ещё помучать?
Ну значит оставляю, и начинаю делать верхнюю…
del
А я прилетел на чесном слове и на одном крыле, зарядил батарейку собирался выходить снова, а у меня правый проп треснут до середины😵, и крашей сегодня не было, пронесло однако, только пропы то кончились, поставил 10/6 пока, не знаю что будет но других нету…
попробовал вроде работает только погоды нет
Странно у меня не летает толком 😃
Я решил отказаться от этого алгоритма вообще, т.к. он для идеальних условий похоже и начал переделивать на error-based… Потом посмотрел на алгоритм от alexmos. То что надо! Сейчас занимаюсь его интеграцией. Там учитиваются, превышения таймингов измерений и вообще весь алгоритм на основе кол-ва ошибок и чистых чтений по сонару и соот-но переход от баро к сонару тоже error-based, что есть правилно…
и вот тут ошибка, по крайней мере на меге это всегда =0… т.е. переход сонар-баро не работал, а код этот вроде из арду, мот и там ошибка 😃
fade = (SONAR_BARO_FUSION_HC-sonarAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
надо к флоату привести… проверь в дебаге на стм-ке…
fade = ((float)(SONAR_BARO_FUSION_HC-sonarAlt))/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
вот и просадка по высоте… проверю, только на пропах 10/6 это жесть, будет висение где-то на 75% газа…
Потом посмотрел на алгоритм от alexmos. То что надо! Сейчас занимаюсь его интеграцией. Там учитиваются, превышения таймингов измерений и вообще весь алгоритм на основе кол-ва ошибок и чистых чтений по сонару и соот-но переход от баро к сонару тоже error-based, что есть правилно…
я пробовал, но как всегда не с того конца залез 😃 … буду пытатся тоже…
надо к флоату привести… проверь в дебаге на стм-ке…
так работает только чтоб увидеть debug[2] = fade*10.0f; от 0 до 9 (0 - 0.9999…)
так работает только чтоб увидеть debug[2] = fade*10.0f; от 0 до 9 (0 - 0.9999…)
ну я на 100 умножал, т.е. на avr-ке 100% идет округление до int, потому надо числитель во флоат насильно преобразовать… кроче это уже оссобенности компилероВ…
Не я имел ввиду, что когда переделал то считать начало, а так тоже не считало, но к сожалению на 10-х пропах протестить не смог, прелесть трёхи в том что можно однонаправленные пропы ставить, сейчас этим и занимаюсь, надо только tri_yaw_middle подправить, вчём прелесть cli не надо перепрошивать 😃
В общем теперь такая картина - коптер плавает от 0 до 2.5м, такое чувство что сонар сдох и только у самой земли толкается, а выше 2.5м аппарат упирается в баро 😦 где-то ещё бяка…
А над какой поверхностью тестируете сонар?
этот раз над асфальтом и в полный штиль…
Как с сонара данные снимаются измерением ширины импульса или с ADC пина? Если второе ставьте RC или LC фильтр на питание сонара.
всё, пока опыты прекращаю - кончились пропы, остановился правый движек (причин не знаю), вошол в планету ввер лапами метров с пяти в асфальт, минус все пропы, подшипник на заднем моторе (есть чем заменить) и серва 😦
блин прямо злой рок какой-то, это ж я вчера с этого движка треснутый проп снял блин, как всё равно суждено ему было грохнутся… я злой…
Погода отличная, пропы только 10, т.е. газ висения где-то на 75%, а летать то хочется, из опыта Multiwii кто-нибудь подскажет - если я увеличу MinThrottle, это мне что - нибудь даст?
Неее, с таким весом и на 700-х движках 10-е пропы ну никак, отлетал батарейку и ладно, а ещё поползал в сантиметре над асфальтом-сонар на такой высоте держит отлично, даже прикольно наблюдать - аппарат соскальзывает с экрана, а не поднятся, не опустится не может - гы-гы как на льду катается:)
10-е пропы ну никак
Поставь по два и закрепи стяжками
Больше нету, это остатки 😦 всё нормально, сейчас надо другим заниматься, платки до ума довести, полетать ещё успею 😃
Не пойму я чего-то, гиры в акро держат просто супер - можно аппарат подвесить и идти курить 😃 , а вот аксель дурить начал - сваливает аппарат через несколько секунд (движки, пропы отбалансированы, мосх на паралоне) может пробовать жестко на раму мосх зацепить? чёт мне кажется т.к. газ висения сместился, мосх в резонанс какой-то попадает из-за слишком мягкого демпфера?
На новом контроллере решил вернуть баро на SPI, и флешку мелкую вернуть, вдруг захочется CC3D спиратить, так что переделываю всё заново…
Как с сонара данные снимаются измерением ширины импульса или с ADC пина? Если второе ставьте RC или LC фильтр на питание сонара.
Ничего, если за Сергея отвечу? В том коде, что я видел, ширину времени меряет. Решение с АЦП было бы более красивым, если б не необходимость дергать триггер сонара…
Вы такой режим с ардуинским сонаром как реализовали?
Не пойму я чего-то, гиры в акро держат просто супер - можно аппарат подвесить и идти курить , а вот аксель дурить начал - сваливает аппарат через несколько секунд (движки, пропы отбалансированы, мосх на паралоне) может пробовать жестко на раму мосх зацепить?
У меня аксель не сваливает, но горизонт уводит в сторону. Хочу понять как калман борется с тряской…
Как с сонара данные снимаются измерением ширины импульса или с ADC пина? Если второе ставьте RC или LC фильтр на питание сонара.
Извиняюсь прохлопал вопрос…