IMU_blokove.txt

	
	(jeste to nemam hotove, zkusim pokracovat pozdeji)
	
void IMU()
{		
	1)
	/* pozice v prostoru - rotace, ktera transformuje globalni souradnice na lokalni
            Pro reprezentaci rotace je pouzit quaternion (ale neni to nutne)
	    Vychozi hodnota neni prilis dulezita, ale predpokladejme, ze quad lezi vodorovne smerem na sever 
	 	(globalni a lokalni souradnice jsou na pocatku schodne)
	*/
	static Quat R = {0,0,0,1};  // za zacatku zadna rotace, reprezentace x,y,z,w
		
	// u funkci ReadGyro, readAcc predpokladam, ze vraci uz normalizovane mereni (rad/s , m/s^2)
	Vec3 GyroXYZ = readGyro(); // vektor s uhlovou rychlosti pro jednotlive osy (v lokalnich souradnicich)
				   // smer je osa rotace, vekost vektoru rychlost otaceni
	Vec3 AccXYZ = readAcc();   // vektor se zmerenou akcelerace, v lokalnich souradnicich
		
	// accSmooth je trochu sporne 
	//  - filter zanese do mereni zpozdeni. Pokud by quad trvale rotoval kolem jedne osy, tak se toto zpozdeni projevi
	//     jako konstantni odchylka
	//  - je otazka, zda filtrovani ma nejaky pozitivni efekt (krome vyhodnocovani, zda vzorek pouzit)
	//  - rozhodne zde nefiltrovat moc
	Vec3 accSmooth = filter(AccXYZ, accFilterState, dT);
		
	/* znam rychlost otaceni pro jednotlive osy (GyroXYZ) a dobu od posledniho mereni (dT), tuto zmenu potrebuji
	     'pridat' do rotace R. Zmeny nejdou itegrovat primo, ale je mozne je prevest na derivaci R (pokud je R 
	     quaternion).
	     
	     Somega = {0, GyroXYZ}; - doplneni na quaternion
	     R' = 1/2 * R * Somega; - derivace quaternionu orientace - uz jde integrovat
	
	     Matematicky je to stejne, jako bych otocil R otocil o GyroXYZ * dT
	 */
	// *** <1> ***
	Quat Somega = {0, GyroXYZ};   // doplneni na quaternion 
	dR = 1/2 * quat_mult(R, sOmega) * dT;
	R += dR;                      // integrujeme dR 
	
	// pokud by nedochazelo k chybam (mereni, zaokrouhlovani,aproximace), R by ted byla orientace v prostoru
	// problem je, ze chyby se budou akumulovat - je treba je odstranit
		
	// odstraneni chyby pomoci ACC
	accMag=vec_norm(accSmooth);   // velikost vektoru
	// ignorovani mereni, ktere jsou prilis daleko od klidoveho stavu - akceleraci zpusobuje pilot a ne gravitace
	if(accMag > 0.7 * g && accMag < 1.3 * g)  {
		// vezmu gravitacni vektor a otocim ho do lokalnich souradnic ( je to snazsi nez otacet mereni )
		Vec3 gravity = {0,0,1};  // NED - gravitace smeruje ve smeru Z, chci jednotkovy vektor
		Vec3 gLoc = vec_rotate(R, gravity);
		// pokud by bylo vse vporadku, tak se budou gLoc a accSmooth stejne.
		// potrebuju najit odchylku mezi temito vektory 
		// pouziju vektorovy soucin (cross product) - smer vysledneho vektoru je kolmy na oba nasobene vektory
		//  a velikost je sin(alpha) - vlastne ziskam osu otaceni a uhel, pomoci ktereho muzu vektory 'srovnat'
		Vec3 accErr = cross(vec_normalize(accSmooth), gLoc);
		// (accErr by mela byt v klidovem stavu mala)
			
		// chybu od akcelerometru vynasobit 'ziskem' - vlastne P regulator, kterym opravuji chybu R
		// (v tomto miste muzu pouzit i PI regulator - I slozka bude kompenzovat offset gyra)
			
		accErr *= accGain;
			
		// ted muzu z accErr vytvorit quaternion a pomoci toho opravit R(podobne jako pro signal gyra). 
		// Ale accErr ma stejny format jako signal z gyra - smer udava osu otaceni, velikot se umerna uhlu.
		// Navic accGain je maly (chci maly vliv Acc, aby poloha nereagovala na zrychleni zpusobena pilotem).
		// - muzu si to zjednodusit - accErr prictu k GyroXYZ (v miste <1>) a prevod a integraci provedu jen jednou.
		<- <1>
		GyroXYZ += accErr;
		....
	}
	v pripade magnetometru muzu postupovat podobne - otocim smer magnetickeho vektoru (pozor ze nemiri na sever vodorovne, ale v Cechach ma sklon 63°) a vypoctu odchylku.
	Problem je, ze tento vypocet by mi ovlivnoval i uhly naklonu a pri presnosti magnetometru by to zpusobovalo velkou chybu v odhadu orientace (i stupen je pro naklon velka chyba). Proto to Cleanflight dela opacne - zmereny magneticky vektor se otoci do svetovych souradnic a Z se vynuluje - zustane jen hodnota v rovine sever-vychod. Z totoho smeru vypoctu odchylu od osy X (sever), pokud pouziju vektorovy soucin, vyjde mi vektor velokosti odpovidajici chybe, ktery ma jen slozku Z (musi byt kolmy na rovinu XY). Tento vektor otozim zpet do lokalnich souradnic a pouziju stejne jako ACC ke korekci chyby.
		
	
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v, float *delta)
{
	struct fp_vector v_tmp = *v;

	// This does a  "proper" matrix rotation using gyro deltas without small-angle approximation
	float mat[3][3];
	float cosx, sinx, cosy, siny, cosz, sinz;
	float coszcosx, sinzcosx, coszsinx, sinzsinx;

	cosx = cosf(delta[0]);
	sinx = sinf(delta[0]);
	cosy = cosf(delta[1]);
	siny = sinf(delta[1]);
	cosz = cosf(delta[2]);
	sinz = sinf(delta[2]);

	coszcosx = cosz * cosx;
	sinzcosx = sinz * cosx;
	coszsinx = sinx * cosz;
	sinzsinx = sinx * sinz;

	mat[0][0] = cosz * cosy;
	mat[0][1] = -cosy * sinz;
	mat[0][2] = siny;
	mat[1][0] = sinzcosx + (coszsinx * siny);
	mat[1][1] = coszcosx - (sinzsinx * siny);
	mat[1][2] = -sinx * cosy;
	mat[2][0] = (sinzsinx) - (coszcosx * siny);
	mat[2][1] = (coszsinx) + (sinzcosx * siny);
	mat[2][2] = cosy * cosx;
	
	/* compute a new value of x,y,z, by previous x,y,z and rotation matrix*/
	v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
	v->Y = v_tmp.X * mat[0][1] + v_tmp.Y * mat[1][1] + v_tmp.Z * mat[2][1];
	v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
}