/* Checks for correctness, comparing results of
* serial and parallel implementations */
int testFLOYD(int n, int num) {
int *a = randomM(n);
int *b = (int *)malloc(n*n*sizeof(int));
memcpy(b, a, n*n*sizeof(int));
floyd(a, n);
f_p(b, n, num);
return compareM(a, b, n);
}
explicit
FlintZZ_pX(const ZZ_pX& a)
{
long da = deg(a);
FlintZZ f_p(ZZ_p::modulus());
fmpz_mod_poly_init2(value, f_p.value, da+1);
for (long i = 0; i <= da; i++) {
FlintZZ f_c(rep(a[i]));
fmpz_mod_poly_set_coeff_fmpz(value, i, f_c.value);
}
}
void NavEstimator::prediction(float Ts)
{
float psidot, tmp, Vgdot;
if(fabsf(xhat_p(2)) < 0.01f)
{
xhat_p(2) = 0.01;
}
for(int i=0;i<N_;i++)
{
psidot = (qhat*sinf(phihat) + rhat*cosf(phihat))/cosf(thetahat);
tmp = -psidot*Vwhat*(xhat_p(4)*cosf(xhat_p(6)) + xhat_p(5)*sinf(xhat_p(6)))/xhat_p(2);
Vgdot = ((Vwhat*cosf(xhat_p(6)) + xhat_p(4))*(-psidot*Vwhat*sinf(xhat_p(6))) + (Vwhat*sinf(xhat_p(6)) + xhat_p(5))*(psidot*Vwhat*cosf(xhat_p(6))))/xhat_p(2);
f_p = Eigen::VectorXf::Zero(7);
f_p(0) = xhat_p(2)*cosf(xhat_p(3));
f_p(1) = xhat_p(2)*sinf(xhat_p(3));
f_p(2) = Vgdot;
f_p(3) = GRAVITY/xhat_p(2)*tanf(phihat)*cosf(xhat_p(3) - xhat_p(6));
f_p(6) = psidot;
A_p = Eigen::MatrixXf::Zero(7,7);
A_p(0,2) = cos(xhat_p(3));
A_p(0,3) = -xhat_p(2)*sinf(xhat_p(3));
A_p(1,2) = sin(xhat_p(3));
A_p(1,3) = xhat_p(2)*cosf(xhat_p(3));
A_p(2,2) = -Vgdot/xhat_p(2);
A_p(2,4) = -psidot*Vwhat*sinf(xhat_p(6))/xhat_p(2);
A_p(2,5) = psidot*Vwhat*cosf(xhat_p(6))/xhat_p(2);
A_p(2,6) = tmp;
A_p(3,2) = -GRAVITY/powf(xhat_p(2),2)*tanf(phihat)*cosf(xhat_p(3) - xhat_p(6));
A_p(3,3) = -GRAVITY/xhat_p(2)*tanf(phihat)*sinf(xhat_p(3) - xhat_p(6));
A_p(3,6) = GRAVITY/xhat_p(2)*tanf(phihat)*sinf(xhat_p(3) - xhat_p(6));
xhat_p += f_p *(Ts/N_);
P_p += (A_p*P_p + P_p*A_p.transpose() + Q_p)*(Ts/N_);
}
}
long StartTest(void)
{
unsigned int i;
funcp_t f_p = (funcp_t)
((char*) __section_begin("MYOVERLAY") + 0); /* +1 for Thumb instr */
//StartTestRom2();
SwitchToOverlay1();
//f_p();
SwitchToOverlay2();
f_p();
//StartTestRam
//StartTestRom2();
return 0;
}
void main()
{
double i;
double root;
int n;
printf("\nn E(eV)\n");
for(n=0; n<15; n++)
{
for(i=-5; i<0; i=i+0.1)
{
root = f_p(i,i+0.1,n);
}
}
}
FlintZZ_pX()
{
FlintZZ f_p(ZZ_p::modulus());
fmpz_mod_poly_init(value, f_p.value);
}
int main()
{
char o,x,d;
char x2[2]={0x00,0x00};
char *str="\x00\x1A";
char utf8_str[512];
int i,len;
long number,*number_p;
o='\032';
x='\x1A';
d =26;
printf("\nnumber %d, number_p %d\n",sizeof(number),sizeof(number_p));
printf("x2 %d, *x2 %d\n",sizeof(x2),sizeof(&x2[0]));
printf("str %d\n",sizeof(str));
printf("utf8_str %d \n",sizeof(utf8_str));
printf("struct myStruct: %d\n",sizeof(struct myStruct));
printf("struct myStruct*: %d\n",sizeof(struct myStruct *));
/*
printf("o=%d,x=%d,d=%d \n",o,x,d);
printf("1 \032\n");
printf("2 \x1A\n");
printf("3 \x001A\n");
memcpy(x2,str,2);
printf("x2=%02x%02x\n",x2[0],x2[1]);
memcpy(x2,str,2);
printf("x2=%02x%02x\n",x2[0],x2[1]);
str="\x001A";
memset(x2,0x00,sizeof(x2));
for(i =0;i< 2;i++)
printf("str[%d]=%02x\n",i,str[i]);
memcpy(x2,str,strlen(str));
printf("x2=%02x%02x\n",x2[0],x2[1]);
str = "\xE7\x9A\x84\xE5\xBE\x88\xE7\xAE\x00\x00";
len=10;
for(i =0;i<len;i++)
{
printf("0x%02x ",(char)str[i]);
}
printf("\n");
len = ucs2_to_utf8(str,5,utf8_str);
printf("len = %d\n",len);
for(i =0;i<len;i++)
{
printf("0x%02x ",utf8_str[i]);
}
printf("\n");
i= 5;
printf("i = %d\n",(i++)+(++i)+(i++));
*/
{
int p = 100,*rp = NULL;
printf("\np = (@x%4x,%d)\n",&p,p);
//int *(*func1)(int *arg1) = test_f1;
func_int_p f_p = test_f1;
printf("\nf_p=(@x%04x,@X%04x)\n",f_p,*f_p);
rp = f_p(&p);
printf("\nrp = (@x%04x,%d)\n",rp,*rp);
}
return 0;
}
