void pidKontroler( Dane_Katy *Zadany_Kat, Dane_Katy *Zmierzony_Kat, Poprzedni_Blad *Poprzedni_Blad, Dane_PID *DanePid)
{
   int16_t BladPitch=1;
   int16_t RozniczkaPitch=0;

   int16_t BladRoll=1;
   int16_t RozniczkaRoll=0;

   BladPitch=Zadany_Kat->Pitch-Zmierzony_Kat->Pitch;
   BladRoll=Zadany_Kat->Roll-Zmierzony_Kat->Roll;

   RozniczkaPitch=(BladPitch-Poprzedni_Blad->Pitch)/dt;
   RozniczkaRoll=(BladRoll-Poprzedni_Blad->Roll)/dt;

   DanePid->Pitch=(KP*BladPitch)+(KD*RozniczkaPitch);
   DanePid->Roll=(KP*BladRoll)+(KD*RozniczkaRoll);

}

pidKontroler(&ZadaneKaty, &ZmierzoneKaty, &PoprzedniBlad, &DanePid);

DanePid->Pitch=(KP*BladPitch)+(KD*RozniczkaPitch);
   DanePid->Roll= (KP*BladRoll)+(KD*RozniczkaRoll);