controller.c

一个飞机控制器的控制器源码

        case TYPE_Double:
        	result = Double_new(thisToken.payload.Double + otherToken.payload.Double);
        	break;
        #endif
        default:
	        fprintf(stderr, "add error. (%d)\n", otherToken.type);
	        exit(1);
    }	    
    va_end(argp);
    return result;
}


Token multiply(Token firsttoken, Token secondtoken) 
{
	int 	i,j;
	Token	result;
	Token	tempToken;
	
	int		firsttokenrow,firsttokencolumn,secondtokenrow,secondtokencolumn;
	firsttokenrow = firsttoken.payload.Matrix->row;
	firsttokencolumn = firsttoken.payload.Matrix->column;
	secondtokenrow = secondtoken.payload.Matrix->row;
	secondtokencolumn = secondtoken.payload.Matrix->column;	
	
	if(firsttokencolumn != secondtokenrow)
	{
		printf("These 2 token can not be multiplied\n");
		exit(0);
	}
	
    int M=firsttokenrow;//A的行数，C的行数
    int N=secondtokencolumn;//B的列数，C的列数
    int K=firsttokencolumn;//A的列数，B的行数

    int lda=K;//A的列
    int ldb=N;//B的列
    int ldc=N;//C的列
    double 	alpha=1.0;
    double	beta =0.0;
    double A[M*K];
    double B[K*N];
    double C[M*N]; 
    
	
	
    for (i = 0; i < firsttokencolumn; i++) {
    	for(j = 0; j< firsttokenrow; j++){
    		tempToken = Matrix_get(firsttoken,j,i);
    		switch(tempToken.type)
    		{
	    		#ifdef TYPE_Double
	    		case TYPE_Double:
	        		A[(i*firsttokenrow+j)]=(double)tempToken.payload.Double;
	        		
	        		break;
	        	#endif
	        	#ifdef TYPE_Int
	        	case TYPE_Int:
	        		A[(i*firsttokenrow+j)]=(double)tempToken.payload.Int;
	        		break;
	        	#endif
	        	default:
	        		exit(1);
    		}
    	}
    }
    
    for (i = 0; i < secondtokencolumn; i++) {
    	for(j = 0; j< secondtokenrow; j++){
        	tempToken = Matrix_get(secondtoken,j,i);
    		switch(tempToken.type)
    		{
	    		#ifdef TYPE_Double
	    		case TYPE_Double:
	        		B[(i*secondtokenrow+j)]=(double)tempToken.payload.Double;
	        		
	        		break;
	        	#endif
	        	#ifdef TYPE_Int
	        	case TYPE_Int:
	        		B[(i*secondtokenrow+j)]=(double)tempToken.payload.Int;
	        		break;
	        	#endif
	        	default:
	        		exit(1);
    		}
    	}
    }
    cblas_dgemm(CblasRowMajor,CblasNoTrans,CblasNoTrans,M, N, K,alpha,A,lda,B,ldb,beta,C,ldc);	
 	result = Matrix_new(M, N, 0);
    for (i = 0; i < N; i++)
    {
		for(j = 0; j< M; j++)
		{	
    		Matrix_set(result,j,i,Double_new((double)C[i*M+j]));
		}
	}

	return result;	
}

Token RecvfromRTlab()
{
	int 	n=0,i,len;
	int 	on =1;
	int 	fd_RT_lab_data_from_RT_lab;
	int 	PORT_RT_lab_data_from_RT_lab= 6000; // same as the remote port of RT_lab model 
	Token 	result;
	struct	RT_lab_data		RT_lab_data_from_RT_lab;
	struct 	sockaddr_in addr_RT_lab_data_from_RT_lab;
	struct  sockaddr_in client_ad;
    int     client_ad_size = sizeof(client_ad);
	//Input RT_lab_data_from_RT_lab
	if((fd_RT_lab_data_from_RT_lab=socket(AF_INET,SOCK_DGRAM,0))==-1)
    {
    		perror("socket fd_RT_lab_data_from_RT_lab");
    		exit(1);
    }
    setsockopt(fd_RT_lab_data_from_RT_lab, SOL_SOCKET,SO_REUSEADDR,&on, sizeof(on));	
	bzero(&addr_RT_lab_data_from_RT_lab,sizeof(addr_RT_lab_data_from_RT_lab)); 	
    addr_RT_lab_data_from_RT_lab.sin_family=AF_INET;
    addr_RT_lab_data_from_RT_lab.sin_port=htons(PORT_RT_lab_data_from_RT_lab); 				
    addr_RT_lab_data_from_RT_lab.sin_addr.s_addr=INADDR_ANY; 	
	if(bind(fd_RT_lab_data_from_RT_lab,(struct sockaddr *)&addr_RT_lab_data_from_RT_lab, sizeof(struct sockaddr))==-1)
    {
    		perror("fd_RT_lab_data_from_RT_lab bind");
    		exit(1); 
    }
    fd_set sd_set;
    struct timeval tv;
   
    FD_ZERO (&sd_set);
    FD_SET  (fd_RT_lab_data_from_RT_lab, &sd_set);  
    // Set the tv structure to the correct timeout value
    tv.tv_sec = 100;

    switch (select (fd_RT_lab_data_from_RT_lab+1, &sd_set, (fd_set*)0, (fd_set*)0, &tv))
    {
    	case -1:
      		exit(-1);
    	case  0:
      		exit(0);
    	default:
	    if (!(FD_ISSET (fd_RT_lab_data_from_RT_lab, &sd_set)))
		{
		  exit(0);
		}
    }
    
    n = recvfrom (fd_RT_lab_data_from_RT_lab,(char *)&RT_lab_data_from_RT_lab, sizeof(RT_lab_data_from_RT_lab), 0, (struct sockaddr *)&client_ad, &client_ad_size);
    len = (sizeof(RT_lab_data_from_RT_lab)-12)/8;
    result = Matrix_new(len,1,0);
    for(i=0;i<len;i++)
    {
    	Matrix_set(result, i, 0, Double_new(RT_lab_data_from_RT_lab.data[i]));
    }
    shutdown(fd_RT_lab_data_from_RT_lab, SHUT_RDWR);
    close(fd_RT_lab_data_from_RT_lab);
    return result;
    
}

int SendtoRTlab(int sd,Token mytoken, struct sockaddr_in send_ad)
{
	int row;
	int column;
	int i,j,n;
	
	struct	RT_lab_data		RT_lab_data_to_RT_lab;	
	row = mytoken.payload.Matrix->row;
	column = mytoken.payload.Matrix->column;
	RT_lab_data_to_RT_lab.dev_id = 1;
	RT_lab_data_to_RT_lab.msg_id = status++;
	RT_lab_data_to_RT_lab.msg_len= (row*column*8);
    for (i = 0; i < row; i++) {
    	for(j = 0; j< column; j++)
        RT_lab_data_to_RT_lab.data[i*column+j] = Matrix_get(mytoken,i,j).payload.Double;
    }
	n=sendto (sd,(char *)&RT_lab_data_to_RT_lab, sizeof(RT_lab_data_to_RT_lab), 0, (struct sockaddr *)&send_ad, sizeof(send_ad));
	return n; 
}

// make a new integer token from the given value.
Token Int_new(int i) {
    Token result;
    result.type = TYPE_Int;
    result.payload.Int = i;
    return result;
}
Token Int_convert(Token token, ...) {
    switch (token.type) {
        #ifdef TYPE_Double
        case TYPE_Double:
        token.payload.Int = DoubletoInt(token.payload.Double);
        break;
        #endif
        // FIXME: not finished
        default:
        fprintf(stderr, "Int_convert(): Conversion from an unsupported type. (%d)\n", token.type);
        break;
    }
    token.type = TYPE_Int;
    return token;
}
Token Int_toString(Token thisToken, ...) {
    return String_new(InttoString(thisToken.payload.Int));
}
Token Int_isCloseTo(Token thisToken, ...) {
    va_list argp;
    Token otherToken;
    Token tolerance;
    va_start(argp, thisToken);
    otherToken = va_arg(argp, Token);
    tolerance = va_arg(argp, Token);
    va_end(argp);
    return Boolean_new(fabs(thisToken.payload.Int - otherToken.payload.Int) < tolerance.payload.Double);
}
Token Int_multiply(Token thisToken, ...) {
    va_list argp;
    Token result;
    Token otherToken;
    va_start(argp, thisToken);
    otherToken = va_arg(argp, Token);
    switch (otherToken.type) {
        case TYPE_Int:
        result = Int_new(thisToken.payload.Int * otherToken.payload.Int);
        break;
        #ifdef TYPE_Double
        case TYPE_Double:
        result = Double_new(thisToken.payload.Int * otherToken.payload.Double);
        break;
        #endif
        // FIXME: not finished
        default:
        fprintf(stderr, "Int_multiply(): Multiply with an unsupported type. (%d)\n", otherToken.type);
        exit(1);
    }
    va_end(argp);
    return result;
}
/* Instead of Int_delete(), we call scalarDelete(). */
inline Token convert_Matrix_Matrix(Token a1) {
    return a1;
}
inline Token multiply_Matrix_Matrix(Token a1, Token a2) {
    return Matrix_multiply(a1, a2);
}
inline double convert_Double_Double(double a) {
    return a;
}
inline double multiply_Double_Double(double a1, double a2) {
    return a1 * a2;
}
inline double subtract_Double_Double(double a1, double a2) {
    return a1 - a2;
}
#define negate_Double(a) -a
inline double add_Double_Double(double a1, double a2) {
    return a1 + a2;
}
inline double divide_Double_Double(double a1, double a2) {
    return a1 / a2;
}
/* Generate shared code for Controller */
Token Scale_scaleOnLeft(Token input, double factor) {
    int i;
    Token result;
    if (input.type == TYPE_Array) {
        result = Array_new(input.payload.Array->size, 0);
        for (i = 0; i < input.payload.Array->size; i++) {
            result.payload.Array->elements[i] = Scale_scaleOnLeft(Array_get(input, i), factor);
        }
        return result;
    } else {
        return functionTable[(int)Double_new(factor).type][FUNC_multiply](Double_new(factor), input);
    }
}
Token Scale_scaleOnRight(Token input, double factor) {
    int i;
    Token result;
    if (input.type == TYPE_Array) {
        result = Array_new(input.payload.Array->size, 0);
        for (i = 0; i < input.payload.Array->size; i++) {
            result.payload.Array->elements[i] = Scale_scaleOnRight(Array_get(input, i), factor);
        }
        return result;
    } else {
        return functionTable[(int)input.type][FUNC_multiply](input, Double_new(factor));
    }
}
/* max and min may be used by the Expression actor. */
#ifndef max
#define max(a,b) ((a)>(b)?(a):(b))