static void cliCMix(char *cmdline)
{
int i, check = 0;
int num_motors = 0;
uint8_t len;
char buf[16];
float mixsum[3];
char *ptr;
len = strlen(cmdline);
if (len == 0) {
cliPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n");
for (i = 0; i < MAX_MOTORS; i++) {
if (mcfg.customMixer[i].throttle == 0.0f)
break;
num_motors++;
printf("#%d:\t", i + 1);
printf("%s\t", ftoa(mcfg.customMixer[i].throttle, buf));
printf("%s\t", ftoa(mcfg.customMixer[i].roll, buf));
printf("%s\t", ftoa(mcfg.customMixer[i].pitch, buf));
printf("%s\r\n", ftoa(mcfg.customMixer[i].yaw, buf));
}
mixsum[0] = mixsum[1] = mixsum[2] = 0.0f;
for (i = 0; i < num_motors; i++) {
mixsum[0] += mcfg.customMixer[i].roll;
mixsum[1] += mcfg.customMixer[i].pitch;
mixsum[2] += mcfg.customMixer[i].yaw;
}
cliPrint("Sanity check:\t");
for (i = 0; i < 3; i++)
cliPrint(fabsf(mixsum[i]) > 0.01f ? "NG\t" : "OK\t");
cliPrint("\r\n");
return;
} else if (strncasecmp(cmdline, "reset", 5) == 0) {
// erase custom mixer
for (i = 0; i < MAX_MOTORS; i++)
mcfg.customMixer[i].throttle = 0.0f;
} else if (strncasecmp(cmdline, "load", 4) == 0) {
ptr = strchr(cmdline, ' ');
if (ptr) {
len = strlen(++ptr);
for (i = 0; ; i++) {
if (mixerNames[i] == NULL) {
cliPrint("Invalid mixer type...\r\n");
break;
}
if (strncasecmp(ptr, mixerNames[i], len) == 0) {
mixerLoadMix(i);
printf("Loaded %s mix...\r\n", mixerNames[i]);
cliCMix("");
break;
}
}
}
} else {
ptr = cmdline;
i = atoi(ptr); // get motor number
if (--i < MAX_MOTORS) {
ptr = strchr(ptr, ' ');
if (ptr) {
mcfg.customMixer[i].throttle = _atof(++ptr);
check++;
}
ptr = strchr(ptr, ' ');
if (ptr) {
mcfg.customMixer[i].roll = _atof(++ptr);
check++;
}
ptr = strchr(ptr, ' ');
if (ptr) {
mcfg.customMixer[i].pitch = _atof(++ptr);
check++;
}
ptr = strchr(ptr, ' ');
if (ptr) {
mcfg.customMixer[i].yaw = _atof(++ptr);
check++;
}
if (check != 4) {
cliPrint("Wrong number of arguments, needs idx thr roll pitch yaw\r\n");
} else {
cliCMix("");
}
} else {
printf("Motor number must be between 1 and %d\r\n", MAX_MOTORS);
}
}
}
static void cliSet(char *cmdline)
{
uint32_t i;
uint32_t len;
const clivalue_t *val;
char *eqptr = NULL;
int32_t value = 0;
float valuef = 0;
len = strlen(cmdline);
if (len == 0 || (len == 1 && cmdline[0] == '*')) {
cliPrint("Current settings: \r\n");
for (i = 0; i < VALUE_COUNT; i++) {
val = &valueTable[i];
printf("%s = ", valueTable[i].name);
cliPrintVar(val, len); // when len is 1 (when * is passed as argument), it will print min/max values as well, for gui
cliPrint("\r\n");
}
} else if ((eqptr = strstr(cmdline, "=")) != NULL) {
// has equal, set var
char *lastNonSpaceCharacter = eqptr;
while (*(lastNonSpaceCharacter - 1) == ' ') {
lastNonSpaceCharacter--;
}
uint8_t variableNameLength = lastNonSpaceCharacter - cmdline;
eqptr++;
len--;
value = atoi(eqptr);
valuef = fastA2F(eqptr);
for (i = 0; i < VALUE_COUNT; i++) {
val = &valueTable[i];
// ensure exact match when setting to prevent setting variables with shorter names
if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) {
if (valuef >= valueTable[i].min && valuef <= valueTable[i].max) { // here we compare the float value since... it should work, RIGHT?
int_float_value_t tmp;
if (valueTable[i].type == VAR_FLOAT)
tmp.float_value = valuef;
else
tmp.int_value = value;
cliSetVar(val, tmp);
printf("%s set to ", valueTable[i].name);
cliPrintVar(val, 0);
} else {
cliPrint("ERR: Value assignment out of range\r\n");
}
return;
}
}
cliPrint("ERR: Unknown variable name\r\n");
} else {
// no equals, check for matching variables.
for (i = 0; i < VALUE_COUNT; i++) {
if (strstr(valueTable[i].name, cmdline)) {
val = &valueTable[i];
printf("%s = ", valueTable[i].name);
cliPrintVar(val, 0);
printf("\r\n");
}
}
}
}
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;
}
static void cliReboot(void) {
cliPrint("\r\nRebooting...");
waitForSerialPortToFinishTransmitting(cliPort);
systemReset(false);
}
static void cliDefaults(char *cmdline)
{
cliPrint("Resetting to defaults...");
resetEEPROM();
cliReboot();
}
void cliProcess(void)
{
if (!cliMode) {
cliMode = 1;
cliPrint("\r\nEntering CLI Mode, type 'exit' to return, or 'help'\r\n");
cliPrompt();
}
while (serialTotalBytesWaiting(core.mainport)) {
uint8_t c = serialRead(core.mainport);
if (c == '\t' || c == '?') {
// do tab completion
const clicmd_t *cmd, *pstart = NULL, *pend = NULL;
int i = bufferIndex;
for (cmd = cmdTable; cmd < cmdTable + CMD_COUNT; cmd++) {
if (bufferIndex && (strncasecmp(cliBuffer, cmd->name, bufferIndex) != 0))
continue;
if (!pstart)
pstart = cmd;
pend = cmd;
}
if (pstart) { /* Buffer matches one or more commands */
for (; ; bufferIndex++) {
if (pstart->name[bufferIndex] != pend->name[bufferIndex])
break;
if (!pstart->name[bufferIndex] && bufferIndex < sizeof(cliBuffer) - 2) {
/* Unambiguous -- append a space */
cliBuffer[bufferIndex++] = ' ';
cliBuffer[bufferIndex] = '\0';
break;
}
cliBuffer[bufferIndex] = pstart->name[bufferIndex];
}
}
if (!bufferIndex || pstart != pend) {
/* Print list of ambiguous matches */
cliPrint("\r\033[K");
for (cmd = pstart; cmd <= pend; cmd++) {
cliPrint(cmd->name);
cliWrite('\t');
}
cliPrompt();
i = 0; /* Redraw prompt */
}
for (; i < bufferIndex; i++)
cliWrite(cliBuffer[i]);
} else if (!bufferIndex && c == 4) {
cliExit(cliBuffer);
return;
} else if (c == 12) {
// clear screen
cliPrint("\033[2J\033[1;1H");
cliPrompt();
} else if (bufferIndex && (c == '\n' || c == '\r')) {
// enter pressed
clicmd_t *cmd = NULL;
clicmd_t target;
cliPrint("\r\n");
cliBuffer[bufferIndex] = 0; // null terminate
target.name = cliBuffer;
target.param = NULL;
cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare);
if (cmd)
cmd->func(cliBuffer + strlen(cmd->name) + 1);
else
cliPrint("ERR: Unknown command, try 'help'");
memset(cliBuffer, 0, sizeof(cliBuffer));
bufferIndex = 0;
// 'exit' will reset this flag, so we don't need to print prompt again
if (!cliMode)
return;
cliPrompt();
} else if (c == 127) {
// backspace
if (bufferIndex) {
cliBuffer[--bufferIndex] = 0;
cliPrint("\010 \010");
}
} else if (bufferIndex < sizeof(cliBuffer) && c >= 32 && c <= 126) {
if (!bufferIndex && c == 32)
continue;
cliBuffer[bufferIndex++] = c;
cliWrite(c);
}
}
}
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 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 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 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 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 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;
}
}
}
static void cliFeature(char *cmdline)
{
uint32_t i;
uint32_t len;
uint32_t mask;
len = strlen(cmdline);
mask = featureMask();
if (len == 0) {
cliPrint("Enabled features: ");
for (i = 0; ; i++) {
if (featureNames[i] == NULL)
break;
if (mask & (1 << i))
printf("%s ", featureNames[i]);
}
cliPrint("\r\n");
} else if (strncasecmp(cmdline, "list", len) == 0) {
cliPrint("Available features: ");
for (i = 0; ; i++) {
if (featureNames[i] == NULL)
break;
printf("%s ", featureNames[i]);
}
cliPrint("\r\n");
return;
} else {
bool remove = false;
if (cmdline[0] == '-') {
// remove feature
remove = true;
cmdline++; // skip over -
len--;
}
for (i = 0; ; i++) {
if (featureNames[i] == NULL) {
cliPrint("Invalid feature name\r\n");
break;
}
if (strncasecmp(cmdline, featureNames[i], len) == 0) {
mask = 1 << i;
#ifndef GPS
if (mask & FEATURE_GPS) {
cliPrint("GPS unavailable\r\n");
break;
}
#endif
#ifndef SONAR
if (mask & FEATURE_SONAR) {
cliPrint("SONAR unavailable\r\n");
break;
}
#endif
if (remove) {
featureClear(mask);
cliPrint("Disabled ");
} else {
featureSet(mask);
cliPrint("Enabled ");
}
printf("%s\r\n", featureNames[i]);
break;
}
}
}
}
static void cliVersion(char *cmdline)
{
UNUSED(cmdline);
cliPrint("Cleanflight/" __TARGET__ " " __DATE__ " / " __TIME__ " (" __REVISION__ ")");
}
static void cliDump(char *cmdline)
{
int i;
char buf[16];
float thr, roll, pitch, yaw;
uint32_t mask;
const clivalue_t *setval;
printf("Current Config: Copy everything below here...\r\n");
// print out aux switches
cliAux("");
// print out current motor mix
printf("mixer %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]);
// print custom mix if exists
if (mcfg.customMixer[0].throttle != 0.0f) {
for (i = 0; i < MAX_MOTORS; i++) {
if (mcfg.customMixer[i].throttle == 0.0f)
break;
thr = mcfg.customMixer[i].throttle;
roll = mcfg.customMixer[i].roll;
pitch = mcfg.customMixer[i].pitch;
yaw = mcfg.customMixer[i].yaw;
printf("cmix %d", i + 1);
if (thr < 0)
printf(" ");
printf("%s", ftoa(thr, buf));
if (roll < 0)
printf(" ");
printf("%s", ftoa(roll, buf));
if (pitch < 0)
printf(" ");
printf("%s", ftoa(pitch, buf));
if (yaw < 0)
printf(" ");
printf("%s\r\n", ftoa(yaw, buf));
}
printf("cmix %d 0 0 0 0\r\n", i + 1);
}
// print enabled features
mask = featureMask();
for (i = 0; ; i++) { // disable all feature first
if (featureNames[i] == NULL)
break;
printf("feature -%s\r\n", featureNames[i]);
}
for (i = 0; ; i++) { // reenable what we want.
if (featureNames[i] == NULL)
break;
if (mask & (1 << i))
printf("feature %s\r\n", featureNames[i]);
}
// print RC MAPPING
for (i = 0; i < 8; i++)
buf[mcfg.rcmap[i]] = rcChannelLetters[i];
buf[i] = '\0';
printf("map %s\r\n", buf);
// print settings
for (i = 0; i < VALUE_COUNT; i++) {
setval = &valueTable[i];
printf("set %s = ", valueTable[i].name);
cliPrintVar(setval, 0);
cliPrint("\r\n");
}
}
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;
///////////////////////////
}
}
}
static void cliVersion(char *cmdline)
{
cliPrint("Afro32 CLI version 2.2 " __DATE__ " / " __TIME__);
}
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;
///////////////////////////
}
}
}
static void cliPrompt(void)
{
cliPrint("\r\n# ");
}
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;
///////////////////////////
}
}
}