예제 #1
0
void initGyro(void)
{
    i2cWrite(MPU6050_ADDRESS, MPU60X0_REG_PWR_MGMT_1, H_RESET);
    delay(5);
    i2cWrite(MPU6050_ADDRESS, MPU60X0_REG_PWR_MGMT_1, PLL_WITH_Z);
    i2cWrite(MPU6050_ADDRESS, MPU60X0_REG_CONFIG, 0); //EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
    i2cWrite(MPU6050_ADDRESS, MPU60X0_REG_GYRO_CONFIG, 0x18); //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec

    //i2cWrite(MPU6050_ADDRESS, MPU60X0_REG_INT_PIN_CFG, 0x02); //INT_PIN_CFG   -- INT_LEVEL=0 ; INT_OPEN=0 ; LATCH_INT_EN=0 ; INT_RD_CLEAR=0 ; FSYNC_INT_LEVEL=0 ; FSYNC_INT_EN=0 ; I2C_BYPASS_EN=1 ; CLKOUT_EN=0

    delay(100);

    computeGyroRTBias();
}
예제 #2
0
void processFlightCommands(void)
{
    uint8_t channel;

    if ( rcActive == true )
    {
		// Read receiver commands
        for (channel = 0; channel < 8; channel++)
            rxCommand[channel] = (float)pwmRead(systemConfig.rcMap[channel]);

        rxCommand[ROLL]  -= systemConfig.midCommand;                  // Roll Range    -1000:1000
        rxCommand[PITCH] -= systemConfig.midCommand;                  // Pitch Range   -1000:1000
        rxCommand[YAW]   -= systemConfig.midCommand;                  // Yaw Range     -1000:1000

        rxCommand[THROTTLE] -= systemConfig.midCommand - MIDCOMMAND;  // Throttle Range 2000:4000
        rxCommand[AUX1]     -= systemConfig.midCommand - MIDCOMMAND;  // Aux1 Range     2000:4000
        rxCommand[AUX2]     -= systemConfig.midCommand - MIDCOMMAND;  // Aux2 Range     2000:4000
        rxCommand[AUX3]     -= systemConfig.midCommand - MIDCOMMAND;  // Aux3 Range     2000:4000
        rxCommand[AUX4]     -= systemConfig.midCommand - MIDCOMMAND;  // Aux4 Range     2000:4000
    }

    // Set past command in detent values
    for (channel = 0; channel < 3; channel++)
    	previousCommandInDetent[channel] = commandInDetent[channel];

    // Apply deadbands and set detent discretes'
    for (channel = 0; channel < 3; channel++)
    {
    	if ((rxCommand[channel] <= DEADBAND) && (rxCommand[channel] >= -DEADBAND))
        {
            rxCommand[channel] = 0;
  	        commandInDetent[channel] = true;
  	    }
        else
  	    {
  	        commandInDetent[channel] = false;
  	        if (rxCommand[channel] > 0)
  	        {
  		        rxCommand[channel] = (rxCommand[channel] - DEADBAND) * DEADBAND_SLOPE;
  	        }
  	        else
  	        {
  	            rxCommand[channel] = (rxCommand[channel] + DEADBAND) * DEADBAND_SLOPE;
  	        }
        }
    }

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

    // Check for low throttle
    if ( rxCommand[THROTTLE] < systemConfig.minCheck )
    {
		// Check for disarm command ( low throttle, left yaw ), will disarm immediately
		if ( (rxCommand[YAW] < (systemConfig.minCheck - MIDCOMMAND)) && (armed == true) )
		{
			armed = false;
			// Zero PID integrators when disarmed
			zeroIntegralError();
			zeroLastError();
		}

		// Check for gyro bias command ( low throttle, left yaw, aft pitch, right roll )
		if ( (rxCommand[YAW  ] < (systemConfig.minCheck - MIDCOMMAND)) &&
		     (rxCommand[ROLL ] > (systemConfig.maxCheck - MIDCOMMAND)) &&
		     (rxCommand[PITCH] < (systemConfig.minCheck - MIDCOMMAND)) )
		{
			computeGyroRTBias();
			pulseMotors(3);
		}

		// Check for arm command ( low throttle, right yaw), must be present for 1 sec before arming
		if ( (rxCommand[YAW] > (systemConfig.maxCheck - MIDCOMMAND) ) && (armed == false) )
		{
			armingTimer++;

			if ( armingTimer > 50 )
			{
				zeroIntegralError();
				armed = true;
				armingTimer = 0;
			}
		}
		else
		{
			armingTimer = 0;
		}
	}

	// Check for armed true and throttle command > minThrottle
    if ( (armed == true) && (rxCommand[THROTTLE] > systemConfig.minThrottle) )
    	holdIntegrators = false;
    else
    	holdIntegrators = true;

    // Check AUX1 for rate or attitude mode
	if ( rxCommand[AUX1] > MIDCOMMAND )
	{
		flightMode = ATTITUDE;
	}
	else
	{
		flightMode = RATE;
	}
}
예제 #3
0
void sensorCLI()
{
    uint8_t  sensorQuery;
    uint8_t  validQuery = false;

    cliBusy = true;

    cliPrint("\nEntering Sensor CLI....\n\n");

    while(true)
    {
        cliPrint("Sensor CLI -> ");

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

		if (validQuery == false)
		    sensorQuery = cliRead();

		cliPrint("\n");

		switch(sensorQuery)
		{
            ///////////////////////////

            case 'a': // Sensor Data
                cliPrintF("\nAccel Scale Factor:        %9.4f, %9.4f, %9.4f\n", eepromConfig.accelScaleFactor[XAXIS],
                                                		                        eepromConfig.accelScaleFactor[YAXIS],
                                                		                        eepromConfig.accelScaleFactor[ZAXIS]);
                cliPrintF("Accel Bias:                %9.4f, %9.4f, %9.4f\n",   eepromConfig.accelBias[XAXIS],
                                                		                        eepromConfig.accelBias[YAXIS],
                                                		                        eepromConfig.accelBias[ZAXIS]);
                cliPrintF("Gyro TC Bias Slope:        %9.4f, %9.4f, %9.4f\n",   eepromConfig.gyroTCBiasSlope[ROLL ],
                                                                		        eepromConfig.gyroTCBiasSlope[PITCH],
                                                                		        eepromConfig.gyroTCBiasSlope[YAW  ]);
                cliPrintF("Gyro TC Bias Intercept:    %9.4f, %9.4f, %9.4f\n",   eepromConfig.gyroTCBiasIntercept[ROLL ],
                		                                                        eepromConfig.gyroTCBiasIntercept[PITCH],
                		                                                        eepromConfig.gyroTCBiasIntercept[YAW  ]);
                cliPrintF("Mag Bias:                  %9.4f, %9.4f, %9.4f\n",   eepromConfig.magBias[XAXIS],
                                                		                        eepromConfig.magBias[YAXIS],
                                                		                        eepromConfig.magBias[ZAXIS]);
                cliPrintF("Accel One G:               %9.4f\n",   accelOneG);
                cliPrintF("Accel Cutoff:              %9.4f\n",   eepromConfig.accelCutoff);
                cliPrintF("KpAcc (MARG):              %9.4f\n",   eepromConfig.KpAcc);
                cliPrintF("KiAcc (MARG):              %9.4f\n",   eepromConfig.KiAcc);
                cliPrintF("KpMag (MARG):              %9.4f\n",   eepromConfig.KpMag);
                cliPrintF("KiMag (MARG):              %9.4f\n",   eepromConfig.KiMag);
                cliPrintF("hdot est/h est Comp Fil A: %9.4f\n",   eepromConfig.compFilterA);
                cliPrintF("hdot est/h est Comp Fil B: %9.4f\n\n", eepromConfig.compFilterB);

                validQuery = false;
                break;

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

            case 'b': // Accel Calibration
                accelCalibration();

                sensorQuery = 'a';
                validQuery = true;
                break;

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

            case 'c': // Magnetometer Calibration
                magCalibration();

                sensorQuery = 'a';
                validQuery = true;
                break;

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

            case 'd': // GyroTemp Calibration
                gyroTempCalibration();
                computeGyroRTBias();

                sensorQuery = 'a';
                validQuery = true;
                break;

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

			case 'x':
			    cliPrint("\nExiting Sensor CLI....\n\n");
			    cliBusy = false;
			    return;
			    break;

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

            case 'B': // Accel Cutoff
                eepromConfig.accelCutoff = readFloatCLI();

                sensorQuery = 'a';
                validQuery = true;
        	    break;

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

            case 'C': // kpAcc, kiAcc
                eepromConfig.KpAcc = readFloatCLI();
                eepromConfig.KiAcc = readFloatCLI();

                sensorQuery = 'a';
                validQuery = true;
                break;

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

            case 'D': // kpMag, kiMag
                eepromConfig.KpMag = readFloatCLI();
                eepromConfig.KiMag = readFloatCLI();

                sensorQuery = 'a';
                validQuery = true;
                break;

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

            case 'E': // h dot est/h est Comp Filter A/B
                eepromConfig.compFilterA = readFloatCLI();
                eepromConfig.compFilterB = readFloatCLI();

                sensorQuery = 'a';
                validQuery = true;
                break;

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

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

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

			case '?':
			   	cliPrint("\n");
			   	cliPrint("'a' Display Sensor Data\n");
			   	cliPrint("'b' Accel Calibration                      'B' Set Accel Cutoff                     BAccelCutoff\n");
			   	cliPrint("'c' Magnetometer Calibration               'C' Set kpAcc/kiAcc                      CKpAcc;KiAcc\n");
			   	cliPrint("'d' Gyro Temp Calibration                  'D' Set kpMag/kiMag                      DKpMag;KiMag\n");
			   	cliPrint("                                           'E' Set h dot est/h est Comp Filter A/B  EA;B\n");
			   	cliPrint("                                           'W' Write EEPROM Parameters\n");
			   	cliPrint("'x' Exit Sensor CLI                        '?' Command Summary\n");
			    cliPrint("\n");
	    	    break;

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

}
예제 #4
0
void updateCommands(void)
{
    uint8_t i;
    static uint8_t commandDelay;

    if(!featureGet(FEATURE_SPEKTRUM) || spektrumFrameComplete())
        computeRC();
    
    // Ground Routines
    if(rcData[THROTTLE] < cfg.minCheck) {
        zeroPIDs(); // Stops integrators from exploding on the ground
        
        if(cfg.auxActivate[OPT_ARM] > 0) {
            if(auxOptions[OPT_ARM] && mode.OK_TO_ARM) { // AUX Arming
                mode.ARMED = 1;
                headfreeReference = stateData.heading;
            } else if(mode.ARMED){ // AUX Disarming
                mode.ARMED = 0;
            }
        } else if(rcData[YAW] > cfg.maxCheck && !mode.ARMED) { // Stick Arming
            if(commandDelay++ == 20) {
                mode.ARMED = 1;
                headfreeReference = stateData.heading;
            }
        } else if(rcData[YAW] < cfg.minCheck && mode.ARMED) { // Stick Disarming
            if(commandDelay++ == 20) {
                mode.ARMED = 0;
            }
        } else if(rcData[YAW] < cfg.minCheck && rcData[PITCH] > cfg.minCheck && !mode.ARMED) {
            if(commandDelay++ == 20) {
                computeGyroRTBias();
        		//pulseMotors(3);
        		// GPS Reset
            }
        } else {
            commandDelay = 0;
        }
    } else if(rcData[THROTTLE] > cfg.maxCheck && !mode.ARMED) {
        if(rcData[YAW] > cfg.maxCheck && rcData[PITCH] < cfg.minCheck) {
            if(commandDelay++ == 20) {
                magCalibration();
            }
        } else if(rcData[YAW] < cfg.minCheck && rcData[PITCH] < cfg.minCheck) {
            if(commandDelay++ == 20) {
                accelCalibration();
                pulseMotors(3);
            }
        } else if (rcData[PITCH] > cfg.maxCheck) {
                cfg.angleTrim[PITCH] += 0.01;
                writeParams();
        } else if (rcData[PITCH] < cfg.minCheck) {
                cfg.angleTrim[PITCH] -= 0.01;
                writeParams();
        } else if (rcData[ROLL] > cfg.maxCheck) {
                cfg.angleTrim[ROLL] += 0.01;
                writeParams();
        } else if (rcData[ROLL] < cfg.minCheck) {
                cfg.angleTrim[ROLL] -= 0.01;
                writeParams();
        } else {
            commandDelay = 0;
        }
    }
    
    // Failsafe
    if(featureGet(FEATURE_FAILSAFE)) {
        if(failsafeCnt > cfg.failsafeOnDelay && mode.ARMED) {
            // Stabilise and set Throttle to failsafe level
            for(i = 0; i < 3; ++i) {
                rcData[i] = cfg.midCommand;
            }
            rcData[THROTTLE] = cfg.failsafeThrottle;
            mode.FAILSAFE = 1;
            if(failsafeCnt > cfg.failsafeOffDelay + cfg.failsafeOnDelay) {
                // Disarm
                mode.ARMED = 0;
                // you will have to switch off first to rearm
                mode.OK_TO_ARM = 0;  
            }
            if(failsafeCnt > cfg.failsafeOnDelay && !mode.ARMED) {
                mode.ARMED = 0;
                // you will have to switch off first to rearm
                mode.OK_TO_ARM = 0;
            }
            ++failsafeCnt;
        } else {
            mode.FAILSAFE = 0;
        }
    }

    // Aux Options
    uint16_t auxOptionMask = 0;
    
    for(i = 0; i < AUX_CHANNELS; ++i) {
        auxOptionMask |= (rcData[AUX1 + i] < cfg.minCheck) << (3 * i) |
                        (rcData[AUX1 + i] > cfg.minCheck && rcData[i] < cfg.minCheck) << (3 * i + 1) |
                        (rcData[AUX1 + i] > cfg.maxCheck) << (3 * i + 2);
    }
    
    for(i = 0; i < AUX_OPTIONS; ++i) {
        auxOptions[i] = (auxOptionMask & cfg.auxActivate[i]) > 0;
    }
    
    if(auxOptions[OPT_ARM] == 0) {
        mode.OK_TO_ARM = 1;
    }
    
    // Passthrough
    if(auxOptions[OPT_PASSTHRU]) {
        mode.PASSTHRU_MODE = 1;
    } else {
        mode.PASSTHRU_MODE = 0;
    }
    
    // Level - TODO Add failsafe and ACC check
    if(auxOptions[OPT_LEVEL] || mode.FAILSAFE) { // 
        if(!mode.LEVEL_MODE) {
            zeroPID(&pids[ROLL_LEVEL_PID]);
            zeroPID(&pids[PITCH_LEVEL_PID]);
            mode.LEVEL_MODE = 1;
        }
    } else {
        mode.LEVEL_MODE = 0;
    }
    
    // Heading 
    if(auxOptions[OPT_HEADING]) {
        if(!mode.HEADING_MODE) {
            mode.HEADING_MODE = 1;
            headingHold = stateData.heading;
        }
    } else {
        mode.HEADING_MODE = 0;
    }
    
    // Headfree
    if(auxOptions[OPT_HEADFREE]) {
        if(!mode.HEADFREE_MODE) {
            mode.HEADFREE_MODE = 1;
            headfreeReference = stateData.heading;
        }
    } else {
        mode.HEADFREE_MODE = 0;
    }
    
    if(auxOptions[OPT_HEADFREE_REF]) {
        headfreeReference = stateData.heading;
    }
    
    // GPS GOES HERE
    
    uint8_t axis;
    uint16_t tmp, tmp2;
    
    // Apply deadbands, rates and expo    
    for (axis = 0; axis < 3; axis++) {
        lastCommandInDetent[axis] = commandInDetent[axis];
        tmp = min(abs(rcData[axis] - cfg.midCommand), 500);
        
        if (tmp > cfg.deadBand[axis]) {
            tmp -= cfg.deadBand[axis];
            commandInDetent[axis] = false;
        } else {
            tmp = 0;
            commandInDetent[axis] = true;
        }
    
        if(axis != 2) { // Roll and Pitch
            tmp2 = tmp / 100;
            command[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
        } else { // Yaw
            command[axis] = tmp;
        }
        
        if (rcData[axis] < cfg.midCommand)
            command[axis] = -command[axis];
    }

    tmp = constrain(rcData[THROTTLE], cfg.minCheck, 2000);
    tmp = (uint32_t) (tmp - cfg.minCheck) * 1000 / (2000 - cfg.minCheck);       // [MINCHECK;2000] -> [0;1000]
    tmp2 = tmp / 100;
    command[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]

    // This will force a reset
    if(fabs(command[THROTTLE] - altitudeThrottleHold) > THROTTLE_HOLD_DEADBAND)
        mode.ALTITUDE_MODE = 0;
    
    // Altitude TODO Add barometer check
    if(auxOptions[OPT_ALTITUDE]) {
        if(!mode.ALTITUDE_MODE) {
            mode.ALTITUDE_MODE = 1;
            altitudeThrottleHold = command[THROTTLE];
            altitudeHold = stateData.altitude;
            zeroPID(&pids[ALTITUDE_PID]);
        }
    } else {
        mode.ALTITUDE_MODE = 0;
    }
}