cmdutil.c

一个C语言写的读入位置跟踪器数据的源程序

    Remarks:
*/
int printmatrix(birddata,buttonmode,displayon,datafilestream)
short * birddata;
short buttonmode;
unsigned char displayon;
FILE * datafilestream;
{
    short i;
	short row;
    float floatdata[9];
    char * printdataformat[3] = {"\t%7.3f\t%7.3f\t%7.3f\n",
				 "\t%7.3f\t%7.3f\t%7.3f\n",
				 "\t%7.3f\t%7.3f\t%7.3f"};
    char * printbuttonformat = "\t%3d";

    /*
	Only compute if display or file is enabled
    */
    if ((displayon) || (datafilestream))
    {
	for (i=0;i<9;i++)
	    floatdata[i] = (float)(birddata[i] * WTF);
    }

    /*
	Display the Data and Button Value (if required)
    */
    if (displayon)
    {
	/*
	    send out three rows of data
	*/
	    for(row=0;row<3;row++)      /* print the rows */
	    {
		    printf(printdataformat[row],
					    floatdata[row + 0],
					    floatdata[row + 3],
					    floatdata[row + 6]);

	    /*
		Display the Button Value if required
	    */
		    if ((row == 2) && (buttonmode != 0))
		printf(printbuttonformat,buttonvalue);
	    }
    }

    /*
	Save the Data to a File...only if one exists!
    */
    if (datafilestream)
    {
	/*
	    send out three rows of data
	*/
	    for(row=0;row<3;row++)      /* print the rows */
	    {
		    fprintf(datafilestream,printdataformat[row],
					    floatdata[row + 0],
					    floatdata[row + 3],
					    floatdata[row + 6]);

	    /*
		Display the Button Value if required
	    */
		    if ((row == 2) && (buttonmode != 0))
			    fprintf(datafilestream,printbuttonformat,buttonvalue);
	    }
    }
    return(9);
}

/*
    printposangles      Print and File the Position and Angle Data

    Prototype in:       cmdutil.h

    Parameters Passed:  birddata - array of birddata
			buttonmode - ON/OFF
			displayon - DISPLAYON/OFF
			datafilestream - file to store data in..if any

    Return Value:       int 6

    Remarks:
*/
int printposangles(birddata,buttonmode,displayon,datafilestream)
short * birddata;
short buttonmode;
unsigned char displayon;
FILE * datafilestream;
{
    short i;
    float floatdata[6];
    char * printdataformat = "\t%7.2f\t%7.2f\t%7.2f\t%7.2f\t%7.2f\t%7.2f";
    char * printbuttonformat = "\t%3d";

    /*
	Only compute if display or file is enabled
    */
    if ((displayon) || (datafilestream))
    {
	for (i=0;i<3;i++)
	    floatdata[i] = (float)(birddata[i] * posk);

	for (i=3;i<6;i++)
	    floatdata[i] = (float)(birddata[i] * ANGK);
    }

    /*
	Display the Data and Button Value (if required)
    */
    if (displayon)
    {
	printf(printdataformat,floatdata[0],floatdata[1],floatdata[2],
			       floatdata[3],floatdata[4],floatdata[5]);
	if (buttonmode != 0)
	    printf(printbuttonformat, buttonvalue);
    }

    /*
	Save the Data to a File...only if one exists!
    */
    if (datafilestream)
    {
	/*
	    print the data to the file
	*/
	fprintf(datafilestream,printdataformat,
		floatdata[0],floatdata[1],floatdata[2],
		floatdata[3],floatdata[4],floatdata[5]);

	/*
	    save the Button Value if required
	*/
	if (buttonmode != 0)
	    fprintf(datafilestream, printbuttonformat, buttonvalue);
    }
    return(6);
}

/*
    printquaternions    Print and File the Quaternion Data

    Prototype in:       cmdutil.h

    Parameters Passed:  birddata - array of birddata
			buttonmode - ON/OFF
			displayon - DISPLAYON/OFF
			datafilestream - file to store data in..if any

    Return Value:       int 4

    Remarks:
*/
int printquaternions(birddata,buttonmode,displayon,datafilestream)
short * birddata;
short buttonmode;
unsigned char displayon;
FILE * datafilestream;
{
    short i;
    float floatdata[4];
    char * printdataformat = "\t%8.4f %8.4f %8.4f %8.4f";
    char * printbuttonformat = "\t%3d";

    /*
	Only compute if display or file is enabled
    */
    if ((displayon) || (datafilestream))
    {
	for (i=0;i<4;i++)
	    floatdata[i] = (float)(birddata[i] * WTF);
    }

    /*
	Display the Data and Button Value (if required)
    */
    if (displayon)
    {
	printf(printdataformat,floatdata[0],floatdata[1],
			       floatdata[2],floatdata[3]);
	if (buttonmode != 0)
	    printf(printbuttonformat, buttonvalue);
    }

    /*
	Save the Data to a File...only if one exists!
    */
    if (datafilestream)
    {
	/*
	    print the data to the file
	*/
	fprintf(datafilestream,printdataformat,
		floatdata[0],floatdata[1],floatdata[2],floatdata[3]);

	/*
	    save the Button Value if required
	*/
	if (buttonmode != 0)
	    fprintf(datafilestream, printbuttonformat, buttonvalue);
    }
    return(4);
}

/*
    printposquaternions Print and File the Position and Quaternion Data

    Prototype in:       cmdutil.h

    Parameters Passed:  birddata - array of birddata
			buttonmode - ON/OFF
			displayon - DISPLAYON/OFF
			datafilestream - file to store data in..if any

    Return Value:       int 7

    Remarks:
*/
int printposquaternions(birddata,buttonmode,displayon,datafilestream)
short * birddata;
short buttonmode;
unsigned char displayon;
FILE * datafilestream;
{
    short i;
    float floatdata[7];
    char * printdataformat = "%7.2f %7.2f %7.2f %8.4f %8.4f %8.4f %8.4f";
    char * printbuttonformat = "\t%3d";

    /*
	Only compute if display or file is enabled
    */
    if ((displayon) || (datafilestream))
    {
	for (i=0;i<3;i++)
	    floatdata[i] = (float)(birddata[i] * posk);

	for (i=3;i<7;i++)
	    floatdata[i] = (float)(birddata[i] * WTF);
    }

    /*
	Display the Data and Button Value (if required)
    */
    if (displayon)
    {
	printf(printdataformat,floatdata[0],floatdata[1],
			       floatdata[2],floatdata[3],
			       floatdata[4],floatdata[5],
			       floatdata[6]);
	if (buttonmode != 0)
	    printf(printbuttonformat, buttonvalue);
    }

    /*
	Save the Data to a File...only if one exists!
    */
    if (datafilestream)
    {
	/*
	    print the data to the file
	*/
	fprintf(datafilestream,printdataformat,
			       floatdata[0],floatdata[1],
			       floatdata[2],floatdata[3],
			       floatdata[4],floatdata[5],
			       floatdata[6]);

	/*
	    save the Button Value if required
	*/
	if (buttonmode != 0)
	    fprintf(datafilestream, printbuttonformat, buttonvalue);
    }
    return(7);
}

/*
    printposmatrix      Print and File the Position and Matrix Data

    Prototype in:       cmdutil.h

    Parameters Passed:  birddata - array of birddata
			buttonmode - ON/OFF
			displayon - DISPLAYON/OFF
			datafilestream - file to store data in..if any

    Return Value:       int 12

    Remarks:
*/
int printposmatrix(birddata,buttonmode,displayon,datafilestream)
short * birddata;
short buttonmode;
unsigned char displayon;
FILE * datafilestream;
{
    short i;
    short row;
    float floatdata[12];
    char * printdataformat1 = "\t%7.2f\t%7.2f\t%7.2f\n";
    char * printdataformat2[3] = {"\t%7.3f\t%7.3f\t%7.3f\n",
				 "\t%7.3f\t%7.3f\t%7.3f\n",
				 "\t%7.3f\t%7.3f\t%7.3f"};
    char * printbuttonformat = "\t%3d";

    /*
	Only compute if display or file is enabled
    */
    if ((displayon) || (datafilestream))
    {
	for (i=0;i<3;i++)
	    floatdata[i] = (float)(birddata[i] * posk);

	for (i=3;i<12;i++)
	    floatdata[i] = (float)(birddata[i] * WTF);

    }

    /*
	Display the Data and Button Value (if required)
    */
    if (displayon)
    {
	printf(printdataformat1,floatdata[0],floatdata[1],
			       floatdata[2]);

	/*
	    send out three rows of data
	*/
	    for(row=0;row<3;row++)      /* print the rows */
	    {
		    printf(printdataformat2[row],
					    floatdata[row + 3],
					    floatdata[row + 6],
					    floatdata[row + 9]);

	    /*
		Display the Button Value if required
	    */
		    if ((row == 2) && (buttonmode != 0))
		printf(printbuttonformat,buttonvalue);
	    }
    }

    /*
	Save the Data to a File...only if one exists!
    */
    if (datafilestream)
    {
	fprintf(datafilestream,printdataformat1,floatdata[0],floatdata[1],
			       floatdata[2]);

	/*
	    send out three rows of data
	*/
	    for(row=0;row<3;row++)      /* print the rows */
	    {
		    fprintf(datafilestream,printdataformat2[row],
					    floatdata[row + 3],
					    floatdata[row + 6],
					    floatdata[row + 9]);

	    /*
		Display the Button Value if required
	    */
		    if ((row == 2) && (buttonmode != 0))
		fprintf(datafilestream,printbuttonformat,buttonvalue);
	    }
    }
    return(12);
}


/*
    printarray          -   Print Array Data

    Prototype in:       cmdutil.h

    Parameters Passed:  dataarray       - character/data array pointer
			size            - number of items to print

    Return Value:       TRUE

    Remarks:            prints each item of dataarray as a decimal number
			followed by a CR LF.
*/
int printarray(dataarray,size)
char * dataarray;
short size;
{
    while (size--)
		printf("%d\n\r",*dataarray++);

    return(TRUE);
}

/*
    dumpbirdarray       -   Dump Data from Address

    Prototype in:       cmdutil.h

    Parameters Passed:  address     - address of data to read
			array       - pointer to character array to store
				      data in

    Return Value:       TRUE if successful
			FALSE otherwise
    Remarks:            reads 18 bytes starting from the bird starting
			at the address sent down to the bird via the
			'a' command.

    NOTE:       The Debug command 'z' must be active prior to calling
		dumpbirdarray
*/
int dumpbirdarray(address,array)
unsigned short address;
unsigned char * array;
{
    static unsigned char dumpcmd[] = {'a',0,0};
	short rxchar;
	short charcount = 0;

    /*
	put address in the dumpcmd..kinda confusing but proper
    */
	*(unsigned short *)&dumpcmd[1] = address;

    /*
	Send the command
    */
    if (send_serial_cmd(dumpcmd,3) != 3)
	return(FALSE);

    /*
	Get the 18 characters back
    */
    while (charcount++ < 18)
    {
	if ((rxchar = waitforchar()) < 0)
	{
			printf("Didn't get back Dump Array from the Bird\n\r");
	    hitkeycontinue();
	    return(FALSE);
	}

	/*
	    Store the character
	*/
	*array++ = (unsigned char) rxchar;
    }
    return(TRUE);
}


/*
    getangles           - Get Angles from User

    Prototype in:       cmdutil.h

    Parameters Passed:  promptstrg      - string used to prompt user
			angle           - pointer to float array

    Return Value:       TRUE if got OK
			ESC_SEL if user selected ESC

    Remarks:            prompts user to enter 3 floating point angles and
			stores them in the angle array
*/
int getangles(promptstrg,angle)
char * promptstrg;
float * angle;
{
	short invalid;
	short i = 0;
    char floatstring[80];
    static float anglevalidlow[] = {-180.0,-90.0,-180.0};
    static float anglevalidhigh[] = {180.0,90.0,180.0};
    static char * anglepromptmsg[]= {"Azimuth","Elevation","Roll"};
	static char * invalidanglemsg = "\n\r** ERROR ** invalid angle\n\r";

    /*
	Put up a Prompt String
    */