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();
}
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;
}
}
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;
///////////////////////////
}
}
}
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;
}
}