controller.c

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

#ifndef PT_NO_STDLIB_H
#include <stdlib.h>
#endif
#ifndef PT_NO_STRING_H
#include <string.h>
#endif
#ifndef PT_NO_MATH_H
#include <math.h>
#endif
#ifndef PT_NO_STDARG_H
#include <stdarg.h>
#endif
#ifndef PT_NO_STDIO_H
#include <stdio.h>
#endif
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <spawn.h>
#include <errno.h>
#include <unistd.h>
#include <sys/select.h>

#include "f2c.h"
#include "clapack.h"
#include "blaswrap.h"
#include "cblas.h"

/* Generate type resolution code for .Controller */
// Constants.
#define MISSING 0
#define boolean unsigned char
/* Infinity is a valid Ptolemy identifier. */
#define Infinity HUGE_VAL
#ifdef linux
/* Linux tends to have NAN. */
#define NaN (__builtin_nanf (""))
#else /*linux*/
#define NaN nanf(0)
#endif /*linux*/
#define false 0
#define true 1
#define TYPE_Token -1
#define TYPE_String 0
#define TYPE_Array 1
#define TYPE_Double 2
#define TYPE_Boolean 3
#define TYPE_Matrix 4
#define TYPE_Int 5
#define FUNC_convert 0
#define FUNC_toString 1
#define FUNC_isCloseTo 2
#define FUNC_multiply 3
#define FUNC_delete 4
typedef struct token Token;
typedef char* StringToken;
// Definition of the array struct.
struct array {
    int size;                                   // size of the array.
    Token* elements;                            // array of Token elements.
    //char elementType;                          // type of the elements.
};
typedef struct array* ArrayToken;
typedef double DoubleToken;
typedef boolean BooleanToken;
#include <stdarg.h>     // Needed Matrix_new va_* macros
struct matrix {
    unsigned int row;            // number of rows.
    unsigned int column;         // number of columns.
    Token *elements;            // matrix of pointers to the elements.
    //unsigned char elementsType;  // type of all the elements.
};
typedef struct matrix* MatrixToken;
typedef int IntToken;
// Token structure containing the specified types.
struct token {         // Base type for tokens.
    char type;         // TYPE field has to be the first field.
    union typeMembers {
        // type member declarations [i.e. Type1Token Type1;]
        StringToken String;
        ArrayToken Array;
        DoubleToken Double;
        BooleanToken Boolean;
        MatrixToken Matrix;
        IntToken Int;
    } payload;
};
#define MAXRECVSIZE 64
struct RT_lab_data  //the data format from the RT_lab 
{
  int  dev_id;            
  int  msg_id;             
  int  msg_len;            
  double data[MAXRECVSIZE];  
};
Token emptyToken; /* Used by *_delete() and others. */
Token String_convert (Token thisToken, ...);
Token String_toString (Token thisToken, ...);
Token String_equals (Token thisToken, ...);
Token String_isCloseTo (Token thisToken, ...);
Token String_multiply (Token thisToken, ...);
Token String_delete (Token thisToken, ...);
Token Array_convert (Token thisToken, ...);
Token Array_toString (Token thisToken, ...);
Token Array_isCloseTo (Token thisToken, ...);
Token Array_multiply (Token thisToken, ...);
Token Array_delete (Token thisToken, ...);
Token Double_convert (Token thisToken, ...);
Token Double_toString (Token thisToken, ...);
Token Double_isCloseTo (Token thisToken, ...);
Token Double_multiply (Token thisToken, ...);
Token Boolean_convert (Token thisToken, ...);
Token Boolean_toString (Token thisToken, ...);
Token Boolean_equals (Token thisToken, ...);
Token Boolean_isCloseTo (Token thisToken, ...);
Token Boolean_multiply (Token thisToken, ...);
Token Matrix_convert (Token thisToken, ...);
Token Matrix_toString (Token thisToken, ...);
Token Matrix_isCloseTo (Token thisToken, ...);
Token Matrix_multiply (Token thisToken, ...);
Token Matrix_delete (Token thisToken, ...);
Token Int_convert (Token thisToken, ...);
Token Int_toString (Token thisToken, ...);
Token Int_isCloseTo (Token thisToken, ...);
Token Int_multiply (Token thisToken, ...);
/* We share one method between all types so as to reduce code size. */
Token unsupportedTypeFunction(Token token, ...) {
    fprintf(stderr, "Attempted to call unsupported method on a type.\n");
    exit(1);
    return emptyToken;
}
/* We share one method between all scalar types so as to reduce code size. */
Token scalarDelete(Token token, ...) {
    /* We need to return something here because all the methods are declared
    * as returning a Token so we can use them in a table of functions.
    */
    return emptyToken;
}
Token String_new(char* s);
Token Array_new(int size, int given, ...);
// Array_get: get an element of an array.
Token Array_get(Token array, int i) {
    return array.payload.Array->elements[i];
}
// Array_set: set an element of an array.
void Array_set(Token array, int i, Token element) {
    array.payload.Array->elements[i] = element;
}
// Array_resize: Change the size of an array,
// preserving those elements that fit.
void Array_resize(Token array, int size) {
    array.payload.Array->size = size;
    // FIXME: Does realloc() initialize memory? If not, then we need to do that.
    array.payload.Array->elements = (Token*) realloc(array.payload.Array->elements, size * sizeof(Token));
}
// Array_insert: Append the specified element to the end of an array.
void Array_insert(Token array, Token token) {
    int oldSize = array.payload.Array->size++;
    Array_resize(array, array.payload.Array->size);
    array.payload.Array->elements[oldSize] = token;
}
Token Double_new(double d);
Token Boolean_new(boolean b);
Token Matrix_new(int row, int column, int given, ...);
Token Matrix_get(Token token, int row, int column) {
    return token.payload.Matrix->elements[column * token.payload.Matrix->row + row];
}
void Matrix_set(Token matrix, int row, int column, Token element) {
    matrix.payload.Matrix->elements[column * matrix.payload.Matrix->row + row] = element;
}
Token Int_new(int i);
#define StringtoInt atoi
#define StringtoDouble atof
#define StringtoLong atol
#define DoubletoInt floor
#define InttoDouble (double)
#define InttoLong (long)
char* InttoString (int i) {
    char* string = (char*) malloc(sizeof(char) * 12);
    sprintf((char*) string, "%d", i);
    return string;
}
char* LongtoString (long long l) {
    char* string = (char*) malloc(sizeof(char) * 22);
    sprintf(string, "%lld", l);
    return string;
}
char* DoubletoString (double d) {
    int index;
    char* string = (char*) malloc(sizeof(char) * 20);
    sprintf(string, "%.14g", d);
    // Make sure that there is a decimal point.
    if (strrchr(string, '.') == NULL) {
        index = strlen(string);
        if (index == 20) {
            string = (char*) realloc(string, sizeof(char) * 22);
        }
        string[index] = '.';
        string[index + 1] = '0';
        string[index + 2] = '\0';
    }
    return string;
}
char* BooleantoString (boolean b) {
    char *results;
    if (b) {
        // AVR does not have strdup
        results = (char*) malloc(sizeof(char) * 5);
        strcpy(results, "true");
    } else {
        results = (char*) malloc(sizeof(char) * 6);
        strcpy(results, "false");
    }
    return results;
}
#define NUM_TYPE 6
#define NUM_FUNC 5
Token (*functionTable[NUM_TYPE][NUM_FUNC])(Token, ...)= {
{String_convert, String_toString, String_equals, unsupportedTypeFunction, String_delete},
{Array_convert, Array_toString, Array_isCloseTo, Array_multiply, Array_delete},
{Double_convert, Double_toString, Double_isCloseTo, Double_multiply, scalarDelete},
{Boolean_convert, Boolean_toString, Boolean_equals, unsupportedTypeFunction, scalarDelete},
{Matrix_convert, Matrix_toString, Matrix_isCloseTo, Matrix_multiply, Matrix_delete},
{Int_convert, Int_toString, Int_isCloseTo, Int_multiply, scalarDelete}
};
/* Make a new integer token from the given value. */
Token String_new(char* s) {
    Token result;
    result.type = TYPE_String;
    result.payload.String = strdup(s);
    return result;
}
Token String_convert(Token token, ...) {
    char* stringPointer;
    switch (token.type) {
        #ifdef TYPE_Boolean
        case TYPE_Boolean:
        stringPointer = BooleantoString(token.payload.Boolean);
        break;
        #endif
        #ifdef TYPE_Int
        case TYPE_Int:
        stringPointer = InttoString(token.payload.Int);
        break;
        #endif
        #ifdef TYPE_Double
        case TYPE_Double:
        stringPointer = DoubletoString(token.payload.Double);
        break;
        #endif
        default:
        // FIXME: not finished
        fprintf(stderr, "String_convert(): Conversion from an unsupported type. (%d)\n", token.type);
        break;
    }
    token.payload.String = stringPointer;
    token.type = TYPE_String;
    return token;
}
Token String_toString(Token thisToken, ...) {
    // Guarrantee to return a new string.
    char* result = (char*) malloc(sizeof(char) * (3 + strlen(thisToken.payload.String)));
    sprintf(result, "\"%s\"", thisToken.payload.String);
    return String_new(result);
}
Token String_equals(Token thisToken, ...) {
    va_list argp;
    Token otherToken;
    va_start(argp, thisToken);
    otherToken = va_arg(argp, Token);
    va_end(argp);
    return Boolean_new(!strcmp(thisToken.payload.String, otherToken.payload.String));
}
Token String_delete(Token token, ...) {
    free(token.payload.String);
    /* We need to return something here because all the methods are declared
    * as returning a Token so we can use them in a table of functions.
    */
    return emptyToken;
}
// Array_new: Create a new array with the specified elements.
// The "size" argument specifies the size of the array, and
// the "given" argument specifies the number of provided elements
// (which will typically be <= size).
// The rest of the arguments are the provided elements (there
// should be "given" of them). The given elements
// should be of type Token *.
Token Array_new(int size, int given, ...) {
    va_list argp;
    int i;
    Token result;
    char elementType;
    result.type = TYPE_Array;
    result.payload.Array = (ArrayToken) malloc(sizeof(struct array));