Пример #1
0
static void cliDefaults(char *cmdline)
{
    cliPrint("Resetting to defaults...\r\n");
    checkFirstTime(true);
    cliPrint("Rebooting...");
    delay(10);
    systemReset(false);
}
Пример #2
0
void systemInit(void)
{
	// Init cycle counter
    cycleCounterInit();

    // SysTick
    SysTick_Config(SystemCoreClock / 1000);

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

    checkFirstTime(false);
	readEEPROM();

	if (eepromConfig.receiverType == SPEKTRUM)
		checkSpektrumBind();

	checkResetType();

	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);  // 2 bits for pre-emption priority, 2 bits for subpriority

	initMixer();

    ledInit();
    cliInit();

    BLUE_LED_ON;

    delay(20000);  // 20 sec total delay for sensor stabilization - probably not long enough.....

    adcInit();
    batteryInit();
    gpsInit();
    i2cInit(I2C1);
    i2cInit(I2C2);
    pwmEscInit(eepromConfig.escPwmRate);
    pwmServoInit(eepromConfig.servoPwmRate);
    rxInit();
    spiInit(SPI2);
    spiInit(SPI3);
    telemetryInit();
    timingFunctionsInit();

    initFirstOrderFilter();
    initGPS();
    initMax7456();
    initPID();

    GREEN_LED_ON;

    initMPU6000();
    initMag(HMC5883L_I2C);
    initPressure(MS5611_I2C);
}
Пример #3
0
void systemInit(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    uint8_t i;

    gpio_config_t gpio_cfg[] = {
        {LED0_GPIO, LED0_PIN, GPIO_Mode_Out_PP},        // PB3 (LED)
        {LED1_GPIO, LED1_PIN, GPIO_Mode_Out_PP},        // PB4 (LED)
    };

    uint8_t gpio_count = sizeof(gpio_cfg) / sizeof(gpio_cfg[0]);

    // Turn on clocks for stuff we use
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE);
    RCC_ClearFlag();

    // Make all GPIO in by default to save power and reduce noise
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    GPIO_Init(GPIOC, &GPIO_InitStructure);

    // Turn off JTAG port 'cause we're using the GPIO for leds
    GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);

    // Configure gpio
    for (i = 0; i < gpio_count; i++) {
        GPIO_InitStructure.GPIO_Pin = gpio_cfg[i].pin;
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
        GPIO_InitStructure.GPIO_Mode = gpio_cfg[i].mode;

        GPIO_Init(gpio_cfg[i].gpio, &GPIO_InitStructure);
    }

    // Init cycle counter
    cycleCounterInit();

    // SysTick
    SysTick_Config(SystemCoreClock / 1000);

    LED0_OFF;
    LED1_OFF;

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);     // 2 bits for pre-emption priority, 2 bits for subpriority

    checkFirstTime(false, false);
    readEEPROM();

    initMixer();

    pwmOutputConfig.escPwmRate = systemConfig.escPwmRate;
    pwmOutputConfig.servoPwmRate = systemConfig.servoPwmRate;

    //i2cInit(SENSOR_I2C);
    pwmInputInit();
    pwmOutputInit(&pwmOutputConfig);
    uartInit(115200);

    //delay(10000);               // 10 sec delay for sensor stabilization - probably not long enough.....

    //initAccel();
    //initGyro();
    //initMag();
    adcInit();
    //initPressure(); // no pressure sensor

    initPID();
}
Пример #4
0
void testInit(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    uint8_t i;

    struct __gpio_config_t {
    GPIO_TypeDef *gpio;
    uint16_t pin;
    GPIOMode_TypeDef mode;
    }
    gpio_cfg[] = {
        {LED0_GPIO, LED0_PIN, GPIO_Mode_Out_PP},        // PB3 (LED)
        {LED1_GPIO, LED1_PIN, GPIO_Mode_Out_PP},        // PB4 (LED)
    };

    uint8_t gpio_count = sizeof(gpio_cfg) / sizeof(gpio_cfg[0]);

    // Turn on clocks for stuff we use
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE);
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE);
    RCC_ClearFlag();

    // Make all GPIO in by default to save power and reduce noise
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    GPIO_Init(GPIOC, &GPIO_InitStructure);

    // Turn off JTAG port 'cause we're using the GPIO for leds
    GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);

    // Configure gpio
    for (i = 0; i < gpio_count; i++) {
        GPIO_InitStructure.GPIO_Pin = gpio_cfg[i].pin;
        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
        GPIO_InitStructure.GPIO_Mode = gpio_cfg[i].mode;

        GPIO_Init(gpio_cfg[i].gpio, &GPIO_InitStructure);
    }

    // Init cycle counter
    cycleCounterInit();

    // SysTick
    SysTick_Config(SystemCoreClock / 1000);

    LED0_OFF;
    LED1_OFF;

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);     // 2 bits for pre-emption priority, 2 bits for subpriority
    
    checkFirstTime(true,true);
    initMixer();
    
    pwmOutputConfig.motorPwmRate = 10*1000;
    pwmOutputConfig.noEsc = true;
    pwmOutputInit(&pwmOutputConfig);
    
    
    i2cInit(SENSOR_I2C);
    initGyro();
    initAccel();
    initMag();
    nrf_init();
    nrf_detect();
}
Пример #5
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;

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

		}
    }
}
Пример #6
0
/*---------------------------------------------------------------------------
     TITLE   : thread_menu
     WORK    : 
     ARG     : void
     RET     : void
---------------------------------------------------------------------------*/
void thread_menu(void const *argument)
{
    (void) argument;
    uint8_t cmd;
    uint8_t show_menu_flag;


    DEBUG_PRINT("Thread Menu\r\n");


    _menu_show_menu();
    _menu_show_cmd();


    for (;;)
    {
    	//if( bDeviceState == CONFIGURED && _menu_get_cmd( &cmd ) == TRUE )
    	if( _menu_get_cmd( &cmd ) == TRUE )
        {
    		show_menu_flag = false;

    		switch( cmd )
    		{
				case '1':
					while( !_menu_received() )
					{
						_menu_printf( "Angle : %d\t %d\t %d\t %d "  , angle[0], angle[1], heading, EstAlt );
						_menu_printf( "rc :\t %d\t %d\t %d\t %d\t \r\n", rcCommand[0], rcCommand[1], rcCommand[2], rcCommand[3] );
						osDelay(100);
					}
					break;

				case '2':
					cmd_bluetooth_setup();
					break;

				case '3':
					cmd_bluetooth_check();
					break;

				case '4':
					mcfg.uart1_type++;

					if( mcfg.uart1_type > _UART1_TYPE_END ) mcfg.uart1_type = 0;
					writeEEPROM(1, true);

					_menu_printf( "the board should be rebooted\r\n" );
					show_menu_flag = true;
					break;

				case '5':
					if (!f.ARMED)
					{
            			checkFirstTime(true);
            		}
					show_menu_flag = true;
					break;

				case '6':
					cmd_bluetooth_change_name();
					break;

				case '7':
					core.useVComMultiwii  = true;
					_menu_printf("\r\nOK\r\n");
					break;

				case '8':
					core.useShowMspCmd = true;
					_menu_printf( "Monitor Start\r\n ");

					while( !_menu_received() )
					{
						osDelay(100);
					}
					core.useShowMspCmd = false;

					_menu_printf( "Monitor End\r\n ");

					break;

				case 'm':
				case 'M':
					_menu_show_menu();
					_menu_show_cmd();
					break;

				case 'z':
					cmd_go_bootloader();
					break;

				default:
					break;

    		}

    		if( show_menu_flag == true )
    		{
				_menu_show_menu();
				_menu_show_cmd();
    		}

        }
        osDelay(1);
    }
}
Пример #7
0
/*!
* @brief	Initializes the whole system and runs the desired application
*
* This is the main function of the project. It calls initialization functions
* of the MCU and the sensors. In the infinite loop it repeatedly checks
* the USART module read buffer and Streams sensor data periodically (100 ms) via USART.
*
*/
int main(void)
{
	/********************* Initialize global variables **********************/

	bmf055_input_state = USART_INPUT_STATE_PRINT_DATA;
	
	/************************* Initializations ******************************/
	
	/*Initialize SAMD20 MCU*/
	system_init();
	
	/*Initialize clock module of SAMD20 MCU - Internal RC clock*/
	//clock_initialize(); // done via conf_clocks.h --> ASF
	
	/*SPI master for communicating with sensors*/
	spi_initialize();
	
	/*eeprom emulator for configuration storage */
	eeprom_emulator_initialize();
	
	/*Initialize timers */
	tc_initialize();
	
	/*Initialize UART for communication with PC*/
	usart_initialize();
	
	/*Enable the system interrupts*/
	system_interrupt_enable_global();/* All interrupts have a priority of level 0 which is the highest. */
	
	/* Initialize the sensors */
	bmf055_sensors_initialize();
	
	readEEPROM();
	checkFirstTime(0);
	//readEEPROM();
	
	configureReceiver();
	initSensors();
	previousTime = micros();
	calibratingG = 400;
	f.SMALL_ANGLES_25=1; // important for gyro only conf
  if(conf.copterType == 0){//0=Bi,1=Tri,2=QUADP,3=QUADX,4=Y4,5=Y6,6=H6P,7=H6X,8=Vtail4
    MULTITYPE      = 4;
    NUMBER_MOTOR   = 2;
  }
  if(conf.copterType == 1){
    MULTITYPE      = 1;
    NUMBER_MOTOR   = 3;
  }
  if(conf.copterType == 2){
    MULTITYPE      = 2;
    NUMBER_MOTOR   = 4;
  }
  if(conf.copterType == 3){
    MULTITYPE      = 3;
    NUMBER_MOTOR   = 4;
  }
  if(conf.copterType == 4){
    MULTITYPE      = 9;
    NUMBER_MOTOR   = 4;
  }
  if(conf.copterType == 5){
    MULTITYPE      = 6;
    NUMBER_MOTOR   = 6;
  }
  if(conf.copterType == 6){
    MULTITYPE      = 7;
    NUMBER_MOTOR   = 6;
  }
  if(conf.copterType == 7){
    MULTITYPE      = 10;
    NUMBER_MOTOR   = 6;
  }     
  if(conf.copterType == 8){
    MULTITYPE      = 17;
    NUMBER_MOTOR   = 4;
  }
  
  initOutput();
	
	/************************** Infinite Loop *******************************/
	while (true)
	{

		
		static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
  static uint8_t beepon = 0;
  uint8_t axis,i;
  int16_t error,errorAngle;
  int16_t delta,deltaSum;
  int16_t PTerm=0,ITerm=0,PTermACC=0,ITermACC=0,PTermGYRO=0,ITermGYRO=0,DTerm=0;
  static int16_t lastGyro[3] = {0,0,0};
  static int16_t delta1[3],delta2[3];
  static int16_t errorGyroI[3] = {0,0,0};
  static int16_t errorAngleI[2] = {0,0};
  static uint32_t rcTime  = 0;
  static uint32_t BeepTime  = 0;
  static uint8_t stickarmed = 0;
  //static int16_t initialThrottleHold;
  
  if(!rcOptions[BOXARM] && stickarmed == 0 && f.ARMED == 0){
    if(rcData[YAW]<conf.MINCHECK && rcData[ROLL]>conf.MAXCHECK){
      conf.calibState=1;
      writeParams(1);
      while(true){
        //blinkLED(10,30,1);
      }      
    }
  } 
 
  while(SetupMode == 1){
    checkSetup();
  }
 
 
  if(conf.RxType == 1 || conf.RxType == 2){
    if (rcFrameComplete) computeRC();
  }
  
  if(!rcOptions[BOXARM] && stickarmed == 0) {
    f.ARMED = 0;
  }

  if (currentTime > rcTime ) { // 50Hz
    rcTime = currentTime + 20000;
    if(failsave < 250)failsave++;
    debug[0] = failsave;
    if(conf.RxType != 1 && conf.RxType != 2){
      computeRC();
    }
  
    if ((rcData[THROTTLE] < conf.MINCHECK && s3D == 0) || (rcData[THROTTLE] > (1500-conf.MIDDLEDEADBAND) && rcData[THROTTLE] < (1500+conf.MIDDLEDEADBAND) && s3D == 1 && f.ARMED == 0)) {
      errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0;
      errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;

      rcDelayCommand++;
      if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK && !f.ARMED) {
        if (rcDelayCommand == 20 && failsave < 20) {
          calibratingG=400;
        }
      }else if (rcData[YAW] > conf.MAXCHECK && rcData[PITCH] > conf.MAXCHECK && !f.ARMED) {
        if (rcDelayCommand == 20) {
          previousTime = micros();
        }
      }else if (conf.activate[BOXARM] > 0) {
        if ( rcOptions[BOXARM] && f.OK_TO_ARM && good_calib) {
	  f.ARMED = 1;
          stickarmed = 0;
        } else if (f.ARMED) f.ARMED = 0;
        rcDelayCommand = 0;
        
      
      } else if ( (rcData[YAW] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) {
        if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
      } else if ( (rcData[YAW] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 0) {
        if (rcDelayCommand == 20 && good_calib) {
	  f.ARMED = 1;
          stickarmed = 1;
        }
  
       } else if ( (rcData[ROLL] < conf.MINCHECK ) && f.ARMED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) {
        if (rcDelayCommand == 20) f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
      } else if ( (rcData[ROLL] > conf.MAXCHECK ) && rcData[PITCH] < conf.MAXCHECK && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED && !rcOptions[BOXARM] && s3D == 0 && conf.ArmRoll == 1) {
        if (rcDelayCommand == 20 && good_calib) {
	  f.ARMED = 1;
          stickarmed = 1;
        }       
        
        
      } else
        rcDelayCommand = 0;
    } else if (rcData[THROTTLE] > conf.MAXCHECK && !f.ARMED) {
      if (rcData[YAW] < conf.MINCHECK && rcData[PITCH] < conf.MINCHECK) {        // throttle=max, yaw=left, pitch=min
        if (rcDelayCommand == 20) calibratingA=400;
        rcDelayCommand++;
      } else if (rcData[PITCH] > conf.MAXCHECK) {
         conf.angleTrim[PITCH]+=2;writeParams(1);
      } else if (rcData[PITCH] < conf.MINCHECK) {
         conf.angleTrim[PITCH]-=2;writeParams(1);
      } else if (rcData[ROLL] > conf.MAXCHECK) {
         conf.angleTrim[ROLL]+=2;writeParams(1);
      } else if (rcData[ROLL] < conf.MINCHECK) {
         conf.angleTrim[ROLL]-=2;writeParams(1);
      } else {
        rcDelayCommand = 0;
      }
    }
    
    

    uint16_t auxState = 0;
    for(i=0;i<4;i++)
      auxState |= (rcData[AUX1+i]<1300)<<(3*i) | (1300<rcData[AUX1+i] && rcData[AUX1+i]<1700)<<(3*i+1) | (rcData[AUX1+i]>1700)<<(3*i+2);
    for(i=0;i<CHECKBOXITEMS;i++)
      rcOptions[i] = (auxState & conf.activate[i])>0;
      
      
     if(failsave > 200 && f.ARMED){
      rcOptions[BOXACC] = 1;
      s3D = 0;
      rcData[THROTTLE] = 1190;
      rcCommand[THROTTLE] = 1190;
    }

    if (rcOptions[BOXACC] && s3D == 0) { 
      // bumpless transfer to Level mode
      if (!f.ACC_MODE) {
        errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
        f.ACC_MODE = 1;
      }  
    } else {
      // failsafe support
      f.ACC_MODE = 0;
    }
    if (rcOptions[BOXBEEP]) { 
      f.FSBEEP = 1;
      if(currentTime > BeepTime){
        BeepTime = currentTime + 50000;
        if(beepon == 0){
          if(conf.RxType == 0){
            //digitalWrite(A2,HIGH); 
          }else{
            //digitalWrite(8,HIGH); 
          }          
          beepon = 1;
        }else{
          if(conf.RxType == 0){
            //digitalWrite(A2,LOW); 
          }else{
            //digitalWrite(8,LOW); 
          }
          beepon = 0;
        }
      }
    } else {
      f.FSBEEP = 0;
      if(conf.RxType == 0){
        //digitalWrite(A2,LOW); 
      }else{
        //digitalWrite(8,LOW); 
      }
    }    

    
    if (rcOptions[BOXHORIZON] && s3D == 0) { 
      // bumpless transfer to Horizon mode
      if (!f.HORIZON_MODE) {
        errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0;
        f.HORIZON_MODE = 1;
      }  
    } else {
      f.HORIZON_MODE = 0;
    }
    
    if (rcOptions[BOX3D] && conf.F3D == 1) {  
      if(f.ARMED == 0 && s3D == 0){
        s3D = 1;
        f.ACC_MODE = 0;
        f.HORIZON_MODE = 0;
      }
    } else if(f.ARMED == 0){
      s3D = 0;
    }   
    

    if (rcOptions[BOXARM] == 0) f.OK_TO_ARM = 1;
  }
 
  computeIMU();
  int16_t prop;
  if (f.HORIZON_MODE)
    prop = max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])); // range [0;500]  
  
  
  if (f.ACC_MODE){
    if(Zadd > 0)Zadd--;
    if(Zadd < 0)Zadd++;
  }else{
    Zadd = 0;
  }

  

  //**** PITCH & ROLL & YAW PID ****    
  for(axis=0;axis<3;axis++) {
    if ((f.ACC_MODE || f.HORIZON_MODE) && axis<2 ) { //LEVEL MODE
      // 50 degrees max inclination
      errorAngle = constrain(2*rcCommand[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
      #ifdef LEVEL_PDF
        PTermACC      = -(int32_t)angle[axis]*conf.P8[PIDLEVEL]/100 ;
      #else  
        PTermACC      = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100 ;                          // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
      #endif
      PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);

      errorAngleI[axis]  = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);    // WindUp     //16 bits is ok here
      ITermACC           = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12;            // 32 bits is needed for calculation:10000*I8 could exceed 32768   16 bits is ok for result
    }
    if ( !f.ACC_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis
      if (abs(rcCommand[axis])<350) error =          rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000      16 bits is ok for result if P8>2 (P>0.2)
                               else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000   16 bits is ok for result if P8>2 (P>0.2)
      error -= gyroData[axis];

      PTermGYRO = rcCommand[axis];
      
      errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);          // WindUp   16 bits is ok here
      if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
      ITermGYRO         = (errorGyroI[axis]/125*conf.I8[axis])>>6;                                   // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
    }
    if ( f.HORIZON_MODE && axis<2) {
      PTerm = ((int32_t)PTermACC*(500-prop) + (int32_t)PTermGYRO*prop)/500;
      ITerm = ((int32_t)ITermACC*(500-prop) + (int32_t)ITermGYRO*prop)/500;
    } else {
      if ( f.ACC_MODE && axis<2) {
        PTerm = PTermACC;
        ITerm = ITermACC;
      } else {
        PTerm = PTermGYRO;
        ITerm = ITermGYRO;
      }
    }
    if (abs(gyroData[axis])<160) PTerm -=          gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation   160*200 = 32000         16 bits is ok for result
                            else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation   

    delta          = gyroData[axis] - lastGyro[axis];                               // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
    lastGyro[axis] = gyroData[axis];
    deltaSum       = delta1[axis]+delta2[axis]+delta;
    delta2[axis]   = delta1[axis];
    delta1[axis]   = delta;
 
    if (abs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5;                       // 16 bits is needed for calculation 640*50 = 32000           16 bits is ok for result 
                      else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5;              // 32 bits is needed for calculation
                      
    axisPID[axis] =  PTerm + ITerm - DTerm;
  }
Пример #8
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;

                ///////////////////////////////
	    }
    }
}
Пример #9
0
static void evaluateCommand(void)
{
    uint32_t i, tmp, junk;
    uint8_t wp_no;
    int32_t lat = 0, lon = 0, alt = 0;

    switch (cmdMSP) {
    case MSP_SET_RAW_RC:
        for (i = 0; i < 8; i++)
            rcData[i] = read16();
        headSerialReply(0);
        break;
    case MSP_SET_ACC_TRIM:
        cfg.angleTrim[PITCH] = read16();
        cfg.angleTrim[ROLL]  = read16();
        headSerialReply(0);
        break;
    case MSP_SET_RAW_GPS:
        f.GPS_FIX = read8();
        GPS_numSat = read8();
        GPS_coord[LAT] = read32();
        GPS_coord[LON] = read32();
        GPS_altitude = read16();
        GPS_speed = read16();
        GPS_update |= 2;        // New data signalisation to GPS functions
        headSerialReply(0);
        break;
    case MSP_SET_PID:
        for (i = 0; i < PIDITEMS; i++) {
            cfg.P8[i] = read8();
            cfg.I8[i] = read8();
            cfg.D8[i] = read8();
        }
        headSerialReply(0);
        break;
    case MSP_SET_BOX:
        for (i = 0; i < numberBoxItems; i++)
            cfg.activate[availableBoxes[i]] = read16();
        headSerialReply(0);
        break;
    case MSP_SET_RC_TUNING:
        cfg.rcRate8 = read8();
        cfg.rcExpo8 = read8();
        cfg.rollPitchRate = read8();
        cfg.yawRate = read8();
        cfg.dynThrPID = read8();
        cfg.thrMid8 = read8();
        cfg.thrExpo8 = read8();
        headSerialReply(0);
        break;
    case MSP_SET_MISC:
        read16(); // powerfailmeter
        mcfg.minthrottle = read16();
        read32(); // mcfg.maxthrottle, mcfg.mincommand
        cfg.failsafe_throttle = read16();
        read16();
        read32();
        cfg.mag_declination = read16() * 10;
        mcfg.vbatscale = read8();           // actual vbatscale as intended
        mcfg.vbatmincellvoltage = read8();  // vbatlevel_warn1 in MWC2.3 GUI
        mcfg.vbatmaxcellvoltage = read8();  // vbatlevel_warn2 in MWC2.3 GUI
        read8();                            // vbatlevel_crit (unused)
        headSerialReply(0);
        break;
    case MSP_SET_MOTOR:
        for (i = 0; i < 8; i++)
            motor_disarmed[i] = read16();
        break;
    case MSP_SELECT_SETTING:
        if (!f.ARMED) {
            mcfg.current_profile = read8();
            if (mcfg.current_profile > 2)
                mcfg.current_profile = 0;
            // this writes new profile index and re-reads it
            writeEEPROM(0, false);
        }
        headSerialReply(0);
        break;
    case MSP_SET_HEAD:
        magHold = read16();
        headSerialReply(0);
        break;
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(VERSION);                // multiwii version
        serialize8(mcfg.mixerConfiguration); // type of multicopter
        serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
        serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | (mcfg.flaps_speed ? CAP_FLAPS : 0));        // "capability"
        break;
    case MSP_STATUS:
        headSerialReply(11);
        serialize16(cycleTime);
        serialize16(i2cGetErrorCounter());
        serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
        // OK, so you waste all the f*****g time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all
        // the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE F*****G LOGIC IN THIS, FUCKWADS.
        // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit
        junk = 0;
        tmp = f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
                    f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
                    rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                    f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD |
                    f.PASSTHRU_MODE << BOXPASSTHRU |
                    rcOptions[BOXBEEPERON] << BOXBEEPERON |
                    rcOptions[BOXLEDMAX] << BOXLEDMAX |
                    rcOptions[BOXLLIGHTS] << BOXLLIGHTS |
                    rcOptions[BOXVARIO] << BOXVARIO |
                    rcOptions[BOXCALIB] << BOXCALIB |
                    rcOptions[BOXGOV] << BOXGOV |
                    rcOptions[BOXOSD] << BOXOSD |
                    f.ARMED << BOXARM;
        for (i = 0; i < numberBoxItems; i++) {
            int flag = (tmp & (1 << availableBoxes[i]));
            if (flag)
                junk |= 1 << i;
        }
        serialize32(junk);
        serialize8(mcfg.current_profile);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
        // Retarded hack until multiwiidorks start using real units for sensor data
        if (acc_1G > 1024) {
            for (i = 0; i < 3; i++)
                serialize16(accSmooth[i] / 8);
        } else {
            for (i = 0; i < 3; i++)
                serialize16(accSmooth[i]);
        }
        for (i = 0; i < 3; i++)
            serialize16(gyroData[i]);
        for (i = 0; i < 3; i++)
            serialize16(magADC[i]);
        break;
    case MSP_SERVO:
        s_struct((uint8_t *)&servo, 16);
        break;
    case MSP_SERVO_CONF:
        headSerialReply(56);
        for (i = 0; i < MAX_SERVOS; i++) {
            serialize16(cfg.servoConf[i].min);
            serialize16(cfg.servoConf[i].max);
            serialize16(cfg.servoConf[i].middle);
            serialize8(cfg.servoConf[i].rate);
        }
        break;
    case MSP_SET_SERVO_CONF:
        headSerialReply(0);
        for (i = 0; i < MAX_SERVOS; i++) {
            cfg.servoConf[i].min = read16();
            cfg.servoConf[i].max = read16();
            cfg.servoConf[i].middle = read16();
            cfg.servoConf[i].rate = read8();
        }
        break;
    case MSP_MOTOR:
        s_struct((uint8_t *)motor, 16);
        break;
    case MSP_RC:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(rcData[i]);
        break;
    case MSP_RAW_GPS:
        headSerialReply(16);
        serialize8(f.GPS_FIX);
        serialize8(GPS_numSat);
        serialize32(GPS_coord[LAT]);
        serialize32(GPS_coord[LON]);
        serialize16(GPS_altitude);
        serialize16(GPS_speed);
        serialize16(GPS_ground_course);
        break;
    case MSP_COMP_GPS:
        headSerialReply(5);
        serialize16(GPS_distanceToHome);
        serialize16(GPS_directionToHome);
        serialize8(GPS_update & 1);
        break;
    case MSP_ATTITUDE:
        headSerialReply(8);
        for (i = 0; i < 2; i++)
            serialize16(angle[i]);
        serialize16(heading);
        serialize16(headFreeModeHold);
        break;
    case MSP_ALTITUDE:
        headSerialReply(6);
        serialize32(EstAlt);
        serialize16(vario);
        break;
    case MSP_ANALOG:
        headSerialReply(5);
        serialize8(vbat);
        serialize16(0); // power meter trash
        serialize16(rssi);
        break;
    case MSP_RC_TUNING:
        headSerialReply(7);
        serialize8(cfg.rcRate8);
        serialize8(cfg.rcExpo8);
        serialize8(cfg.rollPitchRate);
        serialize8(cfg.yawRate);
        serialize8(cfg.dynThrPID);
        serialize8(cfg.thrMid8);
        serialize8(cfg.thrExpo8);
        break;
    case MSP_PID:
        headSerialReply(3 * PIDITEMS);
        for (i = 0; i < PIDITEMS; i++) {
            serialize8(cfg.P8[i]);
            serialize8(cfg.I8[i]);
            serialize8(cfg.D8[i]);
        }
        break;
    case MSP_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
    case MSP_BOX:
        headSerialReply(2 * numberBoxItems);
        for (i = 0; i < numberBoxItems; i++)
            serialize16(cfg.activate[availableBoxes[i]]);
        break;
    case MSP_BOXNAMES:
        // headSerialReply(sizeof(boxnames) - 1);
        serializeBoxNamesReply();
        break;
    case MSP_BOXIDS:
        headSerialReply(numberBoxItems);
        for (i = 0; i < numberBoxItems; i++)
            serialize8(availableBoxes[i]);
        break;
    case MSP_MISC:
        headSerialReply(2 * 6 + 4 + 2 + 4);
        serialize16(0); // intPowerTrigger1 (aka useless trash)
        serialize16(mcfg.minthrottle);
        serialize16(mcfg.maxthrottle);
        serialize16(mcfg.mincommand);
        serialize16(cfg.failsafe_throttle);
        serialize16(0); // plog useless shit
        serialize32(0); // plog useless shit
        serialize16(cfg.mag_declination / 10); // TODO check this shit
        serialize8(mcfg.vbatscale);
        serialize8(mcfg.vbatmincellvoltage);
        serialize8(mcfg.vbatmaxcellvoltage);
        serialize8(0);
        break;
    case MSP_MOTOR_PINS:
        headSerialReply(8);
        for (i = 0; i < 8; i++)
            serialize8(i + 1);
        break;
    case MSP_WP:
        wp_no = read8();    // get the wp number
        headSerialReply(18);
        if (wp_no == 0) {
            lat = GPS_home[LAT];
            lon = GPS_home[LON];
        } else if (wp_no == 16) {
            lat = GPS_hold[LAT];
            lon = GPS_hold[LON];
        }
        serialize8(wp_no);
        serialize32(lat);
        serialize32(lon);
        serialize32(AltHold);           // altitude (cm) will come here -- temporary implementation to test feature with apps
        serialize16(0);                 // heading  will come here (deg)
        serialize16(0);                 // time to stay (ms) will come here
        serialize8(0);                  // nav flag will come here
        break;
    case MSP_SET_WP:
        wp_no = read8();    //get the wp number
        lat = read32();
        lon = read32();
        alt = read32();     // to set altitude (cm)
        read16();           // future: to set heading (deg)
        read16();           // future: to set time to stay (ms)
        read8();            // future: to set nav flag
        if (wp_no == 0) {
            GPS_home[LAT] = lat;
            GPS_home[LON] = lon;
            f.GPS_HOME_MODE = 0;        // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
            f.GPS_FIX_HOME = 1;
            if (alt != 0)
                AltHold = alt;          // temporary implementation to test feature with apps
        } else if (wp_no == 16) {       // OK with SERIAL GPS  --  NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
            GPS_hold[LAT] = lat;
            GPS_hold[LON] = lon;
            if (alt != 0)
                AltHold = alt;          // temporary implementation to test feature with apps
            nav_mode = NAV_MODE_WP;
            GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
        }
        headSerialReply(0);
        break;
    case MSP_RESET_CONF:
        if (!f.ARMED)
            checkFirstTime(true);
        headSerialReply(0);
        break;
    case MSP_ACC_CALIBRATION:
        if (!f.ARMED)
            calibratingA = CALIBRATING_ACC_CYCLES;
        headSerialReply(0);
        break;
    case MSP_MAG_CALIBRATION:
        if (!f.ARMED)
            f.CALIBRATE_MAG = 1;
        headSerialReply(0);
        break;
    case MSP_EEPROM_WRITE:
        writeEEPROM(0, true);
        headSerialReply(0);
        break;
    case MSP_DEBUG:
        headSerialReply(8);
        // make use of this crap, output some useful QA statistics
        debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
        for (i = 0; i < 4; i++)
            serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;

    // Additional commands that are not compatible with MultiWii
    case MSP_ACC_TRIM:
        headSerialReply(4);
        serialize16(cfg.angleTrim[PITCH]);
        serialize16(cfg.angleTrim[ROLL]);
        break;
    case MSP_UID:
        headSerialReply(12);
        serialize32(U_ID_0);
        serialize32(U_ID_1);
        serialize32(U_ID_2);
        break;
    case MSP_GPSSVINFO:
        headSerialReply(1 + (GPS_numCh * 4));
        serialize8(GPS_numCh);
           for (i = 0; i < GPS_numCh; i++){
               serialize8(GPS_svinfo_chn[i]);
               serialize8(GPS_svinfo_svid[i]);
               serialize8(GPS_svinfo_quality[i]);
               serialize8(GPS_svinfo_cno[i]);
            }
        break;
    default:                   // we do not know how to handle the (valid) message, indicate error MSP $M!
        headSerialError(0);
        break;
    }
    tailSerialReply();
}
Пример #10
0
static void evaluateCommand(void)
{
    uint32_t i, tmpu32 = 0;
    uint8_t wp_no;

    switch (cmdMSP)
    {
    case MSP_SET_RAW_RC:
        for (i = 0; i < 8; i++)
            rcDataSAVE[i] = read16();
        headSerialReply(0);
        break;
    case MSP_SET_RAW_GPS:
        f.GPS_FIX = read8();
        GPS_numSat = read8();
        GPS_coord[LAT] = read32();
        GPS_coord[LON] = read32();
        GPS_altitude = read16();
        GPS_speed = read16();
        GPS_update |= 2;        // New data signalisation to GPS functions
        headSerialReply(0);
        break;
    case MSP_SET_PID:
        for (i = 0; i < PIDITEMS; i++)
        {
            cfg.P8[i] = read8();
            cfg.I8[i] = read8();
            cfg.D8[i] = read8();
        }
        headSerialReply(0);
        break;
    case MSP_SET_BOX:
        for (i = 0; i < CHECKBOXITEMS; i++)
        {
            tmpu32 = 0;
            if (cfg.rc_auxch > 4) tmpu32 = read32();
            else tmpu32 = read16();
            cfg.activate[i] = tmpu32;
        }
        headSerialReply(0);
        break;
    case MSP_SET_RC_TUNING:
        cfg.rcRate8 = read8();
        cfg.rcExpo8 = read8();
        cfg.rollPitchRate = read8();
        cfg.yawRate = read8();
        cfg.dynThrPID = read8();
        cfg.thrMid8 = read8();
        cfg.thrExpo8 = read8();
        headSerialReply(0);
        break;
    case MSP_SET_MISC:
        headSerialReply(0);
        break;
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(VERSION);                // multiwii version
        serialize8(cfg.mixerConfiguration); // type of multicopter
        serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
        serialize32(PLATFORM_32BIT);        // "capability"
        break;
    case MSP_STATUS:
        headSerialReply(11);
        serialize16(cycleTime);
        serialize16(i2cGetErrorCounter());
        serialize16(sensors(SENSOR_ACC)                   |
                    sensors(SENSOR_BARO)   << 1           |
                    sensors(SENSOR_MAG)    << 2           |
                    sensors(SENSOR_GPS)    << 3           |
                    sensors(SENSOR_SONAR)  << 4);

        serialize32(f.ANGLE_MODE           << BOXANGLE    |
                    f.HORIZON_MODE         << BOXHORIZON  |
                    f.BARO_MODE            << BOXBARO     |
                    f.MAG_MODE             << BOXMAG      |
                    f.ARMED                << BOXARM      |
                    rcOptions[BOXCAMSTAB]  << BOXCAMSTAB  |
                    f.GPS_HOME_MODE        << BOXGPSHOME  |
                    f.GPS_HOLD_MODE        << BOXGPSHOLD  |
                    f.GPS_LOG_MODE         << BOXGPSLOG   |
                    f.HEADFREE_MODE        << BOXHEADFREE |
                    f.PASSTHRU_MODE        << BOXPASSTHRU |
                    rcOptions[BOXBEEPERON] << BOXBEEPERON |
                    rcOptions[BOXHEADADJ]  << BOXHEADADJ  |
                    rcOptions[BOXOSD]      << BOXOSD);
        serialize8(0);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
        if (!feature(FEATURE_PASS))                               // Just Do the normal stuff
        {
            for (i = 0; i < 3; i++) serialize16((int16_t)accSmooth[i]);
            for (i = 0; i < 3; i++) serialize16((int16_t)gyroData[i]);
            for (i = 0; i < 3; i++) serialize16((int16_t)magADCfloat[i]);
        }
        else                                                      // Just serialize unfiltered AccZ for Balancing
        {
            for (i = 0; i < 2; i++) serialize16(0);
            serialize16((int16_t)accADC[YAW] - (uint16_t)acc_1G); // Put accz into the middle
            for (i = 0; i < 3; i++) serialize16(0);
            for (i = 0; i < 3; i++) serialize16(0);
        }
        break;
    case MSP_SERVO:
        headSerialReply(16);
        for (i = 0; i < 8; i++) serialize16(servo[i]);
        break;
    case MSP_MOTOR:
        headSerialReply(16);
        for (i = 0; i < 8; i++) serialize16(motor[i]);
        break;
    case MSP_RC:
        headSerialReply((cfg.rc_auxch + 4) * 2); // headSerialReply(16);
        for (i = 0; i < cfg.rc_auxch + 4; i++)   // for (i = 0; i < 8; i++)
            serialize16(rcDataSAVE[i]);
        break;
    case MSP_RAW_GPS:
        headSerialReply(14);
        serialize8(f.GPS_FIX);
        serialize8(GPS_numSat);
        serialize32(GPS_coord[LAT]);
        serialize32(GPS_coord[LON]);
        serialize16(GPS_altitude);
        serialize16(GPS_speed);
        break;
    case MSP_COMP_GPS:
        headSerialReply(5);
        serialize16(GPS_distanceToHome);
        serialize16(GPS_directionToHome);
        serialize8(GPS_update & 1);
        break;
    case MSP_ATTITUDE:
        headSerialReply(8);
        for (i = 0; i < 2; i++) serialize16((int16_t)angle[i]);
        serialize16((int16_t)heading);
        serialize16((int16_t)headFreeModeHold);
        break;
    case MSP_ALTITUDE:
        headSerialReply(6);
        if (feature(FEATURE_PASS))
        {
            serialize32(0);
            serialize16(0);
        }
        else
        {
            serialize32((int32_t)EstAlt);
            serialize16((int16_t)vario);
        }
        break;
    case MSP_BAT:
        headSerialReply(5);
        serialize8(vbat);
        serialize16(0);                               // power meter stuff
        serialize16((uint16_t)rssi << 2);             // Upscale 255 to 1020, so 100% (1023) is hard to achieve, who cares?
        break;
    case MSP_RC_TUNING:
        headSerialReply(7);
        serialize8(cfg.rcRate8);
        serialize8(cfg.rcExpo8);
        serialize8(cfg.rollPitchRate);
        serialize8(cfg.yawRate);
        serialize8(cfg.dynThrPID);
        serialize8(cfg.thrMid8);
        serialize8(cfg.thrExpo8);
        break;
    case MSP_PID:
        headSerialReply(3 * PIDITEMS);
        for (i = 0; i < PIDITEMS; i++)
        {
            serialize8(cfg.P8[i]);
            serialize8(cfg.I8[i]);
            serialize8(cfg.D8[i]);
        }
        break;
    case MSP_BOX:
        if (cfg.rc_auxch > 4) headSerialReply(4 * CHECKBOXITEMS);
        else headSerialReply(2 * CHECKBOXITEMS);
        for (i = 0; i < CHECKBOXITEMS; i++)
        {
            tmpu32 = cfg.activate[i];
            if (cfg.rc_auxch > 4) serialize32(tmpu32);
            else serialize16((int16_t)tmpu32);
        }
        break;
    case MSP_BOXNAMES:
        headSerialReply(sizeof(boxnames) - 1);
        serializeNames(boxnames);
        break;
    case MSP_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
    case MSP_MISC:
        headSerialReply(2);
        serialize16(0); // intPowerTrigger1
        break;
    case MSP_MOTOR_PINS:
        headSerialReply(8);
        for (i = 0; i < 8; i++) serialize8(i + 1);
        break;
    case MSP_WP:
        wp_no = read8();    // get the wp number
        headSerialReply(12);
        if (wp_no == 0)
        {
            serialize8(0);                   // wp0
            serialize32(GPS_home[LAT]);
            serialize32(GPS_home[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        }
        else if (wp_no == 16)
        {
            serialize8(16);                  // wp16
            serialize32(GPS_WP[LAT]);
            serialize32(GPS_WP[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        }
        break;
    case MSP_RESET_CONF:
        checkFirstTime(true);
        headSerialReply(0);
        break;
    case MSP_ACC_CALIBRATION:
        calibratingA = true;
        headSerialReply(0);
        break;
    case MSP_MAG_CALIBRATION:
        f.CALIBRATE_MAG = 1;
        headSerialReply(0);
        break;
    case MSP_EEPROM_WRITE:
        writeParams(0);
        headSerialReply(0);
        break;
    case MSP_DEBUG:
        headSerialReply(8);
        for (i = 0; i < 4; i++) serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;
    default:                                                // we do not know how to handle the (valid) message, indicate error MSP $M!
        headSerialError(0);
        break;
    }
    tailSerialReply();
}
Пример #11
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;

			///////////////////////////////
		}
    }
}
Пример #12
0
void systemInit(void)
{
    RCC_ClocksTypeDef rccClocks;

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

	// Init cycle counter
    cycleCounterInit();

    // SysTick
    SysTick_Config(SystemCoreClock / 1000);

    // Turn on peripherial clcoks
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1,   ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2,   ENABLE);

    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1,   ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2,   ENABLE);

    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,  ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,  ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC,  ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD,  ENABLE);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE,  ENABLE);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1,   ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2,   ENABLE);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2,   ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI3,   ENABLE);

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,   ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,   ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,   ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,   ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5,   ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6,   ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,   ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM10,  ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11,  ENABLE);

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);

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

    checkFirstTime(false);
	readEEPROM();

	if (eepromConfig.receiverType == SPEKTRUM)
		checkSpektrumBind();

	checkResetType();

	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);  // 2 bits for pre-emption priority, 2 bits for subpriority

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

	gpsPortClearBuffer       = &uart2ClearBuffer;
    gpsPortNumCharsAvailable = &uart2NumCharsAvailable;
    gpsPortPrintBinary       = &uart2PrintBinary;
    gpsPortRead              = &uart2Read;

	telemPortAvailable       = &uart1Available;
	telemPortPrint           = &uart1Print;
	telemPortPrintF          = &uart1PrintF;
	telemPortRead            = &uart1Read;

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

	initMixer();

    usbInit();

    ledInit();

    uart1Init();
    uart2Init();

    BLUE_LED_ON;

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

    delay(10000);  // 10 seconds of 20 second delay for sensor stabilization

    checkUsbActive();

    #ifdef __VERSION__
        cliPortPrintF("\ngcc version " __VERSION__ "\n");
    #endif

    cliPortPrintF("\nAQ32Plus Firmware V%s, Build Date " __DATE__ " "__TIME__" \n", __AQ32PLUS_VERSION);

    if ((RCC->CR & RCC_CR_HSERDY) != RESET)
    {
        cliPortPrint("\nRunning on external HSE clock....\n");
    }
    else
    {
        cliPortPrint("\nERROR: Running on internal HSI clock....\n");
    }

    RCC_GetClocksFreq(&rccClocks);

    cliPortPrintF("\nHCLK->   %3d MHz\n",   rccClocks.HCLK_Frequency   / 1000000);
    cliPortPrintF(  "PCLK1->  %3d MHz\n",   rccClocks.PCLK1_Frequency  / 1000000);
    cliPortPrintF(  "PCLK2->  %3d MHz\n",   rccClocks.PCLK2_Frequency  / 1000000);
    cliPortPrintF(  "SYSCLK-> %3d MHz\n\n", rccClocks.SYSCLK_Frequency / 1000000);

    initUBLOX();

    delay(10000);  // Remaining 10 seconds of 20 second delay for sensor stabilization - probably not long enough..

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

    adcInit();
    i2cInit(I2C1);
    i2cInit(I2C2);
    pwmServoInit();
    rxInit();
    spiInit(SPI2);
    spiInit(SPI3);
    timingFunctionsInit();

    batteryInit();

    initFirstOrderFilter();
    initMavlink();
    initMax7456();
    initPID();

    GREEN_LED_ON;

    initMPU6000();
    initMag();
    initPressure();
}
Пример #13
0
void systemInit(void)
{
	// Init cycle counter
    cycleCounterInit();

    // SysTick
    SysTick_Config(SystemCoreClock / 1000);

    // Turn on peripherial clocks
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_ADC12,    ENABLE);

	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1,     ENABLE);  // USART1
	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2,     ENABLE);  // ADC2

	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA,    ENABLE);
	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB,    ENABLE);
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC,    ENABLE);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2,   ENABLE);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,   ENABLE);  // PWM Servo Out 1 & 2
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,   ENABLE);  // PWM ESC Out 3 & 4
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,   ENABLE);  // PWM ESC Out 5,6,7, & 8
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6,   ENABLE);  // 500 Hz dt Counter
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7,   ENABLE);  // 100 Hz dt Counter
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM15,  ENABLE);  // PWM ESC Out 1 & 2
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM16,  ENABLE);  // PPM RX
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM17,  ENABLE);  // Spektrum Frame Sync

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);  // Telemetry
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);  // Spektrum RX

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

    checkFirstTime(false);
	readEEPROM();

	if (eepromConfig.receiverType == SPEKTRUM)
		checkSpektrumBind();

    checkResetType();

	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);  // 2 bits for pre-emption priority, 2 bits for subpriority

	initMixer();

	cliInit();
	gpioInit();
    telemetryInit();
    adcInit();

    LED0_OFF;

    delay(20000);  // 20 sec total delay for sensor stabilization - probably not long enough.....

    LED0_ON;

    batteryInit();
    pwmServoInit(eepromConfig.servoPwmRate);

    if (eepromConfig.receiverType == SPEKTRUM)
        spektrumInit();
    else
        ppmRxInit();

    spiInit(SPI2);
    timingFunctionsInit();

    initFirstOrderFilter();
    initPID();

    initMPU6000();
    initMag();
    initPressure();
}
Пример #14
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, %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("\nnDot PID:  %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[NDOT_PID].B,
                   		                                                              eepromConfig.PID[NDOT_PID].P,
                   		                                                              eepromConfig.PID[NDOT_PID].I,
                   		                                                              eepromConfig.PID[NDOT_PID].D,
                   		                                                              eepromConfig.PID[NDOT_PID].windupGuard,
                   		                                                              eepromConfig.PID[NDOT_PID].dErrorCalc ? "Error" : "State");

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

                cliPortPrintF("hDot 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("\nN PID:  %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[N_PID].B,
                   		                                                           eepromConfig.PID[N_PID].P,
                   		                                                           eepromConfig.PID[N_PID].I,
                   		                                                           eepromConfig.PID[N_PID].D,
                   		                                                           eepromConfig.PID[N_PID].windupGuard,
                   		                                                           eepromConfig.PID[N_PID].dErrorCalc ? "Error" : "State");

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

                cliPortPrintF("h 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': // 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", axisPID[ROLL ],
													   axisPID[PITCH],
													   axisPID[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': // Not Used
				cliQuery = 'x';
				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 ((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, ", sBusChannel[index]);

                    cliPortPrintF("%4ld\n", sBusChannel[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;

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

			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' 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;

				///////////////////////////////
		}
    }
}
Пример #15
0
static void evaluateCommand(void)
{
    uint32_t i, j, tmp, junk;
#ifdef GPS
    uint8_t wp_no;
    int32_t lat = 0, lon = 0, alt = 0;
#endif
    const char *build = __DATE__;

    switch (currentPortState->cmdMSP) {
        case MSP_SET_RAW_RC:
            for (i = 0; i < 8; i++)
                rcData[i] = read16();
            headSerialReply(0);
            mspFrameRecieve();
            break;
        case MSP_SET_ACC_TRIM:
            cfg.angleTrim[PITCH] = read16();
            cfg.angleTrim[ROLL]  = read16();
            headSerialReply(0);
            break;
#ifdef GPS
        case MSP_SET_RAW_GPS:
            f.GPS_FIX = read8();
            GPS_numSat = read8();
            GPS_coord[LAT] = read32();
            GPS_coord[LON] = read32();
            GPS_altitude = read16();
            GPS_speed = read16();
            GPS_update |= 2;        // New data signalisation to GPS functions
            headSerialReply(0);
            break;
#endif
        case MSP_SET_PID:
            for (i = 0; i < PIDITEMS; i++) {
                cfg.P8[i] = read8();
                cfg.I8[i] = read8();
                cfg.D8[i] = read8();
            }
            headSerialReply(0);
            break;
        case MSP_SET_BOX:
            for (i = 0; i < numberBoxItems; i++)
                cfg.activate[availableBoxes[i]] = read16();
            headSerialReply(0);
            break;
        case MSP_SET_RC_TUNING:
            cfg.rcRate8 = read8();
            cfg.rcExpo8 = read8();
            cfg.rollPitchRate = read8();
            cfg.yawRate = read8();
            cfg.dynThrPID = read8();
            cfg.thrMid8 = read8();
            cfg.thrExpo8 = read8();
            headSerialReply(0);
            break;
        case MSP_SET_MISC:
            tmp = read16();
            // sanity check
            if (tmp < 1600 && tmp > 1400)
                mcfg.midrc = tmp;
            mcfg.minthrottle = read16();
            mcfg.maxthrottle = read16();
            mcfg.mincommand = read16();
            cfg.failsafe_throttle = read16();
            mcfg.gps_type = read8();
            mcfg.gps_baudrate = read8();
            mcfg.gps_ubx_sbas = read8();
            mcfg.multiwiicurrentoutput = read8();
            mcfg.rssi_aux_channel = read8();
            read8();
            cfg.mag_declination = read16() * 10;
            mcfg.vbatscale = read8();           // actual vbatscale as intended
            mcfg.vbatmincellvoltage = read8();  // vbatlevel_warn1 in MWC2.3 GUI
            mcfg.vbatmaxcellvoltage = read8();  // vbatlevel_warn2 in MWC2.3 GUI
            mcfg.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert
            headSerialReply(0);
            break;
        case MSP_SET_MOTOR:
            for (i = 0; i < 8; i++)
                motor_disarmed[i] = read16();
            headSerialReply(0);
            break;
        case MSP_SELECT_SETTING:
            if (!f.ARMED) {
                mcfg.current_profile = read8();
                if (mcfg.current_profile > 2)
                    mcfg.current_profile = 0;
                // this writes new profile index and re-reads it
                writeEEPROM(0, false);
            }
            headSerialReply(0);
            break;
        case MSP_SET_HEAD:
            magHold = read16();
            headSerialReply(0);
            break;
        case MSP_IDENT:
            headSerialReply(7);
            serialize8(VERSION);                    // multiwii version
            serialize8(mcfg.mixerConfiguration);    // type of multicopter
            serialize8(MSP_VERSION);                // MultiWii Serial Protocol Version
            serialize32(CAP_PLATFORM_32BIT | CAP_BASEFLIGHT_CONFIG | CAP_DYNBALANCE | CAP_FW_FLAPS); // "capability"
            break;
        case MSP_STATUS:
            headSerialReply(11);
            serialize16(cycleTime);
            serialize16(i2cGetErrorCounter());
            serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
            // OK, so you waste all the f*****g time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all
            // the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE F*****G LOGIC IN THIS, FUCKWADS.
            // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit
            junk = 0;
            tmp = f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
                  f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
                  rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                  f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD |
                  f.PASSTHRU_MODE << BOXPASSTHRU |
                  rcOptions[BOXBEEPERON] << BOXBEEPERON |
                  rcOptions[BOXLEDMAX] << BOXLEDMAX |
                  rcOptions[BOXLLIGHTS] << BOXLLIGHTS |
                  rcOptions[BOXVARIO] << BOXVARIO |
                  rcOptions[BOXCALIB] << BOXCALIB |
                  rcOptions[BOXGOV] << BOXGOV |
                  rcOptions[BOXOSD] << BOXOSD |
                  rcOptions[BOXTELEMETRY] << BOXTELEMETRY |
                  rcOptions[BOXSERVO1] << BOXSERVO1 |
                  rcOptions[BOXSERVO2] << BOXSERVO2 |
                  rcOptions[BOXSERVO3] << BOXSERVO3 |
                  f.ARMED << BOXARM;
            for (i = 0; i < numberBoxItems; i++) {
                int flag = (tmp & (1 << availableBoxes[i]));
                if (flag)
                    junk |= 1 << i;
            }
            serialize32(junk);
            serialize8(mcfg.current_profile);
            break;
        case MSP_RAW_IMU:
            headSerialReply(18);
            // Retarded hack until multiwiidorks start using real units for sensor data
            if (acc_1G > 1024) {
                for (i = 0; i < 3; i++)
                    serialize16(accSmooth[i] / 8);
            } else {
                for (i = 0; i < 3; i++)
                    serialize16(accSmooth[i]);
            }
            for (i = 0; i < 3; i++)
                serialize16(gyroData[i]);
            for (i = 0; i < 3; i++)
                serialize16(magADC[i]);
            break;
        case MSP_SERVO:
            s_struct((uint8_t *)&servo, 16);
            break;
        case MSP_SERVO_CONF:
            headSerialReply(56);
            for (i = 0; i < MAX_SERVOS; i++) {
                serialize16(cfg.servoConf[i].min);
                serialize16(cfg.servoConf[i].max);
                serialize16(cfg.servoConf[i].middle);
                serialize8(getOldServoConfig(i));
            }
            break;
        case MSP_SET_SERVO_CONF:
            headSerialReply(0);
            for (i = 0; i < MAX_SERVOS; i++) {
                cfg.servoConf[i].min = read16();
                cfg.servoConf[i].max = read16();
                cfg.servoConf[i].middle = read16();
                tmp = read8();
                // trash old servo direction
                cfg.servoConf[i].rate = tmp & 0xfc;

                // store old style servo direction depending on current mixer configuration
                switch (mcfg.mixerConfiguration) {
                    case MULTITYPE_BI:
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_PITCH);

                        storeOldServoConfig(i, tmp, 4, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 5, 2, INPUT_YAW);

                        break;
                    case MULTITYPE_TRI:
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_YAW);

                        break;
                    case MULTITYPE_FLYING_WING:
                        storeOldServoConfig(i, tmp, 3, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);

                        storeOldServoConfig(i, tmp, 3, 2, INPUT_ROLL);
                        storeOldServoConfig(i, tmp, 4, 2, INPUT_ROLL);

                        break;
                    case MULTITYPE_DUALCOPTER:
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_ROLL);

                        break;
                    case MULTITYPE_SINGLECOPTER:
                        storeOldServoConfig(i, tmp, 3, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH);
                        storeOldServoConfig(i, tmp, 5, 1, INPUT_ROLL);
                        storeOldServoConfig(i, tmp, 6, 1, INPUT_ROLL);

                        storeOldServoConfig(i, tmp, 3, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 4, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 5, 2, INPUT_YAW);
                        storeOldServoConfig(i, tmp, 6, 2, INPUT_YAW);

                        break;
                }
            }
            break;
        case MSP_MOTOR:
            s_struct((uint8_t *)motor, 16);
            break;
        case MSP_RC:
            headSerialReply(16);
            for (i = 0; i < 8; i++)
                serialize16(rcData[i]);
            break;
#ifdef GPS
        case MSP_RAW_GPS:
            headSerialReply(16);
            serialize8(f.GPS_FIX);
            serialize8(GPS_numSat);
            serialize32(GPS_coord[LAT]);
            serialize32(GPS_coord[LON]);
            serialize16(GPS_altitude);
            serialize16(GPS_speed);
            serialize16(GPS_ground_course);
            break;
        case MSP_COMP_GPS:
            headSerialReply(5);
            serialize16(GPS_distanceToHome);
            serialize16(GPS_directionToHome);
            serialize8(GPS_update & 1);
            break;
#endif
        case MSP_ATTITUDE:
            headSerialReply(6);
            for (i = 0; i < 2; i++)
                serialize16(angle[i]);
            serialize16(heading);
            break;
        case MSP_ALTITUDE:
            headSerialReply(6);
            serialize32(EstAlt);
            serialize16(vario);
            break;
        case MSP_ANALOG:
            headSerialReply(7);
            serialize8((uint8_t)constrain(vbat, 0, 255));
            serialize16((uint16_t)constrain(mAhdrawn, 0, 0xFFFF)); // milliamphours drawn from battery
            serialize16(rssi);
            if (mcfg.multiwiicurrentoutput)
                serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps
            else
                serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps
            break;
        case MSP_RC_TUNING:
            headSerialReply(7);
            serialize8(cfg.rcRate8);
            serialize8(cfg.rcExpo8);
            serialize8(cfg.rollPitchRate);
            serialize8(cfg.yawRate);
            serialize8(cfg.dynThrPID);
            serialize8(cfg.thrMid8);
            serialize8(cfg.thrExpo8);
            break;
        case MSP_PID:
            headSerialReply(3 * PIDITEMS);
            for (i = 0; i < PIDITEMS; i++) {
                serialize8(cfg.P8[i]);
                serialize8(cfg.I8[i]);
                serialize8(cfg.D8[i]);
            }
            break;
        case MSP_PIDNAMES:
            headSerialReply(sizeof(pidnames) - 1);
            serializeNames(pidnames);
            break;
        case MSP_BOX:
            headSerialReply(2 * numberBoxItems);
            for (i = 0; i < numberBoxItems; i++)
                serialize16(cfg.activate[availableBoxes[i]]);
            break;
        case MSP_BOXNAMES:
            // headSerialReply(sizeof(boxnames) - 1);
            serializeBoxNamesReply();
            break;
        case MSP_BOXIDS:
            headSerialReply(numberBoxItems);
            for (i = 0; i < numberBoxItems; i++) {
                for  (j = 0; j < CHECKBOXITEMS; j++) {
                    if (boxes[j].permanentId == availableBoxes[i])
                        serialize8(boxes[j].permanentId);
                }
            }
            break;
        case MSP_MISC:
            headSerialReply(2 * 6 + 4 + 2 + 4);
            serialize16(mcfg.midrc);
            serialize16(mcfg.minthrottle);
            serialize16(mcfg.maxthrottle);
            serialize16(mcfg.mincommand);
            serialize16(cfg.failsafe_throttle);
            serialize8(mcfg.gps_type);
            serialize8(mcfg.gps_baudrate);
            serialize8(mcfg.gps_ubx_sbas);
            serialize8(mcfg.multiwiicurrentoutput);
            serialize8(mcfg.rssi_aux_channel);
            serialize8(0);
            serialize16(cfg.mag_declination / 10); // TODO check this shit
            serialize8(mcfg.vbatscale);
            serialize8(mcfg.vbatmincellvoltage);
            serialize8(mcfg.vbatmaxcellvoltage);
            serialize8(mcfg.vbatwarningcellvoltage);
            break;
        case MSP_MOTOR_PINS:
            headSerialReply(8);
            for (i = 0; i < 8; i++)
                serialize8(i + 1);
            break;
#ifdef GPS
        case MSP_WP:
            wp_no = read8();    // get the wp number
            headSerialReply(18);
            if (wp_no == 0) {
                lat = GPS_home[LAT];
                lon = GPS_home[LON];
            } else if (wp_no == 16) {
                lat = GPS_hold[LAT];
                lon = GPS_hold[LON];
            }
            serialize8(wp_no);
            serialize32(lat);
            serialize32(lon);
            serialize32(AltHold);           // altitude (cm) will come here -- temporary implementation to test feature with apps
            serialize16(0);                 // heading  will come here (deg)
            serialize16(0);                 // time to stay (ms) will come here
            serialize8(0);                  // nav flag will come here
            break;
        case MSP_SET_WP:
            wp_no = read8();    //get the wp number
            lat = read32();
            lon = read32();
            alt = read32();     // to set altitude (cm)
            read16();           // future: to set heading (deg)
            read16();           // future: to set time to stay (ms)
            read8();            // future: to set nav flag
            if (wp_no == 0) {
                GPS_home[LAT] = lat;
                GPS_home[LON] = lon;
                f.GPS_HOME_MODE = 0;        // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
                f.GPS_FIX_HOME = 1;
                if (alt != 0)
                    AltHold = alt;          // temporary implementation to test feature with apps
            } else if (wp_no == 16) {       // OK with SERIAL GPS  --  NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
                GPS_hold[LAT] = lat;
                GPS_hold[LON] = lon;
                if (alt != 0)
                    AltHold = alt;          // temporary implementation to test feature with apps
                nav_mode = NAV_MODE_WP;
                GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
            }
            headSerialReply(0);
            break;
#endif /* GPS */
        case MSP_RESET_CONF:
            if (!f.ARMED)
                checkFirstTime(true);
            headSerialReply(0);
            break;
        case MSP_ACC_CALIBRATION:
            if (!f.ARMED)
                calibratingA = CALIBRATING_ACC_CYCLES;
            headSerialReply(0);
            break;
        case MSP_MAG_CALIBRATION:
            if (!f.ARMED)
                f.CALIBRATE_MAG = 1;
            headSerialReply(0);
            break;
        case MSP_EEPROM_WRITE:
            if (f.ARMED) {
                headSerialError(0);
            } else {
                writeEEPROM(0, true);
                headSerialReply(0);
            }
            break;
        case MSP_DEBUG:
            headSerialReply(8);
            // make use of this crap, output some useful QA statistics
            debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000);         // XX0YY [crystal clock : core clock]
            for (i = 0; i < 4; i++)
                serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
            break;

        // Additional commands that are not compatible with MultiWii
        case MSP_ACC_TRIM:
            headSerialReply(4);
            serialize16(cfg.angleTrim[PITCH]);
            serialize16(cfg.angleTrim[ROLL]);
            break;
        case MSP_UID:
            headSerialReply(12);
            serialize32(U_ID_0);
            serialize32(U_ID_1);
            serialize32(U_ID_2);
            break;
#ifdef GPS
        case MSP_GPSSVINFO:
            headSerialReply(1 + (GPS_numCh * 4));
            serialize8(GPS_numCh);
            for (i = 0; i < GPS_numCh; i++) {
                serialize8(GPS_svinfo_chn[i]);
                serialize8(GPS_svinfo_svid[i]);
                serialize8(GPS_svinfo_quality[i]);
                serialize8(GPS_svinfo_cno[i]);
            }
            // Poll new SVINFO from GPS
            gpsPollSvinfo();
            break;
        case MSP_GPSDEBUGINFO:
            headSerialReply(16);
            if (sensors(SENSOR_GPS)) {
                serialize32(GPS_update_rate[1] - GPS_update_rate[0]);
                serialize32(GPS_svinfo_rate[1] - GPS_svinfo_rate[0]);
            } else {
                serialize32(0);
                serialize32(0);
            }
            serialize32(GPS_HorizontalAcc);
            serialize32(GPS_VerticalAcc);
            break;
#endif /* GPS */

        case MSP_SET_CONFIG:
            headSerialReply(0);
            mcfg.mixerConfiguration = read8(); // multitype
            featureClearAll();
            featureSet(read32()); // features bitmap
            mcfg.serialrx_type = read8(); // serialrx_type
            mcfg.board_align_roll = read16(); // board_align_roll
            mcfg.board_align_pitch = read16(); // board_align_pitch
            mcfg.board_align_yaw = read16(); // board_align_yaw
            mcfg.currentscale = read16();
            mcfg.currentoffset = read16();
            /// ???
            break;
        case MSP_CONFIG:
            headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 4);
            serialize8(mcfg.mixerConfiguration);
            serialize32(featureMask());
            serialize8(mcfg.serialrx_type);
            serialize16(mcfg.board_align_roll);
            serialize16(mcfg.board_align_pitch);
            serialize16(mcfg.board_align_yaw);
            serialize16(mcfg.currentscale);
            serialize16(mcfg.currentoffset);
            /// ???
            break;

        case MSP_RCMAP:
            headSerialReply(MAX_INPUTS); // TODO fix this
            for (i = 0; i < MAX_INPUTS; i++)
                serialize8(mcfg.rcmap[i]);
            break;
        case MSP_SET_RCMAP:
            headSerialReply(0);
            for (i = 0; i < MAX_INPUTS; i++)
                mcfg.rcmap[i] = read8();
            break;

        case MSP_REBOOT:
            headSerialReply(0);
            pendReboot = true;
            break;

        case MSP_BUILDINFO:
            headSerialReply(11 + 4 + 4);
            for (i = 0; i < 11; i++)
                serialize8(build[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc
            serialize32(0); // future exp
            serialize32(0); // future exp
            break;

        default:                   // we do not know how to handle the (valid) message, indicate error MSP $M!
            headSerialError(0);
            break;
    }
    tailSerialReply();
}
Пример #16
0
int main(void)
{
    uint8_t i;
    drv_pwm_config_t pwm_params;

#if 0
    // PC12, PA15
    // using this to write asm for bootloader :)
    RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only
    AFIO->MAPR &= 0xF0FFFFFF;
    AFIO->MAPR = 0x02000000;
    GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz
    GPIOA->BRR = 0x8000; // set low 15
    GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz
    GPIOC->BRR = 0x1000; // set low 12
#endif

#if 0
    // using this to write asm for bootloader :)
    RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
    AFIO->MAPR &= 0xF0FFFFFF;
    AFIO->MAPR = 0x02000000;
    GPIOB->BRR = 0x18; // set low 4 & 3
    GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
#endif

    systemInit();

    readEEPROM();
    checkFirstTime(false);

    serialInit(cfg.serial_baudrate);

    // We have these sensors
#ifndef FY90Q
    // AfroFlight32
    sensorsSet(SENSOR_ACC | SENSOR_BARO | SENSOR_MAG);
#else
    // FY90Q
    sensorsSet(SENSOR_ACC);
#endif

    mixerInit(); // this will set useServo var depending on mixer type
    // when using airplane/wing mixer, servo/motor outputs are remapped
    if (cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_FLYING_WING)
        pwm_params.airplane = true;
    pwm_params.usePPM = feature(FEATURE_PPM);
    pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
    pwm_params.useServos = useServo;
    pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
    pwm_params.motorPwmRate = cfg.motor_pwm_rate;
    pwm_params.servoPwmRate = cfg.servo_pwm_rate;

    pwmInit(&pwm_params);

    // configure PWM/CPPM read function. spektrum will override that
    rcReadRawFunc = pwmReadRawRC;

    LED1_ON;
    LED0_OFF;
    for (i = 0; i < 10; i++) {
        LED1_TOGGLE;
        LED0_TOGGLE;
        delay(25);
        BEEP_ON;
        delay(25);
        BEEP_OFF;
    }
    LED0_OFF;
    LED1_OFF;

    // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
    sensorsAutodetect();
    imuInit(); // Mag is initialized inside imuInit

    // Check battery type/voltage
    if (feature(FEATURE_VBAT))
        batteryInit();

    if (feature(FEATURE_SPEKTRUM)) {
        spektrumInit();
        rcReadRawFunc = spektrumReadRawRC;
    } else {
        // spektrum and GPS are mutually exclusive
        // Optional GPS - available only when using PPM, otherwise required pins won't be usable
        if (feature(FEATURE_PPM)) {
            if (feature(FEATURE_GPS))
                gpsInit(cfg.gps_baudrate);
#ifdef SONAR
            if (feature(FEATURE_SONAR))
                Sonar_init();
#endif
        }
    }

    previousTime = micros();
    if (cfg.mixerConfiguration == MULTITYPE_GIMBAL)
        calibratingA = 400;
    calibratingG = 1000;
    f.SMALL_ANGLES_25 = 1;

    // loopy
    while (1) {
        loop();
    }
}
Пример #17
0
void systemInit(void)
{
    RCC_ClocksTypeDef rccClocks;

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

    // Init cycle counter
    cycleCounterInit();

    // SysTick
    SysTick_Config(SystemCoreClock / 1000);

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
                           RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO  |
                           RCC_APB2Periph_TIM1  | RCC_APB2Periph_TIM8  |
                           RCC_APB2Periph_ADC1, ENABLE);

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3  | RCC_APB1Periph_TIM4  |
                           RCC_APB1Periph_TIM5  | RCC_APB1Periph_TIM6  |
                           RCC_APB1Periph_I2C2, ENABLE);

    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);

#ifdef _DTIMING
    timingSetup();
#endif

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

    checkFirstTime(false);
    readEEPROM();

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);  // 2 bits for pre-emption priority, 2 bits for subpriority

    pwmMotorDriverInit();

    cliInit();
    gpioInit();
    adcInit();

    LED2_ON;

    delay(10000);  // 10 seconds of 20 second delay for sensor stabilization

    if (GetVCPConnectMode() != eVCPConnectReset)
    {
        cliPrintF("\r\nUSB startup delay...\r\n");
        delay(3000);

        if (GetVCPConnectMode() == eVCPConnectData)
        {
            cliPrintF("\r\nBGC32 firmware starting up, USB connected...\r\n");
        }
    }
    else
    {
        cliPrintF("\r\nDelaying for usb/serial driver to settle\r\n");
        delay(3000);
        cliPrintF("\r\nBGC32 firmware starting up, serial active...\r\n");
    }

#ifdef __VERSION__
    cliPrintF("\ngcc version " __VERSION__ "\n");
#endif

    cliPrintF("BGC32 Firmware V%s, Build Date " __DATE__ " "__TIME__" \n", __BGC32_VERSION);

    if ((RCC->CR & RCC_CR_HSERDY) != RESET)
    {
        cliPrintF("\nRunning on external HSE clock....\n");
    }
    else
    {
        cliPrintF("\nERROR: Running on internal HSI clock....\n");
    }

    RCC_GetClocksFreq(&rccClocks);

    cliPrintF("\nADCCLK-> %2d MHz\n",   rccClocks.ADCCLK_Frequency / 1000000);
    cliPrintF(  "HCLK->   %2d MHz\n",   rccClocks.HCLK_Frequency   / 1000000);
    cliPrintF(  "PCLK1->  %2d MHz\n",   rccClocks.PCLK1_Frequency  / 1000000);
    cliPrintF(  "PCLK2->  %2d MHz\n",   rccClocks.PCLK2_Frequency  / 1000000);
    cliPrintF(  "SYSCLK-> %2d MHz\n\n", rccClocks.SYSCLK_Frequency / 1000000);

    delay(10000);  // Remaining 10 seconds of 20 second delay for sensor stabilization - probably not long enough..

    LED1_ON;

    i2cInit(I2C2);
    rcInit();
    timingFunctionsInit();

    BKPInit();

    initFirstOrderFilter();
    initPID();
    initSinArray();

    orientIMU();

    initMPU6050();
    // initMag();
}
Пример #18
0
int main(void)
{
    uint8_t i;
    drv_pwm_config_t pwm_params;
    drv_adc_config_t adc_params;

#if 0
    // PC12, PA15
    // using this to write asm for bootloader :)
    RCC->APB2ENR |= RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO; // GPIOA/C+AFIO only
    AFIO->MAPR &= 0xF0FFFFFF;
    AFIO->MAPR = 0x02000000;
    GPIOA->CRH = 0x34444444; // PIN 15 Output 50MHz
    GPIOA->BRR = 0x8000; // set low 15
    GPIOC->CRH = 0x44434444; // PIN 12 Output 50MHz
    GPIOC->BRR = 0x1000; // set low 12
#endif

#if 0
    // using this to write asm for bootloader :)
    RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
    AFIO->MAPR &= 0xF0FFFFFF;
    AFIO->MAPR = 0x02000000;
    GPIOB->BRR = 0x18; // set low 4 & 3
    GPIOB->CRL = 0x44433444; // PIN 4 & 3 Output 50MHz
#endif

    systemInit();
		#ifdef USE_LAME_PRINTF
    init_printf(NULL, _putc);
		#endif

    checkFirstTime(false);
    readEEPROM();

    // configure power ADC
    if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
        adc_params.powerAdcChannel = mcfg.power_adc_channel;
    else {
        adc_params.powerAdcChannel = 0;
        mcfg.power_adc_channel = 0;
    }

    adcInit(&adc_params);

    // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
    sensorsSet(SENSORS_SET);

    mixerInit(); // this will set useServo var depending on mixer type
    // when using airplane/wing mixer, servo/motor outputs are remapped
    if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
        pwm_params.airplane = true;
    else
        pwm_params.airplane = false;
    pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SPEKTRUM); // spektrum support uses UART too
    pwm_params.usePPM = feature(FEATURE_PPM);
    pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum
    pwm_params.useServos = useServo;
    pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
    pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
    pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
    pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
    switch (mcfg.power_adc_channel) {
        case 1:
            pwm_params.adcChannel = PWM2;
            break;
        case 9:
            pwm_params.adcChannel = PWM8;
            break;
        default:
            pwm_params.adcChannel = 0;
        break;
    }

    pwmInit(&pwm_params);

    // configure PWM/CPPM read function. spektrum below will override that
    rcReadRawFunc = pwmReadRawRC;

    if (feature(FEATURE_SPEKTRUM)) {
        spektrumInit();
        rcReadRawFunc = spektrumReadRawRC;
    } else {
        // spektrum and GPS are mutually exclusive
        // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
        if (feature(FEATURE_GPS))
            gpsInit(mcfg.gps_baudrate);
    }
#ifdef SONAR
    // sonar stuff only works with PPM
    if (feature(FEATURE_PPM)) {
        if (feature(FEATURE_SONAR))
            Sonar_init();
    }
#endif

    LED1_ON;
    LED0_OFF;
    for (i = 0; i < 10; i++) {
        LED1_TOGGLE;
        LED0_TOGGLE;
        delay(25);
        BEEP_ON;
        delay(25);
        BEEP_OFF;
    }
    LED0_OFF;
    LED1_OFF;

    // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
    sensorsAutodetect();
    imuInit(); // Mag is initialized inside imuInit

    // Check battery type/voltage
    if (feature(FEATURE_VBAT))
        batteryInit();

    serialInit(mcfg.serial_baudrate);

    previousTime = micros();
    if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
        calibratingA = 400;
    calibratingG = 1000;
    calibratingB = 200;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
    f.SMALL_ANGLES_25 = 1;

    // loopy
    while (1) {
        loop();
    }
}
Пример #19
0
static void evaluateCommand(void)
{
    uint32_t i;
    uint8_t wp_no;

    switch (cmdMSP)
    {
    case MSP_SET_RAW_RC:
        for (i = 0; i < 8; i++)
            rcDataSAVE[i] = read16();
        headSerialReply(0);
        break;
    case MSP_SET_ACC_TRIM:
        cfg.angleTrim[PITCH] = read16();
        cfg.angleTrim[ROLL]  = read16();
        headSerialReply(0);
        break;
    case MSP_SET_RAW_GPS:
        f.GPS_FIX = read8();
        GPS_numSat = read8();
        GPS_coord[LAT] = read32();
        GPS_coord[LON] = read32();
        GPS_altitude = read16();
        GPS_speed = read16();
        GPS_update |= 2;        // New data signalisation to GPS functions
        headSerialReply(0);
        break;
    case MSP_SET_PID:
        for (i = 0; i < PIDITEMS; i++)
        {
            cfg.P8[i] = read8();
            cfg.I8[i] = read8();
            cfg.D8[i] = read8();
        }
        headSerialReply(0);
        break;
    case MSP_SET_BOX:
        for (i = 0; i < CHECKBOXITEMS; i++)
        {
            if (cfg.auxChannels > 4) cfg.activate[i] = read32();
            else cfg.activate[i] = read16();
        }
        headSerialReply(0);
        break;
    case MSP_SET_RC_TUNING:
        cfg.rcRate8 = read8();
        cfg.rcExpo8 = read8();
        cfg.rollPitchRate = read8();
        cfg.yawRate = read8();
        cfg.dynThrPID = read8();
        cfg.thrMid8 = read8();
        cfg.thrExpo8 = read8();
        headSerialReply(0);
        break;
    case MSP_SET_MISC:
        headSerialReply(0);
        break;
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(VERSION);                // multiwii version
        serialize8(cfg.mixerConfiguration); // type of multicopter
        serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
        serialize32(PLATFORM_32BIT);        // "capability"
        break;
    case MSP_STATUS:
        headSerialReply(10);
        serialize16(cycleTime);
        serialize16(i2cGetErrorCounter());
        serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
        serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM |
                    rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                    f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU |
                    rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
        if (!feature(FEATURE_PASS))                               // Just Do the normal stuff
        {
            for (i = 0; i < 3; i++) serialize16(accSmooth[i]);
            for (i = 0; i < 3; i++) serialize16(gyroData[i]);
            for (i = 0; i < 3; i++) serialize16((int16_t)magADCfloat[i]);
        }
        else                                                      // Just serialize unfiltered AccZ for Balancing
        {
            for (i = 0; i < 2; i++) serialize16(0);
            serialize16(accADC[YAW] - acc_1G);                    // Put accz into the middle
            for (i = 0; i < 3; i++) serialize16(0);
            for (i = 0; i < 3; i++) serialize16(0);
        }
        break;
    case MSP_SERVO:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(servo[i]);
        break;
    case MSP_MOTOR:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(motor[i]);
        break;
    case MSP_RC:
        headSerialReply((cfg.auxChannels + 4) * 2); // headSerialReply(16);
        for (i = 0; i < cfg.auxChannels + 4; i++)   // for (i = 0; i < 8; i++)
            serialize16(rcDataSAVE[i]);
        break;
    case MSP_RAW_GPS:
        headSerialReply(14);
        serialize8(f.GPS_FIX);
        serialize8(GPS_numSat);
        if (cfg.gps_debug == 1)
        {
            serialize32(Real_GPS_coord[LAT]);
            serialize32(Real_GPS_coord[LON]);
        }
        else
        {
            serialize32(GPS_coord[LAT]);
            serialize32(GPS_coord[LON]);
        }
        serialize16(GPS_altitude);
        serialize16(GPS_speed);
        break;
    case MSP_COMP_GPS:
        headSerialReply(5);
        serialize16(GPS_distanceToHome);
        serialize16(GPS_directionToHome);
        serialize8(GPS_update & 1);
        break;
    case MSP_ATTITUDE:
        headSerialReply(8);
        for (i = 0; i < 2; i++)
            serialize16(angle[i]);
        serialize16((int16_t)heading);
        serialize16((int16_t)headFreeModeHold);
        break;
    case MSP_ALTITUDE:
        headSerialReply(4);
        if (feature(FEATURE_PASS))
            serialize32(0);
        else
            serialize32(EstAlt);
        break;
    case MSP_BAT:
        headSerialReply(3);
        serialize8(vbat);
        serialize16(0); // power meter trash
        break;
    case MSP_RC_TUNING:
        headSerialReply(7);
        serialize8(cfg.rcRate8);
        serialize8(cfg.rcExpo8);
        serialize8(cfg.rollPitchRate);
        serialize8(cfg.yawRate);
        serialize8(cfg.dynThrPID);
        serialize8(cfg.thrMid8);
        serialize8(cfg.thrExpo8);
        break;
    case MSP_PID:
        headSerialReply(3 * PIDITEMS);
        for (i = 0; i < PIDITEMS; i++)
        {
            serialize8(cfg.P8[i]);
            serialize8(cfg.I8[i]);
            serialize8(cfg.D8[i]);
        }
        break;
    case MSP_BOX:
        if (cfg.auxChannels > 4) headSerialReply(4 * CHECKBOXITEMS);
        else headSerialReply(2 * CHECKBOXITEMS);
        for (i = 0; i < CHECKBOXITEMS; i++)
        {
            if (cfg.auxChannels > 4) serialize32(cfg.activate[i]);
            else serialize16(cfg.activate[i]);
        }
        break;
    case MSP_BOXNAMES:
        headSerialReply(sizeof(boxnames) - 1);
        serializeNames(boxnames);
        break;
    case MSP_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
    case MSP_MISC:
        headSerialReply(2);
        serialize16(0); // intPowerTrigger1
        break;
    case MSP_MOTOR_PINS:
        headSerialReply(8);
        for (i = 0; i < 8; i++)
            serialize8(i + 1);
        break;
    case MSP_WP:
        wp_no = read8();    // get the wp number
        headSerialReply(12);
        if (wp_no == 0)
        {
            serialize8(0);                   // wp0
            serialize32(GPS_home[LAT]);
            serialize32(GPS_home[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        }
        else if (wp_no == 16)
        {
            serialize8(16);                  // wp16
            serialize32(GPS_WP[LAT]);
            serialize32(GPS_WP[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        }
        break;
    case MSP_RESET_CONF:
        checkFirstTime(true);
        headSerialReply(0);
        break;
    case MSP_ACC_CALIBRATION:
        calibratingA = 400;
        headSerialReply(0);
        break;
    case MSP_MAG_CALIBRATION:
        f.CALIBRATE_MAG = 1;
        headSerialReply(0);
        break;
    case MSP_EEPROM_WRITE:
        writeParams(0);
        headSerialReply(0);
        break;
    case MSP_ACC_TRIM:
        headSerialReply(4);
        serialize16(cfg.angleTrim[PITCH]);
        serialize16(cfg.angleTrim[ROLL]);
        break;
    case MSP_DEBUG:
        headSerialReply(8);
        for (i = 0; i < 4; i++) serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;

        // new @Johannes based of MultiWii V2.21
    case MSP_SERVO_CONF:  // @Johannes:
        //s_struct((uint8_t*)&cfg.servoConf[0].min,56); // struct servo_conf_ is 7 bytes length: min:2 / max:2 / middle:2 / rate:1    ----     8 servo =>  8x7 = 56
        headSerialReply(56);
        for(i=0;i<8;i++) { // new servostructure Johannes: 
            serialize16(cfg.servoConf[i].min);
            serialize16(cfg.servoConf[i].max);
            serialize16(cfg.servoConf[i].middle);
            serialize8(cfg.servoConf[i].rate);
        }
        break;
  case MSP_SET_SERVO_CONF: // @Johannes:
        //s_struct_w((uint8_t*)&cfg.servoConf[0].min,56);	 doesn't work Johannes
        headSerialReply(0);
        for(i=0;i<8;i++) { // new servostructure Johannes: 
            cfg.servoConf[i].min =  read16();
            cfg.servoConf[i].max =  read16();
            cfg.servoConf[i].middle =  read16();
            cfg.servoConf[i].rate =  read8();
        }
        break;

		// Additional commands that are not compatible with MultiWii
	case MSP_UID:	                                          
        headSerialReply(12);
        serialize32(U_ID_0);
        serialize32(U_ID_1);
        serialize32(U_ID_2);
        break;
    case MSP_TEMPERATURE:
        headSerialReply(2);
        serialize16(telemTemperature1);
        break;
    case MSP_SONAR:
        headSerialReply(2);
        serialize16(sonarAlt);
        break;
    case MSP_FIRMWARE:
        headSerialReply(sizeof(FIRMWARE) - 1);
        serializeNames(FIRMWARE);
        break;
    default:                                                // we do not know how to handle the (valid) message, indicate error MSP $M!
        headSerialError(0);
        break;
    }
    tailSerialReply();
}
Пример #20
0
int main(void)
{
    uint8_t i;
    drv_pwm_config_t pwm_params;
    drv_adc_config_t adc_params;
    bool sensorsOK = false;
#ifdef SOFTSERIAL_LOOPBACK
    serialPort_t* loopbackPort1 = NULL;
    serialPort_t* loopbackPort2 = NULL;
#endif

    initEEPROM();
    checkFirstTime(false);
    readEEPROM();
    systemInit(mcfg.emf_avoidance);
#ifdef USE_LAME_PRINTF
    init_printf(NULL, _putc);
#endif

    activateConfig();

    // configure power ADC
    if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
        adc_params.powerAdcChannel = mcfg.power_adc_channel;
    else {
        adc_params.powerAdcChannel = 0;
        mcfg.power_adc_channel = 0;
    }

    adcInit(&adc_params);
    // Check battery type/voltage
    if (feature(FEATURE_VBAT))
        batteryInit();
    initBoardAlignment();

    // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
    sensorsSet(SENSORS_SET);
    // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
    sensorsOK = sensorsAutodetect();

    // production debug output
#ifdef PROD_DEBUG
    productionDebug();
#endif

    // if gyro was not detected due to whatever reason, we give up now.
    if (!sensorsOK)
        failureMode(3);

    LED1_ON;
    LED0_OFF;
    for (i = 0; i < 10; i++) {
        LED1_TOGGLE;
        LED0_TOGGLE;
        delay(25);
        BEEP_ON;
        delay(25);
        BEEP_OFF;
    }
    LED0_OFF;
    LED1_OFF;

    imuInit(); // Mag is initialized inside imuInit
    mixerInit(); // this will set core.useServo var depending on mixer type

    serialInit(mcfg.serial_baudrate);

    // when using airplane/wing mixer, servo/motor outputs are remapped
    if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
        pwm_params.airplane = true;
    else
        pwm_params.airplane = false;
    pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too
    pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
    pwm_params.usePPM = feature(FEATURE_PPM);
    pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
    pwm_params.useServos = core.useServo;
    pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
    pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
    pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
    pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
    if (feature(FEATURE_3D))
        pwm_params.idlePulse = mcfg.neutral3d;
    if (pwm_params.motorPwmRate > 500)
        pwm_params.idlePulse = 0; // brushed motors
    pwm_params.servoCenterPulse = mcfg.midrc;
    pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
    switch (mcfg.power_adc_channel) {
        case 1:
            pwm_params.adcChannel = PWM2;
            break;
        case 9:
            pwm_params.adcChannel = PWM8;
            break;
        default:
            pwm_params.adcChannel = 0;
            break;
    }

    pwmInit(&pwm_params);
    core.numServos = pwm_params.numServos;

    // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled
    for (i = 0; i < RC_CHANS; i++)
        rcData[i] = 1502;
    rcReadRawFunc = pwmReadRawRC;
    core.numRCChannels = MAX_INPUTS;

    if (feature(FEATURE_SERIALRX)) {
        switch (mcfg.serialrx_type) {
            case SERIALRX_SPEKTRUM1024:
            case SERIALRX_SPEKTRUM2048:
                spektrumInit(&rcReadRawFunc);
                break;
            case SERIALRX_SBUS:
                sbusInit(&rcReadRawFunc);
                break;
            case SERIALRX_SUMD:
                sumdInit(&rcReadRawFunc);
                break;
            case SERIALRX_MSP:
                mspInit(&rcReadRawFunc);
                break;
        }
    } else { // spektrum and GPS are mutually exclusive
        // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
        // gpsInit will return if FEATURE_GPS is not enabled.
        gpsInit(mcfg.gps_baudrate);
    }
#ifdef SONAR
    // sonar stuff only works with PPM
    if (feature(FEATURE_PPM)) {
        if (feature(FEATURE_SONAR))
            Sonar_init();
    }
#endif

    if (feature(FEATURE_SOFTSERIAL)) {
        //mcfg.softserial_baudrate = 19200; // Uncomment to override config value

        setupSoftSerialPrimary(mcfg.softserial_baudrate, mcfg.softserial_1_inverted);
        setupSoftSerialSecondary(mcfg.softserial_2_inverted);

#ifdef SOFTSERIAL_LOOPBACK
        loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]);
        serialPrint(loopbackPort1, "SOFTSERIAL 1 - LOOPBACK ENABLED\r\n");

        loopbackPort2 = (serialPort_t*)&(softSerialPorts[1]);
        serialPrint(loopbackPort2, "SOFTSERIAL 2 - LOOPBACK ENABLED\r\n");
#endif
        //core.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial.
    }

    if (feature(FEATURE_TELEMETRY))
        initTelemetry();

    previousTime = micros();
    if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
        calibratingA = CALIBRATING_ACC_CYCLES;
    calibratingG = CALIBRATING_GYRO_CYCLES;
    calibratingB = CALIBRATING_BARO_CYCLES;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
    f.SMALL_ANGLE = 1;

    // loopy
    while (1) {
        loop();
#ifdef SOFTSERIAL_LOOPBACK
        if (loopbackPort1) {
            while (serialTotalBytesWaiting(loopbackPort1)) {
                uint8_t b = serialRead(loopbackPort1);
                serialWrite(loopbackPort1, b);
                //serialWrite(core.mainport, 0x01);
                //serialWrite(core.mainport, b);
            };
        }

        if (loopbackPort2) {
            while (serialTotalBytesWaiting(loopbackPort2)) {
#ifndef OLIMEXINO // PB0/D27 and PB1/D28 internally connected so this would result in a continuous stream of data
                serialRead(loopbackPort2);
#else
                uint8_t b = serialRead(loopbackPort2);
                serialWrite(loopbackPort2, b);
                //serialWrite(core.mainport, 0x02);
                //serialWrite(core.mainport, b);
#endif // OLIMEXINO
            };
    }
#endif
    }
}
Пример #21
0
int main(void)
{
    uint8_t i;
    drv_pwm_config_t pwm_params;
    drv_adc_config_t adc_params;
    serialPort_t* loopbackPort = NULL;

    systemInit();
#ifdef USE_LAME_PRINTF
    init_printf(NULL, _putc);
#endif


    checkFirstTime(false);
    readEEPROM();

    // configure power ADC
    if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9))
        adc_params.powerAdcChannel = mcfg.power_adc_channel;
    else {
        adc_params.powerAdcChannel = 0;
        mcfg.power_adc_channel = 0;
    }

    adcInit(&adc_params);
    initBoardAlignment();

    // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
    sensorsSet(SENSORS_SET);

    mixerInit(); // this will set core.useServo var depending on mixer type
    // when using airplane/wing mixer, servo/motor outputs are remapped
    if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING)
        pwm_params.airplane = true;
    else
        pwm_params.airplane = false;
    //pwm_params.useUART = feature(FEATURE_GPS) || feature(FEATURE_SERIALRX); // spektrum/sbus support uses UART too
    pwm_params.useUART = feature(FEATURE_GPS);
    pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
    pwm_params.usePPM = feature(FEATURE_PPM);
    pwm_params.enableInput = !feature(FEATURE_SERIALRX); // disable inputs if using spektrum
    pwm_params.useServos = core.useServo;
    pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
    pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
    pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
    pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
    if (feature(FEATURE_3D))
        pwm_params.idlePulse = mcfg.neutral3d;
    if (pwm_params.motorPwmRate > 500)
        pwm_params.idlePulse = 0; // brushed motors
    pwm_params.servoCenterPulse = mcfg.midrc;
    pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
    switch (mcfg.power_adc_channel) {
        case 1:
            pwm_params.adcChannel = PWM2;
            break;
        case 9:
            pwm_params.adcChannel = PWM8;
            break;
        default:
            pwm_params.adcChannel = 0;
        break;
    }

    pwmInit(&pwm_params);

    // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled
    for (i = 0; i < RC_CHANS; i++)
        rcData[i] = 1502;
    rcReadRawFunc = pwmReadRawRC;
    core.numRCChannels = MAX_INPUTS;

    if (feature(FEATURE_SERIALRX)) {
        switch (mcfg.serialrx_type) {
            case SERIALRX_SPEKTRUM1024:
            case SERIALRX_SPEKTRUM2048:
                spektrumInit(&rcReadRawFunc);
                break;
            case SERIALRX_SBUS:
                sbusInit(&rcReadRawFunc);
                break;
            case SERIALRX_SUMD:
                sumdInit(&rcReadRawFunc);
                break;
            #if defined(SKYROVER)
            case SERIALRX_HEXAIRBOT:
                hexairbotInit(&rcReadRawFunc);
                break;
            #endif
        }
    } else { // spektrum and GPS are mutually exclusive
        // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2.
        // gpsInit will return if FEATURE_GPS is not enabled.
        // Sanity check below - protocols other than NMEA do not support baud rate autodetection
        if (mcfg.gps_type > 0 && mcfg.gps_baudrate < 0)
            mcfg.gps_baudrate = 0;
        gpsInit(mcfg.gps_baudrate);
    }
#ifdef SONAR
    // sonar stuff only works with PPM
    if (feature(FEATURE_PPM)) {
        if (feature(FEATURE_SONAR))
            Sonar_init();
    }
#endif

    LED1_ON;
    LED0_OFF;
    for (i = 0; i < 10; i++) {
        LED1_TOGGLE;
        LED0_TOGGLE;
        delay(25);
        BEEP_ON;
        delay(25);
        BEEP_OFF;
    }
    LED0_OFF;
    LED1_OFF;

    serialInit(mcfg.serial_baudrate);

    DEBUG_PRINT("Booting..\r\n");

    // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
    sensorsAutodetect();
    imuInit(); // Mag is initialized inside imuInit

    // Check battery type/voltage
    if (feature(FEATURE_VBAT))
        batteryInit();

    

    if (feature(FEATURE_SOFTSERIAL)) {
      setupSoftSerial1(mcfg.softserial_baudrate, mcfg.softserial_inverted);
#ifdef SOFTSERIAL_LOOPBACK
      loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
      serialPrint(loopbackPort, "LOOPBACK ENABLED\r\n");
#endif
    }

    if (feature(FEATURE_TELEMETRY))
        initTelemetry();

    previousTime = micros();
    if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL)
        calibratingA = CALIBRATING_ACC_CYCLES;
    calibratingG = CALIBRATING_GYRO_CYCLES;
    calibratingB = CALIBRATING_BARO_CYCLES;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
    f.SMALL_ANGLES_25 = 1;

    DEBUG_PRINT("Start\r\n");

    // loopy
    while (1) {
        loop();
#ifdef SOFTSERIAL_LOOPBACK
        if (loopbackPort) {
            while (serialTotalBytesWaiting(loopbackPort)) {
    
                uint8_t b = serialRead(loopbackPort);
                serialWrite(loopbackPort, b);
                //serialWrite(core.mainport, b);
            };
        }
#endif
    }
}
Пример #22
0
void eepromCLI()
{
    uint32_t c1, c2;

    uint8_t  eepromQuery = 'x';
    uint8_t  validQuery  = false;

    cliBusy = true;

    cliPortPrint("\nEntering EEPROM CLI....\n\n");

    while(true)
    {
        cliPortPrint("EEPROM CLI -> ");

        while ((cliPortAvailable() == false) && (validQuery == false));

        if (validQuery == false)
            eepromQuery = cliPortRead();

        cliPortPrint("\n");

        switch(eepromQuery)
        {
        // 'a' is the standard "print all the information" character
        case 'a': // config struct data
            c1 = eepromConfig.CRCAtEnd[0];

            zeroPIDstates();

            c2 = crc32bEEPROM(&eepromConfig, false);

            cliPortPrintF("Config structure information:\n");
            cliPortPrintF("Version          : %d\n", eepromConfig.version );
            cliPortPrintF("Size             : %d\n", sizeof(eepromConfig) );
            cliPortPrintF("CRC on last read : %08X\n", c1 );
            cliPortPrintF("Current CRC      : %08X\n", c2 );

            if ( c1 != c2 )
                cliPortPrintF("  CRCs differ. Current Config has not yet been saved.\n");

            cliPortPrintF("CRC Flags :\n");
            cliPortPrintF("  History Bad    : %s\n", eepromConfig.CRCFlags & CRC_HistoryBad ? "true" : "false" );
            validQuery = false;
            break;

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

        case 'c': // Write out to Console in Hex.  (RAM -> console)
            // we assume the flyer is not in the air, so that this is ok;
            // these change randomly when not in flight and can mistakenly
            // make one think that the in-memory eeprom struct has changed

            zeroPIDstates();

            cliPortPrintF("\n");

            cliPrintEEPROM(&eepromConfig);

            cliPortPrintF("\n");

            if (crcCheckVal != crc32bEEPROM(&eepromConfig, true))
            {
                cliPortPrint("NOTE: in-memory config CRC invalid; there have probably been changes to\n");
                cliPortPrint("      eepromConfig since the last write to flash/eeprom.\n");
            }

            validQuery = false;
            break;

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

        case 'H': // clear bad history flag
            cliPortPrintF("Clearing Bad History flag.\n");
            eepromConfig.CRCFlags &= ~CRC_HistoryBad;
            validQuery = false;
            break;

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

        case 'C': // Read in from Console in hex.  Console -> RAM
            ;
            uint32_t sz = sizeof(eepromConfig);
            eepromConfig_t e;
            uint8_t *p = (uint8_t*)&e;
            uint8_t *end = (uint8_t*)(&e + 1);
            uint32_t t = millis();
            enum { Timeout = 100 }; // timeout is in ms
            int second_nibble = 0; // 0 or 1
            char c;
            uint32_t chars_encountered = 0;

            cliPortPrintF("Ready to read in config. Expecting %d (0x%03X) bytes as %d\n",
                          sz, sz, sz * 2);
            cliPortPrintF("hexadecimal characters, optionally separated by [ \\n\\r_].\n");
            cliPortPrintF("Times out if no character is received for %dms\n", Timeout);

            memset(p, 0, end - p);

            while (p < end)
            {
                while (!cliPortAvailable() && millis() - t < Timeout) {}
                t = millis();

                c = cliPortAvailable() ? cliPortRead() : '\0';
                int8_t hex = parse_hex(c);
                int ignore = c == ' ' || c == '\n' || c == '\r' || c == '_' ? true : false;

                if (c != '\0') // assume the person isn't sending null chars
                    chars_encountered++;
                if (ignore)
                    continue;
                if (hex == -1)
                    break;

                *p |= second_nibble ? hex : hex << 4;
                p += second_nibble;
                second_nibble ^= 1;
            }

            if (c == 0)
            {
                cliPortPrintF("Did not receive enough hex chars! (got %d, expected %d)\n",
                              (p - (uint8_t*)&e) * 2 + second_nibble, sz * 2);
            }
            else if (p < end || second_nibble)
            {
                cliPortPrintF("Invalid character found at position %d: '%c' (0x%02x)",
                              chars_encountered, c, c);
            }
            // HJI else if (crcCheckVal != crc32bEEPROM(&e, true))
            // HJI {
            // HJI     cliPortPrintF("CRC mismatch! Not writing to in-memory config.\n");
            // HJI     cliPortPrintF("Here's what was received:\n\n");
            // HJI     cliPrintEEPROM(&e);
            // HJI }
            else
            {
                // check to see if the newly received eeprom config
                // actually differs from what's in-memory

                zeroPIDstates();

                int i;
                for (i = 0; i < sz; i++)
                    if (((uint8_t*)&e)[i] != ((uint8_t*)&eepromConfig)[i])
                        break;

                if (i == sz)
                {
                    cliPortPrintF("NOTE: uploaded config was identical to in-memory config.\n");
                }
                else
                {
                    eepromConfig = e;
                    cliPortPrintF("In-memory config updated!\n");
                    cliPortPrintF("NOTE: config not written to EEPROM; use 'W' to do so.\n");
                }

            }

            // eat the next 100ms (or whatever Timeout is) of characters,
            // in case the person pasted too much by mistake or something
            t = millis();
            while (millis() - t < Timeout)
                if (cliPortAvailable())
                    cliPortRead();

            validQuery = false;
            break;

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

        case 'E': // Read in from EEPROM.  (EEPROM -> RAM)
            cliPortPrint("Re-reading EEPROM.\n");
            readEEPROM();
            validQuery = false;
            break;

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

        case 'x': // exit EEPROM CLI
            cliPortPrint("\nExiting EEPROM CLI....\n\n");
            cliBusy = false;
            return;
            break;

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

        case 'W':
        case 'e': // Write out to EEPROM. (RAM -> EEPROM)
            cliPortPrint("\nWriting EEPROM Parameters....\n\n");

            validQuery = false;
            writeEEPROM();
            break;

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

        case 'f': // Write out to sdCard FILE. (RAM -> FILE)
            validQuery = false;
            break;

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

        case 'F': // Read in from sdCard FILE. (FILE -> RAM)
            validQuery = false;
            break;

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

        case 'V': // Reset EEPROM Parameters
            cliPortPrint( "\nEEPROM Parameters Reset....(not rebooting)\n" );
            checkFirstTime(true);
            validQuery = false;
            break;


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

        case '?':
            //                0         1         2         3         4         5         6         7
            //                01234567890123456789012345678901234567890123456789012345678901234567890123456789
            cliPortPrintF("\n");
            cliPortPrintF("'a' Display in-RAM config information\n");
            cliPortPrintF("'c' Write in-RAM -> Console (as Hex)      'C' Read Console (as Hex) -> in-RAM\n");
            cliPortPrintF("'e' Write in-RAM -> EEPROM                'E' Read EEPROM -> in-RAM\n");
            cliPortPrintF("'f' Write in-RAM -> sd FILE (Not yet imp) 'F' Read sd FILE -> in-RAM (Not imp)\n");
            cliPortPrintF("                                          'H' Clear CRC Bad History flag\n");
            cliPortPrintF("                                          'V' Reset in-RAM config to default.\n");
            cliPortPrintF("'x' Exit EEPROM CLI                       '?' Command Summary\n");
            cliPortPrintF("\n");
            cliPortPrintF("For compatability:                        'W' Write in-RAM -> EEPROM\n");
            cliPortPrintF("\n");
            break;

            ///////////////////////////
        }
    }
}
Пример #23
0
static void evaluateCommand(void)
{
    uint32_t i;
    uint8_t wp_no;

    switch (cmdMSP) {
    case MSP_SET_RAW_RC:
        for (i = 0; i < 8; i++)
            rcData[i] = read16();
        headSerialReply(0);
        break;
    case MSP_SET_RAW_GPS:
        f.GPS_FIX = read8();
        GPS_numSat = read8();
        GPS_coord[LAT] = read32();
        GPS_coord[LON] = read32();
        GPS_altitude = read16();
        GPS_speed = read16();
        GPS_update |= 2;        // New data signalisation to GPS functions
        headSerialReply(0);
        break;
    case MSP_SET_PID:
        for (i = 0; i < PIDITEMS; i++) {
            cfg.P8[i] = read8();
            cfg.I8[i] = read8();
            cfg.D8[i] = read8();
        }
        headSerialReply(0);
        break;
    case MSP_SET_BOX:
        for (i = 0; i < CHECKBOXITEMS; i++)
            cfg.activate[i] = read16();
        headSerialReply(0);
        break;
    case MSP_SET_RC_TUNING:
        cfg.rcRate8 = read8();
        cfg.rcExpo8 = read8();
        cfg.rollPitchRate = read8();
        cfg.yawRate = read8();
        cfg.dynThrPID = read8();
        cfg.thrMid8 = read8();
        cfg.thrExpo8 = read8();
        headSerialReply(0);
        break;
    case MSP_SET_MISC:
        headSerialReply(0);
        break;
    case MSP_IDENT:
        headSerialReply(7);
        serialize8(VERSION);                // multiwii version
        serialize8(cfg.mixerConfiguration); // type of multicopter
        serialize8(MSP_VERSION);            // MultiWii Serial Protocol Version
        serialize32(PLATFORM_32BIT);        // "capability"
        break;
    case MSP_STATUS:
        headSerialReply(10);
        serialize16(cycleTime);
        serialize16(i2cGetErrorCounter());
        serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3);
        serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM |
                    rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
                    f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU |
                    rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
        break;
    case MSP_RAW_IMU:
        headSerialReply(18);
        for (i = 0; i < 3; i++)
            serialize16(accSmooth[i]);
        for (i = 0; i < 3; i++)
            serialize16(gyroData[i]);
        for (i = 0; i < 3; i++)
            serialize16(magADC[i]);
        break;
    case MSP_SERVO:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(servo[i]);
        break;
    case MSP_MOTOR:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(motor[i]);
        break;
    case MSP_RC:
        headSerialReply(16);
        for (i = 0; i < 8; i++)
            serialize16(rcData[i]);
        break;
    case MSP_RAW_GPS:
        headSerialReply(14);
        serialize8(f.GPS_FIX);
        serialize8(GPS_numSat);
        serialize32(GPS_coord[LAT]);
        serialize32(GPS_coord[LON]);
        serialize16(GPS_altitude);
        serialize16(GPS_speed);
        break;
    case MSP_COMP_GPS:
        headSerialReply(5);
        serialize16(GPS_distanceToHome);
        serialize16(GPS_directionToHome);
        serialize8(GPS_update & 1);
        break;
    case MSP_ATTITUDE:
        headSerialReply(8);
        for (i = 0; i < 2; i++)
            serialize16(angle[i]);
        serialize16(heading);
        serialize16(headFreeModeHold);
        break;
    case MSP_ALTITUDE:
        headSerialReply(4);
        serialize32(EstAlt);
        break;
    case MSP_BAT:
        headSerialReply(3);
        serialize8(vbat);
        serialize16(0); // power meter trash
        break;
    case MSP_RC_TUNING:
        headSerialReply(7);
        serialize8(cfg.rcRate8);
        serialize8(cfg.rcExpo8);
        serialize8(cfg.rollPitchRate);
        serialize8(cfg.yawRate);
        serialize8(cfg.dynThrPID);
        serialize8(cfg.thrMid8);
        serialize8(cfg.thrExpo8);
        break;
    case MSP_PID:
        headSerialReply(3 * PIDITEMS);
        for (i = 0; i < PIDITEMS; i++) {
            serialize8(cfg.P8[i]);
            serialize8(cfg.I8[i]);
            serialize8(cfg.D8[i]);
        }
        break;
    case MSP_BOX:
        headSerialReply(2 * CHECKBOXITEMS);
        for (i = 0; i < CHECKBOXITEMS; i++)
            serialize16(cfg.activate[i]);
        break;
    case MSP_BOXNAMES:
        headSerialReply(sizeof(boxnames) - 1);
        serializeNames(boxnames);
        break;
    case MSP_PIDNAMES:
        headSerialReply(sizeof(pidnames) - 1);
        serializeNames(pidnames);
        break;
    case MSP_MISC:
        headSerialReply(2);
        serialize16(0); // intPowerTrigger1
        break;
    case MSP_MOTOR_PINS:
        headSerialReply(8);
        for (i = 0; i < 8; i++)
            serialize8(i + 1);
        break;
    case MSP_WP:
        wp_no = read8();    // get the wp number
        headSerialReply(12);
        if (wp_no == 0) {
            serialize8(0);                   // wp0
            serialize32(GPS_home[LAT]);
            serialize32(GPS_home[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        } else if (wp_no == 16) {
            serialize8(16);                  // wp16
            serialize32(GPS_hold[LAT]);
            serialize32(GPS_hold[LON]);
            serialize16(0);                  // altitude will come here
            serialize8(0);                   // nav flag will come here
        }
        break;
    case MSP_RESET_CONF:
        checkFirstTime(true);
        headSerialReply(0);
        break;
    case MSP_ACC_CALIBRATION:
        calibratingA = 400;
        headSerialReply(0);
        break;
    case MSP_MAG_CALIBRATION:
        f.CALIBRATE_MAG = 1;
        headSerialReply(0);
        break;
    case MSP_EEPROM_WRITE:
        writeParams(0);
        headSerialReply(0);
        break;
    case MSP_DEBUG:
        headSerialReply(8);
        for (i = 0; i < 4; i++)
            serialize16(debug[i]);      // 4 variables are here for general monitoring purpose
        break;
    default:                   // we do not know how to handle the (valid) message, indicate error MSP $M!
        headSerialError(0);
        break;
    }
    tailSerialReply();
}
Пример #24
0
void systemInit(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;

    // Turn on clocks for stuff we use
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,   ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,   ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,   ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,   ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1,   ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2,   ENABLE);
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1,     ENABLE);
    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2,     ENABLE);
    RCC_ClearFlag();

    // Make all GPIO in by default to save power and reduce noise
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_All;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    GPIO_Init(GPIOC, &GPIO_InitStructure);

    // Turn off JTAG port 'cause we're using the GPIO for leds
    GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);

    // Init cycle counter
    cycleCounterInit();

    // SysTick
    SysTick_Config(SystemCoreClock / 1000);

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);     // 2 bits for pre-emption priority, 2 bits for subpriority

    checkFirstTime(false);
    readEEPROM();

    ledInit();

    LED0_ON;

    initMixer();

    pwmOutputConfig.escPwmRate   = eepromConfig.escPwmRate;
    pwmOutputConfig.servoPwmRate = eepromConfig.servoPwmRate;

    cliInit(115200);
    i2cInit(I2C2);
    pwmOutputInit(&pwmOutputConfig);
    rxInit();

    delay(20000);               // 20 sec delay for sensor stabilization - probably not long enough.....

    LED1_ON;

    initAccel();
    initGyro();
    initMag();
    initPressure();

    initPID();
}
Пример #25
0
void setup() {
#if !defined(GPS_PROMINI)
    UARTInit(115200);
#endif
    LEDPIN_PINMODE;
    SHIELDLED_PINMODE;
    //POWERPIN_PINMODE;
    //BUZZERPIN_PINMODE;
    //STABLEPIN_PINMODE;
    //POWERPIN_OFF;


    /* Initialize GPIO (sets up clock) */
    GPIOInit();
    init_microsec();
    enable_microsec();
    init_timer16PWM();
    enable_PWMtimer();

    /********  special version of MultiWii to calibrate all attached ESCs ************/
#if defined(ESC_CALIB_CANNOT_FLY)
    writeAllMotors(ESC_CALIB_HIGH);
    delayMs(3000);
    writeAllMotors(ESC_CALIB_LOW);
    delayMs(500);
    while (1) {
        delayMs(5000);
        blinkLED(2,20, 2);
    }
    // statement never reached
#endif

    writeAllMotors(MINCOMMAND);
    delayMs(300);

    readEEPROM();
    checkFirstTime();
    configureReceiver();
#if defined(OPENLRSv2MULTI)
    initOpenLRS();
#endif
    initSensors();
#if defined(I2C_GPS) || defined(GPS_SERIAL) || defined(GPS_FROM_OSD)||defined(I2C_NAV)
    GPS_set_pids();
#endif
    previousTime = micros();
#if defined(GIMBAL)
    calibratingA = 400;
#endif
    calibratingG = 400;
    calibratingB = 200;  // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
#if defined(POWERMETER)
    for(uint8_t i=0;i<=PMOTOR_SUM;i++)
        pMeter[i]=0;
#endif
#if defined(ARMEDTIMEWARNING)
    ArmedTimeWarningMicroSeconds = (ARMEDTIMEWARNING *1000000);
#endif
    /************************************/
#if defined(GPS_SERIAL)
    SerialOpen(GPS_SERIAL,GPS_BAUD);
    delay(400);
    for(uint8_t i=0;i<=5;i++){
        GPS_NewData();
        LEDPIN_ON
        delay(20);
        LEDPIN_OFF
        delay(80);
    }
    if(!GPS_Present){
        SerialEnd(GPS_SERIAL);
        SerialOpen(0,SERIAL_COM_SPEED);
    }
#if !defined(GPS_PROMINI)
    GPS_Present = 1;
#endif
    GPS_Enable = GPS_Present;
#endif
    /************************************/

#if defined(I2C_GPS) || defined(TINY_GPS) || defined(GPS_FROM_OSD)|| defined(I2C_NAV)
    GPS_Enable = 1;
#endif

#if defined(LCD_ETPP) || defined(LCD_LCD03) || defined(OLED_I2C_128x64)
    initLCD();
#endif
#ifdef LCD_TELEMETRY_DEBUG
    telemetry_auto = 1;
#endif
#ifdef LCD_CONF_DEBUG
    configurationLoop();
#endif
#ifdef LANDING_LIGHTS_DDR
    init_landing_lights();
#endif

#if defined(LED_FLASHER)
    init_led_flasher();
    led_flasher_set_sequence(LED_FLASHER_SEQUENCE);
#endif
    f.SMALL_ANGLES_25=1; // important for gyro only conf

    //initialise median filter structures
#ifdef MEDFILTER
#ifdef SONAR
    initMedianFilter(&SonarFilter, 5);
#endif
#endif

    initWatchDog();
}