Ejemplo n.º 1
1
Archivo: rc.c Proyecto: mingpen/OpenNT
int _CRTAPI1
rc_main(
    int argc,
    char**argv
    )
{
    PCHAR       r;
    PCHAR       x;
    PCHAR       s1, s2, s3;
    int         n;
    PCHAR       pchIncludeT;
    ULONG       cchIncludeMax;
    int         fInclude = TRUE;        /* by default, search INCLUDE */
    int         fIncludeCurrentFirst = TRUE; /* by default, add current dir to start of includes */
    int         cDefine = 0;
    int         cUnDefine = 0;
    PCHAR       pszDefine[cDefineMax];
    PCHAR       pszUnDefine[cDefineMax];
    CHAR        szDrive[_MAX_DRIVE];
    CHAR        szDir[_MAX_DIR];
    CHAR        szFName[_MAX_FNAME];
    CHAR        szExt[_MAX_EXT];
    CHAR        szFullPath[_MAX_PATH];
    CHAR        szIncPath[_MAX_PATH];
    CHAR        buf[10];
    CHAR        *szRC;
    PCHAR       *ppargv;
    int         rcpp_argc;

    /* Set up for this run of RC */
    if (_setjmp(jb)) {
        return Nerrors;
    }

    hHeap = RCInit();
    if (hHeap == NULL) {
        SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1120), 0x01000000);
        quit(Msg_Text);
    }

    pchInclude = pchIncludeT = MyAlloc(_MAX_PATH*2);
    cchIncludeMax = _MAX_PATH*2;

    szRC = argv[0];

    /* process the command line switches */
    while ((argc > 1) && (IsSwitchChar(*argv[1]))) {
        switch (toupper(argv[1][1])) {
            case '?':
            case 'H':
                /* print out help, and quit */
                SendError("\n");
                SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(10001),
                        VER_PRODUCTVERSION_STR, VER_PRODUCTBUILD);
                SendError(Msg_Text);
                SendError(GET_MSG(20001));
                SendError("\n");

                return 0;   /* can just return - nothing to cleanup, yet. */

            case 'B':
                if (toupper(argv[1][2]) == 'R') {   /* base resource id */
                    unsigned long id;
                    if (isdigit(argv[1][3]))
                        argv[1] += 3;
                    else if (argv[1][3] == ':')
                        argv[1] += 4;
                    else {
                        argc--;
                        argv++;
                        if (argc <= 1)
                            goto BadId;
                    }
                    if (*(argv[1]) == 0)
                        goto BadId;
                    id = atoi(argv[1]);
                    if (id < 1 || id > 32767)
                        quit(GET_MSG(1210));
                    idBase = (WORD)id;
                    break;

BadId:
                    quit(GET_MSG(1209));
                }
                break;

            case 'C':
                /* Check for the existence of CodePage Number */
                if (argv[1][2])
                    argv[1] += 2;
                else {
                    argc--;
                    argv++;
                }

                /* Now argv point to first digit of CodePage */

                if (!argv[1])
                    quit(GET_MSG(1204));

                uiCodePage = atoi(argv[1]);

                if (uiCodePage == 0)
                    quit(GET_MSG(1205));

                /* Check if uiCodePage exist in registry. */
                if (!IsValidCodePage (uiCodePage))
                    quit(GET_MSG(1206));
                break;

            case 'D':
                /* if not attached to switch, skip to next */
                if (argv[1][2])
                    argv[1] += 2;
                else {
                    argc--;
                    argv++;
                }

                /* remember pointer to string */
                pszDefine[cDefine++] = argv[1];
                if (cDefine > cDefineMax) {
                    SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1105), argv[1]);
                    quit(Msg_Text);
                }
                break;

            case 'F':
                switch (toupper(argv[1][2])) {
                    case 'O':
                        if (argv[1][3])
                            argv[1] += 3;
                        else {
                            argc--;
                            argv++;
                        }
                        if (argc > 1)
                            strcpy(resname, argv[1]);
                        else
                            quit(GET_MSG(1101));

                        break;

                    default:
                        SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1103), argv[1]);
                        quit(Msg_Text);
                }
                break;

            case 'I':
                /* add string to directories to search */
                /* note: format is <path>\0<path>\0\0 */

                /* if not attached to switch, skip to next */
                if (argv[1][2])
                    argv[1] += 2;
                else {
                    argc--;
                    argv++;
                }

                if (!argv[1])
                    quit(GET_MSG(1201));

                if ((strlen(argv[1]) + 1 + strlen(pchInclude)) >= cchIncludeMax) {
                    cchIncludeMax = strlen(pchInclude) + strlen(argv[1]) + _MAX_PATH*2;
                    pchIncludeT = MyAlloc(cchIncludeMax);
                    strcpy(pchIncludeT, pchInclude);
                    MyFree(pchInclude);
                    pchInclude = pchIncludeT;
                    pchIncludeT = pchInclude + strlen(pchIncludeT) + 1;
                }

                /* if not first switch, write over terminator with semicolon */
                if (pchInclude != pchIncludeT)
                    pchIncludeT[-1] = ';';

                /* copy the path */
                while ((*pchIncludeT++ = *argv[1]++) != 0)
                    ;
                break;

            case 'L':
                /* if not attached to switch, skip to next */
                if (argv[1][2])
                    argv[1] += 2;
                else {
                    argc--;
                    argv++;
                }

                if (!argv[1])
                    quit(GET_MSG(1202));
                if (sscanf( argv[1], "%x", &language ) != 1)
                    quit(GET_MSG(1203));

                while (*argv[1]++ != 0)
                    ;

                break;

            case 'M':
                fMacRsrcs = TRUE;
                goto MaybeMore;

            case 'N':
                fAppendNull = TRUE;
                goto MaybeMore;

            case 'P':
                fPreprocessOnly = TRUE;
                break;

            case 'R':
                goto MaybeMore;

            case 'S':
                // find out from BRAD what -S does
                fAFXSymbols = TRUE;
                break;

            case 'U':
                /* if not attached to switch, skip to next */
                if (argv[1][2])
                    argv[1] += 2;
                else {
                    argc--;
                    argv++;
                }

                /* remember pointer to string */
                pszUnDefine[cUnDefine++] = argv[1];
                if (cUnDefine > cDefineMax) {
                    SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1104), argv[1]);
                    quit(Msg_Text);
                }
                break;

            case 'V':
                fVerbose = TRUE; // AFX doesn't set this
                goto MaybeMore;

            case 'X':
                /* remember not to add INCLUDE path */
                fInclude = FALSE;

                // VC seems to feel the current dir s/b added first no matter what...
                // If -X! is specified, don't do that.
                if (argv[1][2] == '!') {
                    fIncludeCurrentFirst = FALSE;
                    argv[1]++;
                }

MaybeMore:      /* check to see if multiple switches, like -xrv */
                if (argv[1][2]) {
                    argv[1][1] = '-';
                    argv[1]++;
                    continue;
                }
                break;

            case 'Z':

                /* if not attached to switch, skip to next */
                if (argv[1][2])
                    argv[1] += 2;
                else {
                    argc--;
                    argv++;
                }

                if (!argv[1])
                    quit(GET_MSG(1211));

                s3 = strchr(argv[1], '/');
                if (s3 == NULL)
                    quit(GET_MSG(1212));

                *s3 = '\0';
                MultiByteToWideChar(CP_ACP, MB_PRECOMPOSED, s3+1, -1, szSubstituteFontName, MAXTOKSTR);

                s1 = argv[1];
                do {
                    s2 = strchr(s1, ',');
                    if (s2 != NULL)
                        *s2 = '\0';

                    if (strlen(s1)) {
                        if (nBogusFontNames >= 16)
                            quit(GET_MSG(1213));

                        pszBogusFontNames[nBogusFontNames] = MyAlloc((strlen(s1)+1) * sizeof(WCHAR));
                        MultiByteToWideChar(CP_ACP, MB_PRECOMPOSED, s1, -1, pszBogusFontNames[nBogusFontNames], MAXTOKSTR);
                        nBogusFontNames += 1;
                    }

                    if (s2 != NULL)
                        *s2++ = ',';
                    }
                while (s1 = s2);

                *s3 =  '/';

                while (*argv[1]++ != 0)
                    ;
                break;

            default:
                SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1106), argv[1]);
                quit(Msg_Text);
        }

        /* get next argument or switch */
        argc--;
        argv++;
    }

    /* make sure we have at least one file name to work with */
    if (argc != 2 || *argv[1] == '\0')
        quit(GET_MSG(1107));

    if (fVerbose) {
        SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(10001),
                VER_PRODUCTVERSION_STR, VER_PRODUCTBUILD);
        printf(Msg_Text);
        printf("%s\n", GET_MSG(10002));
    }

    // Support Multi Code Page

    //  If user did NOT indicate code in command line, we have to set Default
    //     for NLS Conversion

    if (uiCodePage == 0) {

        CHAR *pchCodePageString;

        /* At first, search ENVIRONMENT VALUE */

        if ((pchCodePageString = getenv("RCCODEPAGE")) != NULL) {
            uiCodePage = atoi(pchCodePageString);

            if (uiCodePage == 0 || !IsValidCodePage(uiCodePage))
                quit(GET_MSG(1207));
        }
        else
        {
            /* We use System ANSI Code page (ACP) */
            uiCodePage = GetACP();
        }
    }
    uiDefaultCodePage = uiCodePage;
    if (fVerbose)
        printf("Using codepage %d as default\n", uiDefaultCodePage);

    /* If we have no extension, assumer .rc                             */
    /* If .res extension, make sure we have -fo set, or error           */
    /* Otherwise, just assume file is .rc and output .res (or resname)  */

    //
    // We have to be careful upper casing this, because the codepage
    // of the filename might be in something other than current codepage.
    //
    MultiByteToWideChar(uiCodePage, MB_PRECOMPOSED, argv[1], -1, tokenbuf, MAXSTR+1);
    if (CharUpperBuff(tokenbuf, wcslen(tokenbuf)) == 0)
        _wcsupr(tokenbuf);
    WideCharToMultiByte(uiCodePage, 0, tokenbuf, -1, argv[1], strlen(argv[1]), NULL, NULL);
    _splitpath(argv[1], szDrive, szDir, szFName, szExt);

    if (!(*szDir || *szDrive)) {
        strcpy(szIncPath, ".;");
    } else {
        strcpy(szIncPath, szDrive);
        strcat(szIncPath, szDir);
        strcat(szIncPath, ";.;");
    }

    if ((strlen(szIncPath) + 1 + strlen(pchInclude)) >= cchIncludeMax) {
        cchIncludeMax = strlen(pchInclude) + strlen(szIncPath) + _MAX_PATH*2;
        pchIncludeT = MyAlloc(cchIncludeMax);
        strcpy(pchIncludeT, pchInclude);
        MyFree(pchInclude);
        pchInclude = pchIncludeT;
        pchIncludeT = pchInclude + strlen(pchIncludeT) + 1;
    }

    pchIncludeT = MyAlloc(cchIncludeMax);

    if (fIncludeCurrentFirst) {
        strcpy(pchIncludeT, szIncPath);
        strcat(pchIncludeT, pchInclude);
    } else {
        strcpy(pchIncludeT, pchInclude);
        strcat(pchIncludeT, ";");
        strcat(pchIncludeT, szIncPath);
    }

    MyFree(pchInclude);
    pchInclude = pchIncludeT;
    pchIncludeT = pchInclude + strlen(pchIncludeT) + 1;

    if (!szExt[0]) {
        strcpy(szExt, ".RC");
    }
    else if (strcmp (szExt, ".RES") == 0) {
        quit (GET_MSG(1208));
    }

    _makepath(inname, szDrive, szDir, szFName, szExt);
    if (fPreprocessOnly) {
        _makepath(szPreProcessName, NULL, NULL, szFName, ".rcpp");
    }

    /* Create the name of the .RES file */
    if (resname[0] == 0) {
        // if building a Mac resource file, we use .rsc to match mrc's output
        _makepath(resname, szDrive, szDir, szFName, fMacRsrcs ? ".RSC" : ".RES");
    }

    /* create the temporary file names */
    szTempFileName = MyAlloc(_MAX_PATH);

    _fullpath(szFullPath, resname, _MAX_PATH);
    _splitpath(szFullPath, szDrive, szDir, NULL, NULL);

    _makepath(szTempFileName, szDrive, szDir, "RCXXXXXX", "");
    _mktemp (szTempFileName);
    szTempFileName2 = MyAlloc(_MAX_PATH);
    _makepath(szTempFileName2, szDrive, szDir, "RDXXXXXX", "");
    _mktemp (szTempFileName2);

    ppargv = szRCPP;
    *ppargv++ = "RCPP";
    rcpp_argc = 1;

    /* Open the .RES file (deleting any old versions which exist). */
    if ((fhBin = fopen(resname, "w+b")) == NULL) {
        SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1109), resname);
        quit(Msg_Text);
    }
    else {
        if (fMacRsrcs)
            MySeek(fhBin, MACDATAOFFSET, 0);
        if (fVerbose) {
            SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(10102), resname);
            printf(Msg_Text);
        }

        /* Set up for RCPP. This constructs the command line for it. */
        *ppargv = _strdup("-CP");
        rcpp_argc++ ; ppargv++;
        _itoa(uiCodePage, buf, 10);
        *ppargv = buf;
        rcpp_argc++ ; ppargv++;

        *ppargv = _strdup("-f");
        rcpp_argc++ ; ppargv++;
        *ppargv = _strdup(szTempFileName);
        rcpp_argc++ ; ppargv++;

        *ppargv = _strdup("-g");
        rcpp_argc++ ; ppargv++;
        if (fPreprocessOnly) {
            *ppargv = _strdup(szPreProcessName);
        } else {
            *ppargv = _strdup(szTempFileName2);
        }
        rcpp_argc++ ; ppargv++;

        *ppargv = _strdup("-DRC_INVOKED");
        rcpp_argc++ ; ppargv++;

        if (fAFXSymbols) {
            *ppargv = _strdup("-DAPSTUDIO_INVOKED");
            rcpp_argc++ ; ppargv++;
        }

        if (fMacRsrcs) {
            *ppargv = _strdup("-D_MAC");
            rcpp_argc++ ; ppargv++;
        }

        *ppargv = _strdup("-D_WIN32"); /* to be compatible with C9/VC++ */
        rcpp_argc++ ; ppargv++;

        *ppargv = _strdup("-pc\\:/");
        rcpp_argc++ ; ppargv++;

        *ppargv = _strdup("-E");
        rcpp_argc++ ; ppargv++;

        /* Parse the INCLUDE environment variable */

        if (fInclude) {

            *ppargv = _strdup("-I.");
            rcpp_argc++ ; ppargv++;

            /* add seperator if any -I switches */
            if (pchInclude != pchIncludeT)
                pchIncludeT[-1] = ';';

            /* read 'em */
            x = getenv("INCLUDE");
            if (x == (PCHAR)NULL)
                *pchIncludeT = '\000';
            else {
                if (strlen(pchInclude) + strlen(x) + 1 >= cchIncludeMax) {
                    cchIncludeMax = strlen(pchInclude) + strlen(x) + _MAX_PATH*2;
                    pchIncludeT = MyAlloc(cchIncludeMax);
                    strcpy(pchIncludeT, pchInclude);
                    MyFree(pchInclude);
                    pchInclude = pchIncludeT;
                }

                strcat(pchInclude, x);
                pchIncludeT = pchInclude + strlen(pchInclude);
            }
        }

        /* now put includes on the RCPP command line */
        for (x = pchInclude ; *x ; ) {

            r = x;
            while (*x && *x != ';')
                x = CharNextA(x);

            /* mark if semicolon */
            if (*x)
                *x-- = 0;

            if (*r != '\0' &&       /* empty include path? */
                *r != '%'           /* check for un-expanded stuff */
                // && strchr(r, ' ') == NULL  /* check for whitespace */
                ) {
                /* add switch */
                *ppargv = _strdup("-I");
                rcpp_argc++ ; ppargv++;
                *ppargv = _strdup(r);
                rcpp_argc++ ; ppargv++;
            }

            /* was semicolon, need to fix for searchenv() */
            if (*x) {
                *++x = ';';
                x++;
            }

        }

        /* include defines */
        for (n = 0; n < cDefine; n++) {
            *ppargv = _strdup("-D");
            rcpp_argc++ ; ppargv++;
            *ppargv = pszDefine[n];
            rcpp_argc++ ; ppargv++;
        }

        /* include undefine */
        for (n = 0; n < cUnDefine; n++) {
            *ppargv = _strdup("-U");
            rcpp_argc++ ; ppargv++;
            *ppargv = pszUnDefine[n];
            rcpp_argc++ ; ppargv++;
        }

        if (rcpp_argc > MAX_CMD) {
            quit(GET_MSG(1102));
        }
        if (fVerbose) {
            /* echo the preprocessor command */
            printf("RC:");
            for (n = 0 ; n < rcpp_argc ; n++) {
                wsprintfA(Msg_Text, " %s", szRCPP[n]);
                printf(Msg_Text);
            }
            printf("\n");
        }

        /* Add .rc with rcincludes into szTempFileName */
        if (!RC_PreProcess(inname))
            quit(Msg_Text);

        /* Run the Preprocessor. */
        if (RCPP(rcpp_argc, szRCPP, NULL) != 0)
            quit(GET_MSG(1116));
    }

    if (fPreprocessOnly) {
        sprintf(szBuf, "Preprocessed File Created in: %s\n", szPreProcessName);
        quit(szBuf);
    }

    if (fVerbose)
        printf("\n%s", inname);

    if ((fhInput = fopen(szTempFileName2, "rb")) == NULL_FILE)
        quit(GET_MSG(2180));

    if (!InitSymbolInfo())
        quit(GET_MSG(22103));

    LexInit (fhInput);
    uiCodePage = uiDefaultCodePage;
    ReadRF();               /* create .RES from .RC */
    if (!TermSymbolInfo(fhBin))
        quit(GET_MSG(22204));

    if (!fMacRsrcs)
        MyAlign(fhBin); // Pad end of file so that we can concatenate files

    CleanUpFiles();

    HeapDestroy(hHeap);

    return Nerrors;   // return success, not quitting.
}
Ejemplo n.º 2
0
int main(void)
	{
	// <editor-fold defaultstate="collapsed" desc="Initialization of HW components/modules">
	//*******************************************************************
	// Initialization of HW components/modules
	//===================================================================
	Init();
	TMRInit(2);			// Initialize Timer interface with Priority=2
	BLIInit();			// Initialize Signal interface
	//--------------------------
	BLIAsyncMorse("S", 1);	// dit-dit-dit
	MCMInitT(2.5, 2500);	// Initialize Motor Control for PPM with setting
							// Throttle to HIGH for delay interval to let ESC
							// capture Throttle range
	BLIAsyncMorse("O", 1);	// dah-dah-dah
	//--------------------------
	ADCInit(3);			// Initialize ADC to control battery
	//--------------------------
	RCInit(4);			// Initialize Receiver interface with Priority=4
	//--------------------------
	I2CInit(5, 1);		// Initialize I2C1 module with IPL=5 and Fscl=400 KHz
	//--------------------------
	UARTInitTX(6, 350);	// Initialize UART1 for TX on IPL=6 at 
	// BaudRate =   48	=>   115,200 bps	- ZigBEE
	//--------------------------------------
	// BaudRate =  100	=>   250,000 bps
	// BaudRate =  200	=>   500,000 bps
	// BaudRate =  250	=>   625,000 bps
	// BaudRate =  350	=>   833,333 bps	- SD Logger
	// BaudRate =  500	=> 1,250,000 bps
	// BaudRate = 1000	=> 2,500,000 bps
	//*******************************************************************
	// <editor-fold defaultstate="collapsed" desc="Initializing IMU">
	//==================================================================
	#ifdef __MAG_Use__
	//--------------------------------------------------------------
	// Initialize Magnetometer
	//--------------------------------------------------------------
	if (HMCInit(6, 1, 0))	// Initialize magnetic Sensor
							// ODR  = 6 (max, 75 Hz),
							// Gain = 2 (1.3 Gs)
							// DLPF = 0 (no averaging)
		BLIDeadStop("EM", 2);
	#endif
	//*******************************************************************
	BLIAsyncMorse(	"I", 1);	// dit - dit
	//==================================================================
	#ifdef __MXB_Use__
	//--------------------------------------------------------------
	// Initialize MaxBotix range finder
	//--------------------------------------------------------------
	if ( 0 == MXBInit(3, &TM.MXB) )
		BLIDeadStop("ES", 2);
	#endif
	//==================================================================
	// Initialize motion sensor - rotation rate baseline established at
	// this time - QUAD should be motionless!!!
	//------------------------------------------------------------------
	if ( MPUInit(0, 3 ) )	// Initialize motion Sensor
		// 1 kHz/(0+1) = 1000 Hz (1ms)
		// DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec)
		BLIDeadStop("EA", 2);
	//------------------------------------------------------------------
	BLIAsyncStop();
	//==================================================================
	// </editor-fold>

	//==================================================================
	BLISignalON();
	TMRDelay(2000); 	// Wait for extra 2 sec - to let ESC arm...
	// (finish the song :) )
	BLISignalOFF();
	//===================================================================
	// </editor-fold>

	//*******************************************************************
	// Quadrocopter control variables
	//-------------------------------------------------------------------
	ulong		StartTS		= 0;
	ulong		StepTS;

	RCData		RCNative;		// Control input from Receiver
	RCData		RC;				// Smoothed control input used in all
								// control calculations
	
	DCMData		IMU;			// Orientation data from DCM
	
	MCMData		MC;				// Motor control Variables

	float		BatNomV		= ADCGetBatteryNomVoltage();

	ulong		Alarm		= 0;
	
	#ifdef __CB_To_Model_Front__
		// RC native input rotated to adjust CB orientation to model front
		RCData	RC_CB_To_Model_Front;
		// Control board front does not coincide with the model front
		Matrix	CB_To_Model;
		// Build rotation matrix to adjust for orientation discrepancy
		MatrixYawRotation(__CB_To_Model_Front__, &CB_To_Model);
	#else
		// If CB orientation coincides with the fron of the model
		// we will use Native RC input as base for RC input to
		// Quadrocopter control module
		#define RC_CB_To_Model_Front  RCNative
	#endif


Re_Start:
	//==================================================================
	// Wait for the Receiver ARMing: Control should go Down and then Up
	//------------------------------------------------------------------
	BLIAsyncMorse(	"O", 1);	// doh - doh - doh
	RCSymArm();

	//==================================================================
	BLIAsyncMorse("T", 1);			// doh
	MPUAsyncStop();
	if (MPUCalibrate() != MPU_OK)	
		// Gyro Calibration filed
		BLIDeadStop("EA", 2);
	BLIAsyncStop();
	//==================================================================
	// Start IMU and wait until orientation estimate stabilizes
	//------------------------------------------------------------------
	BLIAsyncMorse(	"E", 1);		// dit
	IMUInit();
	//------------------------------------------------------------------
	QCMReset();			// Initialize (reset) QCM variables
	//------------------------------------------------------------------
	BLIAsyncStop();
	//==================================================================

	//******************************************************************
	// Control variables used to smooth RC receiver data
	//------------------------------------------------------------------
	// Reset Smothed RC data 
	//-------------------------------------------------------------------
	RC.Roll		= 0.0;
	RC.Pitch	= 0.0;
	RC.Yaw		= 0.0;
	//------------------------
	RC.Throttle	= 0.0;
	//------------------------
	RC.Control	= 1;
	//-------------------------------------------------------------------
	
	//*******************************************************************
	// Quadrocopter Control Loop
	//-------------------------------------------------------------------
	BLISignalON();
	while (1)
		{
		// Sets the "frquency" of Control Loop
		Alarm = TMRSetAlarm(10);	// Set Alarm 10 msec in the future
		//============================================================
		// Read commands from receiver - non-blocking call!
		// (we will get out of this call even if connection to the
		// receiver is lost!)
		//============================================================
		// <editor-fold defaultstate="collapsed" desc="Process Receiver feed">
		if ( RCSymRead(&RCNative) )
			{
			//------------------------------------------------------------
			// Normalize Roll and Pitch control input from RC Receiver to
			// +/- 0.35 rad (~20 degrees) and Yaw control input to
			// +/- 3.00 rad (~172 degrees)
			//------------------------------------------------------------
			RCNative.Roll 	= 0.35 * RCNative.Roll;
			RCNative.Pitch	= 0.35 * RCNative.Pitch;
			RCNative.Yaw	= 3.00 * RCNative.Yaw;
			#ifdef __CB_To_Model_Front__
				{
				// Control board front does not coincide with the model front
				Vector	RCInput;
				Vector	RCRotated;
				VectorSet(RCNative.Roll, RCNative.Pitch, RCNative.Yaw, &RCInput);
				MatrixTimesVector(&CB_To_Model, &RCInput, &RCRotated);
				RC_CB_To_Model_Front.Roll		= RCRotated.X;
				RC_CB_To_Model_Front.Pitch		= RCRotated.Y;
				RC_CB_To_Model_Front.Yaw		= RCRotated.Z;
				}
			#endif
			}
		//============================================================
		// Smooth RC data 
		//------------------------------------------------------------
		// Roll, Pitch, and Yaw are smoothed with the IIR(8)
		//------------------------------------------------------------
		RC.Roll		= (RC.Roll	* 7.0	+ RC_CB_To_Model_Front.Roll	) * 0.125;	// 1/8 = 0.125
		RC.Pitch	= (RC.Pitch	* 7.0 	+ RC_CB_To_Model_Front.Pitch) * 0.125;
		RC.Yaw		= (RC.Yaw	* 7.0	+ RC_CB_To_Model_Front.Yaw	) * 0.125;
		//------------------------------------------------------------
		// Throttle is smoothed with the IIR(4) and adjusted to
		// account for actual battery voltage. This is done to
		// improve "hovering" when throttle stick is not moving.
		//------------------------------------------------------------
		// Adjust Native (from RC) throttle to a value corresponding
		float BatAdjTh	= RCNative.Throttle	* BatNomV / ADCGetBatteryVoltage();
		//-----------------------------------------
		RC.Throttle	= (RC.Throttle * 3	+ BatAdjTh) * 0.25;		// 1/4 = 0.25
		//-----------------------------------------
		RC.Control	= RCNative.Control;
		// </editor-fold>
		//============================================================

		//============================================================
		// Implement Motor Cut-Off if RC Control is LOW
		//============================================================
		// <editor-fold defaultstate="collapsed" desc="Process RC Control">
		if ( 0 == RC.Control )
			{
			// Yes, Control is reliably low!
			//--------------------------------------------
			// Override motor control
			//--------------------------------------------
			MC.F = MC.B = MC.L = MC.R = 0;
			MCMSet(&MC);
			//--------------------------------------------
			// Reset Timing series
			StartTS = 0;
			//--------------------------------------------
			// Flight terminated, post EOF to Data Logger
			//--------------------------------------------
			TMRDelay(10);	// Wait for pipe to clear
			UARTPostIfReady( NULL, 0);
			// ... and again - to be sure!
			TMRDelay(10);	// Wait for pipe to clear
			UARTPostIfReady( NULL, 0);
			//----------------------------------
			goto Re_Start;
			}
		// </editor-fold>
		//============================================================

		//============================================================
		// Obtain and process battery charge status
		//============================================================
		// <editor-fold defaultstate="collapsed" desc="Process Battery level">
		float BatteryCharge	= QCMBatteryMgmt();	
		if (BatteryCharge < 0.35)	// BC < 35%
			{
			float MaxLevel = 2.0 * BatteryCharge;
			if (RC.Throttle > MaxLevel)
				RC.Throttle = MaxLevel;
			}
		// </editor-fold>
		//============================================================

		//============================================================
		// Read current orientation and sensor data from the IMU
		// (non-blocking call)
		//============================================================
		if (IMU_OK == IMUGetUpdate(&IMU))
			{
			// Calculate motor control based
			// upon IMU data
			QCMPerformStep(&RC, &IMU, &MC);
			}
		else
			{
			// IMU Failed to provide update -
			// set Motor Control to native throttle
			MC.F = MC.B = MC.L = MC.R = RC.Throttle;
			}
		//============================================================

		//*****************************************
		// Implement Motor Cut-Off if model is
		// dangerously tilted (> 60 degrees) while
		// RC Throttle is low - to protect props 
		//----------------------------------------
		if (IMU.Incl <= 0.5 && RC.Throttle <= 0.40)
			{
			// Override motor control
			MC.F = MC.B = MC.L = MC.R = 0;
			}
		//----------------------------------------

		//*****************************************
		// Update motor control
		//*****************************************
		MCMSet(&MC);
		//-----------------------------------------
		
		//===========================================================
		// Load and post telemetry data (non-blocking call)
		//-----------------------------------------------------------
		// <editor-fold defaultstate="collapsed" desc="Populating Telemetry">
		StepTS =  TMRGetTS(); 
		//-----------------------------------------
		if ( 0 == StartTS )
			// Time stamp of cycle start!
			StartTS =  StepTS;
		//-----------------------------------------
		TM.TS			= StepTS - StartTS;
		//----------------------
		TM.Roll			= IMU.Roll;
		TM.Pitch		= IMU.Pitch;
		TM.Yaw			= IMU.Yaw;
		TM.Inclination	= IMU.Incl;
		TM.Azimuth		= IMU.Azimuth;
		//----------------------
		#ifdef  _TMReport_GYRO_
			#ifdef	_TMReport_GYRO_Rotated
			//------------------------------------------------------
			// We rotate Gyro vector using "partial" DCM built using
			// only Roll and Pitch angles as the actual Yaw does not
			// affect Angualr Velocity by axis
			//------------------------------------------------------
			Matrix		NoYawDCM;
			// Generate partial rotation matrix
			MatrixEulerRotation(IMU.Roll, IMU.Pitch, 0.0, &NoYawDCM);
			// Rotate Gyro vector
			MatrixTimesVector(&NoYawDCM, &IMU.GyroRate, &TM.Gyro);
			#else
			// Just report native Gyro data
			VectorCopy(&IMU.GyroRate, &TM.Gyro);
			#endif
		#endif
		//----------------------
		#ifdef  _TMReport_ACC_
		// Just report native Gyro data
		VectorCopy(&IMU.Gravity, &TM.Acc);
		#endif
		//----------------------
		TM.RollDer		= QSD.RollDer;
		TM.PitchDer		= QSD.PitchDer;
		TM.YawDer		= QSD.YawDer;
		//----------------------
		TM.RC_Throttle	= RC.Throttle;
		TM.RC_Roll		= RC.Roll;
		TM.RC_Pitch		= RC.Pitch;
		TM.RC_Yaw		= RC.Yaw;
		//----------------------
		#ifdef _TMReport_NativeRC_
		TM.RCN_Throttle	= RCNative.Throttle;
		TM.RCN_Roll		= RCNative.Roll;
		TM.RCN_Pitch	= RCNative.Pitch;
		TM.RCN_Yaw		= RCNative.Yaw;
		#endif
		//----------------------
		#ifdef __MXB_Use__
		MXBRead(&TM.MXB);
		#endif
		//----------------------
		#ifdef _TMReport_PID_
		#ifdef _TMReport_PID_Details
		TM.DRProp		= QSD.DeltaRollProp;
		TM.DRDiff		= QSD.DeltaRollDiff;
		TM.DRInt		= QSD.DeltaRollInt;
		#endif
		TM.DRTot		= QSD.DeltaRoll;
		//-------------
		#ifdef _TMReport_PID_Details
		TM.DPProp		= QSD.DeltaPitchProp;
		TM.DPDiff		= QSD.DeltaPitchDiff;
		TM.DPInt		= QSD.DeltaPitchInt;
		#endif
		TM.DPTot		= QSD.DeltaPitch;
		//-------------
		#ifdef _TMReport_PID_Details
		TM.DYProp		= QSD.DeltaYawProp;
		TM.DYDiff		= QSD.DeltaYawDiff;
		TM.DYInt		= QSD.DeltaYawInt;
		#endif
		TM.DYTot		= QSD.DeltaYaw;
		#endif
		//----------------------
		TM.Throttle		= QSD.Throttle;				// Real Throttle
		//----------------------
		TM.Voltage 		= QSD.Voltage;
		// </editor-fold>
		//===========================================================
		UARTPostIfReady( (unsigned char *) &TM, sizeof(TM)	);
		//===========================================================
		// Insert controlled "delay" to slow down the Control Loop
		TMRWaitAlarm(Alarm);
		}
	//*******************************************************************
	return 1;
	}
Ejemplo n.º 3
0
i32 RCMain()
{
	i32 bail = 0;
#ifdef WIN32
	while (!SetCurrentDirectoryA(RC_RESOURCE_PATH) && SetCurrentDirectoryA("..")) {
		bail++;
		if (bail >= 32) {
			printf("Can't find resource folder. Build directory must be located further down in the hierarchy than /res/\n");
			exit(0);
		}
	}
	SetCurrentDirectoryA("..");
#else
    char *fbuf = __FILE__;
    char fpath [8192];
    char *epos = strrchr(fbuf, '/');
    strncpy(fpath, fbuf, (epos - fbuf));
    fpath[(epos - fbuf)] = 0;
    chdir(fpath);
    while (chdir(RC_RESOURCE_PATH) != 0 && chdir("..") == 0) {
		bail++;
		if (bail >= 32) {
			printf("Can't find resource folder. Build directory must be located further down in the hierarchy than /res/\n");
			exit(0);
		}
	}
	chdir("..");
#endif

	char buf[4096];
	
	printf("Result setup...\n");
	ResultInit();
	printf("Done\n");

	printf("Display setup...\n");
	Platform::setDisplay(false, RC_DEFAULT_DISPLAY_WIDTH, RC_DEFAULT_DISPLAY_HEIGHT);
	printf("Done\n");

	printf("Renderer init...\n");
	Renderer::init();
	printf("Done\n");
	
	printf("SceneGraph setup...\n");
	SceneGraph::init();
	printf("Done\n");

	printf("Console setup...\n");
	Console::init();
	printf("Done\n");

	printf("Command setup...\n");
	Command::init();
	printf("Done\n");

	printf("Frame init...\n");
	Platform::initFrame();
	printf("Done\n");

	Console::log("~c9995=============================");
	Console::log("~c3939~g3939   _");
	Console::log("~c3939~g6969  c \"     ~r-[ RenderChimp ]-");
	Console::log("~c6969~g9999   (-       ~r-[ v 0.3.2a ]-\n ");
	Console::log("~c9995=============================\n \n ");
	//Console::log("~c9995Press ~c6966F1~c9995 to bring up the console.");
	Console::log("~c9995Press F1 to bring up the console.");

	sprintf(buf, "/config %sconfig/config.cfg", RC_RESOURCE_PATH);
	Command::run(buf);

#if defined RC_WIN32
	Console::log("Running on Win32 platform");
#else
	Console::log("Running on some platform");
#endif

	printf("Running RCInit...\n");
	RCInit();
	printf("Done\n");

	Console::log("Initiated...");

	rs.enableDepthTest(CMP_LESS);
	rs.enableDepthWrite();

	printf("Running main loop...\n");
	Platform::run();

	return 0;
}
Ejemplo n.º 4
0
int main(void)
	{
	//*******************************************************************
	Init();
	TMRInit(2);			// Initialize Timer interface with Priority=2
	BLIInit();			// Initialize Signal interface
	//--------------------------
	BLIAsyncMorse("S", 1);	// dot-dot-dot
	MCMInitF(50, 2500);	// Initialize Motor Control at 50 Hz with setting
						// Throttle to HIGH for delay interval to let ESC
						// capture Throttle range
	BLIAsyncStop();
	//--------------------------
	RCInit(4);			// Initialize Receiver interface with Priority=4
	//--------------------------
	UARTInitTX(6, 48);	// Initialize UART1 for TX on IPL=6 at 115200 bps
	// This initialization routine accepts BaudRate in multiples
	// of 2400 bps; Thus:
	// BaudRate =   1	=>   2400 bps
	// BaudRate =   2	=>   4800 bps
	// ...
	// BaudRate =  48	=> 115200 bps
	//------------------------------------------------------------
	// High speed
	//------------------------------------------------------------
	// BaudRate =  100	=>   250,000 bps
	// BaudRate =  200	=>   500,000 bps
	// BaudRate =  250	=>   625,000 bps
	// BaudRate =  350	=>   833,333 bps
	// BaudRate =  500	=> 1,250,000 bps
	// BaudRate = 1000	=> 2,500,000 bps
	//*******************************************************************
	BLISignalON();
	TMRDelay(2000); 	// Wait for extra 2 sec - to let ESC arm...
						// (finish the song :) )
	BLISignalOFF();
	//==================================================================
	MCMData		MC;
	RCData		RC;
	//-------------------------------------------------
	BLIAsyncMorse("R", 1);	// dot-doh-dot
	RCArm();
	BLIAsyncStop();
	//-------------------------------------------------
	BLISignalON();
	while(1)
		{
		RCReadWhenReady(&RC);
		//---------------------------------------------	
		if (0 == RC.Control)
			MC.F = MC.B	= MC.L = MC.R	= 0.0;
		else
			{	
			MC.F	= RC.Throttle;
			MC.B	= RC.Throttle;
			//--------------
			MC.L	= RC.Throttle;
			MC.R	= RC.Throttle;
			//--------------
			}
		//---------------------------------------------	
		MCMSet(&MC);
		//---------------------------------------------	
		UARTPostWhenReady((uchar*)&RC, sizeof(RC));
		//---------------------------------------------	
		BLISignalFlip();
		}
	return 1;
	}
Ejemplo n.º 5
0
int main(void)
	{
	// <editor-fold defaultstate="collapsed" desc="Initialization of HW components/modules">
	//*******************************************************************
	// Initialization of HW components/modules
	//===================================================================
	Init();
	TMRInit(2);			// Initialize Timer interface with Priority=2
	BLIInit();			// Initialize Signal interface
	//===================================================================
	BLIAsyncStart(50, 50);	// Fast blinking - initialization
	//===================================================================
	MCMInitT(2.5, 2500);	// Initialize Motor Control for PPM with setting
							// Throttle to HIGH for delay interval to let ESC
							// capture Throttle range
	//--------------------------
	ADCInit(3);			// Initialize ADC to control battery
	//--------------------------
	RCInit(4);			// Initialize Receiver interface with Priority=4
	//--------------------------
	I2CInit(5, 1);		// Initialize I2C1 module with IPL=5 and Fscl=400 KHz
	//--------------------------
	UARTInitTX(6, 350);	// Initialize UART1 for TX on IPL=6 at 
	// BaudRate =   48	=>   115,200 bps	- ZigBEE
	//--------------------------------------
	// BaudRate =  100	=>   250,000 bps
	// BaudRate =  200	=>   500,000 bps
	// BaudRate =  250	=>   625,000 bps
	// BaudRate =  350	=>   833,333 bps	- SD Logger, FTDI
	// BaudRate =  500	=> 1,250,000 bps
	// BaudRate = 1000	=> 2,500,000 bps
	//*******************************************************************
	// Initialize Magnetometer
	//--------------------------------------------------------------
	if (HMCInit(6, 1, 0))	// Initialize magnetic Sensor
							// ODR  = 6 (max, 75 Hz),
							// Gain = 1 (1.3 Gs)
							// DLPF = 0 (no averaging)
		BLIDeadStop("EM", 2);
	//==================================================================
	// Initialize motion sensor - No calibration at this time!!!
	//------------------------------------------------------------------
	if ( MPUInit(0, 3) )	// Initialize motion Sensor
		// 1 kHz/(0+1) = 1000 Hz (1ms)
		// DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec)
		BLIDeadStop("EA", 2);
	//------------------------------------------------------------------
	//==================================================================
	AltimeterInit(4);	// Initialize Altimeter with IPL=4 (if needed)
	// NOTE: All sensors areinitialized, so no more synchronous I2C
	//		 operations are needed. Thus we may start Altimeter to give
	//		 it more time to "warm-up"
	AltimeterReset();	// NOTE: will be reset again later
	//==================================================================
	BLIAsyncStop();		// Initialization complete
	//===================================================================
	// </editor-fold>

	//*******************************************************************
	// Quadrocopter control variables
	//-------------------------------------------------------------------
	// <editor-fold defaultstate="collapsed" desc="Timing control variables">
	ulong		StartTS		= 0;	// Flight start time
	ulong		StepTS;				// Control Loop step start time
	ulong		Alarm		= 0;	// "alarm" variable used to set
									// the duration of the Control Loop
	// </editor-fold>

	// <editor-fold defaultstate="collapsed" desc="Variable required to process RC Feed ">
	//-------------------------------------------------------------------
	RCData		RCFeed;			// Control input from Receiver adjusted
								// by control rates. It may also be
								// corrected (if necessary) to account
								// for "+" or "X" configuration as
								// specified by MODE_CB_to_MF flag
	//---------------------------------------
	RCData		RCSmthd;		// Smoothed control input used in all
								// subsequent control calculations
								//---------------------------------------
	RCData		RCFinal;		// Smoothed RC input adjusted for the
								// Yaw angle to provide "Course Lock" (if
								// required). Final RC input provided to
								// Quarocopter Control Module
	//-------------------------------------------------------------------
	// We need battery Nominal voltage to adjust RC Throttle as battery
	// charge depletes.
	float		BatNomV		= ADCGetBatteryNomVoltage ();
	//-------------------------------------------------------------------
	Matrix		Mtr_CB_To_MF;	// Rotation matrix used to adjust Control
								// Board orientation to Model front
								//---------------------------------------
	Matrix		Mtr_CrsLck;		// Rotation matrix used to adjust RC input
								// to account for current Yaw to implement
								// "Course Lock"
								//---------------------------------------
	if (MODE_CB_to_MF)
		// Initialize Rotation Matrix for pre-set angle
		MatrixYawRotation(__CB_To_MF_Angle__, &Mtr_CB_To_MF);
	else
		// Reset Rotation matrix to Identity
		MatrixSetIdentity(&Mtr_CB_To_MF);
	//-------------------------------------------------------------------
	// </editor-fold>
	
	DCMData		IMU;				// Orientation data from DCM
	MCMData		MC;					// Motor control Variables
	//-------------------------------------------------------------------


	// <editor-fold defaultstate="collapsed" desc="ARMing RC and initializing IMU">
	//==================================================================
	// Wait for the Receiver ARMing: Throttle should go up and then down
	//------------------------------------------------------------------
	BLIAsyncStart(200, 200);		// Really slow
	RCArm();
	BLIAsyncStop();
	//==================================================================
	// </editor-fold>

Re_Start:
	//==================================================================
	// <editor-fold defaultstate="collapsed" desc="Intialize Re-Start">
	//------------------------------------------------------------------
	StartTS		= 0;					// Reset timing of control Loop
	//------------------------------------------------------------------
	BLIAsyncMorse(	"I", 1);			// dit - dit
	RCReadWhenReady (&RCFeed);			// Get reading from receiver
	//------------------------------------------------------------------
	// Safety check:  CONTROL should be UP and THROTTLE - LOW
	//------------------------------------------------------------------
	while 	(
			0 == RCFeed.Control
			||
			RCFeed.Throttle > 0.1
			)
		{
		RCReadWhenReady (&RCFeed);		// Get next reading from receiver
		}
	//------------------------------------------------------------------
	BLIAsyncStop ();
	//==================================================================

	//==================================================================
	// Normalize last read RC values as they will represent the first
	// input into the control algorithm, so should be adjusted accordingly
	//------------------------------------------------------------------
	NormalizeRCFeed(&RCFeed);
	//------------------------------------------------------------------
	// Reset Smothed RC data (initialize filter)
	//------------------------------------------------------------------
	RCDataReset(&RCSmthd);
	//==================================================================
	// Initialize sensors... Performed after ARMing to ensure that model
	// is stable
	//--------------------------------------------------------------
	BLIAsyncStart(100, 50);
	//------------------------------------------------------------------
	// Start IMU and wait until orientation estimate stabilizes
	//------------------------------------------------------------------
	IMUReset();
	//--------------------------------------------------------------
	BLIAsyncStart(50, 100);
	//------------------------------------------------------------------
	// Reset Altimeter
	//------------------------------------------------------------------
	AltimeterReset();
	//------------------------------------------------------------------
	QCMReset ();			// Initialize (reset) QCM variables
	//------------------------------------------------------------------
	// Force update of DCMData as inside the control Loop this
	// call is non-blocking!
	//------------------------------------------------------------------
	if (IMU_OK != IMUGetUpdateWhenReady (&IMU))
		// Failure...
		BLIDeadStop ("A", 1);
	//--------------------------------------------------------------
	BLIAsyncStop();
	//--------------------------------------------------------------
	// </editor-fold>
	//==================================================================

	//*******************************************************************
	// Quadrocopter Control Loop
	//-------------------------------------------------------------------
	BLISignalON();
	while (1)
		{
		//============================================================
		// Timing of control Loop
		//============================================================
		// <editor-fold defaultstate="collapsed" desc="Control Loop timing management">
		// Sets the "frquency" of Control Loop
		Alarm = TMRSetAlarm (10);	// Set Alarm 10 msec in the future
		//------------------------------------------------------------
		StepTS =  TMRGetTS ();		// Capture TS of this step
		if ( 0 == StartTS )			// First iteration of Control?
		  StartTS =  StepTS;		// Yes, capture timestamp
		// </editor-fold>
		//============================================================

		//============================================================
		// Read commands from receiver - non-blocking call!
		// (we will get out of this call even if connection to the
		// receiver is lost!)
		// At the end processed Receiver data is stored in RCSmthd
		//============================================================
		// <editor-fold defaultstate="collapsed" desc="Process Receiver feed">
		if ( RCRead(&RCFeed) )
			{
			//------------------------------------------------------------
			// Every NEW sample need to be "normalized"
			//------------------------------------------------------------
			NormalizeRCFeed(&RCFeed);
			//------------------------------------------------------------
			// Adjust orientation from "+" to "X" if needed
			//------------------------------------------------------------
			if (MODE_CB_to_MF)
				{
				// Control board front does not coincide with the model front
				Vector	RCInput;
				Vector	RCRotated;
				VectorSet(RCFeed.Roll, RCFeed.Pitch, RCFeed.Yaw, &RCInput);
				MatrixTimesVector(&Mtr_CB_To_MF, &RCInput, &RCRotated);
				RCFeed.Roll		= RCRotated.X;
				RCFeed.Pitch	= RCRotated.Y;
				RCFeed.Yaw		= RCRotated.Z;	// NOTE: Yaw is not affected!
				}
			}
		//============================================================
		// Smooth RC data 
		//------------------------------------------------------------
		// Roll, Pitch, and Yaw are smoothed with the IIR(4)
		//------------------------------------------------------------
		RCSmthd.Roll	= (RCSmthd.Roll	* 3.0	+ RCFeed.Roll ) * 0.25;	// 1/4 = 0.25
		RCSmthd.Pitch	= (RCSmthd.Pitch* 3.0 	+ RCFeed.Pitch) * 0.25;
		if (MODE_Course_Lock)
			{
			// In this mode Yaw is integrated over the Control Loop
			// duration (0.01 sec) and normalized to +/-Pi range
			RCSmthd.Yaw = QCMRangeToPi(RCSmthd.Yaw + RCFeed.Yaw * 0.01);
			}
		else
			{
			// Without Course-Lock Yaw is treated as "direct input" and
			// just smothed similar to other control variables.
			RCSmthd.Yaw		= (RCSmthd.Yaw	* 3.0	+ RCFeed.Yaw  ) * 0.25;
			}
		//------------------------------------------------------------
		// Throttle is smoothed with the IIR(4) and adjusted to
		// account for actual battery voltage. This is done to
		// improve "hovering" when throttle stick is not moving.
		//------------------------------------------------------------
		// Adjust Native (from RC) throttle to a value corresponding
		float BatV		= ADCGetBatteryVoltage();
		float BatAdjTh	= RCFeed.Throttle;
		if (BatV > 2.0)		// Sanity check :)
			BatAdjTh	= BatAdjTh	* BatNomV / BatV;
		//-----------------------------------------
		RCSmthd.Throttle	= (RCSmthd.Throttle*3.0	+ BatAdjTh) * 0.25;	// 1/4 = 0.25
		//-----------------------------------------
		RCSmthd.Control		= RCFeed.Control;
		// </editor-fold>
		//============================================================

		//============================================================
		// Implement Motor Cut-Off if RC Control is LOW
		//============================================================
		// <editor-fold defaultstate="collapsed" desc="Process RC Control">
		if ( 0 == RCSmthd.Control )
			{
			// Yes, Control is reliably low!
			//--------------------------------------------
			// Override motor control
			//--------------------------------------------
			MC.F = MC.B = MC.L = MC.R = 0;
			MCMSet(&MC);
			//--------------------------------------------
			// Flight terminated, post EOF to Data Logger
			//--------------------------------------------
			TMRDelay(10);	// Wait for pipe to clear
			UARTPostIfReady( NULL, 0);
			// ... and again - to be sure!
			TMRDelay(10);	// Wait for pipe to clear
			UARTPostIfReady( NULL, 0);
			//----------------------------------
			goto Re_Start;
			}
		// </editor-fold>
		//============================================================

		//============================================================
		// Obtain and process battery charge status
		//============================================================
		// <editor-fold defaultstate="collapsed" desc="Process Battery level">
		float BatteryCharge	= QCMBatteryMgmt();	
		if (BatteryCharge < 0.35)	// BC < 35%
			{
			float MaxLevel = 2.0 * BatteryCharge;
			if (RCSmthd.Throttle > MaxLevel)
				RCSmthd.Throttle = MaxLevel;
			}
		// </editor-fold>
		//============================================================

		//============================================================
		// Read current orientation from the IMU  with sensors' raw
		// measurements (non-blocking call) and, if necessary, perform
		// adjustment to RCSmthd to obtain RCFinal
		//============================================================
		// <editor-fold defaultstate="collapsed" desc="Process IMU data and generate Motor Control">
		// First, unconditionally promote RCSmthd to RCFinal
		RCDataCopy(&RCSmthd, &RCFinal);
		//------------------------------------------------------------
		if (IMU_OK == IMUGetUpdate(&IMU))
			{
			if (MODE_Course_Lock)
				{
				// We need to rotate RC input to adjust for current Yaw
				// to implement "Course Lock" - RC Roll and Pitch commands
				// are interpreted as being applied to the original (pre-
				// takeoff) orientation of the model
				//-------------------------------------------------------
				Vector	RCInput;
				Vector	RCRotated;
				// Build rotation matrix to adjust for orientation discrepancy
				MatrixYawRotation(-IMU.Yaw, &Mtr_CrsLck);
				// Load RC input into RCInput vector
				VectorSet(RCSmthd.Roll, RCSmthd.Pitch, RCSmthd.Yaw, &RCInput);
				// Rotate RCInput vector
				MatrixTimesVector(&Mtr_CrsLck, &RCInput, &RCRotated);
				// Save rotated values
				RCFinal.Roll		= RCRotated.X;
				RCFinal.Pitch		= RCRotated.Y;
				RCFinal.Yaw			= RCRotated.Z;
				}
			// Calculate motor control based
			// upon IMU data
			QCMPerformStep(&RCFinal, &IMU, &MC);
			}
		else
			{
			// IMU Failed to provide update -
			// set Motor Control to RC throttle
			MC.F = MC.B = MC.L = MC.R = RCFinal.Throttle;
			}
		// </editor-fold>
		//============================================================

		//************************************************************
		// Implement Motor Cut-Off to protect props if model is
		// dangerously tilted (> 60 degrees) while RC Throttle is low
		//------------------------------------------------------------
		if (IMU.Incl <= 0.5 && RCFinal.Throttle <= 0.40)
			{
			// Override motor control
			MC.F = MC.B = MC.L = MC.R = 0.0;
			}
		//------------------------------------------------------------

		//************************************************************
		// Update motor control
		//************************************************************
		MCMSet(&MC);
		//------------------------------------------------------------
		
		//===========================================================
		// Load and post telemetry data (non-blocking call)
		//-----------------------------------------------------------
		// <editor-fold defaultstate="collapsed" desc="Populating Telemetry">
		//-----------------------------------------
		TM.TS			= StepTS - StartTS;
		//----------------------
		TM.Roll			= IMU.Roll;
		TM.Pitch		= IMU.Pitch;
		TM.Yaw			= IMU.Yaw;
		TM.Inclination	= IMU.Incl;
		TM.Azimuth		= IMU.Azimuth;
		//----------------------
		#ifdef  _TMReport_GYRO_
			#ifdef	_TMReport_GYRO_Rotated
			//------------------------------------------------------
			// We rotate Gyro vector using "partial" DCM built using
			// only Roll and Pitch angles as the actual Yaw does not
			// affect Angualr Velocity by axis
			//------------------------------------------------------
			Vector		Earth;	// One of the Earth axis in Body frame
			Vector		Cross;	// Temporary vector to hold the cross
								// product
			//------------------------------------------------------
			// Calculate Roll and Pitch rates
			//------------------------------------------------------
			// Retrieve Earth Z-axis and calculate cross-product of
			// Z-axis with Gyro vector
			VectorCrossProduct(DCMEarthZ(&Earth), &IMU.GyroRate, &Cross);
			TM.Gyro.X = Cross.X;
			TM.Gyro.Y = Cross.Y;
			//------------------------------------------------------
			// Calculate Yaw rate
			//------------------------------------------------------
			// Retrieve Earth X-axis and calculate cross-product of
			// X-axis with Gyro vector
			VectorCrossProduct(DCMEarthX(&Earth), &IMU.GyroRate, &Cross);
			TM.Gyro.Z = Cross.Z;
			#else
			// Just report native Gyro data
			VectorCopy(&IMU.GyroRate, &TM.Gyro);
			#endif
		#endif
		//----------------------
		#ifdef  _TMReport_ACC_
		// Just report native Gyro data
		VectorCopy(&IMU.Gravity, &TM.Acc);
		#endif
		//----------------------
		TM.RollDer		= QSD.RollDer;
		TM.PitchDer		= QSD.PitchDer;
		TM.YawDer		= QSD.YawDer;
		//----------------------
		TM.RC_Throttle	= RCFinal.Throttle;
		TM.RC_Roll		= RCFinal.Roll;
		TM.RC_Pitch		= RCFinal.Pitch;
		TM.RC_Yaw		= RCFinal.Yaw;
		//----------------------
		#ifdef _TMReport_RCSmthd_
		TM.RCS_Throttle	= RCSmthd.Throttle;
		TM.RCS_Roll		= RCSmthd.Roll;
		TM.RCS_Pitch	= RCSmthd.Pitch;
		TM.RCS_Yaw		= RCSmthd.Yaw;
		#endif
		//----------------------
		#ifdef _TMReport_RCFeed_
		TM.RCN_Throttle	= RCFeed.Throttle;
		TM.RCN_Roll		= RCFeed.Roll;
		TM.RCN_Pitch	= RCFeed.Pitch;
		TM.RCN_Yaw		= RCFeed.Yaw;
		#endif
		//----------------------
		#ifdef _TMReport_Altimetry_
		if ( 0 == AltimeterGetAltData(	IMU.TS,
								&IMU.Gravity,
								&TM.AltResult) )
			goto EndOfCycle;	// Skip reporting on this step
		#endif
		//----------------------
		#ifdef _TMReport_PID_
		#ifdef _TMReport_PID_Details
		TM.DRProp		= QSD.DeltaRollProp;
		TM.DRDiff		= QSD.DeltaRollDiff;
		TM.DRInt		= QSD.DeltaRollInt;
		#endif
		TM.DRTot		= QSD.DeltaRoll;
		//-------------
		#ifdef _TMReport_PID_Details
		TM.DPProp		= QSD.DeltaPitchProp;
		TM.DPDiff		= QSD.DeltaPitchDiff;
		TM.DPInt		= QSD.DeltaPitchInt;
		#endif
		TM.DPTot		= QSD.DeltaPitch;
		//-------------
		#ifdef _TMReport_PID_Details
		TM.DYProp		= QSD.DeltaYawProp;
		TM.DYDiff		= QSD.DeltaYawDiff;
		TM.DYInt		= QSD.DeltaYawInt;
		#endif
		TM.DYTot		= QSD.DeltaYaw;
		#endif
		//----------------------
		TM.Throttle		= QSD.Throttle;				// Real Throttle
		//----------------------
		TM.Voltage 		= ADCGetBatteryVoltage();
		// </editor-fold>
		//===========================================================
		UARTPostIfReady( (unsigned char *) &TM, sizeof(TM)	);
		//===========================================================
		// Insert controlled "delay" to slow down the Control Loop
		// to pre-set frequency
	EndOfCycle:
		TMRWaitAlarm(Alarm);
		//===========================================================
		}
	//*******************************************************************
	return 1;
	}