void readMPU6000(void)
{
setSPIdivisor(MPU6000_SPI, 2); // 18 MHz SPI clock
ENABLE_MPU6000;
spiTransfer(MPU6000_SPI, MPU6000_ACCEL_XOUT_H | 0x80);
rawAccel[XAXIS].bytes[1] = spiTransfer(MPU6000_SPI, 0x00);
rawAccel[XAXIS].bytes[0] = spiTransfer(MPU6000_SPI, 0x00);
rawAccel[YAXIS].bytes[1] = spiTransfer(MPU6000_SPI, 0x00);
rawAccel[YAXIS].bytes[0] = spiTransfer(MPU6000_SPI, 0x00);
rawAccel[ZAXIS].bytes[1] = spiTransfer(MPU6000_SPI, 0x00);
rawAccel[ZAXIS].bytes[0] = spiTransfer(MPU6000_SPI, 0x00);
rawMPU6000Temperature.bytes[1] = spiTransfer(MPU6000_SPI, 0x00);
rawMPU6000Temperature.bytes[0] = spiTransfer(MPU6000_SPI, 0x00);
rawGyro[ROLL ].bytes[1] = spiTransfer(MPU6000_SPI, 0x00);
rawGyro[ROLL ].bytes[0] = spiTransfer(MPU6000_SPI, 0x00);
rawGyro[PITCH].bytes[1] = spiTransfer(MPU6000_SPI, 0x00);
rawGyro[PITCH].bytes[0] = spiTransfer(MPU6000_SPI, 0x00);
rawGyro[YAW ].bytes[1] = spiTransfer(MPU6000_SPI, 0x00);
rawGyro[YAW ].bytes[0] = spiTransfer(MPU6000_SPI, 0x00);
DISABLE_MPU6000;
}
void readSystemEEPROM(void)
{
uint8_t* start = (uint8_t*)(&systemConfig);
uint8_t* end = (uint8_t*)(&systemConfig + 1);
///////////////////////////////////
setSPIdivisor(EEPROM_SPI, 2); // 18 MHz SPI clock
systemConfigaddr.value = SYSTEM_EEPROM_ADDR;
///////////////////////////////////
ENABLE_EEPROM;
spiTransfer(EEPROM_SPI, READ_DATA);
spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[2]);
spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[1]);
spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[0]);
while (start < end)
*start++ = spiTransfer(EEPROM_SPI, 0x00);
DISABLE_EEPROM;
delayMicroseconds(2);
///////////////////////////////////
if ( crcCheckVal != crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig CRC32B[systemConfig]]
(uint32_t*)(&systemConfig + 1)))
{
evrPush(EVR_SystemCRCFail,0);
systemConfig.CRCFlags |= CRC_HistoryBad;
}
else if ( systemConfig.CRCFlags & CRC_HistoryBad )
{
evrPush(EVR_ConfigBadSystemHistory,0);
}
///////////////////////////////////
if (systemConfig.yawDirection >= 0)
systemConfig.yawDirection = 1.0f;
else
systemConfig.yawDirection = -1.0f;
}
void readSensorEEPROM(void)
{
uint8_t* start = (uint8_t*)(&sensorConfig);
uint8_t* end = (uint8_t*)(&sensorConfig + 1);
///////////////////////////////////
setSPIdivisor(EEPROM_SPI, 2); // 18 MHz SPI clock
sensorConfigAddr.value = SENSOR_EEPROM_ADDR;
///////////////////////////////////
ENABLE_EEPROM;
spiTransfer(EEPROM_SPI, READ_DATA);
spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[2]);
spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[1]);
spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[0]);
while (start < end)
*start++ = spiTransfer(EEPROM_SPI, 0x00);
DISABLE_EEPROM;
delayMicroseconds(2);
///////////////////////////////////
if ( crcCheckVal != crc32B((uint32_t*)(&sensorConfig), // CRC32B[sensorConfig CRC32B[sensorConfig]]
(uint32_t*)(&sensorConfig + 1)))
{
evrPush(EVR_SensorCRCFail,0);
sensorConfig.CRCFlags |= CRC_HistoryBad;
}
else if ( sensorConfig.CRCFlags & CRC_HistoryBad )
{
evrPush(EVR_ConfigBadSensorHistory,0);
}
///////////////////////////////////
accConfidenceDecay = 1.0f / sqrt(sensorConfig.accelCutoff);
}
bool readHMC5983(int16_t *data)
{
uint8_t buf[6];
int16_t dataBuffer[3];
setSPIdivisor(8); // 4.5 MHz SPI clock
ENABLE_HMC5983;
spiTransferByte(HMC5983_DATA_X_MSB_REG + 0x80 + 0x40);
spiTransfer(buf, NULL, 6);
DISABLE_HMC5983;
dataBuffer[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
dataBuffer[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
dataBuffer[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
alignSensors(dataBuffer, data, magAlign);
// check for valid data
if (dataBuffer[X] == -4096 || dataBuffer[Y] == -4096 || dataBuffer[Z] == -4096) {
return false;
} else {
return true;
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
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("\n");
cliPrintF("External HMC5883 in use: %s\n", eepromConfig.externalHMC5883 ? "Yes" : "No");
cliPrintF("External MS5611 in use: %s\n", eepromConfig.externalMS5611 ? "Yes" : "No");
cliPrintF("\n");
cliPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
cliPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPrintF("Gyro Temp Comp 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", eepromConfig.compFilterB);
cliPrint("MPU6000 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPrint("42 Hz\n");
break;
}
cliPrint("Magnetic Variation: ");
if (eepromConfig.magVar >= 0.0f)
cliPrintF("E%6.4f\n\n", eepromConfig.magVar * R2D);
else
cliPrintF("W%6.4f\n\n", -eepromConfig.magVar * R2D);
if (eepromConfig.verticalVelocityHoldOnly)
cliPrint("Vertical Velocity Hold Only\n\n");
else
cliPrint("Vertical Velocity and Altitude Hold\n\n");
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'e': // Toggle External HMC5883 Use
if (eepromConfig.externalHMC5883)
eepromConfig.externalHMC5883 = false;
else
eepromConfig.externalHMC5883 = true;
initMag();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'f': // Toggle External MS5611 Use
if (eepromConfig.externalMS5611)
eepromConfig.externalMS5611 = false;
else
eepromConfig.externalMS5611 = true;
initPressure();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock (within 20 +/- 10%)
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, eepromConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
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 'M': // Magnetic Variation
eepromConfig.magVar = readFloatCLI() * D2R;
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 'A' Set MPU6000 DLPF A0 thru 3, see aq32Plus.h\n");
cliPrint("'b' MPU6000 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
cliPrint(" 'D' Set kpMag/kiMag DkpMag;kiMag\n");
cliPrint("'e' Toggle External HMC5883 State 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPrint("'f' Toggle External MS5611 State 'M' Set Mag Variation (+ East, - West) MMagVar\n");
cliPrint("'v' Toggle Vertical Velocity Hold Only\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n\n");
validQuery = false;
break;
///////////////////////////
}
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPortPrint("Sensor CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliPortRead();
cliPortPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", sensorConfig.accelTCBiasSlope[XAXIS],
sensorConfig.accelTCBiasSlope[YAXIS],
sensorConfig.accelTCBiasSlope[ZAXIS]);
cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", sensorConfig.accelTCBiasIntercept[XAXIS],
sensorConfig.accelTCBiasIntercept[YAXIS],
sensorConfig.accelTCBiasIntercept[ZAXIS]);
cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", sensorConfig.gyroTCBiasSlope[ROLL ],
sensorConfig.gyroTCBiasSlope[PITCH],
sensorConfig.gyroTCBiasSlope[YAW ]);
cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", sensorConfig.gyroTCBiasIntercept[ROLL ],
sensorConfig.gyroTCBiasIntercept[PITCH],
sensorConfig.gyroTCBiasIntercept[YAW ]);
cliPortPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", sensorConfig.magBias[XAXIS],
sensorConfig.magBias[YAXIS],
sensorConfig.magBias[ZAXIS]);
cliPortPrintF("Accel One G: %9.4f\n", accelOneG);
cliPortPrintF("Accel Cutoff: %9.4f\n", sensorConfig.accelCutoff);
cliPortPrintF("KpAcc (MARG): %9.4f\n", sensorConfig.KpAcc);
cliPortPrintF("KiAcc (MARG): %9.4f\n", sensorConfig.KiAcc);
cliPortPrintF("KpMag (MARG): %9.4f\n", sensorConfig.KpMag);
cliPortPrintF("KiMag (MARG): %9.4f\n", sensorConfig.KiMag);
cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", sensorConfig.compFilterA);
cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", sensorConfig.compFilterB);
cliPortPrint("MPU6000 DLPF: ");
switch(sensorConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPortPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPortPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPortPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPortPrint("42 Hz\n");
break;
}
cliPortPrint("Sensor Orientation: ");
switch(sensorConfig.sensorOrientation)
{
case 1:
cliPortPrint("Normal\n\n");
break;
case 2:
cliPortPrint("Rotated 90 Degrees CW\n\n");
break;
case 3:
cliPortPrint("Rotated 180 Degrees\n\n");
break;
case 4:
cliPortPrint("Rotated 90 Degrees CCW\n\n");
break;
default:
cliPortPrint("Normal\n\n");
}
if (sensorConfig.gpsVelocityHoldOnly)
cliPortPrint("GPS Velocity Hold Only\n\n");
else
cliPortPrint("GPS Velocity and Position Hold\n\n");
if (sensorConfig.verticalVelocityHoldOnly)
cliPortPrint("Vertical Velocity Hold Only\n\n");
else
cliPortPrint("Vertical Velocity and Altitude Hold\n\n");
cliPortPrintF("Voltage Monitor Scale: %9.4f\n", sensorConfig.voltageMonitorScale);
cliPortPrintF("Voltage Monitor Bias: %9.4f\n", sensorConfig.voltageMonitorBias);
cliPortPrintF("Number of Battery Cells: %1d\n\n", sensorConfig.batteryCells);
cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", sensorConfig.batteryLow);
cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", sensorConfig.batteryVeryLow);
cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", sensorConfig.batteryMaxLow);
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'g': // Toggle GPS Velocity Hold Only
if (sensorConfig.gpsVelocityHoldOnly)
sensorConfig.gpsVelocityHoldOnly = false;
else
sensorConfig.gpsVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (sensorConfig.verticalVelocityHoldOnly)
sensorConfig.verticalVelocityHoldOnly = false;
else
sensorConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, sensorConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
sensorConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
sensorConfig.KpAcc = readFloatCLI();
sensorConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
sensorConfig.KpMag = readFloatCLI();
sensorConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
sensorConfig.compFilterA = readFloatCLI();
sensorConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Sensor Orientation
sensorConfig.sensorOrientation = (uint8_t)readFloatCLI();
orientSensors();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Set Voltage Monitor Trip Points
sensorConfig.batteryLow = readFloatCLI();
sensorConfig.batteryVeryLow = readFloatCLI();
sensorConfig.batteryMaxLow = readFloatCLI();
thresholds[BATTERY_LOW].value = sensorConfig.batteryLow;
thresholds[BATTERY_VERY_LOW].value = sensorConfig.batteryVeryLow;
thresholds[BATTRY_MAX_LOW].value = sensorConfig.batteryMaxLow;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'V': // Set Voltage Monitor Parameters
sensorConfig.voltageMonitorScale = readFloatCLI();
sensorConfig.voltageMonitorBias = readFloatCLI();
sensorConfig.batteryCells = (uint8_t)readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write Sensor EEPROM Parameters
cliPortPrint("\nWriting Sensor EEPROM Parameters....\n\n");
writeSensorEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3\n");
cliPortPrint("'b' MPU6000 Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CKpAcc;KiAcc\n");
cliPortPrint(" 'D' Set kpMag/kiMag DKpMag;KiMag\n");
cliPortPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPortPrint(" 'F' Set Sensor Orientation F1 thru 4\n");
cliPortPrint("'g' Toggle GPS Velocity Hold Only 'M' Set Voltage Monitor Trip Points Mlow;veryLow;maxLow\n");
cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPortPrint("\n");
break;
///////////////////////////
}
}
}
void initMPU6000(void)
{
///////////////////////////////////
ENABLE_MPU6000;
spiTransfer(MPU6000_SPI, MPU6000_PWR_MGMT_1); // Device Reset
spiTransfer(MPU6000_SPI, BIT_H_RESET);
DISABLE_MPU6000;
delay(150);
ENABLE_MPU6000;
spiTransfer(MPU6000_SPI, MPU6000_PWR_MGMT_1); // Clock Source PPL with Z axis gyro reference
spiTransfer(MPU6000_SPI, MPU_CLK_SEL_PLLGYROZ);
DISABLE_MPU6000;
delayMicroseconds(1);
ENABLE_MPU6000;
spiTransfer(MPU6000_SPI, MPU6000_USER_CTRL); // Disable Primary I2C Interface
spiTransfer(MPU6000_SPI, BIT_I2C_IF_DIS);
DISABLE_MPU6000;
delayMicroseconds(1);
ENABLE_MPU6000;
spiTransfer(MPU6000_SPI, MPU6000_PWR_MGMT_2);
spiTransfer(MPU6000_SPI, 0x00);
DISABLE_MPU6000;
delayMicroseconds(1);
ENABLE_MPU6000;
spiTransfer(MPU6000_SPI, MPU6000_SMPLRT_DIV); // Accel Sample Rate 1000 Hz, Gyro Sample Rate 8000 Hz
spiTransfer(MPU6000_SPI, 0x00);
DISABLE_MPU6000;
delayMicroseconds(1);
ENABLE_MPU6000;
spiTransfer(MPU6000_SPI, MPU6000_CONFIG); // Accel and Gyro DLPF Setting
spiTransfer(MPU6000_SPI, eepromConfig.dlpfSetting);
DISABLE_MPU6000;
delayMicroseconds(1);
ENABLE_MPU6000;
spiTransfer(MPU6000_SPI, MPU6000_ACCEL_CONFIG); // Accel +/- 4 G Full Scale
spiTransfer(MPU6000_SPI, BITS_FS_4G);
DISABLE_MPU6000;
delayMicroseconds(1);
ENABLE_MPU6000;
spiTransfer(MPU6000_SPI, MPU6000_GYRO_CONFIG); // Gyro +/- 1000 DPS Full Scale
spiTransfer(MPU6000_SPI, BITS_FS_1000DPS);
DISABLE_MPU6000;
///////////////////////////////////
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
///////////////////////////////////
delay(100);
computeMPU6000RTData();
}
bool hmc5983DetectSpi(sensor_t *mag)
{
int16_t data[3];
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
spiResetErrorCounter();
uint8_t hmc5983Status = 0;
uint8_t i, calibrationSteps = 2;
setSPIdivisor( 8); // 4.5 MHz SPI clock
ENABLE_HMC5983;
spiTransferByte(HMC5983_CONFIG_REG_A);
spiTransferByte(TS | SENSOR_CONFIG | POSITIVE_BIAS_CONFIGURATION);
DISABLE_HMC5983;
delay(50);
ENABLE_HMC5983;
spiTransferByte(HMC5983_CONFIG_REG_B);
spiTransferByte(SENSOR_INIT_GAIN);
DISABLE_HMC5983;
delay(20);
readHMC5983(data);
for (i = 0; i < 10; i++) {
ENABLE_HMC5983;
spiTransferByte(HMC5983_MODE_REG);
spiTransferByte(OP_MODE_SINGLE);
DISABLE_HMC5983;
delay(20);
while ((hmc5983Status && STATUS_RDY) == 0x00) {
ENABLE_HMC5983;
spiTransferByte(HMC5983_STATUS_REG + 0x80);
hmc5983Status = spiTransferByte(0x00);
DISABLE_HMC5983;
}
if (!readHMC5983(data)) // check for valid data
break; // breaks out of the for loop if sensor saturated.
xyz_total[X] += data[X];
xyz_total[Y] += data[Y];
xyz_total[Z] += data[Z];
}
if (i == 10) // loop completed
calibrationSteps--;
ENABLE_HMC5983;
spiTransferByte(HMC5983_CONFIG_REG_A);
spiTransferByte(SENSOR_CONFIG | NEGATIVE_BIAS_CONFIGURATION);
DISABLE_HMC5983;
delay(20);
readHMC5983(data);
for (i = 0; i < 10; i++) {
ENABLE_HMC5983;
spiTransferByte(HMC5983_MODE_REG);
spiTransferByte(OP_MODE_SINGLE);
DISABLE_HMC5983;
delay(20);
while ((hmc5983Status && STATUS_RDY) == 0x00) {
ENABLE_HMC5983;
spiTransferByte(HMC5983_STATUS_REG + 0x80);
hmc5983Status = spiTransferByte(0x00);
DISABLE_HMC5983;
}
if (!readHMC5983(data))
break;
xyz_total[X] += data[X];
xyz_total[Y] += data[Y];
xyz_total[Z] += data[Z];
}
if (i == 10) // loop completed
calibrationSteps--;
ENABLE_HMC5983;
spiTransferByte(HMC5983_CONFIG_REG_A);
spiTransferByte(TS | SENSOR_CONFIG | NORMAL_MEASUREMENT_CONFIGURATION);
DISABLE_HMC5983;
ENABLE_HMC5983;
spiTransferByte(HMC5983_CONFIG_REG_B);
spiTransferByte(SENSOR_GAIN);
DISABLE_HMC5983;
delay(50);
ENABLE_HMC5983;
spiTransferByte(HMC5983_MODE_REG);
spiTransferByte(OP_MODE_CONTINUOUS);
DISABLE_HMC5983;
delay(20);
readHMC5983(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter() != 0) {
spiResetErrorCounter();
return false;
}
// magGain[X] = fabsf(660.0f * HMC5983_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
// magGain[Y] = fabsf(660.0f * HMC5983_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
// magGain[Z] = fabsf(660.0f * HMC5983_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
mag->init = hmc5983Init;
mag->read = readHMC5983;
return (calibrationSteps == 0);
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPortPrint("Sensor CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliPortRead();
cliPortPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPortPrintF("\n");
cliPortPrintF("External HMC5883 in use: %s\n", eepromConfig.externalHMC5883 ? "Yes" : "No");
cliPortPrintF("External MS5611 in use: %s\n", eepromConfig.externalMS5611 ? "Yes" : "No");
cliPortPrintF("MXR9150 Accel in use: %s\n", eepromConfig.useMXR9150 ? "Yes" : "No");
cliPortPrintF("\n");
if (eepromConfig.useMXR9150 == true)
{
cliPortPrintF("MXR Accel Bias: %9.3f, %9.3f, %9.3f\n", eepromConfig.accelBiasMXR[XAXIS],
eepromConfig.accelBiasMXR[YAXIS],
eepromConfig.accelBiasMXR[ZAXIS]);
cliPortPrintF("MXR Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", eepromConfig.accelScaleFactorMXR[XAXIS],
eepromConfig.accelScaleFactorMXR[YAXIS],
eepromConfig.accelScaleFactorMXR[ZAXIS]);
}
else
{
cliPortPrintF("MPU Accel Bias: %9.3f, %9.3f, %9.3f\n", eepromConfig.accelBiasMPU[XAXIS],
eepromConfig.accelBiasMPU[YAXIS],
eepromConfig.accelBiasMPU[ZAXIS]);
cliPortPrintF("MPU Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", eepromConfig.accelScaleFactorMPU[XAXIS],
eepromConfig.accelScaleFactorMPU[YAXIS],
eepromConfig.accelScaleFactorMPU[ZAXIS]);
}
cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPortPrintF("Internal Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPortPrintF("External Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS + 3],
eepromConfig.magBias[YAXIS + 3],
eepromConfig.magBias[ZAXIS + 3]);
cliPortPrintF("Accel One G: %9.4f\n", accelOneG);
cliPortPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPortPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPortPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPortPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPortPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB);
cliPortPrint("MPU6000 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPortPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPortPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPortPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPortPrint("42 Hz\n");
break;
}
cliPortPrint("Sensor Orientation: ");
switch(eepromConfig.sensorOrientation)
{
case 1:
cliPortPrint("Normal\n");
break;
case 2:
cliPortPrint("Rotated 90 Degrees CW\n");
break;
case 3:
cliPortPrint("Rotated 180 Degrees\n");
break;
case 4:
cliPortPrint("Rotated 90 Degrees CCW\n");
break;
default:
cliPortPrint("Normal\n");
}
if (eepromConfig.verticalVelocityHoldOnly)
cliPortPrint("Vertical Velocity Hold Only\n\n");
else
cliPortPrint("Vertical Velocity and Altitude Hold\n\n");
cliPortPrintF("Voltage Monitor Scale: %9.4f\n", eepromConfig.voltageMonitorScale);
cliPortPrintF("Voltage Monitor Bias: %9.4f\n", eepromConfig.voltageMonitorBias);
cliPortPrintF("Number of Battery Cells: %1d\n\n", eepromConfig.batteryCells);
cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", eepromConfig.batteryLow);
cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", eepromConfig.batteryVeryLow);
cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", eepromConfig.batteryMaxLow);
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'd': // Accel Bias and Scale Factor Calibration
if (eepromConfig.useMXR9150 == true)
accelCalibrationMXR();
else
accelCalibrationMPU();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'e': // Toggle External HMC5883 Use
if (eepromConfig.externalHMC5883 == 0)
eepromConfig.externalHMC5883 = 3;
else
eepromConfig.externalHMC5883 = 0;
initMag();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'f': // Toggle External MS5611 Use
if (eepromConfig.externalMS5611)
eepromConfig.externalMS5611 = false;
else
eepromConfig.externalMS5611 = true;
initPressure();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'g': // Toggle MXR9150 Use
if (eepromConfig.useMXR9150)
eepromConfig.useMXR9150 = false;
else
eepromConfig.useMXR9150 = true;
computeMPU6000RTData();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'h': // MXR Bias
eepromConfig.accelBiasMXR[XAXIS] = readFloatCLI();
eepromConfig.accelBiasMXR[YAXIS] = readFloatCLI();
eepromConfig.accelBiasMXR[ZAXIS] = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock (within 20 +/- 10%)
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, eepromConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
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 'F': // Sensor Orientation
eepromConfig.sensorOrientation = (uint8_t)readFloatCLI();
orientSensors();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'N': // Set Voltage Monitor Trip Points
eepromConfig.batteryLow = readFloatCLI();
eepromConfig.batteryVeryLow = readFloatCLI();
eepromConfig.batteryMaxLow = readFloatCLI();
thresholds[BATTERY_LOW].value = eepromConfig.batteryLow;
thresholds[BATTERY_VERY_LOW].value = eepromConfig.batteryVeryLow;
thresholds[BATTRY_MAX_LOW].value = eepromConfig.batteryMaxLow;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'V': // Set Voltage Monitor Parameters
eepromConfig.voltageMonitorScale = readFloatCLI();
eepromConfig.voltageMonitorBias = readFloatCLI();
eepromConfig.batteryCells = (uint8_t)readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3, see aq32Plus.h\n");
cliPortPrint("'b' MPU6000 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
cliPortPrint("'d' Accel Bias and SF Calibration 'D' Set kpMag/kiMag DkpMag;kiMag\n");
cliPortPrint("'e' Toggle External HMC5883 State 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPortPrint("'f' Toggle External MS5611 State 'F' Set Sensor Orientation F1 thru 4\n");
cliPortPrint("'g' Toggle MXR9150 Use\n");
cliPortPrint(" 'N' Set Voltage Monitor Trip Points Nlow;veryLow;maxLow\n");
cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n");
break;
///////////////////////////
}
}
}
void writeSystemEEPROM(void)
{
uint16_t byteCount;
uint8_t pageIndex;
uint8_t* start = (uint8_t*)(&systemConfig);
uint8_t* end = (uint8_t*)(&systemConfig + 1);
///////////////////////////////////
// there's no reason to write these values to EEPROM, they'll just be noise
zeroPIDstates();
if (systemConfig.CRCFlags & CRC_HistoryBad)
evrPush(EVR_ConfigBadSystemHistory,0);
systemConfig.CRCAtEnd[0] = crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig]
(uint32_t*)(&systemConfig.CRCAtEnd));
///////////////////////////////////
setSPIdivisor(EEPROM_SPI, 2); // 18 MHz SPI clock
systemConfigaddr.value = SYSTEM_EEPROM_ADDR;
///////////////////////////////////
// Sector Erase
writeEnable();
ENABLE_EEPROM;
spiTransfer(EEPROM_SPI, SECTOR_ERASE_64KB);
spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[2]);
spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[1]);
spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[0]);
DISABLE_EEPROM;
delayMicroseconds(2);
eepromBusy();
///////////////////////////////////
// Program Page(s)
for (pageIndex = 0; pageIndex < ((sizeof(systemConfig) / 256) + 1); pageIndex++)
{
writeEnable();
ENABLE_EEPROM;
spiTransfer(EEPROM_SPI, PAGE_PROGRAM_256_BYTES);
spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[2]);
spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[1]);
spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[0]);
for (byteCount = 0; byteCount < 256; byteCount++)
{
spiTransfer(EEPROM_SPI, *start++);
if (start >= end)
break;
}
DISABLE_EEPROM;
delayMicroseconds(2);
eepromBusy();
systemConfigaddr.value += 0x0100;
}
readSystemEEPROM();
}
void writeSensorEEPROM(void)
{
uint16_t byteCount;
uint8_t pageIndex;
uint8_t* start = (uint8_t*)(&sensorConfig);
uint8_t* end = (uint8_t*)(&sensorConfig + 1);
///////////////////////////////////
if (sensorConfig.CRCFlags & CRC_HistoryBad)
evrPush(EVR_ConfigBadSensorHistory,0);
sensorConfig.CRCAtEnd[0] = crc32B((uint32_t*)(&sensorConfig), // CRC32B[sensorConfig]
(uint32_t*)(&sensorConfig.CRCAtEnd));
///////////////////////////////////
setSPIdivisor(EEPROM_SPI, 2); // 18 MHz SPI clock
sensorConfigAddr.value = SENSOR_EEPROM_ADDR;
///////////////////////////////////
// Sector Erase
writeEnable();
ENABLE_EEPROM;
spiTransfer(EEPROM_SPI, SECTOR_ERASE_64KB);
spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[2]);
spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[1]);
spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[0]);
DISABLE_EEPROM;
delayMicroseconds(2);
eepromBusy();
///////////////////////////////////
// Program Page(s)
for (pageIndex = 0; pageIndex < ((sizeof(sensorConfig) / 256) + 1); pageIndex++)
{
writeEnable();
ENABLE_EEPROM;
spiTransfer(EEPROM_SPI, PAGE_PROGRAM_256_BYTES);
spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[2]);
spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[1]);
spiTransfer(EEPROM_SPI, sensorConfigAddr.bytes[0]);
for (byteCount = 0; byteCount < 256; byteCount++)
{
spiTransfer(EEPROM_SPI, *start++);
if (start >= end)
break;
}
DISABLE_EEPROM;
delayMicroseconds(2);
eepromBusy();
sensorConfigAddr.value += 0x0100;
}
readSensorEEPROM();
}
