Exemple #1
0
void cliCom(void)
{
	uint16_t index;

	char mvlkToggleString[5] = { 0, 0, 0, 0, 0 };

    if ((cliPortAvailable() && !validCliCommand))
    {
		cliQuery = cliPortRead();

        if (cliQuery == '#')                       // Check to see if we should toggle mavlink msg state
        {
	    	while (cliPortAvailable == false);

        	readStringCLI(mvlkToggleString, 5);

            if ((mvlkToggleString[0] == '#') &&
            	(mvlkToggleString[1] == '#') &&
                (mvlkToggleString[2] == '#') &&
                (mvlkToggleString[3] == '#'))
	    	{
	    	    if (systemConfig.mavlinkEnabled == false)
	    	    {
	    	 	    systemConfig.mavlinkEnabled  = true;
	    		    systemConfig.activeTelemetry = 0x0000;
	    		}
	    		else
	    		{
	    		    systemConfig.mavlinkEnabled = false;
	    	    }

	    	    if (mvlkToggleString[4] == 'W')
	    	    {
	                cliPortPrint("\nWriting EEPROM Parameters....\n");
	                writeSystemEEPROM();
	    	    }
	    	}
	    }
	}

	validCliCommand = false;

    if ((systemConfig.mavlinkEnabled == false) && (cliQuery != '#'))
    {
        switch (cliQuery)
        {
            ///////////////////////////////

            case 'a': // Rate PIDs
                cliPortPrintF("\nRoll Rate PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[ROLL_RATE_PID].P,
                    		                                                    systemConfig.PID[ROLL_RATE_PID].I,
                    		                                                    systemConfig.PID[ROLL_RATE_PID].D,
                    		                                                    systemConfig.PID[ROLL_RATE_PID].N);

                cliPortPrintF(  "Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[PITCH_RATE_PID].P,
                    		                                                    systemConfig.PID[PITCH_RATE_PID].I,
                    		                                                    systemConfig.PID[PITCH_RATE_PID].D,
                    		                                                    systemConfig.PID[PITCH_RATE_PID].N);

                cliPortPrintF(  "Yaw Rate PID:   %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[YAW_RATE_PID].P,
                    		                                                    systemConfig.PID[YAW_RATE_PID].I,
                    		                                                    systemConfig.PID[YAW_RATE_PID].D,
                    		                                                    systemConfig.PID[YAW_RATE_PID].N);
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'b': // Attitude PIDs
                cliPortPrintF("\nRoll Attitude PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[ROLL_ATT_PID].P,
                   		                                                            systemConfig.PID[ROLL_ATT_PID].I,
                   		                                                            systemConfig.PID[ROLL_ATT_PID].D,
                   		                                                            systemConfig.PID[ROLL_ATT_PID].N);

                cliPortPrintF(  "Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[PITCH_ATT_PID].P,
                   		                                                            systemConfig.PID[PITCH_ATT_PID].I,
                   		                                                            systemConfig.PID[PITCH_ATT_PID].D,
                   		                                                            systemConfig.PID[PITCH_ATT_PID].N);

                cliPortPrintF(  "Heading PID:        %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[HEADING_PID].P,
                   		                                                            systemConfig.PID[HEADING_PID].I,
                   		                                                            systemConfig.PID[HEADING_PID].D,
                   		                                                            systemConfig.PID[HEADING_PID].N);
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'c': // Velocity PIDs
                cliPortPrintF("\nnDot PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[NDOT_PID].P,
                   		                                                   systemConfig.PID[NDOT_PID].I,
                   		                                                   systemConfig.PID[NDOT_PID].D,
                   		                                                   systemConfig.PID[NDOT_PID].N);

                cliPortPrintF(  "eDot PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[EDOT_PID].P,
                   		                                                   systemConfig.PID[EDOT_PID].I,
                   		                                                   systemConfig.PID[EDOT_PID].D,
                   		                                                   systemConfig.PID[EDOT_PID].N);

                cliPortPrintF(  "hDot PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[HDOT_PID].P,
                   		                                                   systemConfig.PID[HDOT_PID].I,
                   		                                                   systemConfig.PID[HDOT_PID].D,
                   		                                                   systemConfig.PID[HDOT_PID].N);
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'd': // Position PIDs
                cliPortPrintF("\nN PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[N_PID].P,
                   		                                                systemConfig.PID[N_PID].I,
                   		                                                systemConfig.PID[N_PID].D,
                   		                                                systemConfig.PID[N_PID].N);

                cliPortPrintF(  "E PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[E_PID].P,
                   		                                                systemConfig.PID[E_PID].I,
                   		                                                systemConfig.PID[E_PID].D,
                   		                                                systemConfig.PID[E_PID].N);

                cliPortPrintF(  "h PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[H_PID].P,
                   		                                                systemConfig.PID[H_PID].I,
                   		                                                systemConfig.PID[H_PID].D,
                   		                                                systemConfig.PID[H_PID].N);
                cliQuery = 'x';
                validCliCommand = false;
              	break;

            ///////////////////////////////

            case 'e': // Loop Delta Times
               	cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz,
                   		                                                    deltaTime500Hz,
                   		                                                    deltaTime100Hz,
                   		                                                    deltaTime50Hz,
                   		                                                    deltaTime10Hz,
                   		                                                    deltaTime5Hz,
                   		                                                    deltaTime1Hz);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'f': // Loop Execution Times
               	cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz,
               	        			                                        executionTime500Hz,
               	        			                                        executionTime100Hz,
               	        			                                        executionTime50Hz,
               	        			                                        executionTime10Hz,
               	        			                                        executionTime5Hz,
               	        			                                        executionTime1Hz);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'g': // 500 Hz Accels
            	cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
            			                               sensors.accel500Hz[YAXIS],
            			                               sensors.accel500Hz[ZAXIS]);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'h': // 100 hz Earth Axis Accels
            	cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS],
            			                               earthAxisAccels[YAXIS],
            			                               earthAxisAccels[ZAXIS]);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'i': // 500 hz Gyros
            	cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D,
            			                                      sensors.gyro500Hz[PITCH] * R2D,
            					                              sensors.gyro500Hz[YAW  ] * R2D,
            					                              mpu6000Temperature);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'j': // 10 Hz Mag Data
                cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS],
                		                               sensors.mag10Hz[YAXIS],
                		                               sensors.mag10Hz[ZAXIS]);

            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'k': // Vertical Axis Variables
            	cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld, %9.4f\n", earthAxisAccels[ZAXIS],
            			                                                   sensors.pressureAlt50Hz,
            					                                           hDotEstimate,
            					                                           hEstimate,
            					                                           ms5611Temperature,
            					                                           aglRead());
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'l': // Attitudes
            	cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D,
            			                               sensors.attitude500Hz[PITCH] * R2D,
            			                               sensors.attitude500Hz[YAW  ] * R2D);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'm': // Axis PIDs
            	cliPortPrintF("%9.4f, %9.4f, %9.4f\n", ratePID[ROLL ],
               			                               ratePID[PITCH],
               			                               ratePID[YAW  ]);
               	validCliCommand = false;
               	break;

            ///////////////////////////////

            case 'n': // GPS Data
               	switch (gpsDataType)
               	{
               	    ///////////////////////

               	    case 0:
               	        cliPortPrintF("%12ld, %12ld, %12ld, %12ld, %12ld, %12ld, %4d, %4d\n", gps.latitude,
               			                                                                      gps.longitude,
               			                                                                      gps.hMSL,
               			                                                                      gps.velN,
               			                                                                      gps.velE,
               			                                                                      gps.velD,
               			                                                                      gps.fix,
               			                                                                      gps.numSats);
               	        break;

               	    ///////////////////////

               	    case 1:
               	    	cliPortPrintF("%3d: ", gps.numCh);

               	    	for (index = 0; index < gps.numCh; index++)
               	    	    cliPortPrintF("%3d  ", gps.chn[index]);

               	    	cliPortPrint("\n");

               	    	break;

               	    ///////////////////////

               	    case 2:
               	    	cliPortPrintF("%3d: ", gps.numCh);

               	    	for (index = 0; index < gps.numCh; index++)
               	    		cliPortPrintF("%3d  ", gps.svid[index]);

               	    	cliPortPrint("\n");

               	    	break;

               	    ///////////////////////

               	    case 3:
               	    	cliPortPrintF("%3d: ", gps.numCh);

               	    	for (index = 0; index < gps.numCh; index++)
               	    		cliPortPrintF("%3d  ", gps.cno[index]);

               	    	cliPortPrint("\n");

               	    	break;

               	    ///////////////////////
               	}

               	validCliCommand = false;
                break;

            ///////////////////////////////

            case 'o':
                cliPortPrintF("%9.4f\n", batteryVoltage);

                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'p': // Primary Spektrum Raw Data
            	cliPortPrintF("%04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X\n", primarySpektrumState.lostFrameCnt,
            			                                                                      primarySpektrumState.rcAvailable,
            			                                                                      primarySpektrumState.values[0],
            			                                                                      primarySpektrumState.values[1],
            			                                                                      primarySpektrumState.values[2],
            			                                                                      primarySpektrumState.values[3],
            			                                                                      primarySpektrumState.values[4],
            			                                                                      primarySpektrumState.values[5],
            			                                                                      primarySpektrumState.values[6],
            			                                                                      primarySpektrumState.values[7]);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'q': // Not Used
                cliQuery = 'x';
               	validCliCommand = false;
               	break;

            ///////////////////////////////

			case 'r':
				if (flightMode == RATE)
					cliPortPrint("Flight Mode:RATE      ");
				else if (flightMode == ATTITUDE)
					cliPortPrint("Flight Mode:ATTITUDE  ");
				else if (flightMode == GPS)
					cliPortPrint("Flight Mode:GPS       ");

				if (headingHoldEngaged == true)
					cliPortPrint("Heading Hold:ENGAGED     ");
				else
					cliPortPrint("Heading Hold:DISENGAGED  ");

				switch (verticalModeState)
				{
					case ALT_DISENGAGED_THROTTLE_ACTIVE:
						cliPortPrint("Alt:Disenaged Throttle Active      ");

						break;

					case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
						cliPortPrint("Alt:Hold Fixed at Engagement Alt   ");

						break;

					case ALT_HOLD_AT_REFERENCE_ALTITUDE:
						cliPortPrint("Alt:Hold at Reference Alt          ");

						break;

					case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
						cliPortPrint("Alt:Velocity Hold at Reference Vel ");

						break;

					case ALT_DISENGAGED_THROTTLE_INACTIVE:
						cliPortPrint("Alt:Disengaged Throttle Inactive   ");

						break;
				}

				if (rxCommand[AUX3] > MIDCOMMAND)
					cliPortPrint("Mode:Simple  ");
				else
					cliPortPrint("Mode:Normal  ");

				if (rxCommand[AUX4] > MIDCOMMAND)
					cliPortPrint("Emergency Bail:Active\n");
				else
					cliPortPrint("Emergency Bail:Inactive\n");

				validCliCommand = false;
				break;

			///////////////////////////////

			case 's': // Raw Receiver Commands
                if ((systemConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0))
                {
		    		for (index = 0; index < maxChannelNum - 1; index++)
                         cliPortPrintF("%4ld, ", spektrumBuf[index]);

                    cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]);
                }
                else if ((systemConfig.receiverType == SPEKTRUM) && (maxChannelNum == 0))
                    cliPortPrint("Invalid Number of Spektrum Channels....\n");
		        else
		        {
		    		for (index = 0; index < 7; index++)
                        cliPortPrintF("%4i, ", ppmRxRead(index));

                    cliPortPrintF("%4i\n", ppmRxRead(7));
                }

            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 't': // Processed Receiver Commands
                for (index = 0; index < 7; index++)
                    cliPortPrintF("%8.2f, ", rxCommand[index]);

                cliPortPrintF("%8.2f\n", rxCommand[7]);

                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'u': // Command in Detent Discretes
            	cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false");
            	cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false");
            	cliPortPrintF("%s\n", commandInDetent[YAW  ] ? " true" : "false");

                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'v': // ESC PWM Outputs
            	cliPortPrintF("%4ld, ", TIM2->CCR1 );
            	cliPortPrintF("%4ld, ", TIM2->CCR2 );
                cliPortPrintF("%4ld, ", TIM15->CCR1);
            	cliPortPrintF("%4ld, ", TIM15->CCR2);
            	cliPortPrintF("%4ld, ", TIM3->CCR1 );
            	cliPortPrintF("%4ld\n", TIM3->CCR2 );

            	validCliCommand = false;
                break;

            ///////////////////////////////

            case 'w': // Servo PWM Outputs
            	cliPortPrintF("%4ld, ", TIM4->CCR1);
            	cliPortPrintF("%4ld, ", TIM4->CCR2);
            	cliPortPrintF("%4ld, ", TIM4->CCR3);
            	cliPortPrintF("%4ld\n", TIM4->CCR4);

                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'x':
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'y': // ESC Calibration
            	escCalibration();

            	cliQuery = 'x';
            	break;

            ///////////////////////////////

            case 'z':
                cliPortPrintF("%5.2f, %5.2f\n", voltageMonitor(),
                		                        adcChannel());
                break;

            ///////////////////////////////

            ///////////////////////////////////////////////////////////////////////
            ///////////////////////////////////////////////////////////////////////
            ///////////////////////////////////////////////////////////////////////
            ///////////////////////////////////////////////////////////////////////

            ///////////////////////////////

            case 'A': // Read Roll Rate PID Values
                readCliPID(ROLL_RATE_PID);
                cliPortPrint( "\nRoll Rate PID Received....\n" );

            	cliQuery = 'a';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'B': // Read Pitch Rate PID Values
                readCliPID(PITCH_RATE_PID);
                cliPortPrint( "\nPitch Rate PID Received....\n" );

            	cliQuery = 'a';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'C': // Read Yaw Rate PID Values
                readCliPID(YAW_RATE_PID);
                cliPortPrint( "\nYaw Rate PID Received....\n" );

            	cliQuery = 'a';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'D': // Read Roll Attitude PID Values
                readCliPID(ROLL_ATT_PID);
                cliPortPrint( "\nRoll Attitude PID Received....\n" );

            	cliQuery = 'b';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'E': // Read Pitch Attitude PID Values
                readCliPID(PITCH_ATT_PID);
                cliPortPrint( "\nPitch Attitude PID Received....\n" );

            	cliQuery = 'b';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'F': // Read Heading Hold PID Values
                readCliPID(HEADING_PID);
                cliPortPrint( "\nHeading PID Received....\n" );

            	cliQuery = 'b';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'G': // Read nDot PID Values
                readCliPID(NDOT_PID);
                cliPortPrint( "\nnDot PID Received....\n" );

            	cliQuery = 'c';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'H': // Read eDot PID Values
                readCliPID(EDOT_PID);
                cliPortPrint( "\neDot PID Received....\n" );

                cliQuery = 'c';
              	validCliCommand = false;
              	break;

            ///////////////////////////////

            case 'I': // Read hDot PID Values
                readCliPID(HDOT_PID);
                cliPortPrint( "\nhDot PID Received....\n" );

              	cliQuery = 'c';
              	validCliCommand = false;
              	break;

       	    ///////////////////////////////

            case 'J': // Read n PID Values
                readCliPID(N_PID);
                cliPortPrint( "\nn PID Received....\n" );

                cliQuery = 'd';
                validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'K': // Read e PID Values
                readCliPID(E_PID);
                cliPortPrint( "\ne PID Received....\n" );

                cliQuery = 'd';
                validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'L': // Read h PID Values
                readCliPID(H_PID);
                cliPortPrint( "\nh PID Received....\n" );

                cliQuery = 'd';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'N': // Mixer CLI
                mixerCLI();

                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'O': // Receiver CLI
                receiverCLI();

                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'P': // Sensor CLI
               	sensorCLI();

               	cliQuery = 'x';
               	validCliCommand = false;
               	break;

            ///////////////////////////////

            case 'Q': // GPS Data Selection
            	gpsDataType = (uint8_t)readFloatCLI();

            	cliPortPrint("\n");

                cliQuery = 'n';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'R': // Reset to Bootloader
            	cliPortPrint("Entering Bootloader....\n\n");
            	delay(100);
            	systemReset(true);
            	break;

            ///////////////////////////////

            case 'S': // Reset System
            	cliPortPrint("\nSystem Reseting....\n\n");
            	delay(100);
            	systemReset(false);
            	break;

            ///////////////////////////////

            case 'T': // Telemetry CLI
                telemetryCLI();

                cliQuery = 'x';
             	validCliCommand = false;
             	break;

            ///////////////////////////////

            case 'U': // EEPROM CLI
                eepromCLI();

                cliQuery = 'x';
             	validCliCommand = false;
             	break;

            ///////////////////////////////

            case 'V': // Write Sensor EEPROM Parameters
                cliPortPrint("\nWriting Sensor EEPROM Parameters....\n\n");

                cliBusy = true;

                writeSensorEEPROM();

                cliBusy = false;

                cliQuery = 'x';
             	validCliCommand = false;
             	break;

            ///////////////////////////////

            case 'W': // Write System EEPROM Parameters
                cliPortPrint("\nWriting System EEPROM Parameters....\n\n");

                cliBusy = true;

                writeSystemEEPROM();

                cliBusy = false;

                cliQuery = 'x';
             	validCliCommand = false;
             	break;

            ///////////////////////////////

            case 'X': // Not Used
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'Y': // Not Used
                computeGeoMagElements();

                cliQuery = 'x';
                break;

            ///////////////////////////////

            case 'Z': // Not Used
                if (usbIsConfigured() == true)
                    cliPortPrint("\nUSB Configured TRUE\n");
                else
                    cliPortPrint("\nUSB Configured FALSE\n");

                if (usbIsConnected() == true)
                    cliPortPrint("\nUSB Connected TRUE\n");
                else
                    cliPortPrint("\nUSB Connected FALSE\n");

                cliQuery = 'x';
                break;

            ///////////////////////////////

            case '?': // Command Summary
            	cliBusy = true;

            	cliPortPrint("\n");
   		        cliPortPrint("'a' Rate PIDs                              'A' Set Roll Rate PID Data   AB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'b' Attitude PIDs                          'B' Set Pitch Rate PID Data  BB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'c' Velocity PIDs                          'C' Set Yaw Rate PID Data    CB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'d' Position PIDs                          'D' Set Roll Att PID Data    DB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'e' Loop Delta Times                       'E' Set Pitch Att PID Data   EB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'f' Loop Execution Times                   'F' Set Hdg Hold PID Data    FB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'g' 500 Hz Accels                          'G' Set nDot PID Data        GB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'h' 100 Hz Earth Axis Accels               'H' Set eDot PID Data        HB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'i' 500 Hz Gyros                           'I' Set hDot PID Data        IB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'j' 10 hz Mag Data                         'J' Set n PID Data           JB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'k' Vertical Axis Variable                 'K' Set e PID Data           KB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'l' Attitudes                              'L' Set h PID Data           LB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("\n");

   		        cliPortPrint("Press space bar for more, or enter a command....\n");

   		        while (cliPortAvailable() == false);

   		        cliQuery = cliPortRead();

   		        if (cliQuery != ' ')
   		        {
   		            validCliCommand = true;
   		            cliBusy = false;
   		        	return;
   		        }

   		        cliPortPrint("\n");
   		        cliPortPrint("'m' Axis PIDs                              'M' Not Used\n");
   		        cliPortPrint("'n' GPS Data                               'N' Mixer CLI\n");
   		        cliPortPrint("'o' Battery Voltage                        'O' Receiver CLI\n");
   		        cliPortPrint("'p' Primary Spektrum Raw Data              'P' Sensor CLI\n");
   		        cliPortPrint("'q' Not Used                               'Q' GPS Data Selection\n");
   		        cliPortPrint("'r' Mode States                            'R' Reset and Enter Bootloader\n");
   		        cliPortPrint("'s' Raw Receiver Commands                  'S' Reset\n");
   		        cliPortPrint("'t' Processed Receiver Commands            'T' Telemetry CLI\n");
   		        cliPortPrint("'u' Command In Detent Discretes            'U' EEPROM CLI\n");
   		        cliPortPrint("'v' Motor PWM Outputs                      'V' Write Sensor EEPROM Parameters\n");
   		        cliPortPrint("'w' Servo PWM Outputs                      'W' Write System EEPROM Parameters\n");
   		        cliPortPrint("'x' Terminate Serial Communication         'X' Not Used\n");
   		        cliPortPrint("\n");

   		        cliPortPrint("Press space bar for more, or enter a command....\n");

   		        while (cliPortAvailable() == false);

   		        cliQuery = cliPortRead();

   		        if (cliQuery != ' ')
   		        {
   		        	validCliCommand = true;
   		        	cliBusy = false;
   		        	return;
   		        }

   		        cliPortPrint("\n");
   		        cliPortPrint("'y' ESC Calibration                        'Y' Not Used\n");
   		        cliPortPrint("'z' ADC Values                             'Z' Not Used\n");
   		        cliPortPrint("                                           '?' Command Summary\n");
   		        cliPortPrint("\n");

  		        cliQuery = 'x';
  		        cliBusy = false;
   		        break;

                ///////////////////////////////
		}
    }
}
Exemple #2
0
void cliCom(void)
{
	uint8_t i2cReadBuff; // TEMP
	uint8_t  index;
	uint8_t  numChannels = 8;

	char mvlkToggleString[5] = { 0, 0, 0, 0, 0 };

	if (eepromConfig.receiverType == PPM)
		numChannels = eepromConfig.ppmChannels;

	if ((cliPortAvailable() && !validCliCommand))
    {
		// Pull one character from buffer to find command
		cliQuery = cliPortRead();

		// Check to see if we should toggle MAVLink state (pound sign)
        if (cliQuery == '#')
        {
	    	while (cliPortAvailable == false);

	    	// Check to see if we have 4 pound signs
        	readStringCLI(mvlkToggleString, 5);
            if ((mvlkToggleString[0] == '#') &&
            	(mvlkToggleString[1] == '#') &&
                (mvlkToggleString[2] == '#') &&
                (mvlkToggleString[3] == '#'))
	    	{
            	// Toggle MAVLink
	    	    if (eepromConfig.mavlinkEnabled == false) 
	    	 	    eepromConfig.mavlinkEnabled  = true;
	    		else
	    		    eepromConfig.mavlinkEnabled = false;

	    	    // Write EEPROM state if pounds were followed by W
	    	    if (mvlkToggleString[4] == 'W')
	    	    {
	                cliPortPrint("\nWriting EEPROM Parameters....\n");
	                writeEEPROM();
	    	    }
	    	}
	    }
	}
	validCliCommand = false;

	// If MAVLink is disabled and we aren't toggling MAVLink, assume CLI command
    if ((eepromConfig.mavlinkEnabled == false) && (cliQuery != '#'))
    {
        switch (cliQuery)
        {
            ///////////////////////////////

            case 'a': // Rate PIDs
                cliPortPrintF("\nRoll Rate PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[ROLL_RATE_PID].P,
                    		                                                    eepromConfig.PID[ROLL_RATE_PID].I,
                    		                                                    eepromConfig.PID[ROLL_RATE_PID].D,
                    		                                                    eepromConfig.PID[ROLL_RATE_PID].Limit);

                cliPortPrintF(  "Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[PITCH_RATE_PID].P,
                    		                                                    eepromConfig.PID[PITCH_RATE_PID].I,
                    		                                                    eepromConfig.PID[PITCH_RATE_PID].D,
                    		                                                    eepromConfig.PID[PITCH_RATE_PID].Limit);

                cliPortPrintF(  "Yaw Rate PID:   %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[YAW_RATE_PID].P,
                    		                                                    eepromConfig.PID[YAW_RATE_PID].I,
                    		                                                    eepromConfig.PID[YAW_RATE_PID].D,
                    		                                                    eepromConfig.PID[YAW_RATE_PID].Limit);
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'b': // Attitude PIDs
                cliPortPrintF("\nRoll Attitude PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[ROLL_ATT_PID].P,
                   		                                                            eepromConfig.PID[ROLL_ATT_PID].I,
                   		                                                            eepromConfig.PID[ROLL_ATT_PID].D,
                   		                                                            eepromConfig.PID[ROLL_ATT_PID].Limit);

                cliPortPrintF(  "Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[PITCH_ATT_PID].P,
                   		                                                            eepromConfig.PID[PITCH_ATT_PID].I,
                   		                                                            eepromConfig.PID[PITCH_ATT_PID].D,
                   		                                                            eepromConfig.PID[PITCH_ATT_PID].Limit);

                cliPortPrintF(  "Heading PID:        %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[HEADING_PID].P,
                   		                                                            eepromConfig.PID[HEADING_PID].I,
                   		                                                            eepromConfig.PID[HEADING_PID].D,
                   		                                                            eepromConfig.PID[HEADING_PID].Limit);
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'c': // Velocity PIDs
                cliPortPrintF("\nnDot PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[NDOT_PID].P,
                   		                                                   eepromConfig.PID[NDOT_PID].I,
                   		                                                   eepromConfig.PID[NDOT_PID].D,
                   		                                                   eepromConfig.PID[NDOT_PID].Limit);

                cliPortPrintF(  "eDot PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[EDOT_PID].P,
                   		                                                   eepromConfig.PID[EDOT_PID].I,
                   		                                                   eepromConfig.PID[EDOT_PID].D,
                   		                                                   eepromConfig.PID[EDOT_PID].Limit);

                cliPortPrintF(  "hDot PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[HDOT_PID].P,
                   		                                                   eepromConfig.PID[HDOT_PID].I,
                   		                                                   eepromConfig.PID[HDOT_PID].D,
                   		                                                   eepromConfig.PID[HDOT_PID].Limit);
                cliQuery = 'x';
                validCliCommand = false;
                break;


            ///////////////////////////////

            case 'd': // Position PIDs
                cliPortPrintF("\nN PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[N_PID].P,
                   		                                                eepromConfig.PID[N_PID].I,
                   		                                                eepromConfig.PID[N_PID].D,
                   		                                                eepromConfig.PID[N_PID].Limit);

                cliPortPrintF(  "E PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[E_PID].P,
                   		                                                eepromConfig.PID[E_PID].I,
                   		                                                eepromConfig.PID[E_PID].D,
                   		                                                eepromConfig.PID[E_PID].Limit);

                cliPortPrintF(  "h PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[H_PID].P,
                   		                                                eepromConfig.PID[H_PID].I,
                   		                                                eepromConfig.PID[H_PID].D,
                   		                                                eepromConfig.PID[H_PID].Limit);
                cliQuery = 'x';
                validCliCommand = false;
              	break;

			///////////////////////////////

			case 'e': // Loop Delta Times
				cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz,
																			deltaTime500Hz,
																			deltaTime100Hz,
																			deltaTime50Hz,
																			deltaTime10Hz,
																			deltaTime5Hz,
																			deltaTime1Hz);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'f': // Loop Execution Times
				cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz,
																			executionTime500Hz,
																			executionTime100Hz,
																			executionTime50Hz,
																			executionTime10Hz,
																			executionTime5Hz,
																			executionTime1Hz);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'g': // 100 Hz Accels
				cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel100Hz[XAXIS],
													   sensors.accel100Hz[YAXIS],
													   sensors.accel100Hz[ZAXIS]);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'h': // 100 hz Earth Axis Accels
				cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS],
													   earthAxisAccels[YAXIS],
													   earthAxisAccels[ZAXIS]);
				validCliCommand = false;
				break;
			///////////////////////////////

			case 'i': // 500 hz Gyros
				cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D,
															  sensors.gyro500Hz[PITCH] * R2D,
															  sensors.gyro500Hz[YAW  ] * R2D,
															  mpu6000Temperature);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'j': // 10 Hz Mag Data
				cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS],
													   sensors.mag10Hz[YAXIS],
													   sensors.mag10Hz[ZAXIS]);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'k': // Vertical Axis Variables
				cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld, %9.4f\n", earthAxisAccels[ZAXIS],
																		   sensors.pressureAlt50Hz,
																		   hDotEstimate,
																		   hEstimate,
																		   ms5611Temperature,
																		   aglRead());
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'l': // Attitudes
				cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D,
													   sensors.attitude500Hz[PITCH] * R2D,
													   sensors.attitude500Hz[YAW  ] * R2D);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'm': // Axis PIDs
				cliPortPrintF("%9.4f, %9.4f, %9.4f\n", ratePID[ROLL ],
													   ratePID[PITCH],
													   ratePID[YAW  ]);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'n': // GPS Data
				cliPortPrintF("ITOW:%12ld, LAT:%12ld, LONG:%12ld, HEAD:%12ld, HEIGHT:%12ld, HMSL:%12ld, FIX:%4d, NUMSAT:%4d\n",
						gps.iTOW,
						gps.latitude,
						gps.longitude,
						gps.hDop,
						gps.height,
						gps.hMSL,
						gps.fix,
						gps.numSats);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'o':
				cliPortPrintF("%9.4f\n", batteryVoltage);

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'p':
				cameraCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'q':
				adcCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////FLIGHT MODE

			case 'r':
				if (flightMode == RATE)
					cliPortPrint("Flight Mode:RATE      ");
				else if (flightMode == ATTITUDE)
					cliPortPrint("Flight Mode:ATTITUDE  ");
				else if (flightMode == GPS)
					cliPortPrint("Flight Mode:GPS       ");

				if (headingHoldEngaged == true)
					cliPortPrint("Heading Hold:ENGAGED     ");
				else
					cliPortPrint("Heading Hold:DISENGAGED  ");

				switch (verticalModeState)
				{
					case ALT_DISENGAGED_THROTTLE_ACTIVE:
						cliPortPrint("Alt:Disenaged Throttle Active      ");

						break;

					case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
						cliPortPrint("Alt:Hold Fixed at Engagement Alt   ");

						break;

					case ALT_HOLD_AT_REFERENCE_ALTITUDE:
						cliPortPrint("Alt:Hold at Reference Alt          ");

						break;

					case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
						cliPortPrint("Alt:Velocity Hold at Reference Vel ");

						break;

					case ALT_DISENGAGED_THROTTLE_INACTIVE:
						cliPortPrint("Alt:Disengaged Throttle Inactive   ");

						break;
				}

				if (rxCommand[AUX3] > MIDCOMMAND)
					cliPortPrint("Mode:Simple  ");
				else
					cliPortPrint("Mode:Normal  ");

				if (rxCommand[AUX4] > MIDCOMMAND)
					cliPortPrint("Emergency Bail:Active\n");
				else
					cliPortPrint("Emergency Bail:Inactive\n");

				validCliCommand = false;
				break;

			///////////////////////////////

			case 's': // Raw Receiver Commands
				if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0))
                {
		    		for (index = 0; index < maxChannelNum - 1; index++)
                         cliPortPrintF("%4ld, ", spektrumBuf[index]);

                    cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]);
			    }
				else if (eepromConfig.receiverType == SBUS)
				{
		    		for (index = 0; index < 7; index++)
                         cliPortPrintF("%4ld, ", sBusRead(index));

                    cliPortPrintF("%4ld\n", sBusRead(7));

				}
				else
				{
					for (index = 0; index < numChannels - 1; index++)
						cliPortPrintF("%4i, ", Inputs[index].pulseWidth);

					cliPortPrintF("%4i\n", Inputs[numChannels - 1].pulseWidth);
				}

				validCliCommand = false;
				break;

			///////////////////////////////

			case 't': // Processed Receiver Commands
				for (index = 0; index < numChannels - 1; index++)
					cliPortPrintF("%8.2f, ", rxCommand[index]);

				cliPortPrintF("%8.2f\n", rxCommand[numChannels - 1]);

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'u': // Command in Detent Discretes
				cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false");
				cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false");
				cliPortPrintF("%s\n", commandInDetent[YAW  ] ? " true" : "false");

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'v': // ESC PWM Outputs
				cliPortPrintF("%4ld, ", TIM8->CCR4);
				cliPortPrintF("%4ld, ", TIM8->CCR3);
				cliPortPrintF("%4ld, ", TIM8->CCR2);
				cliPortPrintF("%4ld, ", TIM8->CCR1);
				cliPortPrintF("%4ld, ", TIM2->CCR1);
				cliPortPrintF("%4ld, ", TIM2->CCR2);
				cliPortPrintF("%4ld, ", TIM3->CCR1);
				cliPortPrintF("%4ld\n", TIM3->CCR2);

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'w': // Servo PWM Outputs
				cliPortPrintF("%4ld, ", TIM5->CCR3);
				cliPortPrintF("%4ld, ", TIM5->CCR2);
				cliPortPrintF("%4ld\n", TIM5->CCR1);

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'x':
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'y': // ESC Calibration
				escCalibration();

				cliQuery = 'x';
				break;

			///////////////////////////////

			case 'z':	// ADC readings
				cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f\n", adcValue(1),
																				   adcValue(2),
																				   adcValue(3),
																				   adcValue(4),
																				   adcValue(5),
																				   adcValue(6),
																				   adcValue(7));
				break;

			///////////////////////////////

			///////////////////////////////////////////////////////////////////////
			///////////////////////////////////////////////////////////////////////
			///////////////////////////////////////////////////////////////////////
			///////////////////////////////////////////////////////////////////////

			///////////////////////////////

			case 'A': // Read Roll Rate PID Values
				readCliPID(ROLL_RATE_PID);
				cliPortPrint( "\nRoll Rate PID Received....\n" );

				cliQuery = 'a';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'B': // Read Pitch Rate PID Values
				readCliPID(PITCH_RATE_PID);
				cliPortPrint( "\nPitch Rate PID Received....\n" );

				cliQuery = 'a';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'C': // Read Yaw Rate PID Values
				readCliPID(YAW_RATE_PID);
				cliPortPrint( "\nYaw Rate PID Received....\n" );

				cliQuery = 'a';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'D': // Read Roll Attitude PID Values
				readCliPID(ROLL_ATT_PID);
				cliPortPrint( "\nRoll Attitude PID Received....\n" );

				cliQuery = 'b';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'E': // Read Pitch Attitude PID Values
				readCliPID(PITCH_ATT_PID);
				cliPortPrint( "\nPitch Attitude PID Received....\n" );

				cliQuery = 'b';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'F': // Read Heading Hold PID Values
				readCliPID(HEADING_PID);
				cliPortPrint( "\nHeading PID Received....\n" );

				cliQuery = 'b';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'G': // Read nDot PID Values
				readCliPID(NDOT_PID);
				cliPortPrint( "\nnDot PID Received....\n" );

				cliQuery = 'c';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'H': // Read eDot PID Values
				readCliPID(EDOT_PID);
				cliPortPrint( "\neDot PID Received....\n" );

				cliQuery = 'c';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'I': // Read hDot PID Values
				readCliPID(HDOT_PID);
				cliPortPrint( "\nhDot PID Received....\n" );

				cliQuery = 'c';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'J': // Read n PID Values
				readCliPID(N_PID);
				cliPortPrint( "\nn PID Received....\n" );

				cliQuery = 'd';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'K': // Read e PID Values
				readCliPID(E_PID);
				cliPortPrint( "\ne PID Received....\n" );

				cliQuery = 'd';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'L': // Read h PID Values
				readCliPID(H_PID);
				cliPortPrint( "\nh PID Received....\n" );

				cliQuery = 'd';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'M': // MAX7456 CLI
				max7456CLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'N': // Mixer CLI
				mixerCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'O': // Receiver CLI
				receiverCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'P': // Sensor CLI
				sensorCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'Q': // GPS Type Selection
				cliBusy = true;
				eepromConfig.gpsType = (uint8_t)readFloatCLI();
				switch(eepromConfig.gpsType) {
					case NO_GPS:
						cliPortPrint("GPS Module Disabled\n");
						break;
					case MEDIATEK_3329_BINARY:
						cliPortPrint("GPS Module set to MEDIATEK3329 (Binary)\n");
						break;
					case MEDIATEK_3329_NMEA:
						cliPortPrint("GPS Module set to MEDIATEK3329 (NMEA)\n");
						break;
					case UBLOX:
						cliPortPrint("GPS Module set to UBLOX\n");
						break;
					default:
						cliPortPrint("Invalid GPS module type. Use 0-3 (NONE, MEDIATEK BINARY, MEDIATEK NMEA, UBLOX\n");
						break;
				}
				initGPS();
				cliBusy = false;
                cliQuery = 'x';
                validCliCommand = false;
				break;

			///////////////////////////////

			case 'R': // Reset to Bootloader
				cliPortPrint("Entering Bootloader....\n\n");
				delay(100);
				systemReset(true);
				break;

			///////////////////////////////

			case 'S': // Reset System
				cliPortPrint("\nSystem Reseting....\n\n");
				delay(100);
				systemReset(false);
				break;

			///////////////////////////////

			case 'T': // Telemetry CLI
				telemetryCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'U': // EEPROM CLI
				eepromCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'V': // Reset EEPROM Parameters
				cliPortPrint( "\nEEPROM Parameters Reset....\n" );
				checkFirstTime(true);
				cliPortPrint("\nSystem Resetting....\n\n");
				delay(100);
				systemReset(false);
				break;

			///////////////////////////////

			case 'W': // Write EEPROM Parameters
				cliPortPrint("\nWriting EEPROM Parameters....\n");
				writeEEPROM();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'X': // Environmental Sensor Bus CLI
				esbCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'Y': // ADC CLI
				 adcCLI();

				 cliQuery = 'x';
				 validCliCommand = false;
				 break;

			///////////////////////////////

			case 'Z': // Not Used
				computeGeoMagElements();

				cliQuery = 'x';
				break;

			///////////////////////////////

			case '?': // Command Summary
				cliBusy = true;

				cliPortPrint("\n");
				cliPortPrint("'a' Rate PIDs                              'A' Set Roll Rate PID Data   AP;I;D;N\n");
				cliPortPrint("'b' Attitude PIDs                          'B' Set Pitch Rate PID Data  BP;I;D;N\n");
				cliPortPrint("'c' Velocity PIDs                          'C' Set Yaw Rate PID Data    CP;I;D;N\n");
				cliPortPrint("'d' Position PIDs                          'D' Set Roll Att PID Data    DP;I;D;N\n");
				cliPortPrint("'e' Loop Delta Times                       'E' Set Pitch Att PID Data   EP;I;D;N\n");
				cliPortPrint("'f' Loop Execution Times                   'F' Set Hdg Hold PID Data    FP;I;D;N\n");
				cliPortPrint("'g' 500 Hz Accels                          'G' Set nDot PID Data        GP;I;D;N\n");
				cliPortPrint("'h' 100 Hz Earth Axis Accels               'H' Set eDot PID Data        HP;I;D;N\n");
				cliPortPrint("'i' 500 Hz Gyros                           'I' Set hDot PID Data        IP;I;D;N\n");
				cliPortPrint("'j' 10 hz Mag Data                         'J' Set n PID Data           JP;I;D;N\n");
				cliPortPrint("'k' Vertical Axis Variable                 'K' Set e PID Data           KP;I;D;N\n");
				cliPortPrint("'l' Attitudes                              'L' Set h PID Data           LP;I;D;N\n");
				cliPortPrint("'m' Axis PIDs                              'M' MAX7456 CLI\n");
				cliPortPrint("'n' GPS Data                               'N' Mixer CLI\n");
				cliPortPrint("'o' Battery Voltage                        'O' Receiver CLI\n");
				cliPortPrint("'p' Camera CLI                             'P' Sensor CLI\n");
				cliPortPrint("'q' ADC CLI                                'Q' GPS Data Selection\n");
				cliPortPrint("'r' Mode States                            'R' Reset and Enter Bootloader\n");
				cliPortPrint("'s' Raw Receiver Commands                  'S' Reset\n");
				cliPortPrint("'t' Processed Receiver Commands            'T' Telemetry CLI\n");
				cliPortPrint("'u' Command In Detent Discretes            'U' EEPROM CLI\n");
				cliPortPrint("'v' Motor PWM Outputs                      'V' Reset EEPROM Parameters\n");
				cliPortPrint("'w' Servo PWM Outputs                      'W' Write EEPROM Parameters\n");
				cliPortPrint("'x' Terminate Serial Communication         'X' ESB CLI\n");
				cliPortPrint("'y' ESC Calibration/Motor Verification     'Y' ADC CLI\n");
				cliPortPrint("'z' ADC Values                             'Z' WMM Test\n");
				cliPortPrint("                                           '?' Command Summary\n");
				cliPortPrint("\n");

				cliQuery = 'x';
				cliBusy = false;
				break;

			///////////////////////////////
		}
    }
}
Exemple #3
0
void cliCom(void)
{
	uint8_t  index;
	char mvlkToggleString[5] = { 0, 0, 0, 0, 0 };

    if ((cliPortAvailable() && !validCliCommand))
    {
		cliQuery = cliPortRead();

        if (cliQuery == '#')                       // Check to see if we should toggle mavlink msg state
        {
	    	while (cliPortAvailable == false);

        	readStringCLI(mvlkToggleString, 5);

            if ((mvlkToggleString[0] == '#') &&
            	(mvlkToggleString[1] == '#') &&
                (mvlkToggleString[2] == '#') &&
                (mvlkToggleString[3] == '#'))
	    	{
	    	    if (eepromConfig.mavlinkEnabled == false)
	    	    {
	    	 	    eepromConfig.mavlinkEnabled  = true;
	    		    eepromConfig.activeTelemetry = 0x0000;
	    		}
	    		else
	    		{
	    		    eepromConfig.mavlinkEnabled = false;
	    	    }

	    	    if (mvlkToggleString[4] == 'W')
	    	    {
	                cliPortPrint("\nWriting EEPROM Parameters....\n");
	                writeEEPROM();
	    	    }
	    	}
	    }
	}

	validCliCommand = false;

    if ((eepromConfig.mavlinkEnabled == false) && (cliQuery != '#'))
    {
		switch (cliQuery)
        {
            ///////////////////////////////

            case 'a': // Rate PIDs
                cliPortPrintF("\nRoll Rate PID:  %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[ROLL_RATE_PID].B,
                                		                                                   eepromConfig.PID[ROLL_RATE_PID].P,
                    		                                                               eepromConfig.PID[ROLL_RATE_PID].I,
                    		                                                               eepromConfig.PID[ROLL_RATE_PID].D,
                     		                                                               eepromConfig.PID[ROLL_RATE_PID].windupGuard,
                    		                                                               eepromConfig.PID[ROLL_RATE_PID].dErrorCalc ? "Error" : "State");

                cliPortPrintF("Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n",   eepromConfig.PID[PITCH_RATE_PID].B,
                                		                                                   eepromConfig.PID[PITCH_RATE_PID].P,
                    		                                                               eepromConfig.PID[PITCH_RATE_PID].I,
                    		                                                               eepromConfig.PID[PITCH_RATE_PID].D,
                    		                                                               eepromConfig.PID[PITCH_RATE_PID].windupGuard,
                    		                                                               eepromConfig.PID[PITCH_RATE_PID].dErrorCalc ? "Error" : "State");

                cliPortPrintF("Yaw Rate PID:   %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n",   eepromConfig.PID[YAW_RATE_PID].B,
                                  		                                                   eepromConfig.PID[YAW_RATE_PID].P,
                    		                                                               eepromConfig.PID[YAW_RATE_PID].I,
                    		                                                               eepromConfig.PID[YAW_RATE_PID].D,
                    		                                                               eepromConfig.PID[YAW_RATE_PID].windupGuard,
                    		                                                               eepromConfig.PID[YAW_RATE_PID].dErrorCalc ? "Error" : "State");
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'b': // Attitude PIDs
                cliPortPrintF("\nRoll Attitude PID:  %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[ROLL_ATT_PID].B,
                  		                                                                       eepromConfig.PID[ROLL_ATT_PID].P,
                   		                                                                       eepromConfig.PID[ROLL_ATT_PID].I,
                   		                                                                       eepromConfig.PID[ROLL_ATT_PID].D,
                   		                                                                       eepromConfig.PID[ROLL_ATT_PID].windupGuard,
                   		                                                                       eepromConfig.PID[ROLL_ATT_PID].dErrorCalc ? "Error" : "State");

                cliPortPrintF("Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n",   eepromConfig.PID[PITCH_ATT_PID].B,
                   		                                                                       eepromConfig.PID[PITCH_ATT_PID].P,
                   		                                                                       eepromConfig.PID[PITCH_ATT_PID].I,
                   		                                                                       eepromConfig.PID[PITCH_ATT_PID].D,
                   		                                                                       eepromConfig.PID[PITCH_ATT_PID].windupGuard,
                   		                                                                       eepromConfig.PID[PITCH_ATT_PID].dErrorCalc ? "Error" : "State");

                cliPortPrintF("Heading PID:        %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n",   eepromConfig.PID[HEADING_PID].B,
                   		                                                                       eepromConfig.PID[HEADING_PID].P,
                   		                                                                       eepromConfig.PID[HEADING_PID].I,
                   		                                                                       eepromConfig.PID[HEADING_PID].D,
                   		                                                                       eepromConfig.PID[HEADING_PID].windupGuard,
                   		                                                                       eepromConfig.PID[HEADING_PID].dErrorCalc ? "Error" : "State");
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'c': // Velocity PIDs
                cliPortPrintF("\nhDot PID:  %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n",   eepromConfig.PID[HDOT_PID].B,
                   		                                                                eepromConfig.PID[HDOT_PID].P,
                   		                                                                eepromConfig.PID[HDOT_PID].I,
                   		                                                                eepromConfig.PID[HDOT_PID].D,
                   		                                                                eepromConfig.PID[HDOT_PID].windupGuard,
                   		                                                                eepromConfig.PID[HDOT_PID].dErrorCalc ? "Error" : "State");
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'd': // Position PIDs
                cliPortPrintF("\nh PID:  %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n",   eepromConfig.PID[H_PID].B,
                   		                                                             eepromConfig.PID[H_PID].P,
                   		                                                             eepromConfig.PID[H_PID].I,
                   		                                                             eepromConfig.PID[H_PID].D,
                   		                                                             eepromConfig.PID[H_PID].windupGuard,
                   		                                                             eepromConfig.PID[H_PID].dErrorCalc ? "Error" : "State");
                cliQuery = 'x';
                validCliCommand = false;
              	break;

            ///////////////////////////////

            case 'e': // Loop Delta Times
           	    cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz,
                    		                                                deltaTime500Hz,
               		                                                        deltaTime100Hz,
               		                                                        deltaTime50Hz,
               		                                                        deltaTime10Hz,
               		                                                        deltaTime5Hz,
               		                                                        deltaTime1Hz);
        	validCliCommand = false;
        	break;

            ///////////////////////////////

            case 'f': // Loop Execution Times
               	cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz,
               	        			                                        executionTime500Hz,
               	        			                                        executionTime100Hz,
               	        			                                        executionTime50Hz,
               	        			                                        executionTime10Hz,
               	        			                                        executionTime5Hz,
               	        			                                        executionTime1Hz);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'g': // 500 Hz Accels
            	cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
            			                               sensors.accel500Hz[YAXIS],
            			                               sensors.accel500Hz[ZAXIS]);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'h': // 100 hz Earth Axis Accels
            	cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS],
            			                               earthAxisAccels[YAXIS],
            			                               earthAxisAccels[ZAXIS]);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'i': // 500 hz Gyros
            	cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D,
            			                                      sensors.gyro500Hz[PITCH] * R2D,
            					                              sensors.gyro500Hz[YAW  ] * R2D,
             					                              mpuTemperature);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'j': // 10 Hz Mag Data
            	cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS],
            			                               sensors.mag10Hz[YAXIS],
            			                               sensors.mag10Hz[ZAXIS]);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'k': // Vertical Axis Variables
            	cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", earthAxisAccels[ZAXIS],
            			                                      sensors.pressureAlt50Hz,
            					                              hDotEstimate,
            					                              hEstimate);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'l': // Attitudes
            	cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D,
            			                               sensors.attitude500Hz[PITCH] * R2D,
            			                               sensors.attitude500Hz[YAW  ] * R2D);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'm': // Axis PIDs
            	cliPortPrintF("%9.4f, %9.4f, %9.4f\n", axisPID[ROLL ],
               			                               axisPID[PITCH],
               			                               axisPID[YAW  ]);
               	validCliCommand = false;
               	break;

            ///////////////////////////////

            case 'o':
                cliPortPrintF("%9.4f\n", batteryVoltage);

                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'p': // Primary Spektrum Raw Data
            	cliPortPrintF("%04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X\n", primarySpektrumState.lostFrameCnt,
            			                                                                      primarySpektrumState.rcAvailable,
            			                                                                      primarySpektrumState.values[0],
            			                                                                      primarySpektrumState.values[1],
            			                                                                      primarySpektrumState.values[2],
            			                                                                      primarySpektrumState.values[3],
            			                                                                      primarySpektrumState.values[4],
            			                                                                      primarySpektrumState.values[5],
            			                                                                      primarySpektrumState.values[6],
            			                                                                      primarySpektrumState.values[7]);
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'q': // Not Used
                cliQuery = 'x';
               	validCliCommand = false;
               	break;

            ///////////////////////////////

            case 'r':
        	    if (flightMode == RATE)
        	    	cliPortPrint("Flight Mode = RATE      ");
        	    else if (flightMode == ATTITUDE)
        	    	cliPortPrint("Flight Mode = ATTITUDE  ");

        	    if (headingHoldEngaged == true)
        	        cliPortPrint("Heading Hold = ENGAGED     ");
        	    else
        	        cliPortPrint("Heading Hold = DISENGAGED  ");

        	    cliPortPrint("Alt Hold = ");

                switch (verticalModeState)
        	    {
        	    	case ALT_DISENGAGED_THROTTLE_ACTIVE:
		                cliPortPrint("Alt Disenaged Throttle Active\n");

        	    	    break;

        	    	case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
		                cliPortPrint("Alt Hold Fixed at Engagement Alt\n");

        	    	    break;

        	    	case ALT_HOLD_AT_REFERENCE_ALTITUDE:
		                cliPortPrint("Alt Hold at Reference Alt\n");

        	    	    break;

        	    	case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
		                cliPortPrint("V Velocity Hold at Reference Vel\n");

        	     	    break;

        	     	case ALT_DISENGAGED_THROTTLE_INACTIVE:
        	    	    cliPortPrint("Alt Disengaged Throttle Inactive\n");

        	    	    break;
                }

        	    validCliCommand = false;
        	    break;

            ///////////////////////////////

            case 's': // Raw Receiver Commands
                if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0))
                {
		    		for (index = 0; index < maxChannelNum - 1; index++)
                         cliPortPrintF("%4ld, ", spektrumBuf[index]);

                    cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]);
                }
                else if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum == 0))
                    cliPortPrint("Invalid Number of Spektrum Channels....\n");
		        else
		        {
		    		for (index = 0; index < 7; index++)
                        cliPortPrintF("%4i, ", ppmRxRead(index));

                    cliPortPrintF("%4i\n", ppmRxRead(7));
                }

            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 't': // Processed Receiver Commands
                for (index = 0; index < 7; index++)
                    cliPortPrintF("%8.2f, ", rxCommand[index]);

                cliPortPrintF("%8.2f\n", rxCommand[7]);

                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'u': // Command in Detent Discretes
            	cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false");
            	cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false");
            	cliPortPrintF("%s\n", commandInDetent[YAW  ] ? " true" : "false");

                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'v': // ESC PWM Outputs
            	cliPortPrintF("%4ld, ", TIM4->CCR4);
            	cliPortPrintF("%4ld, ", TIM4->CCR3);
                cliPortPrintF("%4ld, ", TIM4->CCR2);
            	cliPortPrintF("%4ld, ", TIM4->CCR1);
            	cliPortPrintF("%4ld, ", TIM1->CCR4);
            	cliPortPrintF("%4ld\n", TIM1->CCR1);

            	validCliCommand = false;
                break;

            ///////////////////////////////

            case 'x':
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'y': // ESC Calibration
            	escCalibration();

            	cliQuery = 'x';
            	break;

            ///////////////////////////////

            case 'z': // Voltage monitor ADC, Battery voltage
                cliPortPrintF("%7.2f, %5.2f\n", voltageMonitor(), batteryVoltage);

                break;

            ///////////////////////////////

            ///////////////////////////////////////////////////////////////////////
            ///////////////////////////////////////////////////////////////////////
            ///////////////////////////////////////////////////////////////////////
            ///////////////////////////////////////////////////////////////////////

            ///////////////////////////////

            case 'A': // Read Roll Rate PID Values
                readCliPID(ROLL_RATE_PID);
                cliPortPrint( "\nRoll Rate PID Received....\n" );

            	cliQuery = 'a';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'B': // Read Pitch Rate PID Values
                readCliPID(PITCH_RATE_PID);
                cliPortPrint( "\nPitch Rate PID Received....\n" );

            	cliQuery = 'a';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'C': // Read Yaw Rate PID Values
                readCliPID(YAW_RATE_PID);
                cliPortPrint( "\nYaw Rate PID Received....\n" );

            	cliQuery = 'a';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'D': // Read Roll Attitude PID Values
                readCliPID(ROLL_ATT_PID);
                cliPortPrint( "\nRoll Attitude PID Received....\n" );

            	cliQuery = 'b';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'E': // Read Pitch Attitude PID Values
                readCliPID(PITCH_ATT_PID);
                cliPortPrint( "\nPitch Attitude PID Received....\n" );

            	cliQuery = 'b';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'F': // Read Heading Hold PID Values
                readCliPID(HEADING_PID);
                cliPortPrint( "\nHeading PID Received....\n" );

            	cliQuery = 'b';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'I': // Read hDot PID Values
                readCliPID(HDOT_PID);
                cliPortPrint( "\nhDot PID Received....\n" );

              	cliQuery = 'c';
              	validCliCommand = false;
              	break;

       	    ///////////////////////////////

            case 'L': // Read h PID Values
                readCliPID(H_PID);
                cliPortPrint( "\nh PID Received....\n" );

                cliQuery = 'd';
            	validCliCommand = false;
            	break;

            ///////////////////////////////

            case 'N': // Mixer CLI
                mixerCLI();

                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'O': // Receiver CLI
                receiverCLI();

                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'P': // Sensor CLI
               	sensorCLI();

               	cliQuery = 'x';
               	validCliCommand = false;
               	break;

            ///////////////////////////////

            case 'R': // Reset to Bootloader
            	cliPortPrint("Entering Bootloader....\n\n");
            	delay(100);
            	systemReset(true);
            	break;

            ///////////////////////////////

            case 'S': // Reset System
            	cliPortPrint("\nSystem Reseting....\n\n");
            	delay(100);
            	systemReset(false);
            	break;

            ///////////////////////////////

            case 'T': // Telemetry CLI
                telemetryCLI();

                cliQuery = 'x';
             	validCliCommand = false;
              	break;

            ///////////////////////////////

            case 'U': // EEPROM CLI
                eepromCLI();

                cliQuery = 'x';
              	validCliCommand = false;
               	break;

            ///////////////////////////////

            case 'V': // Reset EEPROM Parameters
                cliPortPrint( "\nEEPROM Parameters Reset....\n" );
                checkFirstTime(true);
                cliPortPrint("\nSystem Resetting....\n\n");
                delay(100);
                systemReset(false);
                break;

            ///////////////////////////////

            case 'W': // Write EEPROM Parameters
                cliPortPrint("\nWriting EEPROM Parameters....\n");
                writeEEPROM();

                cliQuery = 'x';
             	validCliCommand = false;
             	break;

            ///////////////////////////////

            case 'X': // Not Used
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'Y': // Not Used
                cliQuery = 'x';
                break;

            ///////////////////////////////

            case 'Z': // Not Used
                cliQuery = 'x';
                break;

            ///////////////////////////////

            case '?': // Command Summary
            	cliBusy = true;

            	cliPortPrint("\n");
   		        cliPortPrint("'a' Rate PIDs                              'A' Set Roll Rate PID Data   AB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'b' Attitude PIDs                          'B' Set Pitch Rate PID Data  BB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'c' Velocity PIDs                          'C' Set Yaw Rate PID Data    CB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'d' Position PIDs                          'D' Set Roll Att PID Data    DB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'e' Loop Delta Times                       'E' Set Pitch Att PID Data   EB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'f' Loop Execution Times                   'F' Set Hdg Hold PID Data    FB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'g' 500 Hz Accels                          'G' Not Used\n");
   		        cliPortPrint("'h' 100 Hz Earth Axis Accels               'H' Not Used\n");
   		        cliPortPrint("'i' 500 Hz Gyros                           'I' Set hDot PID Data        IB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("'j' 10 hz Mag Data                         'J' Not Used\n");
   		        cliPortPrint("'k' Vertical Axis Variable                 'K' Not Used\n");
   		        cliPortPrint("'l' Attitudes                              'L' Set h PID Data           LB;P;I;D;windupGuard;dErrorCalc\n");
   		        cliPortPrint("\n");

   		        cliPortPrint("Press space bar for more, or enter a command....\n");

   		        while (cliPortAvailable() == false);

   		        cliQuery = cliPortRead();

   		        if (cliQuery != ' ')
   		        {
   		            validCliCommand = true;
   		            cliBusy = false;
   		        	return;
   		        }

   		        cliPortPrint("\n");
   		        cliPortPrint("'m' Axis PIDs                              'M' Not Used\n");
   		        cliPortPrint("'n' Not Used                               'N' Mixer CLI\n");
   		        cliPortPrint("'o' Battery Voltage                        'O' Receiver CLI\n");
   		        cliPortPrint("'p' Not Used                               'P' Sensor CLI\n");
   		        cliPortPrint("'q' Primary Spektrum Raw Data              'Q' Not Used\n");
   		        cliPortPrint("'r' Mode States                            'R' Reset and Enter Bootloader\n");
   		        cliPortPrint("'s' Raw Receiver Commands                  'S' Reset\n");
   		        cliPortPrint("'t' Processed Receiver Commands            'T' Telemetry CLI\n");
   		        cliPortPrint("'u' Command In Detent Discretes            'U' EEPROM CLI\n");
   		        cliPortPrint("'v' Motor PWM Outputs                      'V' Reset EEPROM Parameters\n");
   		        cliPortPrint("'w' Not Used                               'W' Write EEPROM Parameters\n");
   		        cliPortPrint("'x' Terminate Serial Communication         'X' Not Used\n");
   		        cliPortPrint("\n");

   		        cliPortPrint("Press space bar for more, or enter a command....\n");

   		        while (cliPortAvailable() == false);

   		        cliQuery = cliPortRead();

   		        if (cliQuery != ' ')
   		        {
   		        	validCliCommand = true;
   		        	cliBusy = false;
   		        	return;
   		        }

   		        cliPortPrint("\n");
   		        cliPortPrint("'y' ESC Calibration                        'Y' Not Used\n");
   		        cliPortPrint("'z' ADC Values                             'Z' Not Used\n");
   		        cliPortPrint("'#####' Toggle MavLink Msg State           '?' Command Summary\n");
   		        cliPortPrint("\n");

  		        cliQuery = 'x';
  		        cliBusy = false;
   		        break;

                ///////////////////////////////
	    }
    }
}
Exemple #4
0
void cliCom(void)
{
	uint8_t  index;
	uint8_t  numChannels = 8;
	char mvlkToggleString[5] = { 0, 0, 0, 0, 0 };

	if (eepromConfig.receiverType == PPM)
		numChannels = eepromConfig.ppmChannels;

	if ((cliPortAvailable() && !validCliCommand))
    {
		cliQuery = cliPortRead();

        if (cliQuery == '#')                       // Check to see if we should toggle mavlink msg state
        {
	    	while (cliPortAvailable == false);

        	readStringCLI(mvlkToggleString, 5);

            if ((mvlkToggleString[0] == '#') &&
            	(mvlkToggleString[1] == '#') &&
                (mvlkToggleString[2] == '#') &&
                (mvlkToggleString[3] == '#'))
	    	{
	    	    if (eepromConfig.mavlinkEnabled == false)
	    	    {
	    	 	    eepromConfig.mavlinkEnabled  = true;
	    		    eepromConfig.activeTelemetry = 0x0000;
	    		}
	    		else
	    		{
	    		    eepromConfig.mavlinkEnabled = false;
	    	    }

	    	    if (mvlkToggleString[4] == 'W')
	    	    {
	                cliPortPrint("\nWriting EEPROM Parameters....\n");
	                writeEEPROM();
	    	    }
	    	}
	    }
	}

	validCliCommand = false;

    if ((eepromConfig.mavlinkEnabled == false) && (cliQuery != '#'))
    {
        switch (cliQuery)
        {
            ///////////////////////////////

            case 'a': // Rate PIDs
                cliPortPrintF("\nRoll Rate PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[ROLL_RATE_PID].P,
                    		                                                    eepromConfig.PID[ROLL_RATE_PID].I,
                    		                                                    eepromConfig.PID[ROLL_RATE_PID].D,
                    		                                                    eepromConfig.PID[ROLL_RATE_PID].Limit);

                cliPortPrintF(  "Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[PITCH_RATE_PID].P,
                    		                                                    eepromConfig.PID[PITCH_RATE_PID].I,
                    		                                                    eepromConfig.PID[PITCH_RATE_PID].D,
                    		                                                    eepromConfig.PID[PITCH_RATE_PID].Limit);

                cliPortPrintF(  "Yaw Rate PID:   %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[YAW_RATE_PID].P,
                    		                                                    eepromConfig.PID[YAW_RATE_PID].I,
                    		                                                    eepromConfig.PID[YAW_RATE_PID].D,
                    		                                                    eepromConfig.PID[YAW_RATE_PID].Limit);
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'b': // Attitude PIDs
                cliPortPrintF("\nRoll Attitude PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[ROLL_ATT_PID].P,
                   		                                                            eepromConfig.PID[ROLL_ATT_PID].I,
                   		                                                            eepromConfig.PID[ROLL_ATT_PID].D,
                   		                                                            eepromConfig.PID[ROLL_ATT_PID].Limit);

                cliPortPrintF(  "Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[PITCH_ATT_PID].P,
                   		                                                            eepromConfig.PID[PITCH_ATT_PID].I,
                   		                                                            eepromConfig.PID[PITCH_ATT_PID].D,
                   		                                                            eepromConfig.PID[PITCH_ATT_PID].Limit);

                cliPortPrintF(  "Heading PID:        %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[HEADING_PID].P,
                   		                                                            eepromConfig.PID[HEADING_PID].I,
                   		                                                            eepromConfig.PID[HEADING_PID].D,
                   		                                                            eepromConfig.PID[HEADING_PID].Limit);
                cliQuery = 'x';
                validCliCommand = false;
                break;

            ///////////////////////////////

            case 'c': // Velocity PIDs
                cliPortPrintF("\nnDot PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[NDOT_PID].P,
                   		                                                   eepromConfig.PID[NDOT_PID].I,
                   		                                                   eepromConfig.PID[NDOT_PID].D,
                   		                                                   eepromConfig.PID[NDOT_PID].Limit);

                cliPortPrintF(  "eDot PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[EDOT_PID].P,
                   		                                                   eepromConfig.PID[EDOT_PID].I,
                   		                                                   eepromConfig.PID[EDOT_PID].D,
                   		                                                   eepromConfig.PID[EDOT_PID].Limit);

                cliPortPrintF(  "hDot PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[HDOT_PID].P,
                   		                                                   eepromConfig.PID[HDOT_PID].I,
                   		                                                   eepromConfig.PID[HDOT_PID].D,
                   		                                                   eepromConfig.PID[HDOT_PID].Limit);
                cliQuery = 'x';
                validCliCommand = false;
                break;


            ///////////////////////////////

            case 'd': // Position PIDs
                cliPortPrintF("\nN PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[N_PID].P,
                   		                                                eepromConfig.PID[N_PID].I,
                   		                                                eepromConfig.PID[N_PID].D,
                   		                                                eepromConfig.PID[N_PID].Limit);

                cliPortPrintF(  "E PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[E_PID].P,
                   		                                                eepromConfig.PID[E_PID].I,
                   		                                                eepromConfig.PID[E_PID].D,
                   		                                                eepromConfig.PID[E_PID].Limit);

                cliPortPrintF(  "h PID:  %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[H_PID].P,
                   		                                                eepromConfig.PID[H_PID].I,
                   		                                                eepromConfig.PID[H_PID].D,
                   		                                                eepromConfig.PID[H_PID].Limit);
                cliQuery = 'x';
                validCliCommand = false;
              	break;

			///////////////////////////////

			case 'e': // Loop Delta Times
				cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz,
																			deltaTime500Hz,
																			deltaTime100Hz,
																			deltaTime50Hz,
																			deltaTime10Hz,
																			deltaTime5Hz,
																			deltaTime1Hz);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'f': // Loop Execution Times
				cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz,
																			executionTime500Hz,
																			executionTime100Hz,
																			executionTime50Hz,
																			executionTime10Hz,
																			executionTime5Hz,
																			executionTime1Hz);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'g': // 100 Hz Accels
				cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel100Hz[XAXIS],
													   sensors.accel100Hz[YAXIS],
													   sensors.accel100Hz[ZAXIS]);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'h': // 100 hz Earth Axis Accels
				cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS],
													   earthAxisAccels[YAXIS],
													   earthAxisAccels[ZAXIS]);
				validCliCommand = false;
				break;
			///////////////////////////////

			case 'i': // 500 hz Gyros
				cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D,
															  sensors.gyro500Hz[PITCH] * R2D,
															  sensors.gyro500Hz[YAW  ] * R2D,
															  mpu6000Temperature);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'j': // 10 Hz Mag Data
				cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS],
													   sensors.mag10Hz[YAXIS],
													   sensors.mag10Hz[ZAXIS]);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'k': // Vertical Axis Variables
				cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld, %9.4f\n", earthAxisAccels[ZAXIS],
																		   sensors.pressureAlt50Hz,
																		   hDotEstimate,
																		   hEstimate,
																		   ms5611Temperature,
																		   aglRead());
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'l': // Attitudes
				cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D,
													   sensors.attitude500Hz[PITCH] * R2D,
													   sensors.attitude500Hz[YAW  ] * R2D);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'm': // Axis PIDs
				cliPortPrintF("%9.4f, %9.4f, %9.4f\n", ratePID[ROLL ],
													   ratePID[PITCH],
													   ratePID[YAW  ]);
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'n': // GPS Data
				switch (gpsDataType)
				{
					///////////////////////

					case 0:
						cliPortPrintF("%12ld, %12ld, %12ld, %12ld, %12ld, %12ld, %4d, %4d\n", gps.latitude,
																							  gps.longitude,
																							  gps.hMSL,
																							  gps.velN,
																							  gps.velE,
																							  gps.velD,
																							  gps.fix,
																							  gps.numSats);
						break;

					///////////////////////

					case 1:
						cliPortPrintF("%3d: ", gps.numCh);

						for (index = 0; index < gps.numCh; index++)
							cliPortPrintF("%3d  ", gps.chn[index]);

						cliPortPrint("\n");

						break;

					///////////////////////

					case 2:
						cliPortPrintF("%3d: ", gps.numCh);

						for (index = 0; index < gps.numCh; index++)
							cliPortPrintF("%3d  ", gps.svid[index]);

						cliPortPrint("\n");

						break;

					///////////////////////

					case 3:
						cliPortPrintF("%3d: ", gps.numCh);

						for (index = 0; index < gps.numCh; index++)
							cliPortPrintF("%3d  ", gps.cno[index]);

						cliPortPrint("\n");

						break;

					///////////////////////
				}

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'o':
				cliPortPrintF("%9.4f\n", batteryVoltage);

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'p': // Using for Nav Debug
//				cliPortPrintF("CurrentLLA:%d,%d, CurrentWP:%d,%d, NextWP:%d,%d\n",
//						currentPosition.latitude,
//						currentPosition.longitude,
//						fromWaypoint.latitude,
//						fromWaypoint.longitude,
//						toWaypoint.latitude,
//						toWaypoint.longitude);
				cliPortPrintF("xt:%.9f, xte:%.9f, tae: %.9f, dist:%.3f\n",
						crossTrack, crossTrackError, trackAngleError, distanceToNextWaypoint);
				//cliQuery = 'x';
				//validCliCommand = false;
				break;

			///////////////////////////////

			case 'q': // Using for Nav Debug
            	cliPortPrintF("State:%d, WP:%d, P:%.1f, R:%.1f, Y:%.1f, Curr:%.1f, Des:%.1f, Dist:%.8f\n",
            									nextNavState,
            									waypointIndex,
            									ratePID[PITCH],
            									ratePID[ROLL],
            									ratePID[YAW],
            									currentHeading,
            									desiredHeading,
            									distanceToNextWaypoint);
				//cliQuery = 'x';
				//validCliCommand = false;
				break;

			///////////////////////////////

			case 'r':
				if (flightMode == RATE)
					cliPortPrint("Flight Mode:RATE      ");
				else if (flightMode == ATTITUDE)
					cliPortPrint("Flight Mode:ATTITUDE  ");
				else if (flightMode == GPS)
					cliPortPrint("Flight Mode:GPS       ");

				if (headingHoldEngaged == true)
					cliPortPrint("Heading Hold:ENGAGED     ");
				else
					cliPortPrint("Heading Hold:DISENGAGED  ");

				switch (verticalModeState)
				{
					case ALT_DISENGAGED_THROTTLE_ACTIVE:
						cliPortPrint("Alt:Disenaged Throttle Active      ");

						break;

					case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
						cliPortPrint("Alt:Hold Fixed at Engagement Alt   ");

						break;

					case ALT_HOLD_AT_REFERENCE_ALTITUDE:
						cliPortPrint("Alt:Hold at Reference Alt          ");

						break;

					case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
						cliPortPrint("Alt:Velocity Hold at Reference Vel ");

						break;

					case ALT_DISENGAGED_THROTTLE_INACTIVE:
						cliPortPrint("Alt:Disengaged Throttle Inactive   ");

						break;
				}

				if (rxCommand[AUX3] > MIDCOMMAND)
					cliPortPrint("Mode:Simple  ");
				else
					cliPortPrint("Mode:Normal  ");

				if (rxCommand[AUX4] > MIDCOMMAND)
					cliPortPrint("Emergency Bail:Active\n");
				else
					cliPortPrint("Emergency Bail:Inactive\n");

				validCliCommand = false;
				break;

			///////////////////////////////

			case 's': // Raw Receiver Commands
				if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0))
                {
		    		for (index = 0; index < maxChannelNum - 1; index++)
                         cliPortPrintF("%4ld, ", spektrumBuf[index]);

                    cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]);
			    }
				else if (eepromConfig.receiverType == SBUS)
				{
		    		for (index = 0; index < 7; index++)
                         cliPortPrintF("%4ld, ", sBusRead(index));

                    cliPortPrintF("%4ld\n", sBusRead(7));

				}
				else
				{
					for (index = 0; index < numChannels - 1; index++)
						cliPortPrintF("%4i, ", Inputs[index].pulseWidth);

					cliPortPrintF("%4i\n", Inputs[numChannels - 1].pulseWidth);
				}

				validCliCommand = false;
				break;

			///////////////////////////////

			case 't': // Processed Receiver Commands
				for (index = 0; index < numChannels - 1; index++)
					cliPortPrintF("%8.2f, ", rxCommand[index]);

				cliPortPrintF("%8.2f\n", rxCommand[numChannels - 1]);

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'u': // Command in Detent Discretes
				cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false");
				cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false");
				cliPortPrintF("%s\n", commandInDetent[YAW  ] ? " true" : "false");

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'v': // ESC PWM Outputs
				cliPortPrintF("%4ld, ", TIM8->CCR4);
				cliPortPrintF("%4ld, ", TIM8->CCR3);
				cliPortPrintF("%4ld, ", TIM8->CCR2);
				cliPortPrintF("%4ld, ", TIM8->CCR1);
				cliPortPrintF("%4ld, ", TIM2->CCR1);
				cliPortPrintF("%4ld, ", TIM2->CCR2);
				cliPortPrintF("%4ld, ", TIM3->CCR1);
				cliPortPrintF("%4ld\n", TIM3->CCR2);

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'w': // Servo PWM Outputs
				cliPortPrintF("%4ld, ", TIM5->CCR3);
				cliPortPrintF("%4ld, ", TIM5->CCR2);
				cliPortPrintF("%4ld\n", TIM5->CCR1);

				validCliCommand = false;
				break;

			///////////////////////////////

			case 'x':
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'y': // ESC Calibration
				escCalibration();

				cliQuery = 'x';
				break;

			///////////////////////////////

			case 'z':	// ADC readings
				cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f\n", adcValue(1),
																				   adcValue(2),
																				   adcValue(3),
																				   adcValue(4),
																				   adcValue(5),
																				   adcValue(6),
																				   adcValue(7));
				break;

			///////////////////////////////

			///////////////////////////////////////////////////////////////////////
			///////////////////////////////////////////////////////////////////////
			///////////////////////////////////////////////////////////////////////
			///////////////////////////////////////////////////////////////////////

			///////////////////////////////

			case 'A': // Read Roll Rate PID Values
				readCliPID(ROLL_RATE_PID);
				cliPortPrint( "\nRoll Rate PID Received....\n" );

				cliQuery = 'a';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'B': // Read Pitch Rate PID Values
				readCliPID(PITCH_RATE_PID);
				cliPortPrint( "\nPitch Rate PID Received....\n" );

				cliQuery = 'a';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'C': // Read Yaw Rate PID Values
				readCliPID(YAW_RATE_PID);
				cliPortPrint( "\nYaw Rate PID Received....\n" );

				cliQuery = 'a';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'D': // Read Roll Attitude PID Values
				readCliPID(ROLL_ATT_PID);
				cliPortPrint( "\nRoll Attitude PID Received....\n" );

				cliQuery = 'b';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'E': // Read Pitch Attitude PID Values
				readCliPID(PITCH_ATT_PID);
				cliPortPrint( "\nPitch Attitude PID Received....\n" );

				cliQuery = 'b';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'F': // Read Heading Hold PID Values
				readCliPID(HEADING_PID);
				cliPortPrint( "\nHeading PID Received....\n" );

				cliQuery = 'b';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'G': // Read nDot PID Values
				readCliPID(NDOT_PID);
				cliPortPrint( "\nnDot PID Received....\n" );

				cliQuery = 'c';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'H': // Read eDot PID Values
				readCliPID(EDOT_PID);
				cliPortPrint( "\neDot PID Received....\n" );

				cliQuery = 'c';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'I': // Read hDot PID Values
				readCliPID(HDOT_PID);
				cliPortPrint( "\nhDot PID Received....\n" );

				cliQuery = 'c';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'J': // Read n PID Values
				readCliPID(N_PID);
				cliPortPrint( "\nn PID Received....\n" );

				cliQuery = 'd';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'K': // Read e PID Values
				readCliPID(E_PID);
				cliPortPrint( "\ne PID Received....\n" );

				cliQuery = 'd';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'L': // Read h PID Values
				readCliPID(H_PID);
				cliPortPrint( "\nh PID Received....\n" );

				cliQuery = 'd';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'M': // MAX7456 CLI
				max7456CLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'N': // Mixer CLI
				mixerCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'O': // Receiver CLI
				receiverCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'P': // Sensor CLI
				sensorCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'Q': // GPS Data Selection
				gpsDataType = (uint8_t)readFloatCLI();

				cliPortPrint("\n");

				cliQuery = 'n';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'R': // Reset to Bootloader
				cliPortPrint("Entering Bootloader....\n\n");
				delay(100);
				systemReset(true);
				break;

			///////////////////////////////

			case 'S': // Reset System
				cliPortPrint("\nSystem Reseting....\n\n");
				delay(100);
				systemReset(false);
				break;

			///////////////////////////////

			case 'T': // Telemetry CLI
				telemetryCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'U': // EEPROM CLI
				eepromCLI();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'V': // Reset EEPROM Parameters
				cliPortPrint( "\nEEPROM Parameters Reset....\n" );
				checkFirstTime(true);
				cliPortPrint("\nSystem Resetting....\n\n");
				delay(100);
				systemReset(false);
				break;

			///////////////////////////////

			case 'W': // Write EEPROM Parameters
				cliPortPrint("\nWriting EEPROM Parameters....\n");
				writeEEPROM();

				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'X': // Not Used
				cliQuery = 'x';
				validCliCommand = false;
				break;

			///////////////////////////////

			case 'Y': // ADC CLI
				 adcCLI();

				 cliQuery = 'x';
				 validCliCommand = false;
				 break;

			///////////////////////////////

			case 'Z': // Not Used
				computeGeoMagElements();

				cliQuery = 'x';
				break;

			///////////////////////////////

			//////////////////////////////////////////////////////////////////////////////////
			//   Communicator Messages   /////////////////////////////////////////////////////
			//////////////////////////////////////////////////////////////////////////////////

			///////////////////////////////

			case '@': // Communicator Messages
			{
				uint8_t msgType = cliPortRead();
				switch (msgType) //update this
				{
					case 's': // Write out vehicle status
						if (statusType++ < 3)
						{
							cliPortPrint("0");
							cliPortPrintF("%1d", armed);
							cliPortPrintF("%1d", flightMode);
							cliPortPrintF("%1d", autoNavMode);
							cliPortPrintF("%1d", verticalModeState);
							cliPortPrintF("%1d", execUp);
							writeShort(batteryVoltage * 100);
							writeShort(hEstimate * 100);
							writeShort(sensors.attitude500Hz[ROLL] * R2D * 10);
							writeShort(sensors.attitude500Hz[PITCH] * R2D * 10);
							writeShort(sensors.attitude500Hz[YAW] * R2D * 10);
							writeShort(rxCommand[0]);
							writeShort(rxCommand[1]);
							writeShort(rxCommand[2]);
							writeShort(rxCommand[3]);
							cliPortPrint("\n"); // 25 bytes
						}
						else
						{
							cliPortPrint("1");
							writeShort(rxCommand[4]);
							writeShort(rxCommand[5]);
							writeShort(rxCommand[6]);
							writeShort(rxCommand[7]);
							writeShort(TIM8->CCR4);
							writeShort(TIM8->CCR3);
							writeShort(TIM8->CCR2);
							writeShort(TIM8->CCR1);
							writeShort(TIM2->CCR1);
							writeShort(TIM2->CCR2);
							writeShort(TIM3->CCR1);
							writeShort(TIM3->CCR2);
							cliPortPrint("\n"); // 26 bytes
							statusType = 0;
						}
						break;
					case 'O': // Read in waypoints
					{
						int index = readFloatCLI();
						if (index >= 0)
						{
						  eepromConfig.route[index].latitude = readFloatCLI();
						  eepromConfig.route[index].longitude = readFloatCLI();
						  eepromConfig.route[index].altitude = readFloatCLI();
						  eepromConfig.route[index].speed = 1;
						  eepromConfig.route[index].type = 0;
						}
						else if (index == -1)
							eepromConfig.storedWaypointCount = readFloatCLI();
						break;
					}
					case 'o': // Write out waypoints
					{
						int index = readFloatCLI();
						if (index < 0)
							cliPortPrintF("%d\n", eepromConfig.storedWaypointCount);
						else
						{
							cliPortPrintF("%d,%d,%d,%d\n",
								index,
								eepromConfig.route[index].latitude,
								eepromConfig.route[index].longitude,
								eepromConfig.route[index].altitude,
								eepromConfig.route[index].speed,
								eepromConfig.route[index].type);
						}
					}
					  break;
					case '^': // Write out AutoNav status
					{
						int type = readFloatCLI();
						if (type == 0) // send shortened position data
						{
							cliPortPrintF("%d,%d,%.1f\n",
									gps.latitude,
									gps.longitude,
									sensors.attitude500Hz[YAW]*R2D);
						}
						if (type == 1)
						{
							cliPortPrintF("%d,%d,%d\n",
								  gps.hMSL,
								  gps.heading,
								  gps.speed);
						}
						if (type == 2)
						{
							cliPortPrintF("%d,%d,%d\n",
								  gps.numSats,
								  gps.hDop,
								  gps.fix);
						}
						if (type == 3) // send stored home position
						{
							cliPortPrintF("%d,%d,%d\n",
								  homePosition.latitude,
								  homePosition.longitude,
								  homePosition.altitude);
						}
					    break;
					}
					case '!': // send software version
						cliPortPrintF("%s\n", __AQ32PLUS_VERSION);
						break;
					case '<': // send autoNav status
						cliPortPrintF("%d\n", getAutoNavState());
						break;
					case '>': // setup autopilot states
						setAutoNavState(readFloatCLI());
						break;
					case 'M': // Read in mode setup
					{
						int slot = readFloatCLI();
						eepromConfig.mode[slot].modeType = readFloatCLI();
						eepromConfig.mode[slot].channel = readFloatCLI();
						eepromConfig.mode[slot].state = readFloatCLI();
						eepromConfig.mode[slot].minChannelValue = readFloatCLI();
						eepromConfig.mode[slot].maxChannelValue = readFloatCLI();
						break;
					}
					case 'm': // Write out mode setup
					{
						int slot = readFloatCLI();
						cliPortPrintF("%d,%d,%d,%d,%d,%d\n",
								slot,
								eepromConfig.mode[slot].modeType,
								eepromConfig.mode[slot].channel,
								eepromConfig.mode[slot].state,
								eepromConfig.mode[slot].minChannelValue,
								eepromConfig.mode[slot].maxChannelValue);
						break;
					}
					case 'A': // Read in AutoNav PIDs
					{
						int pidType = readFloatCLI();
						readCliPID(pidType);
						break;
					}
		            case 'a': // Write out AutoNav PIDs
		                cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f,", eepromConfig.PID[AUTONAV_ROLL_PID].P,
		                    		                                 eepromConfig.PID[AUTONAV_ROLL_PID].I,
		                    		                                 eepromConfig.PID[AUTONAV_ROLL_PID].D,
		                    		                                 eepromConfig.PID[AUTONAV_ROLL_PID].Limit);

		                cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f,", eepromConfig.PID[AUTONAV_PITCH_PID].P,
		                    		                                 eepromConfig.PID[AUTONAV_PITCH_PID].I,
		                    		                                 eepromConfig.PID[AUTONAV_PITCH_PID].D,
		                    		                                 eepromConfig.PID[AUTONAV_PITCH_PID].Limit);

		                cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[AUTONAV_YAW_PID].P,
		                    		                                  eepromConfig.PID[AUTONAV_YAW_PID].I,
		                    		                                  eepromConfig.PID[AUTONAV_YAW_PID].D,
		                    		                                  eepromConfig.PID[AUTONAV_YAW_PID].Limit);
		                break;
		            case 'C':
		            	eepromConfig.xteScaling = readFloatCLI();
		            	break;
		            case 'c':
		            	cliPortPrintF("%.3f\n", eepromConfig.xteScaling);
		            	break;
				}
				cliQuery = 'x';
				validCliCommand = false;
				break;
			}

			///////////////////////////////

			//////////////////////////////////////////////////////////////////////////////////
			//////////////////////////////////////////////////////////////////////////////////
			//////////////////////////////////////////////////////////////////////////////////

			///////////////////////////////


			case '?': // Command Summary
				cliBusy = true;

				cliPortPrint("\n");
				cliPortPrint("'a' Rate PIDs                              'A' Set Roll Rate PID Data   AP;I;D;N\n");
				cliPortPrint("'b' Attitude PIDs                          'B' Set Pitch Rate PID Data  BP;I;D;N\n");
				cliPortPrint("'c' Velocity PIDs                          'C' Set Yaw Rate PID Data    CP;I;D;N\n");
				cliPortPrint("'d' Position PIDs                          'D' Set Roll Att PID Data    DP;I;D;N\n");
				cliPortPrint("'e' Loop Delta Times                       'E' Set Pitch Att PID Data   EP;I;D;N\n");
				cliPortPrint("'f' Loop Execution Times                   'F' Set Hdg Hold PID Data    FP;I;D;N\n");
				cliPortPrint("'g' 500 Hz Accels                          'G' Set nDot PID Data        GP;I;D;N\n");
				cliPortPrint("'h' 100 Hz Earth Axis Accels               'H' Set eDot PID Data        HP;I;D;N\n");
				cliPortPrint("'i' 500 Hz Gyros                           'I' Set hDot PID Data        IP;I;D;N\n");
				cliPortPrint("'j' 10 hz Mag Data                         'J' Set n PID Data           JP;I;D;N\n");
				cliPortPrint("'k' Vertical Axis Variable                 'K' Set e PID Data           KP;I;D;N\n");
				cliPortPrint("'l' Attitudes                              'L' Set h PID Data           LP;I;D;N\n");
				cliPortPrint("\n");

				cliPortPrint("Press space bar for more, or enter a command....\n");

				while (cliPortAvailable() == false);

				cliQuery = cliPortRead();

				if (cliQuery != ' ')
				{
					validCliCommand = true;
					cliBusy = false;
					return;
				}

				cliPortPrint("\n");
				cliPortPrint("'m' Axis PIDs                              'M' MAX7456 CLI\n");
				cliPortPrint("'n' GPS Data                               'N' Mixer CLI\n");
				cliPortPrint("'o' Battery Voltage                        'O' Receiver CLI\n");
				cliPortPrint("'p' Not Used                               'P' Sensor CLI\n");
				cliPortPrint("'q' Not Used                               'Q' GPS Data Selection\n");
				cliPortPrint("'r' Mode States                            'R' Reset and Enter Bootloader\n");
				cliPortPrint("'s' Raw Receiver Commands                  'S' Reset\n");
				cliPortPrint("'t' Processed Receiver Commands            'T' Telemetry CLI\n");
				cliPortPrint("'u' Command In Detent Discretes            'U' EEPROM CLI\n");
				cliPortPrint("'v' Motor PWM Outputs                      'V' Reset EEPROM Parameters\n");
				cliPortPrint("'w' Servo PWM Outputs                      'W' Write EEPROM Parameters\n");
				cliPortPrint("'x' Terminate Serial Communication         'X' Not Used\n");
				cliPortPrint("\n");

				cliPortPrint("Press space bar for more, or enter a command....\n");

				while (cliPortAvailable() == false);

				cliQuery = cliPortRead();

				if (cliQuery != ' ')
				{
					validCliCommand = true;
					cliBusy = false;
					return;
				}

				cliPortPrint("\n");
				cliPortPrint("'y' ESC Calibration/Motor Verification     'Y' ADC CLI\n");
				cliPortPrint("'z' ADC Values                             'Z' WMM Test\n");
				cliPortPrint("                                           '?' Command Summary\n");
				cliPortPrint("\n");

				cliQuery = 'x';
				cliBusy = false;
				break;

				///////////////////////////////

		}
    }
}