Создание собственной системы стабилизации
Сделал “примерку”, как и ожидалось автотрассировщик заткнулся на 67% хотя очевидно, что плата разведётся, придётся помучатся только с двумя SPI - двухслойке быть 😃 это радует, надо попробовать “Топором” развести, хотя лучше по старинке в ручную - долго, но зато как хочется так и делается, плюс контроль самостоятельный 😃
49-ю лапу надо через кондёр 2.2Мкф не забыть на корпус пустить, а то я как-то с горяча её прямо на корпус повесил…
Полетал с утра, минус проп - на посадке, ветер сильный, нервный тик вроде вылечил, проблема другого плана, на сонаре как прибитый высоту держит даже на экране и в ветер, а вот сонар + баро и баро не пошли есть периодические “выстреливания” вверх и обратно спускатся не желает - выстрелил и висит на той высоте, опять выстрелил и опять висит, вобщем думаю надо переделывать алгоритм, и оба устройства привязывать к земле (старту) - буду пробовать…
в общем сделал что-то такое:
// **** 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;
#ifdef SONAR //&& defined(SONAR_BARO_FUSION)
debug[0]=EstAlt;
//debug[1]=baroHigh / 10;
debug[1] = BaroHome;
if(!f.ARMED) { //init offset till motors not armed
BaroHome = BaroHome*0.9f + (baroHigh*10.0f/(cfg.baro_tab_size))*0.1f; // play with optimal coef. here
SonarHome = sonarAlt;
}
//mix baro/sonar
if(BaroHome!=0) {
baroHigh = baroHigh-BaroHome/10.0f;
SonarPostAlt = sonarAlt -SonarHome;
if(sonarAlt<SONAR_BARO_FUSION_LC) {
EstAlt = EstAlt*cfg.baro_noise_lpf + SonarPostAlt*(1.0f - cfg.baro_noise_lpf);
debug[2] = BaroHome+sonarAlt;
} else if(SonarPostAlt>=SONAR_BARO_FUSION_LC && SonarPostAlt<SONAR_BARO_FUSION_HC) {
float fade = SONAR_BARO_FUSION_RATIO;
if(fade==0.0) fade = (SONAR_BARO_FUSION_HC-SonarPostAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
fade = constrain(fade, 0.0, 1.0);
EstAlt = EstAlt*(1.0f - cfg.baro_noise_lpf) + ((SonarPostAlt)*fade + (baroHigh*10.0f/(cfg.baro_tab_size - 1))*(1-fade))*(1-cfg.baro_noise_lpf);
} else {
EstAlt = EstAlt*cfg.baro_noise_lpf + (baroHigh*10.0f/(cfg.baro_tab_size - 1))*(1-cfg.baro_noise_lpf); // additional LPF to reduce baro noise
}
}
#elif (sonarAlt<SONAR_MIN)||(sonarAlt>SONAR_MAX)
EstAlt = EstAlt*cfg.baro_noise_lpf + (baroHigh*10.0f/(cfg.baro_tab_size - 1))*(1-cfg.baro_noise_lpf); // additional LPF to reduce baro noise
#endif
Ха-ха вот и ответ на вопрос почему коптер прыгает, а дело в том что барометр умудряется уплыть на 0.5-1 метр 😦 надо баро получше или постоянно смешивать…
В общем ещё один косяк
#ifdef SONAR //&& defined(SONAR_BARO_FUSION)
debug[0]=SonarPostAlt;
debug[2]=BaroHome;
debug[1] = baroPostHigh;
if(!f.ARMED) { //init offset till motors not armed
BaroHome = BaroHome*0.9f + (baroHigh*10.0f/(cfg.baro_tab_size))*0.1f; // play with optimal coef. here
SonarHome = sonarAlt;
}
//mix baro/sonar
if(BaroHome!=0) {
baroPostHigh = baroHigh-BaroHome/10.0f; // приравниваем к нулю
SonarPostAlt = sonarAlt -SonarHome; // приравниваем к нулю
if(sonarAlt>=SONAR_MIN && sonarAlt<SONAR_MAX){
if(SonarPostAlt<SONAR_BARO_FUSION_LC) {
EstAlt = EstAlt*cfg.baro_noise_lpf + SonarPostAlt*(1.0f - cfg.baro_noise_lpf);
//debug[2] = baroHigh;
} else if(SonarPostAlt>=SONAR_BARO_FUSION_LC && SonarPostAlt<SONAR_BARO_FUSION_HC) {
float fade = SONAR_BARO_FUSION_RATIO;
if(fade==0.0) fade = (SONAR_BARO_FUSION_HC-SonarPostAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
fade = constrain(fade, 0.0, 1.0);
EstAlt = EstAlt*(1.0f - cfg.baro_noise_lpf) + ((SonarPostAlt)*fade + (baroPostHigh*10.0f/(cfg.baro_tab_size - 1))*(1-fade))*(1-cfg.baro_noise_lpf);
}
}
else {
EstAlt = EstAlt*cfg.baro_noise_lpf + (baroPostHigh*10.0f/(cfg.baro_tab_size - 1))*(1-cfg.baro_noise_lpf); // должно считать только по баро притянутому к земле
}
}
//#elif // отключил для проверки
// EstAlt = EstAlt*cfg.baro_noise_lpf + (baroHigh*10.0f/(cfg.baro_tab_size - 1))*(1-cfg.baro_noise_lpf); // отключил для проверки
#endif
так вот, если пальцем заткнуть сонар, начинает почему-то считать не по baroPostHigh = примерно 0, а по baroHigh*10.0f который практически высота по давлению над уровнем моря😵 откуда он его берёт?
Ну что, оставить или ещё помучать? на горизонтальные полосы не смотреть - это глюк преобразования
в общем сделал что-то такое:
тоже лопатил сегодня этот кодярник… чутка отрефакторил т.к. там ну оч. грязный код
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) {
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;
также думал о том как корректировать баро… тут сонар приводится к показаниям баро, а надо наоборот думаю, т.е. сонар брать “чистым” (от нуля), а на баро уже оффсет считать и корректировать его соот-но…
Я только что бяку нашел - всему причиной моя тупая невнимательность… теперь заработало, на столе как надо - дёргаю сонар переходит сразу на баро и никаких дёрганий по высоте - только б погода не подкачала 😃 тот код выше он работает, только я ещё проверку сделал на адекватность сонара - если сырые показания меньше 1 - идём по баро с сонаром что-то не то, ну а верхней границей служит SONAR_BARO_FUSION_HC…
все бяки с cfg. - в Cli выведены, а что мешает в MultiWii сделать? памяти маловато?
if(fade == 0.0) {
fade = (SONAR_BARO_FUSION_HC-sonarAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
}
а если fade не равно нулю что будет?
все бяки с cfg. - в Cli выведены, а что мешает в MultiWii сделать? памяти маловато?
лень ))
а если fade не равно нулю что будет?
т.е. тогда придефайненый (статический) вес сонар/баро будет…
#define SONAR_BARO_FUSION_RATIO 0.0 amount of each sensor value, 0 = proportionnel between LC and HC
ага понял, да и баро проблемотично к сонару привязат …
вобщем остановился пока на таком варианте:
#ifdef SONAR //&& defined(SONAR_BARO_FUSION)
debug[0]=tempAlt;
tempAlt = baroHigh*10.0f/(cfg.baro_tab_size - 1);
if(!f.ARMED) { //init offset till motors not armed
BaroHome = BaroHome*0.8f + tempAlt*0.2f; // play with optimal coef. here
// SonarHome = sonarAlt;
}
//mix baro/sonar
if(BaroHome!=0) {
//baroPostHigh = baroHigh-(BaroHome / 10.0f);
// SonarPostAlt = sonarAlt - SonarHome;
debug[1]=BaroHome;
debug[2] = BaroAlt;
if(sonarAlt>=SONAR_MIN && sonarAlt<SONAR_MAX){
if(sonarAlt<SONAR_BARO_FUSION_LC) {
EstAlt = EstAlt*cfg.baro_noise_lpf + (sonarAlt+BaroHome)*(1.0f - cfg.baro_noise_lpf);
} else if(sonarAlt>=SONAR_BARO_FUSION_LC && sonarAlt<SONAR_BARO_FUSION_HC) {
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);
EstAlt = EstAlt*(1.0f - cfg.baro_noise_lpf) + ((sonarAlt+BaroHome)*fade + (baroHigh*10.0f/(cfg.baro_tab_size - 1))*(1-fade))*(1-cfg.baro_noise_lpf);
}
}
else {
EstAlt = EstAlt*cfg.baro_noise_lpf + (baroHigh*10.0f/(cfg.baro_tab_size - 1))*(1-cfg.baro_noise_lpf); // additional LPF to reduce baro noise
}
}
только с проверкой на адекватность сонара
if(sonarAlt>=SONAR_MIN && sonarAlt<SONAR_MAX){ // если сонар больше или равен единице ну и т.д.
тот код выше он работает
если ты сам понимаешь что написал в этой строчке, то возможно я тебе поверю 😉
baroPostHigh = baroHigh-BaroHome/10.0f; // приравниваем к нулю
может так?!
baroPostHigh = baroHigh/(cfg.baro_tab_size - 1) - BaroHome/10.0f;
------------------------------------------------------------
if(!f.ARMED) { //init offset till motors not armed
BaroHome = BaroHome*0.9f + (baroHigh*10.0f/(cfg.baro_tab_size))*0.1f; // play with optimal coef. here
SonarHome = sonarAlt;
}
почему не
if(!f.ARMED) { //init offset till motors not armed
BaroHome = BaroHome*0.9f + (baroHigh*10.0f/(cfg.baro_tab_size - 1))*0.1f; // play with optimal coef. here
SonarHome = sonarAlt;
}
кстати
#elif
EstAlt = EstAlt*cfg.baro_noise_lpf + (baroHigh*10.0f/(cfg.baro_tab_size - 1))*(1-cfg.baro_noise_lpf); // additional LPF to reduce baro noise
#endif
#elif не катит ARM он хитрый нет дефайна (#ifdef SONAR ) даже смотреть дальше не буду, досвиданья весь альхольд без сонара 😦
по порядку, я же говорил невнимательность 😃
если ты сам понимаешь что написал в этой строчке, то возможно я тебе поверю
это бред…
может так?!
проверял 😦
почему не
ну и так далее
это всё удалено 😃
сонар притянутый к баро работает сейчас переделываю всё заново 😃
может так?!
попробую ещё раз, не получалось сразу, EstAlt вообще не считался, хотя может что-то учудил 😃
по логике один в один с моим приведенным выше, только более зашумленным ))
- почему везде не использовать tempAlt вместо baroHigh*10.0f/(cfg.baro_tab_size - 1)
- нафиг ты инвертируешь фильтр? в одном случае EstAlt*cfg.baro_noise_lpf, в другом EstAlt*(1.0f - cfg.baro_noise_lpf)
- почему фильтр не держать в конце?
EstAlt = EstAlt*0.6f + tempAlt*0.4f; // additional LPF to reduce baro noise
ооооооооо… мляяяяяяяяяя Ж)
посмотри что ты написал… ну так нельзя!
EstAlt = EstAlt*(1.0f - cfg.baro_noise_lpf) + ((sonarAlt+BaroHome)*fade + (baroHigh*10.0f/(cfg.baro_tab_size - 1))*(1-fade))*(1-cfg.baro_noise_lpf);
А теперь
#ifdef SONAR //&& defined(SONAR_BARO_FUSION)
debug[0]=tempAlt;
tempAlt = baroHigh*10.0f/(cfg.baro_tab_size - 1);
if(!f.ARMED) { //init offset till motors not armed
BaroHome = BaroHome*0.8f + tempAlt*0.2f; // play with optimal coef. here
// SonarHome = sonarAlt;
}
//mix baro/sonar
if(BaroHome!=0) {
baroPostHigh = baroHigh/(cfg.baro_tab_size - 1) - BaroHome/10.0f;
SonarPostAlt = sonarAlt - SonarHome;
debug[1]=BaroHome;
debug[2] = BaroAlt;
if(sonarAlt>=SONAR_MIN && sonarAlt<SONAR_MAX){
if(sonarAlt<SONAR_BARO_FUSION_LC) {
EstAlt = EstAlt*cfg.baro_noise_lpf + (SonarPostAlt)*(1.0f - cfg.baro_noise_lpf);
} else if(SonarPostAlt>=SONAR_BARO_FUSION_LC && SonarPostAlt<SONAR_BARO_FUSION_HC) {
float fade = SONAR_BARO_FUSION_RATIO;
if(fade==0.0) fade = (SONAR_BARO_FUSION_HC-SonarPostAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
fade = constrain(fade, 0.0, 1.0);
EstAlt = EstAlt*cfg.baro_noise_lpf + ((SonarPostAlt)*fade + (baroPostHigh*10.0f)*(1-fade))*(1-cfg.baro_noise_lpf);
}
}
else {
EstAlt = EstAlt*cfg.baro_noise_lpf + (baroPostHigh*10.0f)*(1-cfg.baro_noise_lpf); // additional LPF to reduce baro noise
}
}
тьфу не раскоментировал SonarHome:)
А теперь
так проще мне кажется
#ifdef SONAR //&& defined(SONAR_BARO_FUSION)
debug[0]=tempAlt;
tempAlt = baroHigh*10.0f/(cfg.baro_tab_size - 1);
if(!f.ARMED) { //init offset till motors not armed
BaroHome = BaroHome*0.8f + tempAlt*0.2f; // play with optimal coef. here
SonarHome = sonarAlt;
}
//mix baro/sonar
if(BaroHome!=0) {
baroPostHigh = tempAlt - BaroHome;
SonarPostAlt = sonarAlt - SonarHome;
debug[1]=BaroHome;
debug[2] = BaroAlt;
if(sonarAlt>=SONAR_MIN && sonarAlt<SONAR_MAX){
if(sonarAlt<SONAR_BARO_FUSION_LC) {
EstAlt = EstAlt*cfg.baro_noise_lpf + (SonarPostAlt)*(1.0f - cfg.baro_noise_lpf);
} else if(SonarPostAlt<SONAR_BARO_FUSION_HC) {
float fade = SONAR_BARO_FUSION_RATIO;
if(fade==0.0) fade = (SONAR_BARO_FUSION_HC-SonarPostAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
fade = constrain(fade, 0.0, 1.0);
EstAlt = EstAlt*cfg.baro_noise_lpf + (SonarPostAlt*fade + baroPostHigh*(1-fade))*(1-cfg.baro_noise_lpf);
}
}
else {
EstAlt = EstAlt*cfg.baro_noise_lpf + baroPostHigh*(1-cfg.baro_noise_lpf); // additional LPF to reduce baro noise
}
}
А можно ли перед #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
Ну что, оставить или ещё помучать?
Ну значит оставляю, и начинаю делать верхнюю…