X-Authentication-Warning: delorie.com: mailnull set sender to djgpp-bounces using -f
Message-ID: <3C94FA1B.8E6A9BA3@roosnek.nl>
Date: Sun, 17 Mar 2002 21:18:35 +0100
From: Roosnek <nico DOT r AT roosnek DOT nl>
X-Mailer: Mozilla 4.5 [en] (WinNT; I)
X-Accept-Language: en,pdf
MIME-Version: 1.0
To: djgpp AT delorie DOT com
Subject: parsing errors
Content-Type: multipart/mixed;
 boundary="------------96DA1854B2E5F31FB869BEA5"
Reply-To: djgpp AT delorie DOT com

This is a multi-part message in MIME format.
--------------96DA1854B2E5F31FB869BEA5
Content-Type: text/plain; charset=us-ascii
Content-Transfer-Encoding: 7bit

Hello,

I got in the attached subroutine the same severe parsing errors with the
gcc-3.03 using de latest version of DJGPP. With the previous version of
DJGPP with the gcc-2.95 compiler I got no error what so ever. I cannot
see any error in the macro defintion. I hope that is  not a bug.
Please could you advice about this problem.

Sincerly,

Nico Roosnek


--------------96DA1854B2E5F31FB869BEA5
Content-Type: text/plain; charset=us-ascii;
 name="NAVI.C"
Content-Transfer-Encoding: 7bit
Content-Disposition: inline;
 filename="NAVI.C"

#include <malloc.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <conio.h>
#include <time.h>
#include <errno.h>
#include <dos.h>
enum operation {START,INFO,MRU,GPS};
extern int read_man,automatic;
#define sqr(a) ((a)*(a))
struct d6 {
	double t;
	double x,y,z;
	double vx,vy,vz;
};
struct d3 {
	double x;
	double y;
	double z;
};

#ifdef UTM_REPRESENTATION
void DRGPUT(short LD,short LM,double SLAT,short NORS,
			short LOD,short LOM,double SLON,short EORW,
			double *NORTH,double *EAST,short *ZONE);
#endif
struct d3_3 {
		double x[3];
		double y[3];
		double z[3];
};

extern unsigned long long int t_new;
extern double trafo_s_w[9]; // matrix for transformation from ship to world coordinates
extern double trafo_w_s[9]; // matrix for transformation from world to ship coordinates
extern struct d6 ship_wrld;
extern struct d3 aerial_pos_ship;

double x[3],y[3],z[3];
extern double mes_x[3],mes_y[3],mes_z[3];
extern double alpha[2],beta[2],eta[2],offset_alpha,offset_beta;
extern double mes_alpha[2],mes_beta[2],mes_eta[2];

extern double GPS_TIME_OLD,GPS_TIME,GPS_SS,GPS_LAT,GPS_LON,GPS_SSLAT,GPS_SSLON,GPS_Z1,GPS_Z2;


/* state-vector in ship coordinates
	beta  -> roll     (list)
	alpha -> pitch    (trim)
	eta   -> heading

	x[3]          x,y,z
	y[3]
	z[3]
	alpha[2]	  eta,alpha,beta
	beta[2]
   eta[2]


	v_off[2] velocity offset due to flow in worldcoordinates
				 5 minutes response time
	do not use second order diff. for roll and pitch
	s^2 phi + w / Q s phi + w^2 phi = 0
	no driving force

	offset in alpha and beta 5 minutes response time?
*/
double P_x[6],P_y[6],P_z[6],P_2_x[6],P_2_y[6],P_2_z[6],P_alpha[3],P_beta[3],P_eta[3];

//double P_nav[171];
double omega_alpha,kappa_alpha,omega_beta,kappa_beta;

// add here system noise   x[2] and y[2] are actual dummies

#define update(var,n)                                                \
	for(k=0;k<n*(n+1)/2;k++)a[k]=P_##var[k];                          \
	for(k=0;k<n;k++)a[(k+1)*(k+2)/2-1]+=R[k];                         \
	symsol(1,a,x,x,n,&det);     /*x is not used*/                       \
	if(det<1e-30)                                                       \
		printf("determinant very small");                               \
	else {                                                              \
		symsol(3,a,x,x,n,&det); /*x is not used*/                       \
		matmul_tri(K,P_##var,a,n);                                      \
		for(k=0;k<n;k++){                                               \
			delx[k]=0;                                                  \
			for(l=0,kl=k*(k+1)/2;l<n;l++,kl++){                         \
				delx[k]+=(mes_##var##[l]-new_##var##[l])*K[kl];         \
				if(l>=k)kl+=l;                                          \
			}                                                           \
		}                                                               \
		for(k=0;k<n;k++)                                                \
			  var[k]=new_##var##[k]+delx[k];                            \
		matmul_tri(a,P_##var,K,n);                                      \
		for(k=0;k<(n+1)*n/2;k++)P_##var##[k]-=a[k];                     \
	}
#define update_2(var,n)                                                   \
	for(k=0;k<n*(n+1)/2;k++)a[k]=P_2_##var##[k];                          \
	for(k=0;k<n;k++)a[(k+1)*(k+2)/2-1]+=R[k];                           \
	symsol(1,a,x,x,n,&det);     /*x is not used*/                       \
	if(det<1e-30)                                                       \
		printf("determinant very small");                               \
	else {                                                              \
		symsol(3,a,x,x,n,&det); /*x is not used*/                       \
		matmul_tri(K,P_2_##var,a,n);                                      \
		for(k=0;k<n;k++){                                               \
			delx[k]=0;                                                  \
			for(l=0,kl=k*(k+1)/2;l<n;l++,kl++){                         \
				delx[k]+=(mes_##var##[l]-new_##var##[l])*K[kl];         \
				if(l>=k)kl+=l;                                          \
			}                                                           \
		}                                                               \
		for(k=0;k<n;k++)                                                \
			  var[k]=new_##var##[k]+delx[k];                            \
		matmul_tri(a,P_2_##var,K,n);                                      \
		for(k=0;k<(n+1)*n/2;k++)P_2_##var##[k]-=a[k];                     \
	}

void update_state_vector(short oper){
	int i,j,k,l,kl;
	double delt,new_alpha[2],new_beta[2],new_eta[2],det;
	double new_x[3],new_y[3],new_z[3],a[6],R[3],K[6],delx[3];
	static double vx_off=0,vy_off=0,vz_off=0;
	static int GPS_start=1,MRU_start=1;
	struct d3 aerial_pos_wrld;

	static double mem_mes_x[2][3],mem_new_x[2][3];
    static double hgh_mes_x[2],hgh_new_x[2];
    static double low_mes_x[2],low_new_x[2];
    static double mem_mes_y[2][3],mem_new_y[2][3];
    static double hgh_mes_y[2],hgh_new_y[2];
    static double low_mes_y[2],low_new_y[2];
    static double mem_mes_z[3][3],mem_new_z[3][3];
    static double hgh_mes_z[3],hgh_new_z[3];
    static double low_mes_z[3],low_new_z[3];
    static double acoef[3],bcoef[3];
    static short update_prv=0;
    static unsigned long long t,t_gps=0;
    #ifdef realtime
    unsigned long long t_new;
	__asm__ volatile (".byte 0x0f,0x31" : "=A" (t_new));
	#endif
	if(oper==START){
		t=t_new;
		for(kl=0;kl<6;kl++)
			P_x[kl]=P_y[kl]=P_z[kl]=P_2_x[kl]=P_2_y[kl]=P_2_z[kl]=0;
		for(k=0;k<3;k++){
			kl=(k+3)*k/2;
			P_x[kl]=P_y[kl]=P_z[kl]=P_2_x[kl]=P_2_y[kl]=P_2_z[kl]=1e2;              // approx 10 meter
		}
		for(kl=0;kl<3;kl++)
			P_alpha[kl]=P_beta[kl]=P_eta[kl]=0;
		P_alpha[0]=P_beta[0]=P_eta[0]=sqr(.015);    // approx 1 degree
		GPS_start=1;
		MRU_start=1;
		alpha[0]=beta[0]=eta[0]=0;
		return;
    }
   #define clock_frequency 500e6
	delt=(t_new-t)/clock_frequency; // clockticks / cpu clock frequency
	t=t_new;
#define create_priori_angle( phi )                                        \
		new_##phi##[0]=phi##[0]+phi##[1]*delt;                          \
		new_##phi##[1]=phi##[1];                                        \
		P_##phi##[0]+=.0001*delt;P_##phi##[2]+=.001*delt

#define create_priori_trans(x)                                          \
		new_##x##[0]=x##[0]+(x##[1]+x##[2]*delt*.5)*delt;               \
		new_##x##[1]=x##[1]+x##[2]*delt;                                \
		new_##x##[2]=x##[2];                                            \
		P_##x##[0]+=.0001*delt;P_##x##[2]+=.001*delt; /*P_##x##[5]+=.01*delt*/ \
		P_2_##x##[0]+=.0001*delt;P_2_##x##[2]+=.001*delt /*;P_2_##x##[5]+=.01*delt*/
   
   create_priori_angle(alpha);
	create_priori_angle(beta);
	create_priori_angle(eta);

   create_priori_trans(x);
	create_priori_trans(y);
	create_priori_trans(z);
	//if(oper==MRU)oper=INFO;
	#define initiate_filtercoef(omega,Q)    \
		acoef[0]=1/(1+.5*omega*(1/Q+.5*omega)); \
		acoef[1]=-2*acoef[0];                   \
		acoef[2]=acoef[0];                      \
		bcoef[1]=acoef[0]*(.5*sqr(omega)-2);    \
		bcoef[2]=acoef[0]*(1+.5*omega*(-1/Q+.5*omega))
    #define filter(a) \
		mem_##a[0]=a-bcoef[1]*mem_##a[1]-bcoef[2]*mem_##a[2]; \
		hgh_##a=acoef[0]*mem_##a[0]+acoef[1]*mem_##a[1]+acoef[2]*mem_##a[2]; \
		low_##a=a-hgh_##a;\
		mem_##a[2]=mem_##a[1];mem_##a[1]=mem_##a[0]
	/* no acceleration in initiation of mem*/
	#define initiate_memory_of_filter(type)                  \
		for(i=0;i<2;i++){                               \
			double valx=##type##_x[i]/(1+bcoef[1]+bcoef[2]), \
				   valy=##type##_y[i]/(1+bcoef[1]+bcoef[2]), \
				   valz=##type##_z[i]/(1+bcoef[1]+bcoef[2]); \
			for(j=0;j<3;j++){                           \
				mem_##type##_x[i][j]=valx;                   \
				mem_##type##_y[i][j]=valy;                   \
				mem_##type##_z[i][j]=valz;                   \
			}                                           \
		}                                               \
		mem_##type##_z[2][0]=mem_##type##_z[2][1]=mem_##type##_z[2][2]=0
	switch(oper){
		double xtemp,ytemp;
		case INFO:
		     for(k=0;k<2;k++){
				x[k]=new_x[k];y[k]=new_y[k];z[k]=new_z[k];
				alpha[k]=new_alpha[k];beta[k]=new_beta[k];eta[k]=new_eta[k];
             }
			 z[2]=new_z[2];
			 break;
		case MRU: {
			// rotate mesurement to worldframe. z is alright
			xtemp=mes_x[0];ytemp=mes_y[0];
			mes_x[0]= xtemp*cos(eta[0])+ytemp*sin(eta[0]);
			mes_y[0]=-xtemp*sin(eta[0])+ytemp*cos(eta[0]);
			xtemp=mes_x[1];ytemp=mes_y[1];
			mes_x[1]= xtemp*cos(eta[0])+ytemp*sin(eta[0]);
			mes_y[1]=-xtemp*sin(eta[0])+ytemp*cos(eta[0]);
			//mes_z[1]+=vz_off;
			if(MRU_start){
				MRU_start=0;
				alpha[0]=mes_alpha[0];alpha[1]=mes_alpha[1];
				beta[0]=mes_beta[0];beta[1]=mes_beta[1];
				eta[0]=mes_eta[0];eta[1]=mes_eta[1];
				//x[0]=mes_x[0];y[0]=mes_y[0];z[0]=mes_z[0];
                x[1]=mes_x[1];
				y[1]=mes_y[1];
				z[1]=mes_z[1];z[2]=mes_z[2];

                initiate_filtercoef(.2*6.28/.1,.5);   // fall off .2 Hz update 10 Hz
                initiate_memory_of_filter(mes);
				initiate_memory_of_filter(new);
                break;
			} else {
				R[0]=R[1]=1e-4;
				update(alpha,2);
				update(beta,2);
				update(eta,2);
			}
			for(i=0;i<2;i++){
				filter(mes_x[i]);
				filter(mes_y[i]);
				filter(mes_z[i]);
				filter(new_x[i]);
				filter(new_y[i]);
				filter(new_z[i]);
			}
			filter(mes_z[2]);filter(new_z[2]);

			// use only high or low frequency in the update
			#define use(pass,var,n) \
				for(i=0;i<n;i++)new_##var[i]=##pass##_new_##var[i];\
				for(i=0;i<n;i++)mes_##var[i]=##pass##_mes_##var[i]
			#define add_other_part(pass,var,n) for(i=0;i<n;i++)var[i]+=pass##_new_##var[i]
			#define update_dum(var,n)for(i=0;i<n;i++)var[i]=hgh_mes_##var##[i]
			#define print_intermediate(var) { char buf[100]; \
			   sprintf(buf,"##var## hgh up %+4.2lf %+4.2lf mes %+4.2lf %+4.2lf low %+4.2lf %+4.2lf ", \
			   ##var##[0],##var##[1],hgh_mes_##var##[0],hgh_mes_##var##[1],low_new_##var##[0],low_new_##var##[1]); \
			   _settextposition(19, 1);_outtext( buf );\
			   }

			#define plot_disp(var) {    \
				static int mes_##var##_prv=40,##var##_prv=40;\
				_settextposition(18,mes_##var##_prv);_outtext(" "); \
				_settextposition(18,##var##_prv);_outtext(" ");\
				mes_##var##_prv=max(1,min(80,(int)(40*mes_##var##[0]/3+40)));\
				##var##_prv=max(1,min(80,(int)(40*##var##[0]/3+40)));\
				_settextposition(18,mes_##var##_prv);_outtext("*");\
				_settextposition(18,##var##_prv);_outtext(".");\
				}
			R[0]=R[1]=R[2]=1e-4;

			use(hgh,x,2);
			update_dum(x,2);
			add_other_part(low,x,2);
			use(hgh,y,2);
			update_2(y,2);
			//print_intermediate (y);
			add_other_part(low,y,2);
			use(hgh,z,3);update_2(z,3);add_other_part(low,z,3);
			if(update_prv!=oper){
                update_prv=oper;

			}
			break;
			}
		case GPS:  // transfer gps aerial coordinates to ship
			//convert_GPSdata(R);
			{
			static double mes_x_prv,mes_y_prv,mes_z_prv;
			R[0]=1e2;R[1]=1e2;R[2]=1e10;  // GPS data 4 meter variance and the velocity very bad
			for(k=0;k<2;k++){
				alpha[k]=new_alpha
				[k];beta[k]=new_beta[k];eta[k]=new_eta[k];
			}
			create_world_ship_trafo(trafo_s_w,trafo_w_s,alpha,beta,eta);
			ship_to_wrld_offset(&aerial_pos_ship,&aerial_pos_wrld);
			// subtract aerial_offset
			mes_x[0]+=aerial_pos_wrld.x;  // gps the right direction
			mes_y[0]+=aerial_pos_wrld.y;
			mes_z[0]+=aerial_pos_wrld.z;
            //printf(" %4.3lf %4.3lf %4.3lf",mes_x[0],mes_y[0],mes_z[0]);
			if(GPS_start){
				GPS_start=0;
				new_x[0]=x[0]=mes_x[0];
				new_y[0]=y[0]=mes_y[0];
				new_z[0]=z[0]=mes_z[0];
				x[1]=y[1]=z[1]=0;  //no velocity to start with
                initiate_filtercoef(.2*6.28/.1,.5);   // fall off .2 Hz update 10 Hz
                initiate_memory_of_filter(new);  // mmru filter
                goto the_end_of_gps_update;
			 } else {
				double delt=(t_new-t_gps)/clock_frequency;
				//if(delt==0)delt=1e4;
				mes_x[1]=(mes_x[0]-mes_x_prv)/delt;
				mes_y[1]=(mes_y[0]-mes_y_prv)/delt;
				mes_z[1]=(mes_z[0]-mes_z_prv)/delt;
				vx_off=mes_x[1];
				vy_off=mes_y[1];
				vz_off=mes_z[1];
				update(x,2);
				update(y,2);
				update(z,3);

				x[0]=mes_x[0];
				y[0]=mes_y[0];
				z[0]=mes_z[0];
			}
      the_end_of_gps_update:
			t_gps=t_new;
			mes_x_prv=mes_x[0];
			mes_y_prv=mes_y[0];
			mes_z_prv=mes_z[0];
			}
	}
	ship_wrld.x=x[0];
	ship_wrld.y=y[0];
	ship_wrld.z=z[0];
	/*
	{
	char buf[200];
	sprintf(buf,"-> %+6.2lf %+6.2lf %+6.2lf %+4.2lf %+4.2lf %+4.2lf %+3.0lf %+3.0lf %+3.0lf",
			x[0],y[0],z[0],x[1],y[1],z[1],alpha[0]*radgrad,beta[0]*radgrad,eta[0]*radgrad);
	_settextposition(10, 1);_outtext( buf );
	sprintf(buf,"off %+4.2lf %+4.2lf %+4.2lf",vx_off,vy_off,vz_off);
	_settextposition(13, 1);_outtext( buf );
	sprintf(buf,"P %+4.2lf %+4.2lf %+4.2lf %+4.2lf %+4.2lf %+4.2lf   ",P_x[0],P_y[0],P_z[0],P_x[2],P_y[2],P_z[2]);
	_settextposition(14, 1);_outtext( buf );
	if(!automatic){
		while(!kbhit());
		if(do_commands())exit(99);
	}
	}
	*/
}

--------------96DA1854B2E5F31FB869BEA5--