Mail Archives: djgpp/2002/03/17/15:21:07
| 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
|
| 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--
- Raw text -