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 if (eepromConfig.useMpu6050 == true) { cliPortPrint("\nUsing MPU6050....\n\n"); 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]); } else { cliPortPrint("\nUsing ADXL345/MPU3050....\n\n"); } cliPortPrintF("Accel Scale Factor: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelScaleFactor[XAXIS], eepromConfig.accelScaleFactor[YAXIS], eepromConfig.accelScaleFactor[ZAXIS]); cliPortPrintF("Accel Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelBias[XAXIS], eepromConfig.accelBias[YAXIS], eepromConfig.accelBias[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("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS], eepromConfig.magBias[YAXIS], eepromConfig.magBias[ZAXIS]); 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("KpMag (MARG): %9.4f\n", eepromConfig.KpMag); 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); if (eepromConfig.useMpu6050 == true) { cliPortPrint("MPU6050 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; } } if (eepromConfig.useMs5611 == true) cliPortPrint("\nUsing MS5611....\n\n"); else cliPortPrint("\nUsing BMP085....\n\n"); if (eepromConfig.verticalVelocityHoldOnly == true) 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': // MPU Calibration if (eepromConfig.useMpu6050 == true) mpu6050Calibration(); else mpu3050Calibration(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'c': // Magnetometer Calibration magCalibration(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'd': // Accel Calibration if (eepromConfig.useMpu6050 == false) accelCalibrationADXL345(); else accelCalibrationMPU(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'm': // Toggle MPU3050/MPU6050 if (eepromConfig.useMpu6050) { eepromConfig.useMpu6050 = false; initAdxl345(); initMpu3050(); } else { eepromConfig.useMpu6050 = true; initMpu6050(); } sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'p': // Toggle BMP085/MS5611 if (eepromConfig.useMs5611) { eepromConfig.useMs5611 = false; initBmp085(); } else { eepromConfig.useMs5611 = true; initMs5611(); } 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 MPU6050 Digital Low Pass Filter if (eepromConfig.useMpu6050 == true) { 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; } i2cWrite(I2C2, MPU6050_ADDRESS, MPU6050_CONFIG, eepromConfig.dlpfSetting); // Accel and Gyro DLPF Setting sensorQuery = 'a'; validQuery = true; } break; /////////////////////////// case 'B': // Accel Cutoff eepromConfig.accelCutoff = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // kpAcc eepromConfig.KpAcc = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // kpMag eepromConfig.KpMag = 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 '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"); validQuery = false; writeEEPROM(); break; /////////////////////////// case '?': cliPortPrint("\n"); if (eepromConfig.useMpu6050 == true) cliPortPrint("'a' Display Sensor Data 'A' Set MPU6050 DLPF A0 thru 3\n"); else cliPortPrint("'a' Display Sensor Data\n"); cliPortPrint("'b' MPU Calibration 'B' Set Accel Cutoff BAccelCutoff\n"); cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc CKpAcc\n"); cliPortPrint("'d' Accel Calibration 'D' Set kpMag DKpMag\n"); cliPortPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n"); cliPortPrint("'m' Toggle MPU3050/MPU6050\n"); cliPortPrint(" 'N' Set Voltage Monitor Trip Points Nlow;veryLow;maxLow\n"); cliPortPrint("'p' Toggle BMP085/MS5611\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 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 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("MPU Accel Bias: %9.3f, %9.3f, %9.3f\n", sensorConfig.accelBiasMPU[XAXIS], sensorConfig.accelBiasMPU[YAXIS], sensorConfig.accelBiasMPU[ZAXIS]); cliPortPrintF("MPU Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", sensorConfig.accelScaleFactorMPU[XAXIS], sensorConfig.accelScaleFactorMPU[YAXIS], sensorConfig.accelScaleFactorMPU[ZAXIS]); 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; } cliPortPrintF("Voltage Monitor Scale: %9.4f\n\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); if (sensorConfig.gpsVelocityHoldOnly) cliPortPrint("GPS Velocity Hold Only\n"); else cliPortPrint("GPS Velocity and Position Hold\n"); if (sensorConfig.verticalVelocityHoldOnly) cliPortPrint("Vertical Velocity Hold Only\n\n"); else cliPortPrint("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 'd': // Accel Bias and Scale Factor Calibration accelCalibrationMPU(); 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 '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' Accel Bias and SF Calibration 'D' Set kpMag/kiMag DKpMag;KiMag\n"); cliPortPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\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; /////////////////////////// } } }