573 | | // implementace v PC |
574 | | /* t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); |
575 | | * t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/ |
576 | | tmp=prevod(sin(Thetaref*x_est[3]/32768.),15); |
577 | | if (tmp>32767) t_sin =32767; else t_sin=tmp; |
578 | | tmp=prevod(cos(Thetaref*x_est[3]/32768.),15); |
579 | | if (tmp>32767) t_cos =32767; else t_cos=tmp; |
580 | | |
581 | | tmp=((int32)cB*x_est[2])>>15; // q15*q13 -> q13 |
582 | | // q15*q13 + q13*q15 + q15*q13?? |
583 | | x_est[0]=((int32)cA*x_est[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13 |
584 | | x_est[1]=((int32)cA*x_est[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13 |
585 | | x_est[2]=x_est[2]; |
| 573 | x_est[0]=x_est[0]; |
587 | | x_est[3]=(((int32)x_est[3]<<15)+(int32)cG*x_est[2])>>15; |
588 | | |
589 | | if(x_est[3]>(1<<15)) x_est[3]-=2*(1<<15); |
590 | | if(x_est[3]<-(1<<15)) x_est[3]+=2*(1<<15); |
591 | | |
| 575 | x_est[1]=(((int32)x_est[1]<<15)+(int32)cG*x_est[0])>>15; |
| 576 | |
| 577 | // if(x_est[1]>(1<<15)) x_est[1]-=2*(1<<15); |
| 578 | // if(x_est[1]<-(1<<15)) x_est[1]+=2*(1<<15); |
| 579 | |
| 580 | |
| 581 | ///////// end of copy /////////////// |
| 582 | mmultAU(PSI,Uf,PSIU,2,2); |
| 583 | |
610 | | thorton_fast(Uf,Df,PSIU,Q,G,Dfold,4); |
611 | | |
| 593 | thorton_fast(Uf,Df,PSIU,Q,G,Dfold,2); |
| 594 | |
| 595 | |
| 596 | // implementace v PC |
| 597 | /* t_sin=prevod(sin(Thetaref*x_est[3]/32768.),15); |
| 598 | * t_cos=prevod(cos(Thetaref*x_est[3]/32768.),15);*/ |
| 599 | tmp=prevod(sin(Thetaref*x_est[1]/32768.),15); |
| 600 | if (tmp>32767) t_sin =32767; else t_sin=tmp; |
| 601 | tmp=prevod(cos(Thetaref*x_est[1]/32768.),15); |
| 602 | if (tmp>32767) t_cos =32767; else t_cos=tmp; |
| 603 | |
| 604 | { |
| 605 | C[0]=((int32)cB*t_sin)>>15; |
| 606 | tmp=((int32)cH*x_est[0])>>15; // ! cH =cB with different scale!! |
| 607 | C[1]=((int32)tmp*t_cos)>>15; |
| 608 | C[2]=-((int32)cB*t_cos)>>15; |
| 609 | C[3]=((int32)tmp*t_sin)>>15; |
| 610 | } |
| 611 | |
| 612 | |
| 613 | { |
| 614 | int Ui[4]; for (int i=0;i<4; i++) Ui[i]=(int)Uf[i]; |
| 615 | int Ci[4]; for (int i=0;i<4; i++) Ci[i]=(int)C[i]; |
| 616 | |
| 617 | imat Ud(Ui,2,2); |
| 618 | imat Cd(Ci,2,2); |
| 619 | imat CU = Cd*Ud/(1<<15); |
| 620 | log_level.store(logU,get_from_ivec(ivec(CU._data(),CU._datasize()))); |
| 621 | |
| 622 | int di[2]; for (int i=0;i<2; i++) di[i]=(int)Df[i]; |
| 623 | ivec dd(di,2); |
| 624 | imat U(Ui,2,2); |
| 625 | mat U2=to_mat(U)/32768; |
| 626 | mat PP=U2*diag(to_vec(dd))*U2.T(); |
| 627 | vec pp(PP._data(),4); |
| 628 | log_level.store(logP,pp); |
| 629 | } |
| 630 | { |
| 631 | int Gi[4]; for (int i=0;i<4; i++) Gi[i]=(int)G[i]; |
| 632 | ivec Gd(Gi,4); |
| 633 | log_level.store(logG,get_from_ivec(Gd)); |
| 634 | } |
| 635 | |
| 636 | |
| 637 | tmp=((int32)cB*x_est[0])>>15; // q15*q13 -> q13 |
| 638 | // q15*q13 + q13*q15 + q15*q13?? |
| 639 | y_est[0]=((int32)cA*y_old[0]+(int32)tmp*t_sin+(int32)cC*(uf[0]))>>15; // in q13 |
| 640 | y_est[1]=((int32)cA*y_old[1]-(int32)tmp*t_cos+(int32)cC*(uf[1]))>>15; // q13 |
| 641 | |
| 642 | |
| 643 | int16 difz[2]; |
| 644 | difz[0]=(Y_mes[0]-y_est[0]); // shift to q15!! |
| 645 | difz[1]=(Y_mes[1]-y_est[1]);//<<2; |
| 646 | |
| 647 | y_old[0] = Y_mes[0]; |
| 648 | y_old[1] = Y_mes[1]; |
613 | | int Ui[16]; for (int i=0;i<16; i++) Ui[i]=(int)Uf[i]; |
614 | | |
615 | | ivec Ud(Ui,16); |
616 | | log_level.store(logU,get_from_ivec(Ud)); |
617 | | |
618 | | int di[16]; for (int i=0;i<16; i++) di[i]=(int)Df[i]; |
619 | | ivec dd(di,4); |
620 | | imat U(Ui,4,4); |
621 | | mat U2=to_mat(U)/32768; |
622 | | mat PP=U2*diag(to_vec(dd))*U2.T(); |
623 | | vec pp(PP._data(),16); |
624 | | log_level.store(logP,pp); |
625 | | } |
626 | | { |
627 | | int Gi[16]; for (int i=0;i<16; i++) Gi[i]=(int)G[i]; |
628 | | ivec Gd(Gi,16); |
629 | | log_level.store(logG,get_from_ivec(Gd)); |
630 | | } |
631 | | |
632 | | |
633 | | int16 difz[2]; |
634 | | difz[0]=(Y_mes[0]-x_est[0]); // shift to q15!! |
635 | | difz[1]=(Y_mes[1]-x_est[1]);//<<2; |
636 | | |
637 | | { |
638 | | int Di[4]; for (int i=0;i<4; i++) Di[i]=(int)Df[i]; |
639 | | ivec dd(Di,4);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1]; |
| 650 | int Di[2]; for (int i=0;i<2; i++) Di[i]=(int)Df[i]; |
| 651 | ivec dd(Di,2);//;dd(0)=Y_mes[0];dd(1)=Y_mes[1]; dd(2)=difz[0]; dd(3)=difz[1]; |
651 | | (mu)(0)=zprevod(x_est[0],15)*Iref; |
652 | | (mu)(1)=zprevod(x_est[1],15)*Iref; |
653 | | (mu)(2)=zprevod(x_est[2],15)*Wref; |
654 | | (mu)(3)=zprevod(x_est[3],15)*Thetaref; |
| 666 | (mu)(0)=zprevod(x_est[0],15)*Wref; |
| 667 | (mu)(1)=zprevod(x_est[1],15)*Thetaref; |