void magCalibration(void)
{
uint16_t calibrationCounter = 0;
uint16_t population[2][3];
float d[600][3]; // 600 Samples = 60 seconds of data at 10 Hz
float sphereOrigin[3];
float sphereRadius;
magCalibrating = true;
cliPrintF("\nMagnetometer Calibration:\n\n");
cliPrintF("Rotate magnetometer around all axes multiple times\n");
cliPrintF("Must complete within 60 seconds....\n\n");
cliPrintF(" Send a character when ready to begin and another when complete\n\n");
while (cliAvailable() == false);
cliPrintF(" Start rotations.....\n");
getChar();
while ((cliAvailable() == false) && (calibrationCounter < 600))
{
if (readMag() == true)
{
d[calibrationCounter][XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS];
d[calibrationCounter][YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS];
d[calibrationCounter][ZAXIS] = (float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS];
calibrationCounter++;
}
delay(100);
}
cliPrintF("\n\nMagnetometer Bias Calculation, %3ld samples collected out of 600 max)\n", calibrationCounter);
sphereFit(d, calibrationCounter, 100, 0.0f, population, sphereOrigin, &sphereRadius);
eepromConfig.magBias[XAXIS] = sphereOrigin[XAXIS];
eepromConfig.magBias[YAXIS] = sphereOrigin[YAXIS];
eepromConfig.magBias[ZAXIS] = sphereOrigin[ZAXIS];
magCalibrating = false;
}
void accelCalibrationMXR(void)
{
float noseUp = 0.0f;
float noseDown = 0.0f;
float leftWingDown = 0.0f;
float rightWingDown = 0.0f;
float upSideDown = 0.0f;
float rightSideUp = 0.0f;
float xBias = 0.0f;
float yBias = 0.0f;
float zBias = 0.0f;
int16_t index;
accelCalibrating = true;
cliPrint("\nMXR9150 Accelerometer Calibration:\n\n");
///////////////////////////////////
cliPrint("Place accelerometer right side up\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
rightSideUp += mxr9150ZAxis();
xBias += mxr9150XAxis();
yBias += mxr9150YAxis();
delayMicroseconds(1000);
}
rightSideUp /= 5000.0f;
cliPrint("Place accelerometer up side down\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
upSideDown += mxr9150ZAxis();
xBias += mxr9150XAxis();
yBias += mxr9150YAxis();
delayMicroseconds(1000);
}
upSideDown /= 5000.0f;
///////////////////////////////////
cliPrint("Place accelerometer left edge down\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
leftWingDown += mxr9150YAxis();
zBias += mxr9150ZAxis();
delayMicroseconds(1000);
}
leftWingDown /= 5000.0f;
cliPrint("Place accelerometer right edge down\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
rightWingDown += mxr9150YAxis();
zBias += mxr9150ZAxis();
delayMicroseconds(1000);
}
rightWingDown /= 5000.0f;
///////////////////////////////////
cliPrint("Place accelerometer rear edge down\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
noseUp += mxr9150XAxis();
zBias += mxr9150ZAxis();
delayMicroseconds(1000);
}
noseUp /= 5000.0f;
cliPrint("Place accelerometer front edge down\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
noseDown += mxr9150XAxis();
zBias += mxr9150ZAxis();
delayMicroseconds(1000);
}
noseDown /= 5000.0f;
///////////////////////////////////
eepromConfig.accelBiasMXR[ZAXIS] = zBias / 20000.0f;
eepromConfig.accelScaleFactorMXR[ZAXIS] = (2.0f * 9.8065f) / fabsf(rightSideUp - upSideDown);
eepromConfig.accelBiasMXR[YAXIS] = yBias / 10000.0f;
eepromConfig.accelScaleFactorMXR[YAXIS] = (2.0f * 9.8065f) / fabsf(leftWingDown - rightWingDown);
eepromConfig.accelBiasMXR[XAXIS] = xBias / 10000.0f;
eepromConfig.accelScaleFactorMXR[XAXIS] = (2.0f * 9.8065f) / fabsf(noseUp - noseDown);
///////////////////////////////////
accelCalibrating = false;
}
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;
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 accelCalibration(void)
{
double noseUpMXR = 0.0f;
double noseDownMXR = 0.0f;
double leftWingDownMXR = 0.0f;
double rightWingDownMXR = 0.0f;
double upSideDownMXR = 0.0f;
double rightSideUpMXR = 0.0f;
int16_t index;
accelCalibrating = true;
cliPrint("\nAccelerometer Calibration:\n\n");
///////////////////////////////////
cliPrint("Place accelerometer right side up\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
rightSideUpMXR += mxr9150ZAxis();;
delayMicroseconds(1000);
}
rightSideUpMXR /= 5000.0;
cliPrint("Place accelerometer up side down\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
upSideDownMXR += mxr9150ZAxis();
delayMicroseconds(1000);
}
upSideDownMXR /= 5000.0;
eepromConfig.accelBiasMXR[ZAXIS] = (float)((rightSideUpMXR + upSideDownMXR) / 2.0);
eepromConfig.accelScaleFactorMXR[ZAXIS] = (float)((2.0 * 9.8065) / (abs(rightSideUpMXR - eepromConfig.accelBiasMXR[ZAXIS]) +
abs(upSideDownMXR - eepromConfig.accelBiasMXR[ZAXIS])));
///////////////////////////////////
cliPrint("Place accelerometer left edge down\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
leftWingDownMXR += mxr9150YAxis();
delayMicroseconds(1000);
}
leftWingDownMXR /= 5000.0;
cliPrint("Place accelerometer right edge down\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
rightWingDownMXR += mxr9150YAxis();
delayMicroseconds(1000);
}
rightWingDownMXR /= 5000.0;
eepromConfig.accelBiasMXR[YAXIS] = (float)((leftWingDownMXR + rightWingDownMXR) / 2.0);
eepromConfig.accelScaleFactorMXR[YAXIS] = (float)((2.0 * 9.8065) / (abs(leftWingDownMXR - eepromConfig.accelBiasMXR[YAXIS]) +
abs(rightWingDownMXR - eepromConfig.accelBiasMXR[YAXIS])));
///////////////////////////////////
cliPrint("Place accelerometer rear edge down\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
noseUpMXR += mxr9150XAxis();
delayMicroseconds(1000);
}
noseUpMXR /= 5000.0;
cliPrint("Place accelerometer front edge down\n");
cliPrint(" Send a character when ready to proceed\n\n");
while (cliAvailable() == false);
cliRead();
cliPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
noseDownMXR += mxr9150XAxis();
delayMicroseconds(1000);
}
noseDownMXR /= 5000.0;
eepromConfig.accelBiasMXR[XAXIS] = (float)((noseUpMXR + noseDownMXR) / 2.0);
eepromConfig.accelScaleFactorMXR[XAXIS] = (float)((2.0 * 9.8065) / (abs(noseUpMXR - eepromConfig.accelBiasMXR[XAXIS]) +
abs(noseDownMXR - eepromConfig.accelBiasMXR[XAXIS])));
///////////////////////////////////
accelCalibrating = false;
}
void mixerCLI()
{
float tempFloat;
uint8_t index;
uint8_t rows, columns;
uint8_t mixerQuery;
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Mixer CLI....\n\n");
while(true)
{
cliPrint("Mixer CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
mixerQuery = cliRead();
cliPrint("\n");
switch(mixerQuery)
{
///////////////////////////
case 'a': // Mixer Configuration
cliPrint("\nMixer Configuration: ");
switch (eepromConfig.mixerConfiguration)
{
case MIXERTYPE_GIMBAL:
cliPrint("MIXERTYPE GIMBAL\n");
break;
///////////////////////
case MIXERTYPE_FLYING_WING:
cliPrint("MIXERTYPE FLYING WING\n");
break;
///////////////////////
case MIXERTYPE_BI:
cliPrint("MIXERTYPE BICOPTER\n");
break;
///////////////////////
case MIXERTYPE_TRI:
cliPrint("MIXERTYPE TRICOPTER\n");
break;
///////////////////////
case MIXERTYPE_QUADP:
cliPrint("MIXERTYPE QUAD PLUS\n");
break;
case MIXERTYPE_QUADX:
cliPrint("MIXERTYPE QUAD X\n");
break;
case MIXERTYPE_VTAIL4_NO_COMP:
cliPrint("MULTITYPE VTAIL NO COMP\n");
break;
case MIXERTYPE_VTAIL4_Y_COMP:
cliPrint("MULTITYPE VTAIL Y COMP\n");
break;
case MIXERTYPE_VTAIL4_RY_COMP:
cliPrint("MULTITYPE VTAIL RY COMP\n");
break;
case MIXERTYPE_VTAIL4_PY_COMP:
cliPrint("MULTITYPE VTAIL PY COMP\n");
break;
case MIXERTYPE_VTAIL4_RP_COMP:
cliPrint("MULTITYPE VTAIL RP COMP\n");
break;
case MIXERTYPE_VTAIL4_RPY_COMP:
cliPrint("MULTITYPE VTAIL RPY COMP\n");
break;
case MIXERTYPE_Y4:
cliPrint("MIXERTYPE Y4\n");
break;
///////////////////////
case MIXERTYPE_HEX6P:
cliPrint("MIXERTYPE HEX PLUS\n");
break;
case MIXERTYPE_HEX6X:
cliPrint("MIXERTYPE HEX X\n");
break;
case MIXERTYPE_Y6:
cliPrint("MIXERTYPE Y6\n");
break;
///////////////////////
case MIXERTYPE_FREEMIX:
cliPrint("MIXERTYPE FREE MIX\n");
break;
}
cliPrintF("Number of Motors: %1d\n", numberMotor);
cliPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate);
cliPrintF("Servo PWM Rate: %3ld\n", eepromConfig.servoPwmRate);
if ( eepromConfig.mixerConfiguration == MIXERTYPE_BI )
{
cliPrintF("BiCopter Left Servo Min: %4ld\n", (uint16_t)eepromConfig.biLeftServoMin);
cliPrintF("BiCopter Left Servo Mid: %4ld\n", (uint16_t)eepromConfig.biLeftServoMid);
cliPrintF("BiCopter Left Servo Max: %4ld\n", (uint16_t)eepromConfig.biLeftServoMax);
cliPrintF("BiCopter Right Servo Min: %4ld\n", (uint16_t)eepromConfig.biRightServoMin);
cliPrintF("BiCopter Right Servo Mid: %4ld\n", (uint16_t)eepromConfig.biRightServoMid);
cliPrintF("BiCopter Right Servo Max: %4ld\n", (uint16_t)eepromConfig.biRightServoMax);
}
if ( eepromConfig.mixerConfiguration == MIXERTYPE_FLYING_WING )
{
cliPrintF("Roll Direction Left: %4ld\n", (uint16_t)eepromConfig.rollDirectionLeft);
cliPrintF("Roll Direction Right: %4ld\n", (uint16_t)eepromConfig.rollDirectionRight);
cliPrintF("Pitch Direction Left: %4ld\n", (uint16_t)eepromConfig.pitchDirectionLeft);
cliPrintF("Pitch Direction Right: %4ld\n", (uint16_t)eepromConfig.pitchDirectionRight);
cliPrintF("Wing Left Minimum: %4ld\n", (uint16_t)eepromConfig.wingLeftMinimum);
cliPrintF("Wing Left Maximum: %4ld\n", (uint16_t)eepromConfig.wingLeftMaximum);
cliPrintF("Wing Right Minimum: %4ld\n", (uint16_t)eepromConfig.wingRightMinimum);
cliPrintF("Wing Right Maximum: %4ld\n", (uint16_t)eepromConfig.wingRightMaximum);
}
if ( eepromConfig.mixerConfiguration == MIXERTYPE_GIMBAL )
{
cliPrintF("Gimbal Roll Servo Min: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMin);
cliPrintF("Gimbal Roll Servo Mid: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMid);
cliPrintF("Gimbal Roll Servo Max: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMax);
cliPrintF("Gimbal Roll Servo Gain: %7.3f\n", eepromConfig.gimbalRollServoGain);
cliPrintF("Gimbal Pitch Servo Min: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMin);
cliPrintF("Gimbal Pitch Servo Mid: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMid);
cliPrintF("Gimbal Pitch Servo Max: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMax);
cliPrintF("Gimbal Pitch Servo Gain: %7.3f\n", eepromConfig.gimbalPitchServoGain);
}
if ( eepromConfig.mixerConfiguration == MIXERTYPE_TRI )
{
cliPrintF("TriCopter Yaw Servo Min: %4ld\n", (uint16_t)eepromConfig.triYawServoMin);
cliPrintF("TriCopter Yaw Servo Mid: %4ld\n", (uint16_t)eepromConfig.triYawServoMid);
cliPrintF("TriCopter Yaw Servo Max: %4ld\n", (uint16_t)eepromConfig.triYawServoMax);
}
if (eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_Y_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RY_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_PY_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RP_COMP ||
eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RPY_COMP)
{
cliPrintF("V Tail Angle %6.2f\n", eepromConfig.vTailAngle);
}
cliPrintF("Yaw Direction: %2d\n\n", (uint16_t)eepromConfig.yawDirection);
validQuery = false;
break;
///////////////////////////
case 'b': // Free Mix Matrix
cliPrintF("\nNumber of Free Mixer Motors: %1d\n Roll Pitch Yaw\n", eepromConfig.freeMixMotors);
for ( index = 0; index < eepromConfig.freeMixMotors; index++ )
{
cliPrintF("Motor%1d %6.3f %6.3f %6.3f\n", index,
eepromConfig.freeMix[index][ROLL ],
eepromConfig.freeMix[index][PITCH],
eepromConfig.freeMix[index][YAW ]);
}
cliPrint("\n");
validQuery = false;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Mixer CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read Mixer Configuration
eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI();
initMixer();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read ESC and Servo PWM Update Rates
eepromConfig.escPwmRate = (uint16_t)readFloatCLI();
eepromConfig.servoPwmRate = (uint16_t)readFloatCLI();
pwmOutputConfig.escPwmRate = eepromConfig.escPwmRate;
pwmOutputConfig.servoPwmRate = eepromConfig.servoPwmRate;
pwmOutputInit(&pwmOutputConfig);
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // Read BiCopter Left Servo Parameters
eepromConfig.biLeftServoMin = readFloatCLI();
eepromConfig.biLeftServoMid = readFloatCLI();
eepromConfig.biLeftServoMax = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Read BiCopter Right Servo Parameters
eepromConfig.biRightServoMin = readFloatCLI();
eepromConfig.biRightServoMid = readFloatCLI();
eepromConfig.biRightServoMax = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // Read Flying Wing Servo Directions
eepromConfig.rollDirectionLeft = readFloatCLI();
eepromConfig.rollDirectionRight = readFloatCLI();
eepromConfig.pitchDirectionLeft = readFloatCLI();
eepromConfig.pitchDirectionRight = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read Flying Wing Servo Limits
eepromConfig.wingLeftMinimum = readFloatCLI();
eepromConfig.wingLeftMaximum = readFloatCLI();
eepromConfig.wingRightMinimum = readFloatCLI();
eepromConfig.wingRightMaximum = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'G': // Read Free Mix Motor Number
eepromConfig.freeMixMotors = (uint8_t)readFloatCLI();
initMixer();
mixerQuery = 'b';
validQuery = true;
break;
///////////////////////////
case 'H': // Read Free Mix Matrix Element
rows = (uint8_t)readFloatCLI();
columns = (uint8_t)readFloatCLI();
eepromConfig.freeMix[rows][columns] = readFloatCLI();
mixerQuery = 'b';
validQuery = true;
break;
///////////////////////////
case 'I': // Read Gimbal Roll Servo Parameters
eepromConfig.gimbalRollServoMin = readFloatCLI();
eepromConfig.gimbalRollServoMid = readFloatCLI();
eepromConfig.gimbalRollServoMax = readFloatCLI();
eepromConfig.gimbalRollServoGain = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'J': // Read Gimbal Pitch Servo Parameters
eepromConfig.gimbalPitchServoMin = readFloatCLI();
eepromConfig.gimbalPitchServoMid = readFloatCLI();
eepromConfig.gimbalPitchServoMax = readFloatCLI();
eepromConfig.gimbalPitchServoGain = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'K': // Read TriCopter YawServo Parameters
eepromConfig.triYawServoMin = readFloatCLI();
eepromConfig.triYawServoMid = readFloatCLI();
eepromConfig.triYawServoMax = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'L': // Read V Tail Angle
eepromConfig.vTailAngle = readFloatCLI();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Read yaw direction
tempFloat = readFloatCLI();
if (tempFloat >= 0.0)
tempFloat = 1.0;
else
tempFloat = -1.0;
eepromConfig.yawDirection = tempFloat;
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A1 thru 17, see baseFlightPlus.h\n");
cliPrint("'b' Free Mixer Configuration 'B' Set PWM Rates BESC;Servo\n");
cliPrint(" 'C' Set BiCopter Left Servo Parameters CMin;Mid;Max\n");
cliPrint(" 'D' Set BiCopter Right Servo Parameters DMin;Mid;Max\n");
cliPrint(" 'E' Set Flying Wing Servo Directions ERollLeft;RollRight;PitchLeft;PitchRight\n");
cliPrint(" 'F' Set Flying Wing Servo Limits FLeftMin;LeftMax;RightMin;RightMax\n");
cliPrint(" 'G' Set Number of FreeMix Motors GNumber\n");
cliPrint(" 'H' Set FreeMix Matrix Element HRow;Column;Element\n");
cliPrint(" 'I' Set Gimbal Roll Servo Parameters IMin;Mid;Max;Gain\n");
cliPrint(" 'J' Set Gimbal Pitch Servo Parameters JMin;Mid;Max;Gain\n");
cliPrint(" 'K' Set TriCopter Servo Parameters KMin;Mid;Max\n");
cliPrint(" 'L' Set V Tail Angle LAngle\n");
cliPrint(" 'M' Set Yaw Direction M1 or M-1\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
void receiverCLI()
{
char rcOrderString[9];
float tempFloat;
uint8_t index;
uint8_t receiverQuery;
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Receiver CLI....\n\n");
while(true)
{
cliPrint("Receiver CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
receiverQuery = cliRead();
cliPrint("\n");
switch(receiverQuery)
{
///////////////////////////
case 'a': // Receiver Configuration
cliPrint("\nReceiver Type: ");
switch(eepromConfig.receiverType)
{
case PARALLEL_PWM:
cliPrint("Parallel\n");
break;
case SERIAL_PWM:
cliPrint("Serial\n");
break;
case SPEKTRUM:
cliPrint("Spektrum\n");
break;
}
cliPrint("Current RC Channel Assignment: ");
for (index = 0; index < 8; index++)
rcOrderString[eepromConfig.rcMap[index]] = rcChannelLetters[index];
rcOrderString[index] = '\0';
cliPrint(rcOrderString); cliPrint("\n");
cliPrintF("Spektrum Resolution: %s\n", eepromConfig.spektrumHires ? "11 Bit Mode" : "10 Bit Mode");
cliPrintF("Number of Spektrum Channels: %2d\n", eepromConfig.spektrumChannels);
cliPrintF("Mid Command: %4ld\n", (uint16_t)eepromConfig.midCommand);
cliPrintF("Min Check: %4ld\n", (uint16_t)eepromConfig.minCheck);
cliPrintF("Max Check: %4ld\n", (uint16_t)eepromConfig.maxCheck);
cliPrintF("Min Throttle: %4ld\n", (uint16_t)eepromConfig.minThrottle);
cliPrintF("Max Thottle: %4ld\n\n", (uint16_t)eepromConfig.maxThrottle);
tempFloat = eepromConfig.rateScaling * 180000.0 / PI;
cliPrintF("Max Rate Command: %6.2f DPS\n", tempFloat);
tempFloat = eepromConfig.attitudeScaling * 180000.0 / PI;
cliPrintF("Max Attitude Command: %6.2f Degrees\n\n", tempFloat);
cliPrintF("Arm Delay Count: %3d Frames\n", eepromConfig.armCount);
cliPrintF("Disarm Delay Count: %3d Frames\n\n", eepromConfig.disarmCount);
validQuery = false;
break;
///////////////////////////
case 'b': // Read Max Rate Value
eepromConfig.rateScaling = readFloatCLI() / 180000 * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Read Max Attitude Value
eepromConfig.attitudeScaling = readFloatCLI() / 180000 * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Receiver CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read RX Input Type
eepromConfig.receiverType = (uint8_t)readFloatCLI();
cliPrint( "\nReceiver Type Changed....\n");
cliPrint("\nSystem Resetting....\n");
delay(100);
writeEEPROM();
systemReset(false);
break;
///////////////////////////
case 'B': // Read RC Control Order
readStringCLI( rcOrderString, 8 );
parseRcChannels( rcOrderString );
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // Read Spektrum Resolution
eepromConfig.spektrumHires = (uint8_t)readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Read Number of Spektrum Channels
eepromConfig.spektrumChannels = (uint8_t)readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // Read RC Control Points
eepromConfig.midCommand = readFloatCLI();
eepromConfig.minCheck = readFloatCLI();
eepromConfig.maxCheck = readFloatCLI();
eepromConfig.minThrottle = readFloatCLI();
eepromConfig.maxThrottle = readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read Arm/Disarm Counts
eepromConfig.armCount = (uint8_t)readFloatCLI();
eepromConfig.disarmCount = (uint8_t)readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Receiver Configuration Data 'A' Set RX Input Type AX, 1=Parallel, 2=Serial, 3=Spektrum\n");
cliPrint("'b' Set Maximum Rate Command 'B' Set RC Control Order BTAER1234\n");
cliPrint("'c' Set Maximum Attitude Command 'C' Set Spektrum Resolution C0 or C1\n");
cliPrint(" 'D' Set Number of Spektrum Channels D6 thru D12\n");
cliPrint(" 'E' Set RC Control Points EmidCmd;minChk;maxChk;minThrot;maxThrot\n");
cliPrint(" 'F' Set Arm/Disarm Counts FarmCount;disarmCount\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Receiver CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
void gpsCLI()
{
USART_InitTypeDef USART_InitStructure;
uint8_t gpsQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering GPS CLI....\n\n");
while(true)
{
cliPrint("GPS CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
gpsQuery = cliRead();
cliPrint("\n");
switch(gpsQuery)
{
///////////////////////////
case 'a': // GPS Installation Data
cliPrint("\n");
switch(eepromConfig.gpsType)
{
///////////////
case NO_GPS:
cliPrint("No GPS Installed....\n\n");
break;
///////////////
case MEDIATEK_3329_BINARY:
cliPrint("MediaTek 3329 GPS installed, Binary Mode....\n\n");
break;
///////////////
case MEDIATEK_3329_NMEA:
cliPrint("MediaTek 3329 GPS Installed, NMEA Mode....\n\n");
break;
///////////////
case UBLOX:
cliPrint("UBLOX GPS Installed, Binary Mode....\n\n");
break;
///////////////
}
cliPrintF("GPS Baud Rate: %6ld\n\n", eepromConfig.gpsBaudRate);
validQuery = false;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting GPS CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set GPS Installed State to False
eepromConfig.gpsType = NO_GPS;
gpsQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Set GPS Type to MediaTek 3329 Binary
eepromConfig.gpsType = MEDIATEK_3329_BINARY;
initGPS();
gpsQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // Set GPS Type to MediaTek 3329 NMEA
eepromConfig.gpsType = MEDIATEK_3329_NMEA;
initGPS();
gpsQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Set GPS Type to UBLOX Binary
eepromConfig.gpsType = UBLOX;
initGPS();
gpsQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'S': // Read GPS Baud Rate
eepromConfig.gpsBaudRate = (uint16_t)readFloatCLI();
USART_StructInit(&USART_InitStructure);
USART_InitStructure.USART_BaudRate = eepromConfig.gpsBaudRate;
USART_Init(USART2, &USART_InitStructure);
gpsQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Display GPS Installation Data 'A' Set GPS Type to No GPS\n");
cliPrint(" 'B' Set GPS Type to MediaTek 3329 Binary\n");
cliPrint(" 'C' Set GPS Type to MediaTek 3329 NMEA\n");
cliPrint(" 'D' Set GPS Type to UBLOX\n");
cliPrint(" 'S' Set GPS Baud Rate\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit GPS CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
void max7456CLI()
{
uint8_t max7456query = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering MAX7456 CLI....\n\n");
resetMax7456();
while(true)
{
if (!validQuery) cliPrint("MAX7456 CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
max7456query = cliRead();
if (!validQuery) cliPrint("\n");
switch(max7456query)
{
///////////////////////
case 'a': // OSD Configuration
cliPrint("\nMAX7456 OSD Status: ");
if (eepromConfig.osdEnabled)
cliPrint("Enabled\n");
else
cliPrint("Disabled\n");
cliPrint("OSD Default Video Standard: ");
if (eepromConfig.defaultVideoStandard)
cliPrint("PAL\n");
else
cliPrint("NTSC\n");
cliPrint("OSD Display Units: ");
if (eepromConfig.metricUnits)
cliPrint("Metric\n");
else
cliPrint("English\n");
cliPrint("OSD Altitude Display: ");
if (eepromConfig.osdDisplayAlt)
cliPrint("On\n");
else
cliPrint("Off\n");
cliPrint("OSD Artifical Horizon Display: ");
if (eepromConfig.osdDisplayAH)
cliPrint("On\n");
else
cliPrint("Off\n");
cliPrint("OSD Attitude Display: ");
if (eepromConfig.osdDisplayAtt)
cliPrint("On\n");
else
cliPrint("Off\n");
cliPrint("OSD Heading Display: ");
if (eepromConfig.osdDisplayHdg)
cliPrint("On\n");
else
cliPrint("Off\n");
cliPrint("\n");
validQuery = false;
break;
///////////////////////
case 'b': // Enable OSD Altitude Display
eepromConfig.osdDisplayAlt = true;
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'c': // Enable OSD Artifical Horizon Display
eepromConfig.osdDisplayAH = true;
eepromConfig.osdDisplayAtt = false;
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'd': // Enable OSD Attitude Display
eepromConfig.osdDisplayAtt = true;
eepromConfig.osdDisplayAH = false;
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'e': // Enable OSD Heading Display
eepromConfig.osdDisplayHdg = true;
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'q': // Set English Display Units
eepromConfig.metricUnits = false;
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'r': // Reset MAX7456
resetMax7456();
cliPrint("\nMAX7456 Reset....\n\n");
break;
///////////////////////
case 's': // Show character set
showMax7456Font();
cliPrint("\nMAX7456 Character Set Displayed....\n\n");
break;
///////////////////////
case 't': // Download font
downloadMax7456Font();
break;
///////////////////////
case 'u': // Toggle OSD enabled status
if (eepromConfig.osdEnabled) // If Enabled
eepromConfig.osdEnabled = false; // Set Disabled
else
{ // If Disabled
eepromConfig.osdEnabled = true; // Set Enabled
initMax7456(); // and call init procedure
}
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'v': // Toggle default video standard
if (eepromConfig.defaultVideoStandard) // If PAL
eepromConfig.defaultVideoStandard = NTSC; // Set NTSC
else // If NTSC
eepromConfig.defaultVideoStandard = PAL; // Set PAL
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'x':
cliPrint("\nExiting MAX7456 CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////
case 'B': // Disable OSD Altitude Display
eepromConfig.osdDisplayAlt = false;
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'C': // Disable OSD Artifical Horizon Display
eepromConfig.osdDisplayAH = false;
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'D': // Disable OSD Attitude Display
eepromConfig.osdDisplayAtt = false;
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'E': // Disable OSD Heading Display
eepromConfig.osdDisplayHdg = false;
max7456query = 'a';
validQuery = true;
break;
///////////////////////
case 'Q': // Set Metric Display Units
eepromConfig.metricUnits = true;
max7456query = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' OSD Configuration\n");
cliPrint("'b' Enable OSD Altitude Display 'B' Disable OSD Altitude Display\n");
cliPrint("'c' Enable OSD Artificial Horizon Display 'C' Disable OSD Artificial Horizon Display\n");
cliPrint("'d' Enable OSD Attitude Display 'D' Disable OSD Attitude Display\n");
cliPrint("'e' Enable OSD Heading Display 'E' Disable OSD Heading Display\n");
cliPrint("'q' Set English Display Units 'Q' Set Metric Display Units\n");
cliPrint("'r' Reset MAX7456\n");
cliPrint("'s' Display MAX7456 Character Set\n");
cliPrint("'t' Download Font to MAX7456\n");
cliPrint("'u' Change OSD Installed State\n");
cliPrint("'v' Change Default Video Standard 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////
}
}
}
void eepromCLI()
{
uint32_t c1, c2;
uint8_t eepromQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering EEPROM CLI....\n\n");
while(true)
{
cliPrint("EEPROM CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
eepromQuery = cliRead();
cliPrint("\n");
switch(eepromQuery)
{
// 'a' is the standard "print all the information" character
case 'a': // config struct data
c1 = eepromConfig.CRCAtEnd[0];
zeroPIDintegralError();
zeroPIDstates();
c2 = crc32bEEPROM(&eepromConfig, false);
cliPrintF("Config structure information:\n");
cliPrintF("Version : %d\n", eepromConfig.version );
cliPrintF("Size : %d\n", sizeof(eepromConfig) );
cliPrintF("CRC on last read : %08X\n", c1 );
cliPrintF("Current CRC : %08X\n", c2 );
if ( c1 != c2 )
cliPrintF(" CRCs differ. Current Config has not yet been saved.\n");
cliPrintF("CRC Flags :\n");
cliPrintF(" 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
zeroPIDintegralError();
zeroPIDstates();
cliPrintF("\n");
cliPrintEEPROM(&eepromConfig);
cliPrintF("\n");
if (crcCheckVal != crc32bEEPROM(&eepromConfig, true))
{
cliPrint("NOTE: in-memory config CRC invalid; there have probably been changes to\n");
cliPrint(" eepromConfig since the last write to flash/eeprom.\n");
}
validQuery = false;
break;
///////////////////////////
case 'H': // clear bad history flag
cliPrintF("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;
cliPrintF("Ready to read in config. Expecting %d (0x%03X) bytes as %d\n",
sz, sz, sz * 2);
cliPrintF("hexadecimal characters, optionally separated by [ \\n\\r_].\n");
cliPrintF("Times out if no character is received for %dms\n", Timeout);
memset(p, 0, end - p);
while (p < end)
{
while (!cliAvailable() && millis() - t < Timeout) {}
t = millis();
c = cliAvailable() ? cliRead() : '\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)
{
cliPrintF("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)
{
cliPrintF("Invalid character found at position %d: '%c' (0x%02x)",
chars_encountered, c, c);
}
else if (crcCheckVal != crc32bEEPROM(&e, true))
{
cliPrintF("CRC mismatch! Not writing to in-memory config.\n");
cliPrintF("Here's what was received:\n\n");
cliPrintEEPROM(&e);
}
else
{
// check to see if the newly received eeprom config
// actually differs from what's in-memory
zeroPIDintegralError();
zeroPIDstates();
int i;
for (i = 0; i < sz; i++)
if (((uint8_t*)&e)[i] != ((uint8_t*)&eepromConfig)[i])
break;
if (i == sz)
{
cliPrintF("NOTE: uploaded config was identical to in-memory config.\n");
}
else
{
eepromConfig = e;
cliPrintF("In-memory config updated!\n");
cliPrintF("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 (cliAvailable())
cliRead();
validQuery = false;
break;
///////////////////////////
case 'E': // Read in from EEPROM. (EEPROM -> RAM)
cliPrint("Re-reading EEPROM.\n");
readEEPROM();
validQuery = false;
break;
///////////////////////////
case 'x': // exit EEPROM CLI
cliPrint("\nExiting EEPROM CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'W':
case 'e': // Write out to EEPROM. (RAM -> EEPROM)
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
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
cliPrint( "\nEEPROM Parameters Reset....(not rebooting)\n" );
checkFirstTime(true);
validQuery = false;
break;
///////////////////////////
case '?':
// 0 1 2 3 4 5 6 7
// 01234567890123456789012345678901234567890123456789012345678901234567890123456789
cliPrintF("\n");
cliPrintF("'a' Display in-RAM config information\n");
cliPrintF("'c' Write in-RAM -> Console (as Hex) 'C' Read Console (as Hex) -> in-RAM\n");
cliPrintF("'e' Write in-RAM -> EEPROM 'E' Read EEPROM -> in-RAM\n");
cliPrintF("'f' Write in-RAM -> sd FILE (Not yet imp) 'F' Read sd FILE -> in-RAM (Not imp)\n");
cliPrintF(" 'H' Clear CRC Bad History flag\n");
cliPrintF(" 'V' Reset in-RAM config to default.\n");
cliPrintF("'x' Exit EEPROM CLI '?' Command Summary\n");
cliPrintF("\n");
cliPrintF("For compatability: 'W' Write in-RAM -> EEPROM\n");
cliPrintF("\n");
break;
///////////////////////////
}
}
}
void escCalibration(void)
{
escCalibrating = true;
armed = false;
cliPrint("\nESC Calibration:\n\n");
cliPrint("!!!! CAUTION - Remove all propellers and disconnect !!!!\n");
cliPrint("!!!! flight battery before proceeding any further !!!!\n\n");
cliPrint("Type 'Y' to continue, anything other character exits\n\n");
while (cliAvailable() == false);
temp = cliRead();
if (temp != 'Y')
{
cliPrint("ESC Calibration Canceled!!\n\n");
escCalibrating = false;
return;
}
///////////////////////////////////
cliPrint("For ESC Calibration:\n");
cliPrint(" Enter 'h' for Max Command....\n");
cliPrint(" Enter 'm' for Mid Command....\n");
cliPrint(" Enter 'l' for Min Command....\n");
cliPrint(" Enter 'x' to exit....\n\n");
cliPrint("For Motor Order Verification:\n");
cliPrint(" Enter '0' to turn off all motors....\n");
cliPrint(" Enter '1' to turn on Motor1....\n");
cliPrint(" Enter '2' to turn on Motor2....\n");
cliPrint(" Enter '3' to turn on Motor3....\n");
cliPrint(" Enter '4' to turn on Motor4....\n");
cliPrint(" Enter '5' to turn on Motor5....\n");
cliPrint(" Enter '6' to turn on Motor6....\n");
cliPrint(" Enter '7' to turn on Motor7....\n");
cliPrint(" Enter '8' to turn on Motor8....\n\n");
///////////////////////////////////
while(true)
{
while (cliAvailable() == false);
temp = cliRead();
switch (temp)
{
case 'h':
cliPrint("Applying Max Command....\n\n");
writeAllMotors(eepromConfig.maxThrottle);
break;
case 'm':
cliPrint("Applying Mid Command....\n\n");
writeAllMotors(eepromConfig.midCommand);
break;
case 'l':
cliPrint("Applying Min Command....\n\n");
writeAllMotors(MINCOMMAND);
break;
case 'x':
cliPrint("Applying Min Command, Exiting Calibration....\n\n");
writeAllMotors(MINCOMMAND);
escCalibrating = false;
return;
break;
case '0':
cliPrint("Motors at Min Command....\n\n");
writeAllMotors(MINCOMMAND);
break;
case '1':
cliPrint("Motor1 at Min Throttle....\n\n");
pwmEscWrite(0, eepromConfig.minThrottle);
break;
case '2':
cliPrint("Motor2 at Min Throttle....\n\n");
pwmEscWrite(1, eepromConfig.minThrottle);
break;
case '3':
cliPrint("Motor3 at Min Throttle....\n\n");
pwmEscWrite(2, eepromConfig.minThrottle);
break;
case '4':
cliPrint("Motor4 at Min Throttle....\n\n");
pwmEscWrite(3, eepromConfig.minThrottle);
break;
case '5':
cliPrint("Motor5 at Min Throttle....\n\n");
pwmEscWrite(4, eepromConfig.minThrottle);
break;
case '6':
cliPrint("Motor6 at Min Throttle....\n\n");
pwmEscWrite(5, eepromConfig.minThrottle);
break;
case '7':
cliPrint("Motor7 at Min Throttle....\n\n");
pwmEscWrite(6, eepromConfig.minThrottle);
break;
case '8':
cliPrint("Motor8 at Min Throttle....\n\n");
pwmEscWrite(7, eepromConfig.minThrottle);
break;
case '?':
cliPrint("For ESC Calibration:\n");
cliPrint(" Enter 'h' for Max Command....\n");
cliPrint(" Enter 'm' for Mid Command....\n");
cliPrint(" Enter 'l' for Min Command....\n");
cliPrint(" Enter 'x' to exit....\n\n");
cliPrint("For Motor Order Verification:\n");
cliPrint(" Enter '0' to turn off all motors....\n");
cliPrint(" Enter '1' to turn on Motor1....\n");
cliPrint(" Enter '2' to turn on Motor2....\n");
cliPrint(" Enter '3' to turn on Motor3....\n");
cliPrint(" Enter '4' to turn on Motor4....\n");
cliPrint(" Enter '5' to turn on Motor5....\n");
cliPrint(" Enter '6' to turn on Motor6....\n");
cliPrint(" Enter '7' to turn on Motor7....\n");
cliPrint(" Enter '8' to turn on Motor8....\n\n");
break;
}
}
}
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("MPU Accel Bias: %9.3f, %9.3f, %9.3f\n", eepromConfig.accelBiasMPU[XAXIS],
eepromConfig.accelBiasMPU[YAXIS],
eepromConfig.accelBiasMPU[ZAXIS]);
cliPrintF("MPU Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", eepromConfig.accelScaleFactorMPU[XAXIS],
eepromConfig.accelScaleFactorMPU[YAXIS],
eepromConfig.accelScaleFactorMPU[ZAXIS]);
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;
}
if (eepromConfig.verticalVelocityHoldOnly)
cliPrint("Vertical Velocity Hold Only\n\n");
else
cliPrint("Vertical Velocity and Altitude Hold\n\n");
cliPrintF("Voltage Monitor Scale: %9.4f\n", eepromConfig.voltageMonitorScale);
cliPrintF("Voltage Monitor Bias: %9.4f\n", eepromConfig.voltageMonitorBias);
cliPrintF("Number of Battery Cells: %1d\n\n", eepromConfig.batteryCells);
cliPrintF("Battery Low Setpoint: %4.2f volts\n", eepromConfig.batteryLow);
cliPrintF("Battery Very Low Setpoint: %4.2f volts\n", eepromConfig.batteryVeryLow);
cliPrintF("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
accelCalibrationMPU();
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
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': // 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
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3\n");
cliPrint("'b' MPU6000 Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CKpAcc;KiAcc\n");
cliPrint("'d' Accel Bias and SF Calibration 'D' Set kpMag/kiMag DKpMag;KiMag\n");
cliPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPrint(" 'M' Set Voltage Monitor Trip Points Mlow;veryLow;maxLow\n");
cliPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPrintF("\nEntering Sensor CLI....\n\n");
while (true)
{
cliPrintF("Sensor CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = getChar();
//cliRead(sensorQuery, 1);
cliPrintF("\n");
switch (sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
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("MPU6000 DLPF: ");
switch (eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPrintF("256 Hz\n");
break;
case DLPF_188HZ:
cliPrintF("188 Hz\n");
break;
case DLPF_98HZ:
cliPrintF("98 Hz\n");
break;
case DLPF_42HZ:
cliPrintF("42 Hz\n");
break;
}
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6050Calibration
mpu6050Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
// HJI case 'c': // Magnetometer Calibration
// HJI magCalibration();
// HJI sensorQuery = 'a';
// HJI validQuery = true;
// HJI break;
///////////////////////////
case 'x':
cliPrintF("\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;
}
i2cWrite(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, 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 'W': // Write EEPROM Parameters
cliPrintF("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrintF("\n");
cliPrintF("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3\n");
cliPrintF("'b' MPU6050 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPrintF(" 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
// HJI cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
cliPrintF(" 'D' Set kpMag/kiMag DkpMag;kiMag\n");
cliPrintF(" 'W' Write EEPROM Parameters\n");
cliPrintF("'x' Exit Sensor CLI '?' Command Summary\n\n");
break;
///////////////////////////
}
}
}
void mixerCLI()
{
float tempFloat;
uint8_t mixerQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPrint("\nEntering Mixer CLI....\n\n");
while(true)
{
cliPrint("Mixer CLI -> ");
while ((cliAvailable() == false) && (validQuery == false));
if (validQuery == false)
mixerQuery = cliRead();
cliPrint("\n");
switch(mixerQuery)
{
///////////////////////////
case 'a': // Mixer Configuration
cliPrint("\nMixer Configuration: ");
switch (eepromConfig.mixerConfiguration)
{
case MIXERTYPE_QUADX:
cliPrint("MIXERTYPE QUAD X\n");
break;
case MIXERTYPE_HEX6X:
cliPrint(" MIXERTYPE HEX X\n");
break;
}
cliPrintF("Number of Motors: %1d\n", numberMotor);
cliPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate);
cliPrintF("Servo PWM Rate: %3ld\n\n", eepromConfig.servoPwmRate);
validQuery = false;
break;
///////////////////////////
case 'x':
cliPrint("\nExiting Mixer CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read Mixer Configuration
eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI();
initMixer();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read ESC and Servo PWM Update Rates
eepromConfig.escPwmRate = (uint16_t)readFloatCLI();
eepromConfig.servoPwmRate = (uint16_t)readFloatCLI();
pwmEscInit(eepromConfig.escPwmRate);
pwmServoInit(eepromConfig.servoPwmRate);
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Read yaw direction
tempFloat = readFloatCLI();
if (tempFloat >= 0.0)
tempFloat = 1.0;
else
tempFloat = -1.0;
eepromConfig.yawDirection = tempFloat;
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
break;
///////////////////////////
case '?':
cliPrint("\n");
cliPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A1 thru 21, see aq32Plus.h\n");
cliPrint("'b' Free Mixer Configuration 'B' Set PWM Rates BESC;Servo\n");
cliPrint(" 'C' Set BiCopter Left Servo Parameters CMin;Mid;Max\n");
cliPrint(" 'D' Set BiCopter Right Servo Parameters DMin;Mid;Max\n");
cliPrint(" 'E' Set Flying Wing Servo Directions ERollLeft;RollRight;PitchLeft;PitchRight\n");
cliPrint(" 'F' Set Flying Wing Servo Limits FLeftMin;LeftMax;RightMin;RightMax\n");
cliPrint(" 'G' Set Number of FreeMix Motors GNumber\n");
cliPrint(" 'H' Set FreeMix Matrix Element HRow;Column;Element\n");
cliPrint(" 'I' Set Gimbal Roll Servo Parameters IMin;Mid;Max;Gain\n");
cliPrint(" 'J' Set Gimbal Pitch Servo Parameters JMin;Mid;Max;Gain\n");
cliPrint(" 'K' Set TriCopter Servo Parameters KMin;Mid;Max\n");
cliPrint(" 'L' Set V Tail Angle LAngle\n");
cliPrint(" 'M' Set Yaw Direction M1 or M-1\n");
cliPrint(" 'W' Write EEPROM Parameters\n");
cliPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPrint("\n");
break;
///////////////////////////
}
}
}