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

alex_f73
shrimp:

остановился на том что к процессору stm32F прикрутил барометр; давление-температуру меряет, но я крепко задумался: ведь если не хватит вычислительной мощности у него

Олег Syberian на STM32F4 в MPX не только Калмана крутит но и OSD рисует ну и датчики опрашивает, и на все хватает.

shrimp

Я перестраховываюсь, так как опыт программирования микропроцессоров минимальный. Но все говорят что хватит и ещё останется. В самом карйнем случае если нехватит, можно попробовать передавать на другой комп с быстрым FPU данные через ethernet и пускай он считает.

leprud
shrimp:

к процессору stm32F прикрутил барометр; давление-температуру меряет, но я крепко задумался: ведь если не хватит вычислительной мощности у него

Даже у 168/2560 меги хватает скорости 😃

shrimp:

В самом карйнем случае если нехватит, можно попробовать передавать на другой комп с быстрым FPU данные через ethernet

Точно опыта нету…

shrimp

Кстати, может народ позитивную RTOS посоветует? До этого всё делал под FreeRTOS , но вот кажется эта chibios.org значительно удобнее.

SergDoc

А я опять за старое, не лызе бацьку, i у крынку не лызе… нижняя плата не хочет пока влазить в посадочное место 45Х45, мыслю над переносом лишнего на верхнюю - светодиоды почти все туда ушли EEProм-ка туда же, разъёмы телеметрии и программирования (всё равно разъём при снятой верхней плате будет, я конечно с выходами переборщил (12 шт. надо они мне?) 4 чисто на сервы 8 на моторы ну или некоторые вариации, в общем мудрю чего-то… дожди…

shrimp

А фото будет как оно всё выглядит?

SergDoc

я пока только корректирую схему для разводки, потом разведу - закажу или изготовлю платки, потом только фотосессию устраивать можно будет 😃

Gapey
SergDoc:

нижняя плата не хочет пока влазить в посадочное место 45Х45, мыслю над переносом лишнего на верхнюю

лучше нижнюю развести 50х60 венеся серворазЪёмы за габарит 50х50 , как это сделано у BLACK VORTEX … и место освободится и разЪёмы будут легкодоступны после установки второго этажа … остальные разЪёмы можно угловые паять , а трехрядные угловые нежелательно , они ощутимо тяжелее …

SergDoc

я сейчас так и делаю, оставляю “хвост” - последние штрихи и буду пробовать разводить…
P.S. проц приехал (STM32F407VET6) - здоровый зараза после 48 лапых:)

Ну вот что вышло со схемой нижней платы, пошел на пальцах разводить 😃

SergDoc

Сделал “примерку”, как и ожидалось автотрассировщик заткнулся на 67% хотя очевидно, что плата разведётся, придётся помучатся только с двумя SPI - двухслойке быть 😃 это радует, надо попробовать “Топором” развести, хотя лучше по старинке в ручную - долго, но зато как хочется так и делается, плюс контроль самостоятельный 😃

SergDoc

49-ю лапу надо через кондёр 2.2Мкф не забыть на корпус пустить, а то я как-то с горяча её прямо на корпус повесил…

SergDoc

Полетал с утра, минус проп - на посадке, ветер сильный, нервный тик вроде вылечил, проблема другого плана, на сонаре как прибитый высоту держит даже на экране и в ветер, а вот сонар + баро и баро не пошли есть периодические “выстреливания” вверх и обратно спускатся не желает - выстрелил и висит на той высоте, опять выстрелил и опять висит, вобщем думаю надо переделывать алгоритм, и оба устройства привязывать к земле (старту) - буду пробовать…

в общем сделал что-то такое:

// **** 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 
SergDoc

Ха-ха вот и ответ на вопрос почему коптер прыгает, а дело в том что барометр умудряется уплыть на 0.5-1 метр 😦 надо баро получше или постоянно смешивать…

SergDoc

В общем ещё один косяк


#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 который практически высота по давлению над уровнем моря😵 откуда он его берёт?

SergDoc

Ну что, оставить или ещё помучать? на горизонтальные полосы не смотреть - это глюк преобразования

mahowik
SergDoc:

в общем сделал что-то такое:

тоже лопатил сегодня этот кодярник… чутка отрефакторил т.к. там ну оч. грязный код

  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;

также думал о том как корректировать баро… тут сонар приводится к показаниям баро, а надо наоборот думаю, т.е. сонар брать “чистым” (от нуля), а на баро уже оффсет считать и корректировать его соот-но…

SergDoc

Я только что бяку нашел - всему причиной моя тупая невнимательность… теперь заработало, на столе как надо - дёргаю сонар переходит сразу на баро и никаких дёрганий по высоте - только б погода не подкачала 😃 тот код выше он работает, только я ещё проверку сделал на адекватность сонара - если сырые показания меньше 1 - идём по баро с сонаром что-то не то, ну а верхней границей служит SONAR_BARO_FUSION_HC…

все бяки с cfg. - в Cli выведены, а что мешает в MultiWii сделать? памяти маловато?

mahowik:

if(fade == 0.0) {
fade = (SONAR_BARO_FUSION_HC-sonarAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
}

а если fade не равно нулю что будет?

mahowik
SergDoc:

все бяки с cfg. - в Cli выведены, а что мешает в MultiWii сделать? памяти маловато?

лень ))

SergDoc:

а если fade не равно нулю что будет?

т.е. тогда придефайненый (статический) вес сонар/баро будет…

#define SONAR_BARO_FUSION_RATIO 0.0  amount of each sensor value, 0 = proportionnel between LC and HC
SergDoc

ага понял, да и баро проблемотично к сонару привязат …
вобщем остановился пока на таком варианте:


#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){ // если сонар больше или равен единице ну и т.д.
mahowik
SergDoc:

тот код выше он работает

если ты сам понимаешь что написал в этой строчке, то возможно я тебе поверю 😉

SergDoc:

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;
 }
SergDoc

кстати

 #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 ) даже смотреть дальше не буду, досвиданья весь альхольд без сонара 😦

по порядку, я же говорил невнимательность 😃

mahowik:

если ты сам понимаешь что написал в этой строчке, то возможно я тебе поверю

это бред…

mahowik:

может так?!

проверял 😦

mahowik:

почему не

ну и так далее

это всё удалено 😃
сонар притянутый к баро работает сейчас переделываю всё заново 😃

mahowik:

может так?!

попробую ещё раз, не получалось сразу, EstAlt вообще не считался, хотя может что-то учудил 😃