char *readStringCLI(char *data, uint8_t length)
{
uint8_t index = 0;
uint8_t timeout = 0;
do
{
if (cliPortAvailable() == false)
{
delay(10);
timeout++;
}
else
{
data[index] = cliPortRead();
timeout = 0;
index++;
}
}
while ((index == 0 || data[index-1] != ';') && (timeout < 5) && (index < length));
data[index] = '\0';
return data;
}
float readFloatCLI(void)
{
uint8_t index = 0;
uint8_t timeout = 0;
char data[13] = "";
do
{
if (cliPortAvailable() == false)
{
delay(10);
timeout++;
}
else
{
data[index] = cliPortRead();
timeout = 0;
index++;
}
}
while ((index == 0 || data[index-1] != ';') && (timeout < 5) && (index < sizeof(data)-1));
data[index] = '\0';
return stringToFloat(data);
}
void cliCom(void)
{
uint8_t index;
char mvlkToggleString[5] = { 0, 0, 0, 0, 0 };
if ((cliPortAvailable() && !validCliCommand))
{
cliQuery = cliPortRead();
if (cliQuery == '#') // Check to see if we should toggle mavlink msg state
{
while (cliPortAvailable == false);
readStringCLI(mvlkToggleString, 5);
if ((mvlkToggleString[0] == '#') &&
(mvlkToggleString[1] == '#') &&
(mvlkToggleString[2] == '#') &&
(mvlkToggleString[3] == '#'))
{
if (eepromConfig.mavlinkEnabled == false)
{
eepromConfig.mavlinkEnabled = true;
eepromConfig.activeTelemetry = 0x0000;
}
else
{
eepromConfig.mavlinkEnabled = false;
}
if (mvlkToggleString[4] == 'W')
{
cliPortPrint("\nWriting EEPROM Parameters....\n");
writeEEPROM();
}
}
}
}
validCliCommand = false;
if ((eepromConfig.mavlinkEnabled == false) && (cliQuery != '#'))
{
switch (cliQuery)
{
///////////////////////////////
case 'a': // Rate PIDs
cliPortPrintF("\nRoll Rate PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[ROLL_RATE_PID].B,
eepromConfig.PID[ROLL_RATE_PID].P,
eepromConfig.PID[ROLL_RATE_PID].I,
eepromConfig.PID[ROLL_RATE_PID].D,
eepromConfig.PID[ROLL_RATE_PID].windupGuard,
eepromConfig.PID[ROLL_RATE_PID].dErrorCalc ? "Error" : "State");
cliPortPrintF("Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[PITCH_RATE_PID].B,
eepromConfig.PID[PITCH_RATE_PID].P,
eepromConfig.PID[PITCH_RATE_PID].I,
eepromConfig.PID[PITCH_RATE_PID].D,
eepromConfig.PID[PITCH_RATE_PID].windupGuard,
eepromConfig.PID[PITCH_RATE_PID].dErrorCalc ? "Error" : "State");
cliPortPrintF("Yaw Rate PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[YAW_RATE_PID].B,
eepromConfig.PID[YAW_RATE_PID].P,
eepromConfig.PID[YAW_RATE_PID].I,
eepromConfig.PID[YAW_RATE_PID].D,
eepromConfig.PID[YAW_RATE_PID].windupGuard,
eepromConfig.PID[YAW_RATE_PID].dErrorCalc ? "Error" : "State");
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'b': // Attitude PIDs
cliPortPrintF("\nRoll Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[ROLL_ATT_PID].B,
eepromConfig.PID[ROLL_ATT_PID].P,
eepromConfig.PID[ROLL_ATT_PID].I,
eepromConfig.PID[ROLL_ATT_PID].D,
eepromConfig.PID[ROLL_ATT_PID].windupGuard,
eepromConfig.PID[ROLL_ATT_PID].dErrorCalc ? "Error" : "State");
cliPortPrintF("Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[PITCH_ATT_PID].B,
eepromConfig.PID[PITCH_ATT_PID].P,
eepromConfig.PID[PITCH_ATT_PID].I,
eepromConfig.PID[PITCH_ATT_PID].D,
eepromConfig.PID[PITCH_ATT_PID].windupGuard,
eepromConfig.PID[PITCH_ATT_PID].dErrorCalc ? "Error" : "State");
cliPortPrintF("Heading PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[HEADING_PID].B,
eepromConfig.PID[HEADING_PID].P,
eepromConfig.PID[HEADING_PID].I,
eepromConfig.PID[HEADING_PID].D,
eepromConfig.PID[HEADING_PID].windupGuard,
eepromConfig.PID[HEADING_PID].dErrorCalc ? "Error" : "State");
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'c': // Velocity PIDs
cliPortPrintF("\nhDot PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[HDOT_PID].B,
eepromConfig.PID[HDOT_PID].P,
eepromConfig.PID[HDOT_PID].I,
eepromConfig.PID[HDOT_PID].D,
eepromConfig.PID[HDOT_PID].windupGuard,
eepromConfig.PID[HDOT_PID].dErrorCalc ? "Error" : "State");
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'd': // Position PIDs
cliPortPrintF("\nh PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[H_PID].B,
eepromConfig.PID[H_PID].P,
eepromConfig.PID[H_PID].I,
eepromConfig.PID[H_PID].D,
eepromConfig.PID[H_PID].windupGuard,
eepromConfig.PID[H_PID].dErrorCalc ? "Error" : "State");
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'e': // Loop Delta Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz,
deltaTime500Hz,
deltaTime100Hz,
deltaTime50Hz,
deltaTime10Hz,
deltaTime5Hz,
deltaTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'f': // Loop Execution Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz,
executionTime500Hz,
executionTime100Hz,
executionTime50Hz,
executionTime10Hz,
executionTime5Hz,
executionTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'g': // 500 Hz Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'h': // 100 hz Earth Axis Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS],
earthAxisAccels[YAXIS],
earthAxisAccels[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'i': // 500 hz Gyros
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D,
sensors.gyro500Hz[PITCH] * R2D,
sensors.gyro500Hz[YAW ] * R2D,
mpuTemperature);
validCliCommand = false;
break;
///////////////////////////////
case 'j': // 10 Hz Mag Data
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS],
sensors.mag10Hz[YAXIS],
sensors.mag10Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'k': // Vertical Axis Variables
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt50Hz,
hDotEstimate,
hEstimate);
validCliCommand = false;
break;
///////////////////////////////
case 'l': // Attitudes
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D,
sensors.attitude500Hz[PITCH] * R2D,
sensors.attitude500Hz[YAW ] * R2D);
validCliCommand = false;
break;
///////////////////////////////
case 'm': // Axis PIDs
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", axisPID[ROLL ],
axisPID[PITCH],
axisPID[YAW ]);
validCliCommand = false;
break;
///////////////////////////////
case 'o':
cliPortPrintF("%9.4f\n", batteryVoltage);
validCliCommand = false;
break;
///////////////////////////////
case 'p': // Primary Spektrum Raw Data
cliPortPrintF("%04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X\n", primarySpektrumState.lostFrameCnt,
primarySpektrumState.rcAvailable,
primarySpektrumState.values[0],
primarySpektrumState.values[1],
primarySpektrumState.values[2],
primarySpektrumState.values[3],
primarySpektrumState.values[4],
primarySpektrumState.values[5],
primarySpektrumState.values[6],
primarySpektrumState.values[7]);
validCliCommand = false;
break;
///////////////////////////////
case 'q': // Not Used
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'r':
if (flightMode == RATE)
cliPortPrint("Flight Mode = RATE ");
else if (flightMode == ATTITUDE)
cliPortPrint("Flight Mode = ATTITUDE ");
if (headingHoldEngaged == true)
cliPortPrint("Heading Hold = ENGAGED ");
else
cliPortPrint("Heading Hold = DISENGAGED ");
cliPortPrint("Alt Hold = ");
switch (verticalModeState)
{
case ALT_DISENGAGED_THROTTLE_ACTIVE:
cliPortPrint("Alt Disenaged Throttle Active\n");
break;
case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
cliPortPrint("Alt Hold Fixed at Engagement Alt\n");
break;
case ALT_HOLD_AT_REFERENCE_ALTITUDE:
cliPortPrint("Alt Hold at Reference Alt\n");
break;
case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
cliPortPrint("V Velocity Hold at Reference Vel\n");
break;
case ALT_DISENGAGED_THROTTLE_INACTIVE:
cliPortPrint("Alt Disengaged Throttle Inactive\n");
break;
}
validCliCommand = false;
break;
///////////////////////////////
case 's': // Raw Receiver Commands
if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0))
{
for (index = 0; index < maxChannelNum - 1; index++)
cliPortPrintF("%4ld, ", spektrumBuf[index]);
cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]);
}
else if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum == 0))
cliPortPrint("Invalid Number of Spektrum Channels....\n");
else
{
for (index = 0; index < 7; index++)
cliPortPrintF("%4i, ", ppmRxRead(index));
cliPortPrintF("%4i\n", ppmRxRead(7));
}
validCliCommand = false;
break;
///////////////////////////////
case 't': // Processed Receiver Commands
for (index = 0; index < 7; index++)
cliPortPrintF("%8.2f, ", rxCommand[index]);
cliPortPrintF("%8.2f\n", rxCommand[7]);
validCliCommand = false;
break;
///////////////////////////////
case 'u': // Command in Detent Discretes
cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false");
cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false");
cliPortPrintF("%s\n", commandInDetent[YAW ] ? " true" : "false");
validCliCommand = false;
break;
///////////////////////////////
case 'v': // ESC PWM Outputs
cliPortPrintF("%4ld, ", TIM4->CCR4);
cliPortPrintF("%4ld, ", TIM4->CCR3);
cliPortPrintF("%4ld, ", TIM4->CCR2);
cliPortPrintF("%4ld, ", TIM4->CCR1);
cliPortPrintF("%4ld, ", TIM1->CCR4);
cliPortPrintF("%4ld\n", TIM1->CCR1);
validCliCommand = false;
break;
///////////////////////////////
case 'x':
validCliCommand = false;
break;
///////////////////////////////
case 'y': // ESC Calibration
escCalibration();
cliQuery = 'x';
break;
///////////////////////////////
case 'z': // Voltage monitor ADC, Battery voltage
cliPortPrintF("%7.2f, %5.2f\n", voltageMonitor(), batteryVoltage);
break;
///////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////
case 'A': // Read Roll Rate PID Values
readCliPID(ROLL_RATE_PID);
cliPortPrint( "\nRoll Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'B': // Read Pitch Rate PID Values
readCliPID(PITCH_RATE_PID);
cliPortPrint( "\nPitch Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'C': // Read Yaw Rate PID Values
readCliPID(YAW_RATE_PID);
cliPortPrint( "\nYaw Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'D': // Read Roll Attitude PID Values
readCliPID(ROLL_ATT_PID);
cliPortPrint( "\nRoll Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'E': // Read Pitch Attitude PID Values
readCliPID(PITCH_ATT_PID);
cliPortPrint( "\nPitch Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'F': // Read Heading Hold PID Values
readCliPID(HEADING_PID);
cliPortPrint( "\nHeading PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'I': // Read hDot PID Values
readCliPID(HDOT_PID);
cliPortPrint( "\nhDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'L': // Read h PID Values
readCliPID(H_PID);
cliPortPrint( "\nh PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'N': // Mixer CLI
mixerCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'O': // Receiver CLI
receiverCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'P': // Sensor CLI
sensorCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'R': // Reset to Bootloader
cliPortPrint("Entering Bootloader....\n\n");
delay(100);
systemReset(true);
break;
///////////////////////////////
case 'S': // Reset System
cliPortPrint("\nSystem Reseting....\n\n");
delay(100);
systemReset(false);
break;
///////////////////////////////
case 'T': // Telemetry CLI
telemetryCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'U': // EEPROM CLI
eepromCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'V': // Reset EEPROM Parameters
cliPortPrint( "\nEEPROM Parameters Reset....\n" );
checkFirstTime(true);
cliPortPrint("\nSystem Resetting....\n\n");
delay(100);
systemReset(false);
break;
///////////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n");
writeEEPROM();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'X': // Not Used
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Y': // Not Used
cliQuery = 'x';
break;
///////////////////////////////
case 'Z': // Not Used
cliQuery = 'x';
break;
///////////////////////////////
case '?': // Command Summary
cliBusy = true;
cliPortPrint("\n");
cliPortPrint("'a' Rate PIDs 'A' Set Roll Rate PID Data AB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'b' Attitude PIDs 'B' Set Pitch Rate PID Data BB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'c' Velocity PIDs 'C' Set Yaw Rate PID Data CB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'d' Position PIDs 'D' Set Roll Att PID Data DB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'e' Loop Delta Times 'E' Set Pitch Att PID Data EB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'f' Loop Execution Times 'F' Set Hdg Hold PID Data FB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'g' 500 Hz Accels 'G' Not Used\n");
cliPortPrint("'h' 100 Hz Earth Axis Accels 'H' Not Used\n");
cliPortPrint("'i' 500 Hz Gyros 'I' Set hDot PID Data IB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'j' 10 hz Mag Data 'J' Not Used\n");
cliPortPrint("'k' Vertical Axis Variable 'K' Not Used\n");
cliPortPrint("'l' Attitudes 'L' Set h PID Data LB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("\n");
cliPortPrint("Press space bar for more, or enter a command....\n");
while (cliPortAvailable() == false);
cliQuery = cliPortRead();
if (cliQuery != ' ')
{
validCliCommand = true;
cliBusy = false;
return;
}
cliPortPrint("\n");
cliPortPrint("'m' Axis PIDs 'M' Not Used\n");
cliPortPrint("'n' Not Used 'N' Mixer CLI\n");
cliPortPrint("'o' Battery Voltage 'O' Receiver CLI\n");
cliPortPrint("'p' Not Used 'P' Sensor CLI\n");
cliPortPrint("'q' Primary Spektrum Raw Data 'Q' Not Used\n");
cliPortPrint("'r' Mode States 'R' Reset and Enter Bootloader\n");
cliPortPrint("'s' Raw Receiver Commands 'S' Reset\n");
cliPortPrint("'t' Processed Receiver Commands 'T' Telemetry CLI\n");
cliPortPrint("'u' Command In Detent Discretes 'U' EEPROM CLI\n");
cliPortPrint("'v' Motor PWM Outputs 'V' Reset EEPROM Parameters\n");
cliPortPrint("'w' Not Used 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Terminate Serial Communication 'X' Not Used\n");
cliPortPrint("\n");
cliPortPrint("Press space bar for more, or enter a command....\n");
while (cliPortAvailable() == false);
cliQuery = cliPortRead();
if (cliQuery != ' ')
{
validCliCommand = true;
cliBusy = false;
return;
}
cliPortPrint("\n");
cliPortPrint("'y' ESC Calibration 'Y' Not Used\n");
cliPortPrint("'z' ADC Values 'Z' Not Used\n");
cliPortPrint("'#####' Toggle MavLink Msg State '?' Command Summary\n");
cliPortPrint("\n");
cliQuery = 'x';
cliBusy = false;
break;
///////////////////////////////
}
}
}
void cliCom(void)
{
uint16_t index;
char mvlkToggleString[5] = { 0, 0, 0, 0, 0 };
if ((cliPortAvailable() && !validCliCommand))
{
cliQuery = cliPortRead();
if (cliQuery == '#') // Check to see if we should toggle mavlink msg state
{
while (cliPortAvailable == false);
readStringCLI(mvlkToggleString, 5);
if ((mvlkToggleString[0] == '#') &&
(mvlkToggleString[1] == '#') &&
(mvlkToggleString[2] == '#') &&
(mvlkToggleString[3] == '#'))
{
if (systemConfig.mavlinkEnabled == false)
{
systemConfig.mavlinkEnabled = true;
systemConfig.activeTelemetry = 0x0000;
}
else
{
systemConfig.mavlinkEnabled = false;
}
if (mvlkToggleString[4] == 'W')
{
cliPortPrint("\nWriting EEPROM Parameters....\n");
writeSystemEEPROM();
}
}
}
}
validCliCommand = false;
if ((systemConfig.mavlinkEnabled == false) && (cliQuery != '#'))
{
switch (cliQuery)
{
///////////////////////////////
case 'a': // Rate PIDs
cliPortPrintF("\nRoll Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[ROLL_RATE_PID].P,
systemConfig.PID[ROLL_RATE_PID].I,
systemConfig.PID[ROLL_RATE_PID].D,
systemConfig.PID[ROLL_RATE_PID].N);
cliPortPrintF( "Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[PITCH_RATE_PID].P,
systemConfig.PID[PITCH_RATE_PID].I,
systemConfig.PID[PITCH_RATE_PID].D,
systemConfig.PID[PITCH_RATE_PID].N);
cliPortPrintF( "Yaw Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[YAW_RATE_PID].P,
systemConfig.PID[YAW_RATE_PID].I,
systemConfig.PID[YAW_RATE_PID].D,
systemConfig.PID[YAW_RATE_PID].N);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'b': // Attitude PIDs
cliPortPrintF("\nRoll Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[ROLL_ATT_PID].P,
systemConfig.PID[ROLL_ATT_PID].I,
systemConfig.PID[ROLL_ATT_PID].D,
systemConfig.PID[ROLL_ATT_PID].N);
cliPortPrintF( "Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[PITCH_ATT_PID].P,
systemConfig.PID[PITCH_ATT_PID].I,
systemConfig.PID[PITCH_ATT_PID].D,
systemConfig.PID[PITCH_ATT_PID].N);
cliPortPrintF( "Heading PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[HEADING_PID].P,
systemConfig.PID[HEADING_PID].I,
systemConfig.PID[HEADING_PID].D,
systemConfig.PID[HEADING_PID].N);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'c': // Velocity PIDs
cliPortPrintF("\nnDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[NDOT_PID].P,
systemConfig.PID[NDOT_PID].I,
systemConfig.PID[NDOT_PID].D,
systemConfig.PID[NDOT_PID].N);
cliPortPrintF( "eDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[EDOT_PID].P,
systemConfig.PID[EDOT_PID].I,
systemConfig.PID[EDOT_PID].D,
systemConfig.PID[EDOT_PID].N);
cliPortPrintF( "hDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[HDOT_PID].P,
systemConfig.PID[HDOT_PID].I,
systemConfig.PID[HDOT_PID].D,
systemConfig.PID[HDOT_PID].N);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'd': // Position PIDs
cliPortPrintF("\nN PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[N_PID].P,
systemConfig.PID[N_PID].I,
systemConfig.PID[N_PID].D,
systemConfig.PID[N_PID].N);
cliPortPrintF( "E PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[E_PID].P,
systemConfig.PID[E_PID].I,
systemConfig.PID[E_PID].D,
systemConfig.PID[E_PID].N);
cliPortPrintF( "h PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[H_PID].P,
systemConfig.PID[H_PID].I,
systemConfig.PID[H_PID].D,
systemConfig.PID[H_PID].N);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'e': // Loop Delta Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz,
deltaTime500Hz,
deltaTime100Hz,
deltaTime50Hz,
deltaTime10Hz,
deltaTime5Hz,
deltaTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'f': // Loop Execution Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz,
executionTime500Hz,
executionTime100Hz,
executionTime50Hz,
executionTime10Hz,
executionTime5Hz,
executionTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'g': // 500 Hz Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'h': // 100 hz Earth Axis Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS],
earthAxisAccels[YAXIS],
earthAxisAccels[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'i': // 500 hz Gyros
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D,
sensors.gyro500Hz[PITCH] * R2D,
sensors.gyro500Hz[YAW ] * R2D,
mpu6000Temperature);
validCliCommand = false;
break;
///////////////////////////////
case 'j': // 10 Hz Mag Data
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS],
sensors.mag10Hz[YAXIS],
sensors.mag10Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'k': // Vertical Axis Variables
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld, %9.4f\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt50Hz,
hDotEstimate,
hEstimate,
ms5611Temperature,
aglRead());
validCliCommand = false;
break;
///////////////////////////////
case 'l': // Attitudes
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D,
sensors.attitude500Hz[PITCH] * R2D,
sensors.attitude500Hz[YAW ] * R2D);
validCliCommand = false;
break;
///////////////////////////////
case 'm': // Axis PIDs
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", ratePID[ROLL ],
ratePID[PITCH],
ratePID[YAW ]);
validCliCommand = false;
break;
///////////////////////////////
case 'n': // GPS Data
switch (gpsDataType)
{
///////////////////////
case 0:
cliPortPrintF("%12ld, %12ld, %12ld, %12ld, %12ld, %12ld, %4d, %4d\n", gps.latitude,
gps.longitude,
gps.hMSL,
gps.velN,
gps.velE,
gps.velD,
gps.fix,
gps.numSats);
break;
///////////////////////
case 1:
cliPortPrintF("%3d: ", gps.numCh);
for (index = 0; index < gps.numCh; index++)
cliPortPrintF("%3d ", gps.chn[index]);
cliPortPrint("\n");
break;
///////////////////////
case 2:
cliPortPrintF("%3d: ", gps.numCh);
for (index = 0; index < gps.numCh; index++)
cliPortPrintF("%3d ", gps.svid[index]);
cliPortPrint("\n");
break;
///////////////////////
case 3:
cliPortPrintF("%3d: ", gps.numCh);
for (index = 0; index < gps.numCh; index++)
cliPortPrintF("%3d ", gps.cno[index]);
cliPortPrint("\n");
break;
///////////////////////
}
validCliCommand = false;
break;
///////////////////////////////
case 'o':
cliPortPrintF("%9.4f\n", batteryVoltage);
validCliCommand = false;
break;
///////////////////////////////
case 'p': // Primary Spektrum Raw Data
cliPortPrintF("%04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X\n", primarySpektrumState.lostFrameCnt,
primarySpektrumState.rcAvailable,
primarySpektrumState.values[0],
primarySpektrumState.values[1],
primarySpektrumState.values[2],
primarySpektrumState.values[3],
primarySpektrumState.values[4],
primarySpektrumState.values[5],
primarySpektrumState.values[6],
primarySpektrumState.values[7]);
validCliCommand = false;
break;
///////////////////////////////
case 'q': // Not Used
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'r':
if (flightMode == RATE)
cliPortPrint("Flight Mode:RATE ");
else if (flightMode == ATTITUDE)
cliPortPrint("Flight Mode:ATTITUDE ");
else if (flightMode == GPS)
cliPortPrint("Flight Mode:GPS ");
if (headingHoldEngaged == true)
cliPortPrint("Heading Hold:ENGAGED ");
else
cliPortPrint("Heading Hold:DISENGAGED ");
switch (verticalModeState)
{
case ALT_DISENGAGED_THROTTLE_ACTIVE:
cliPortPrint("Alt:Disenaged Throttle Active ");
break;
case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
cliPortPrint("Alt:Hold Fixed at Engagement Alt ");
break;
case ALT_HOLD_AT_REFERENCE_ALTITUDE:
cliPortPrint("Alt:Hold at Reference Alt ");
break;
case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
cliPortPrint("Alt:Velocity Hold at Reference Vel ");
break;
case ALT_DISENGAGED_THROTTLE_INACTIVE:
cliPortPrint("Alt:Disengaged Throttle Inactive ");
break;
}
if (rxCommand[AUX3] > MIDCOMMAND)
cliPortPrint("Mode:Simple ");
else
cliPortPrint("Mode:Normal ");
if (rxCommand[AUX4] > MIDCOMMAND)
cliPortPrint("Emergency Bail:Active\n");
else
cliPortPrint("Emergency Bail:Inactive\n");
validCliCommand = false;
break;
///////////////////////////////
case 's': // Raw Receiver Commands
if ((systemConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0))
{
for (index = 0; index < maxChannelNum - 1; index++)
cliPortPrintF("%4ld, ", spektrumBuf[index]);
cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]);
}
else if ((systemConfig.receiverType == SPEKTRUM) && (maxChannelNum == 0))
cliPortPrint("Invalid Number of Spektrum Channels....\n");
else
{
for (index = 0; index < 7; index++)
cliPortPrintF("%4i, ", ppmRxRead(index));
cliPortPrintF("%4i\n", ppmRxRead(7));
}
validCliCommand = false;
break;
///////////////////////////////
case 't': // Processed Receiver Commands
for (index = 0; index < 7; index++)
cliPortPrintF("%8.2f, ", rxCommand[index]);
cliPortPrintF("%8.2f\n", rxCommand[7]);
validCliCommand = false;
break;
///////////////////////////////
case 'u': // Command in Detent Discretes
cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false");
cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false");
cliPortPrintF("%s\n", commandInDetent[YAW ] ? " true" : "false");
validCliCommand = false;
break;
///////////////////////////////
case 'v': // ESC PWM Outputs
cliPortPrintF("%4ld, ", TIM2->CCR1 );
cliPortPrintF("%4ld, ", TIM2->CCR2 );
cliPortPrintF("%4ld, ", TIM15->CCR1);
cliPortPrintF("%4ld, ", TIM15->CCR2);
cliPortPrintF("%4ld, ", TIM3->CCR1 );
cliPortPrintF("%4ld\n", TIM3->CCR2 );
validCliCommand = false;
break;
///////////////////////////////
case 'w': // Servo PWM Outputs
cliPortPrintF("%4ld, ", TIM4->CCR1);
cliPortPrintF("%4ld, ", TIM4->CCR2);
cliPortPrintF("%4ld, ", TIM4->CCR3);
cliPortPrintF("%4ld\n", TIM4->CCR4);
validCliCommand = false;
break;
///////////////////////////////
case 'x':
validCliCommand = false;
break;
///////////////////////////////
case 'y': // ESC Calibration
escCalibration();
cliQuery = 'x';
break;
///////////////////////////////
case 'z':
cliPortPrintF("%5.2f, %5.2f\n", voltageMonitor(),
adcChannel());
break;
///////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////
case 'A': // Read Roll Rate PID Values
readCliPID(ROLL_RATE_PID);
cliPortPrint( "\nRoll Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'B': // Read Pitch Rate PID Values
readCliPID(PITCH_RATE_PID);
cliPortPrint( "\nPitch Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'C': // Read Yaw Rate PID Values
readCliPID(YAW_RATE_PID);
cliPortPrint( "\nYaw Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'D': // Read Roll Attitude PID Values
readCliPID(ROLL_ATT_PID);
cliPortPrint( "\nRoll Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'E': // Read Pitch Attitude PID Values
readCliPID(PITCH_ATT_PID);
cliPortPrint( "\nPitch Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'F': // Read Heading Hold PID Values
readCliPID(HEADING_PID);
cliPortPrint( "\nHeading PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'G': // Read nDot PID Values
readCliPID(NDOT_PID);
cliPortPrint( "\nnDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'H': // Read eDot PID Values
readCliPID(EDOT_PID);
cliPortPrint( "\neDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'I': // Read hDot PID Values
readCliPID(HDOT_PID);
cliPortPrint( "\nhDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'J': // Read n PID Values
readCliPID(N_PID);
cliPortPrint( "\nn PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'K': // Read e PID Values
readCliPID(E_PID);
cliPortPrint( "\ne PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'L': // Read h PID Values
readCliPID(H_PID);
cliPortPrint( "\nh PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'N': // Mixer CLI
mixerCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'O': // Receiver CLI
receiverCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'P': // Sensor CLI
sensorCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Q': // GPS Data Selection
gpsDataType = (uint8_t)readFloatCLI();
cliPortPrint("\n");
cliQuery = 'n';
validCliCommand = false;
break;
///////////////////////////////
case 'R': // Reset to Bootloader
cliPortPrint("Entering Bootloader....\n\n");
delay(100);
systemReset(true);
break;
///////////////////////////////
case 'S': // Reset System
cliPortPrint("\nSystem Reseting....\n\n");
delay(100);
systemReset(false);
break;
///////////////////////////////
case 'T': // Telemetry CLI
telemetryCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'U': // EEPROM CLI
eepromCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'V': // Write Sensor EEPROM Parameters
cliPortPrint("\nWriting Sensor EEPROM Parameters....\n\n");
cliBusy = true;
writeSensorEEPROM();
cliBusy = false;
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'W': // Write System EEPROM Parameters
cliPortPrint("\nWriting System EEPROM Parameters....\n\n");
cliBusy = true;
writeSystemEEPROM();
cliBusy = false;
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'X': // Not Used
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Y': // Not Used
computeGeoMagElements();
cliQuery = 'x';
break;
///////////////////////////////
case 'Z': // Not Used
if (usbIsConfigured() == true)
cliPortPrint("\nUSB Configured TRUE\n");
else
cliPortPrint("\nUSB Configured FALSE\n");
if (usbIsConnected() == true)
cliPortPrint("\nUSB Connected TRUE\n");
else
cliPortPrint("\nUSB Connected FALSE\n");
cliQuery = 'x';
break;
///////////////////////////////
case '?': // Command Summary
cliBusy = true;
cliPortPrint("\n");
cliPortPrint("'a' Rate PIDs 'A' Set Roll Rate PID Data AB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'b' Attitude PIDs 'B' Set Pitch Rate PID Data BB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'c' Velocity PIDs 'C' Set Yaw Rate PID Data CB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'d' Position PIDs 'D' Set Roll Att PID Data DB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'e' Loop Delta Times 'E' Set Pitch Att PID Data EB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'f' Loop Execution Times 'F' Set Hdg Hold PID Data FB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'g' 500 Hz Accels 'G' Set nDot PID Data GB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'h' 100 Hz Earth Axis Accels 'H' Set eDot PID Data HB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'i' 500 Hz Gyros 'I' Set hDot PID Data IB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'j' 10 hz Mag Data 'J' Set n PID Data JB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'k' Vertical Axis Variable 'K' Set e PID Data KB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("'l' Attitudes 'L' Set h PID Data LB;P;I;D;windupGuard;dErrorCalc\n");
cliPortPrint("\n");
cliPortPrint("Press space bar for more, or enter a command....\n");
while (cliPortAvailable() == false);
cliQuery = cliPortRead();
if (cliQuery != ' ')
{
validCliCommand = true;
cliBusy = false;
return;
}
cliPortPrint("\n");
cliPortPrint("'m' Axis PIDs 'M' Not Used\n");
cliPortPrint("'n' GPS Data 'N' Mixer CLI\n");
cliPortPrint("'o' Battery Voltage 'O' Receiver CLI\n");
cliPortPrint("'p' Primary Spektrum Raw Data 'P' Sensor CLI\n");
cliPortPrint("'q' Not Used 'Q' GPS Data Selection\n");
cliPortPrint("'r' Mode States 'R' Reset and Enter Bootloader\n");
cliPortPrint("'s' Raw Receiver Commands 'S' Reset\n");
cliPortPrint("'t' Processed Receiver Commands 'T' Telemetry CLI\n");
cliPortPrint("'u' Command In Detent Discretes 'U' EEPROM CLI\n");
cliPortPrint("'v' Motor PWM Outputs 'V' Write Sensor EEPROM Parameters\n");
cliPortPrint("'w' Servo PWM Outputs 'W' Write System EEPROM Parameters\n");
cliPortPrint("'x' Terminate Serial Communication 'X' Not Used\n");
cliPortPrint("\n");
cliPortPrint("Press space bar for more, or enter a command....\n");
while (cliPortAvailable() == false);
cliQuery = cliPortRead();
if (cliQuery != ' ')
{
validCliCommand = true;
cliBusy = false;
return;
}
cliPortPrint("\n");
cliPortPrint("'y' ESC Calibration 'Y' Not Used\n");
cliPortPrint("'z' ADC Values 'Z' Not Used\n");
cliPortPrint(" '?' Command Summary\n");
cliPortPrint("\n");
cliQuery = 'x';
cliBusy = false;
break;
///////////////////////////////
}
}
}
void cliCom(void)
{
uint8_t index;
uint8_t numChannels = 8;
char mvlkToggleString[5] = { 0, 0, 0, 0, 0 };
if (eepromConfig.receiverType == PPM)
numChannels = eepromConfig.ppmChannels;
if ((cliPortAvailable() && !validCliCommand))
{
cliQuery = cliPortRead();
if (cliQuery == '#') // Check to see if we should toggle mavlink msg state
{
while (cliPortAvailable == false);
readStringCLI(mvlkToggleString, 5);
if ((mvlkToggleString[0] == '#') &&
(mvlkToggleString[1] == '#') &&
(mvlkToggleString[2] == '#') &&
(mvlkToggleString[3] == '#'))
{
if (eepromConfig.mavlinkEnabled == false)
{
eepromConfig.mavlinkEnabled = true;
eepromConfig.activeTelemetry = 0x0000;
}
else
{
eepromConfig.mavlinkEnabled = false;
}
if (mvlkToggleString[4] == 'W')
{
cliPortPrint("\nWriting EEPROM Parameters....\n");
writeEEPROM();
}
}
}
}
validCliCommand = false;
if ((eepromConfig.mavlinkEnabled == false) && (cliQuery != '#'))
{
switch (cliQuery)
{
///////////////////////////////
case 'a': // Rate PIDs
cliPortPrintF("\nRoll Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[ROLL_RATE_PID].P,
eepromConfig.PID[ROLL_RATE_PID].I,
eepromConfig.PID[ROLL_RATE_PID].D,
eepromConfig.PID[ROLL_RATE_PID].Limit);
cliPortPrintF( "Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[PITCH_RATE_PID].P,
eepromConfig.PID[PITCH_RATE_PID].I,
eepromConfig.PID[PITCH_RATE_PID].D,
eepromConfig.PID[PITCH_RATE_PID].Limit);
cliPortPrintF( "Yaw Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[YAW_RATE_PID].P,
eepromConfig.PID[YAW_RATE_PID].I,
eepromConfig.PID[YAW_RATE_PID].D,
eepromConfig.PID[YAW_RATE_PID].Limit);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'b': // Attitude PIDs
cliPortPrintF("\nRoll Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[ROLL_ATT_PID].P,
eepromConfig.PID[ROLL_ATT_PID].I,
eepromConfig.PID[ROLL_ATT_PID].D,
eepromConfig.PID[ROLL_ATT_PID].Limit);
cliPortPrintF( "Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[PITCH_ATT_PID].P,
eepromConfig.PID[PITCH_ATT_PID].I,
eepromConfig.PID[PITCH_ATT_PID].D,
eepromConfig.PID[PITCH_ATT_PID].Limit);
cliPortPrintF( "Heading PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[HEADING_PID].P,
eepromConfig.PID[HEADING_PID].I,
eepromConfig.PID[HEADING_PID].D,
eepromConfig.PID[HEADING_PID].Limit);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'c': // Velocity PIDs
cliPortPrintF("\nnDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[NDOT_PID].P,
eepromConfig.PID[NDOT_PID].I,
eepromConfig.PID[NDOT_PID].D,
eepromConfig.PID[NDOT_PID].Limit);
cliPortPrintF( "eDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[EDOT_PID].P,
eepromConfig.PID[EDOT_PID].I,
eepromConfig.PID[EDOT_PID].D,
eepromConfig.PID[EDOT_PID].Limit);
cliPortPrintF( "hDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[HDOT_PID].P,
eepromConfig.PID[HDOT_PID].I,
eepromConfig.PID[HDOT_PID].D,
eepromConfig.PID[HDOT_PID].Limit);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'd': // Position PIDs
cliPortPrintF("\nN PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[N_PID].P,
eepromConfig.PID[N_PID].I,
eepromConfig.PID[N_PID].D,
eepromConfig.PID[N_PID].Limit);
cliPortPrintF( "E PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[E_PID].P,
eepromConfig.PID[E_PID].I,
eepromConfig.PID[E_PID].D,
eepromConfig.PID[E_PID].Limit);
cliPortPrintF( "h PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[H_PID].P,
eepromConfig.PID[H_PID].I,
eepromConfig.PID[H_PID].D,
eepromConfig.PID[H_PID].Limit);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'e': // Loop Delta Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz,
deltaTime500Hz,
deltaTime100Hz,
deltaTime50Hz,
deltaTime10Hz,
deltaTime5Hz,
deltaTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'f': // Loop Execution Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz,
executionTime500Hz,
executionTime100Hz,
executionTime50Hz,
executionTime10Hz,
executionTime5Hz,
executionTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'g': // 100 Hz Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel100Hz[XAXIS],
sensors.accel100Hz[YAXIS],
sensors.accel100Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'h': // 100 hz Earth Axis Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS],
earthAxisAccels[YAXIS],
earthAxisAccels[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'i': // 500 hz Gyros
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D,
sensors.gyro500Hz[PITCH] * R2D,
sensors.gyro500Hz[YAW ] * R2D,
mpu6000Temperature);
validCliCommand = false;
break;
///////////////////////////////
case 'j': // 10 Hz Mag Data
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS],
sensors.mag10Hz[YAXIS],
sensors.mag10Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'k': // Vertical Axis Variables
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld, %9.4f\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt50Hz,
hDotEstimate,
hEstimate,
ms5611Temperature,
aglRead());
validCliCommand = false;
break;
///////////////////////////////
case 'l': // Attitudes
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D,
sensors.attitude500Hz[PITCH] * R2D,
sensors.attitude500Hz[YAW ] * R2D);
validCliCommand = false;
break;
///////////////////////////////
case 'm': // Axis PIDs
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", ratePID[ROLL ],
ratePID[PITCH],
ratePID[YAW ]);
validCliCommand = false;
break;
///////////////////////////////
case 'n': // GPS Data
switch (gpsDataType)
{
///////////////////////
case 0:
cliPortPrintF("%12ld, %12ld, %12ld, %12ld, %12ld, %12ld, %4d, %4d\n", gps.latitude,
gps.longitude,
gps.hMSL,
gps.velN,
gps.velE,
gps.velD,
gps.fix,
gps.numSats);
break;
///////////////////////
case 1:
cliPortPrintF("%3d: ", gps.numCh);
for (index = 0; index < gps.numCh; index++)
cliPortPrintF("%3d ", gps.chn[index]);
cliPortPrint("\n");
break;
///////////////////////
case 2:
cliPortPrintF("%3d: ", gps.numCh);
for (index = 0; index < gps.numCh; index++)
cliPortPrintF("%3d ", gps.svid[index]);
cliPortPrint("\n");
break;
///////////////////////
case 3:
cliPortPrintF("%3d: ", gps.numCh);
for (index = 0; index < gps.numCh; index++)
cliPortPrintF("%3d ", gps.cno[index]);
cliPortPrint("\n");
break;
///////////////////////
}
validCliCommand = false;
break;
///////////////////////////////
case 'o':
cliPortPrintF("%9.4f\n", batteryVoltage);
validCliCommand = false;
break;
///////////////////////////////
case 'p': // Using for Nav Debug
// cliPortPrintF("CurrentLLA:%d,%d, CurrentWP:%d,%d, NextWP:%d,%d\n",
// currentPosition.latitude,
// currentPosition.longitude,
// fromWaypoint.latitude,
// fromWaypoint.longitude,
// toWaypoint.latitude,
// toWaypoint.longitude);
cliPortPrintF("xt:%.9f, xte:%.9f, tae: %.9f, dist:%.3f\n",
crossTrack, crossTrackError, trackAngleError, distanceToNextWaypoint);
//cliQuery = 'x';
//validCliCommand = false;
break;
///////////////////////////////
case 'q': // Using for Nav Debug
cliPortPrintF("State:%d, WP:%d, P:%.1f, R:%.1f, Y:%.1f, Curr:%.1f, Des:%.1f, Dist:%.8f\n",
nextNavState,
waypointIndex,
ratePID[PITCH],
ratePID[ROLL],
ratePID[YAW],
currentHeading,
desiredHeading,
distanceToNextWaypoint);
//cliQuery = 'x';
//validCliCommand = false;
break;
///////////////////////////////
case 'r':
if (flightMode == RATE)
cliPortPrint("Flight Mode:RATE ");
else if (flightMode == ATTITUDE)
cliPortPrint("Flight Mode:ATTITUDE ");
else if (flightMode == GPS)
cliPortPrint("Flight Mode:GPS ");
if (headingHoldEngaged == true)
cliPortPrint("Heading Hold:ENGAGED ");
else
cliPortPrint("Heading Hold:DISENGAGED ");
switch (verticalModeState)
{
case ALT_DISENGAGED_THROTTLE_ACTIVE:
cliPortPrint("Alt:Disenaged Throttle Active ");
break;
case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
cliPortPrint("Alt:Hold Fixed at Engagement Alt ");
break;
case ALT_HOLD_AT_REFERENCE_ALTITUDE:
cliPortPrint("Alt:Hold at Reference Alt ");
break;
case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
cliPortPrint("Alt:Velocity Hold at Reference Vel ");
break;
case ALT_DISENGAGED_THROTTLE_INACTIVE:
cliPortPrint("Alt:Disengaged Throttle Inactive ");
break;
}
if (rxCommand[AUX3] > MIDCOMMAND)
cliPortPrint("Mode:Simple ");
else
cliPortPrint("Mode:Normal ");
if (rxCommand[AUX4] > MIDCOMMAND)
cliPortPrint("Emergency Bail:Active\n");
else
cliPortPrint("Emergency Bail:Inactive\n");
validCliCommand = false;
break;
///////////////////////////////
case 's': // Raw Receiver Commands
if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0))
{
for (index = 0; index < maxChannelNum - 1; index++)
cliPortPrintF("%4ld, ", spektrumBuf[index]);
cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]);
}
else if (eepromConfig.receiverType == SBUS)
{
for (index = 0; index < 7; index++)
cliPortPrintF("%4ld, ", sBusRead(index));
cliPortPrintF("%4ld\n", sBusRead(7));
}
else
{
for (index = 0; index < numChannels - 1; index++)
cliPortPrintF("%4i, ", Inputs[index].pulseWidth);
cliPortPrintF("%4i\n", Inputs[numChannels - 1].pulseWidth);
}
validCliCommand = false;
break;
///////////////////////////////
case 't': // Processed Receiver Commands
for (index = 0; index < numChannels - 1; index++)
cliPortPrintF("%8.2f, ", rxCommand[index]);
cliPortPrintF("%8.2f\n", rxCommand[numChannels - 1]);
validCliCommand = false;
break;
///////////////////////////////
case 'u': // Command in Detent Discretes
cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false");
cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false");
cliPortPrintF("%s\n", commandInDetent[YAW ] ? " true" : "false");
validCliCommand = false;
break;
///////////////////////////////
case 'v': // ESC PWM Outputs
cliPortPrintF("%4ld, ", TIM8->CCR4);
cliPortPrintF("%4ld, ", TIM8->CCR3);
cliPortPrintF("%4ld, ", TIM8->CCR2);
cliPortPrintF("%4ld, ", TIM8->CCR1);
cliPortPrintF("%4ld, ", TIM2->CCR1);
cliPortPrintF("%4ld, ", TIM2->CCR2);
cliPortPrintF("%4ld, ", TIM3->CCR1);
cliPortPrintF("%4ld\n", TIM3->CCR2);
validCliCommand = false;
break;
///////////////////////////////
case 'w': // Servo PWM Outputs
cliPortPrintF("%4ld, ", TIM5->CCR3);
cliPortPrintF("%4ld, ", TIM5->CCR2);
cliPortPrintF("%4ld\n", TIM5->CCR1);
validCliCommand = false;
break;
///////////////////////////////
case 'x':
validCliCommand = false;
break;
///////////////////////////////
case 'y': // ESC Calibration
escCalibration();
cliQuery = 'x';
break;
///////////////////////////////
case 'z': // ADC readings
cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f\n", adcValue(1),
adcValue(2),
adcValue(3),
adcValue(4),
adcValue(5),
adcValue(6),
adcValue(7));
break;
///////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////
case 'A': // Read Roll Rate PID Values
readCliPID(ROLL_RATE_PID);
cliPortPrint( "\nRoll Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'B': // Read Pitch Rate PID Values
readCliPID(PITCH_RATE_PID);
cliPortPrint( "\nPitch Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'C': // Read Yaw Rate PID Values
readCliPID(YAW_RATE_PID);
cliPortPrint( "\nYaw Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'D': // Read Roll Attitude PID Values
readCliPID(ROLL_ATT_PID);
cliPortPrint( "\nRoll Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'E': // Read Pitch Attitude PID Values
readCliPID(PITCH_ATT_PID);
cliPortPrint( "\nPitch Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'F': // Read Heading Hold PID Values
readCliPID(HEADING_PID);
cliPortPrint( "\nHeading PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'G': // Read nDot PID Values
readCliPID(NDOT_PID);
cliPortPrint( "\nnDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'H': // Read eDot PID Values
readCliPID(EDOT_PID);
cliPortPrint( "\neDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'I': // Read hDot PID Values
readCliPID(HDOT_PID);
cliPortPrint( "\nhDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'J': // Read n PID Values
readCliPID(N_PID);
cliPortPrint( "\nn PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'K': // Read e PID Values
readCliPID(E_PID);
cliPortPrint( "\ne PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'L': // Read h PID Values
readCliPID(H_PID);
cliPortPrint( "\nh PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'M': // MAX7456 CLI
max7456CLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'N': // Mixer CLI
mixerCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'O': // Receiver CLI
receiverCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'P': // Sensor CLI
sensorCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Q': // GPS Data Selection
gpsDataType = (uint8_t)readFloatCLI();
cliPortPrint("\n");
cliQuery = 'n';
validCliCommand = false;
break;
///////////////////////////////
case 'R': // Reset to Bootloader
cliPortPrint("Entering Bootloader....\n\n");
delay(100);
systemReset(true);
break;
///////////////////////////////
case 'S': // Reset System
cliPortPrint("\nSystem Reseting....\n\n");
delay(100);
systemReset(false);
break;
///////////////////////////////
case 'T': // Telemetry CLI
telemetryCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'U': // EEPROM CLI
eepromCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'V': // Reset EEPROM Parameters
cliPortPrint( "\nEEPROM Parameters Reset....\n" );
checkFirstTime(true);
cliPortPrint("\nSystem Resetting....\n\n");
delay(100);
systemReset(false);
break;
///////////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n");
writeEEPROM();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'X': // Not Used
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Y': // ADC CLI
adcCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Z': // Not Used
computeGeoMagElements();
cliQuery = 'x';
break;
///////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
// Communicator Messages /////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
case '@': // Communicator Messages
{
uint8_t msgType = cliPortRead();
switch (msgType) //update this
{
case 's': // Write out vehicle status
if (statusType++ < 3)
{
cliPortPrint("0");
cliPortPrintF("%1d", armed);
cliPortPrintF("%1d", flightMode);
cliPortPrintF("%1d", autoNavMode);
cliPortPrintF("%1d", verticalModeState);
cliPortPrintF("%1d", execUp);
writeShort(batteryVoltage * 100);
writeShort(hEstimate * 100);
writeShort(sensors.attitude500Hz[ROLL] * R2D * 10);
writeShort(sensors.attitude500Hz[PITCH] * R2D * 10);
writeShort(sensors.attitude500Hz[YAW] * R2D * 10);
writeShort(rxCommand[0]);
writeShort(rxCommand[1]);
writeShort(rxCommand[2]);
writeShort(rxCommand[3]);
cliPortPrint("\n"); // 25 bytes
}
else
{
cliPortPrint("1");
writeShort(rxCommand[4]);
writeShort(rxCommand[5]);
writeShort(rxCommand[6]);
writeShort(rxCommand[7]);
writeShort(TIM8->CCR4);
writeShort(TIM8->CCR3);
writeShort(TIM8->CCR2);
writeShort(TIM8->CCR1);
writeShort(TIM2->CCR1);
writeShort(TIM2->CCR2);
writeShort(TIM3->CCR1);
writeShort(TIM3->CCR2);
cliPortPrint("\n"); // 26 bytes
statusType = 0;
}
break;
case 'O': // Read in waypoints
{
int index = readFloatCLI();
if (index >= 0)
{
eepromConfig.route[index].latitude = readFloatCLI();
eepromConfig.route[index].longitude = readFloatCLI();
eepromConfig.route[index].altitude = readFloatCLI();
eepromConfig.route[index].speed = 1;
eepromConfig.route[index].type = 0;
}
else if (index == -1)
eepromConfig.storedWaypointCount = readFloatCLI();
break;
}
case 'o': // Write out waypoints
{
int index = readFloatCLI();
if (index < 0)
cliPortPrintF("%d\n", eepromConfig.storedWaypointCount);
else
{
cliPortPrintF("%d,%d,%d,%d\n",
index,
eepromConfig.route[index].latitude,
eepromConfig.route[index].longitude,
eepromConfig.route[index].altitude,
eepromConfig.route[index].speed,
eepromConfig.route[index].type);
}
}
break;
case '^': // Write out AutoNav status
{
int type = readFloatCLI();
if (type == 0) // send shortened position data
{
cliPortPrintF("%d,%d,%.1f\n",
gps.latitude,
gps.longitude,
sensors.attitude500Hz[YAW]*R2D);
}
if (type == 1)
{
cliPortPrintF("%d,%d,%d\n",
gps.hMSL,
gps.heading,
gps.speed);
}
if (type == 2)
{
cliPortPrintF("%d,%d,%d\n",
gps.numSats,
gps.hDop,
gps.fix);
}
if (type == 3) // send stored home position
{
cliPortPrintF("%d,%d,%d\n",
homePosition.latitude,
homePosition.longitude,
homePosition.altitude);
}
break;
}
case '!': // send software version
cliPortPrintF("%s\n", __AQ32PLUS_VERSION);
break;
case '<': // send autoNav status
cliPortPrintF("%d\n", getAutoNavState());
break;
case '>': // setup autopilot states
setAutoNavState(readFloatCLI());
break;
case 'M': // Read in mode setup
{
int slot = readFloatCLI();
eepromConfig.mode[slot].modeType = readFloatCLI();
eepromConfig.mode[slot].channel = readFloatCLI();
eepromConfig.mode[slot].state = readFloatCLI();
eepromConfig.mode[slot].minChannelValue = readFloatCLI();
eepromConfig.mode[slot].maxChannelValue = readFloatCLI();
break;
}
case 'm': // Write out mode setup
{
int slot = readFloatCLI();
cliPortPrintF("%d,%d,%d,%d,%d,%d\n",
slot,
eepromConfig.mode[slot].modeType,
eepromConfig.mode[slot].channel,
eepromConfig.mode[slot].state,
eepromConfig.mode[slot].minChannelValue,
eepromConfig.mode[slot].maxChannelValue);
break;
}
case 'A': // Read in AutoNav PIDs
{
int pidType = readFloatCLI();
readCliPID(pidType);
break;
}
case 'a': // Write out AutoNav PIDs
cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f,", eepromConfig.PID[AUTONAV_ROLL_PID].P,
eepromConfig.PID[AUTONAV_ROLL_PID].I,
eepromConfig.PID[AUTONAV_ROLL_PID].D,
eepromConfig.PID[AUTONAV_ROLL_PID].Limit);
cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f,", eepromConfig.PID[AUTONAV_PITCH_PID].P,
eepromConfig.PID[AUTONAV_PITCH_PID].I,
eepromConfig.PID[AUTONAV_PITCH_PID].D,
eepromConfig.PID[AUTONAV_PITCH_PID].Limit);
cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[AUTONAV_YAW_PID].P,
eepromConfig.PID[AUTONAV_YAW_PID].I,
eepromConfig.PID[AUTONAV_YAW_PID].D,
eepromConfig.PID[AUTONAV_YAW_PID].Limit);
break;
case 'C':
eepromConfig.xteScaling = readFloatCLI();
break;
case 'c':
cliPortPrintF("%.3f\n", eepromConfig.xteScaling);
break;
}
cliQuery = 'x';
validCliCommand = false;
break;
}
///////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///////////////////////////////
case '?': // Command Summary
cliBusy = true;
cliPortPrint("\n");
cliPortPrint("'a' Rate PIDs 'A' Set Roll Rate PID Data AP;I;D;N\n");
cliPortPrint("'b' Attitude PIDs 'B' Set Pitch Rate PID Data BP;I;D;N\n");
cliPortPrint("'c' Velocity PIDs 'C' Set Yaw Rate PID Data CP;I;D;N\n");
cliPortPrint("'d' Position PIDs 'D' Set Roll Att PID Data DP;I;D;N\n");
cliPortPrint("'e' Loop Delta Times 'E' Set Pitch Att PID Data EP;I;D;N\n");
cliPortPrint("'f' Loop Execution Times 'F' Set Hdg Hold PID Data FP;I;D;N\n");
cliPortPrint("'g' 500 Hz Accels 'G' Set nDot PID Data GP;I;D;N\n");
cliPortPrint("'h' 100 Hz Earth Axis Accels 'H' Set eDot PID Data HP;I;D;N\n");
cliPortPrint("'i' 500 Hz Gyros 'I' Set hDot PID Data IP;I;D;N\n");
cliPortPrint("'j' 10 hz Mag Data 'J' Set n PID Data JP;I;D;N\n");
cliPortPrint("'k' Vertical Axis Variable 'K' Set e PID Data KP;I;D;N\n");
cliPortPrint("'l' Attitudes 'L' Set h PID Data LP;I;D;N\n");
cliPortPrint("\n");
cliPortPrint("Press space bar for more, or enter a command....\n");
while (cliPortAvailable() == false);
cliQuery = cliPortRead();
if (cliQuery != ' ')
{
validCliCommand = true;
cliBusy = false;
return;
}
cliPortPrint("\n");
cliPortPrint("'m' Axis PIDs 'M' MAX7456 CLI\n");
cliPortPrint("'n' GPS Data 'N' Mixer CLI\n");
cliPortPrint("'o' Battery Voltage 'O' Receiver CLI\n");
cliPortPrint("'p' Not Used 'P' Sensor CLI\n");
cliPortPrint("'q' Not Used 'Q' GPS Data Selection\n");
cliPortPrint("'r' Mode States 'R' Reset and Enter Bootloader\n");
cliPortPrint("'s' Raw Receiver Commands 'S' Reset\n");
cliPortPrint("'t' Processed Receiver Commands 'T' Telemetry CLI\n");
cliPortPrint("'u' Command In Detent Discretes 'U' EEPROM CLI\n");
cliPortPrint("'v' Motor PWM Outputs 'V' Reset EEPROM Parameters\n");
cliPortPrint("'w' Servo PWM Outputs 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Terminate Serial Communication 'X' Not Used\n");
cliPortPrint("\n");
cliPortPrint("Press space bar for more, or enter a command....\n");
while (cliPortAvailable() == false);
cliQuery = cliPortRead();
if (cliQuery != ' ')
{
validCliCommand = true;
cliBusy = false;
return;
}
cliPortPrint("\n");
cliPortPrint("'y' ESC Calibration/Motor Verification 'Y' ADC CLI\n");
cliPortPrint("'z' ADC Values 'Z' WMM Test\n");
cliPortPrint(" '?' Command Summary\n");
cliPortPrint("\n");
cliQuery = 'x';
cliBusy = false;
break;
///////////////////////////////
}
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPortPrint("Sensor CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliPortRead();
cliPortPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", sensorConfig.accelTCBiasSlope[XAXIS],
sensorConfig.accelTCBiasSlope[YAXIS],
sensorConfig.accelTCBiasSlope[ZAXIS]);
cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", sensorConfig.accelTCBiasIntercept[XAXIS],
sensorConfig.accelTCBiasIntercept[YAXIS],
sensorConfig.accelTCBiasIntercept[ZAXIS]);
cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", sensorConfig.gyroTCBiasSlope[ROLL ],
sensorConfig.gyroTCBiasSlope[PITCH],
sensorConfig.gyroTCBiasSlope[YAW ]);
cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", sensorConfig.gyroTCBiasIntercept[ROLL ],
sensorConfig.gyroTCBiasIntercept[PITCH],
sensorConfig.gyroTCBiasIntercept[YAW ]);
cliPortPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", sensorConfig.magBias[XAXIS],
sensorConfig.magBias[YAXIS],
sensorConfig.magBias[ZAXIS]);
cliPortPrintF("Accel One G: %9.4f\n", accelOneG);
cliPortPrintF("Accel Cutoff: %9.4f\n", sensorConfig.accelCutoff);
cliPortPrintF("KpAcc (MARG): %9.4f\n", sensorConfig.KpAcc);
cliPortPrintF("KiAcc (MARG): %9.4f\n", sensorConfig.KiAcc);
cliPortPrintF("KpMag (MARG): %9.4f\n", sensorConfig.KpMag);
cliPortPrintF("KiMag (MARG): %9.4f\n", sensorConfig.KiMag);
cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", sensorConfig.compFilterA);
cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", sensorConfig.compFilterB);
cliPortPrint("MPU6000 DLPF: ");
switch(sensorConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPortPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPortPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPortPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPortPrint("42 Hz\n");
break;
}
cliPortPrint("Sensor Orientation: ");
switch(sensorConfig.sensorOrientation)
{
case 1:
cliPortPrint("Normal\n\n");
break;
case 2:
cliPortPrint("Rotated 90 Degrees CW\n\n");
break;
case 3:
cliPortPrint("Rotated 180 Degrees\n\n");
break;
case 4:
cliPortPrint("Rotated 90 Degrees CCW\n\n");
break;
default:
cliPortPrint("Normal\n\n");
}
if (sensorConfig.gpsVelocityHoldOnly)
cliPortPrint("GPS Velocity Hold Only\n\n");
else
cliPortPrint("GPS Velocity and Position Hold\n\n");
if (sensorConfig.verticalVelocityHoldOnly)
cliPortPrint("Vertical Velocity Hold Only\n\n");
else
cliPortPrint("Vertical Velocity and Altitude Hold\n\n");
cliPortPrintF("Voltage Monitor Scale: %9.4f\n", sensorConfig.voltageMonitorScale);
cliPortPrintF("Voltage Monitor Bias: %9.4f\n", sensorConfig.voltageMonitorBias);
cliPortPrintF("Number of Battery Cells: %1d\n\n", sensorConfig.batteryCells);
cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", sensorConfig.batteryLow);
cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", sensorConfig.batteryVeryLow);
cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", sensorConfig.batteryMaxLow);
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'g': // Toggle GPS Velocity Hold Only
if (sensorConfig.gpsVelocityHoldOnly)
sensorConfig.gpsVelocityHoldOnly = false;
else
sensorConfig.gpsVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (sensorConfig.verticalVelocityHoldOnly)
sensorConfig.verticalVelocityHoldOnly = false;
else
sensorConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
sensorConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, sensorConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
sensorConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
sensorConfig.KpAcc = readFloatCLI();
sensorConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
sensorConfig.KpMag = readFloatCLI();
sensorConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
sensorConfig.compFilterA = readFloatCLI();
sensorConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Sensor Orientation
sensorConfig.sensorOrientation = (uint8_t)readFloatCLI();
orientSensors();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'M': // Set Voltage Monitor Trip Points
sensorConfig.batteryLow = readFloatCLI();
sensorConfig.batteryVeryLow = readFloatCLI();
sensorConfig.batteryMaxLow = readFloatCLI();
thresholds[BATTERY_LOW].value = sensorConfig.batteryLow;
thresholds[BATTERY_VERY_LOW].value = sensorConfig.batteryVeryLow;
thresholds[BATTRY_MAX_LOW].value = sensorConfig.batteryMaxLow;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'V': // Set Voltage Monitor Parameters
sensorConfig.voltageMonitorScale = readFloatCLI();
sensorConfig.voltageMonitorBias = readFloatCLI();
sensorConfig.batteryCells = (uint8_t)readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write Sensor EEPROM Parameters
cliPortPrint("\nWriting Sensor EEPROM Parameters....\n\n");
writeSensorEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3\n");
cliPortPrint("'b' MPU6000 Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CKpAcc;KiAcc\n");
cliPortPrint(" 'D' Set kpMag/kiMag DKpMag;KiMag\n");
cliPortPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPortPrint(" 'F' Set Sensor Orientation F1 thru 4\n");
cliPortPrint("'g' Toggle GPS Velocity Hold Only 'M' Set Voltage Monitor Trip Points Mlow;veryLow;maxLow\n");
cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPortPrint("\n");
break;
///////////////////////////
}
}
}
void eepromCLI()
{
char c;
sensorConfig_t sensorRam;
systemConfig_t systemRam;
uint8_t eepromQuery = 'x';
uint8_t *p;
uint8_t *end;
uint8_t secondNibble;
uint8_t validQuery = false;
uint16_t i;
uint32_t c1, c2;
uint32_t size;
uint32_t time;
uint32_t charsEncountered;
///////////////////////////////////////////////////////////////////////////////
cliBusy = true;
cliPortPrint("\nEntering EEPROM CLI....\n\n");
while(true)
{
cliPortPrint("EEPROM CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
eepromQuery = cliPortRead();
cliPortPrint("\n");
switch(eepromQuery)
{
///////////////////////////
case 'a': // config struct data
c1 = sensorConfig.CRCAtEnd[0];
c2 = crc32B((uint32_t*)(&sensorConfig), // CRC32B[sensorConfig]
(uint32_t*)(&sensorConfig.CRCAtEnd));
cliPortPrintF("Sensor EEPROM structure information:\n");
cliPortPrintF("Version : %d\n", sensorConfig.version);
cliPortPrintF("Size : %d\n", sizeof(sensorConfig));
cliPortPrintF("CRC on last read : %08X\n", c1);
cliPortPrintF("Current CRC : %08X\n", c2);
if ( c1 != c2 )
cliPortPrintF(" CRCs differ. Current Sensor Config has not yet been saved.\n");
cliPortPrintF("CRC Flags :\n");
cliPortPrintF(" History Bad : %s\n", sensorConfig.CRCFlags & CRC_HistoryBad ? "true" : "false" );
validQuery = false;
break;
///////////////////////////
case 'b': // Write out to Console in Hex. (RAM -> console)
// we assume the flyer is not in the air, so that this is ok;
cliPortPrintF("\n");
cliPrintSensorEEPROM();
cliPortPrintF("\n");
if (crcCheckVal != crc32B((uint32_t*)(&sensorConfig), // CRC32B[sensorConfig CRC32B[sensorConfig]]
(uint32_t*)(&sensorConfig + 1)))
{
cliPortPrint("NOTE: in-memory sensor config CRC invalid; there have probably been\n");
cliPortPrint(" changes to sensor config since the last write to flash/eeprom.\n");
}
validQuery = false;
break;
///////////////////////////
case 'c': // Read Sensor Config -> RAM
cliPortPrint("Re-reading Sensor EEPROM.\n");
readSensorEEPROM();
validQuery = false;
break;
///////////////////////////
case 'd': // Read Console -> Sensor RAM
charsEncountered = 0;
secondNibble = 0;
size = sizeof(sensorConfig);
time = millis();
p = (uint8_t*)&sensorRam;
end = (uint8_t*)(&sensorRam + 1);
///////////////////////
cliPortPrintF("Ready to read in sensor config. Expecting %d (0x%03X) bytes as %d\n", size, size, size * 2);
cliPortPrintF("hexadecimal characters, optionally separated by [ \\n\\r_].\n");
cliPortPrintF("Times out if no character is received for %dms\n", TIMEOUT);
memset(p, 0, end - p);
while (p < end)
{
while (!cliPortAvailable() && millis() - time < TIMEOUT) {}
time = millis();
c = cliPortAvailable() ? cliPortRead() : '\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
charsEncountered++;
if (ignore)
continue;
if (hex == -1)
break;
*p |= secondNibble ? hex : hex << 4;
p += secondNibble;
secondNibble ^= 1;
}
if (c == 0)
{
cliPortPrintF("Did not receive enough hex chars! (got %d, expected %d)\n",
(p - (uint8_t*)&sensorRam) * 2 + secondNibble, size * 2);
}
else if (p < end || secondNibble)
{
cliPortPrintF("Invalid character found at position %d: '%c' (0x%02x)",
charsEncountered, c, c);
}
// HJI else if (crcCheckVal != crc32B((uint32_t*)(&sensorConfig), // CRC32B[sensorConfig CRC32B[sensorConfig]]
// HJI (uint32_t*)(&sensorConfig + 1)))
// HJI {
// HJI cliPortPrintF("CRC mismatch! Not writing to in-memory config.\n");
// HJI cliPortPrintF("Here's what was received:\n\n");
// HJI cliPrintSensorEEPROM();
// HJI }
else
{
// check to see if the newly received sytem config
// actually differs from what's in-memory
for (i = 0; i < size; i++)
if (((uint8_t*)&sensorRam)[i] != ((uint8_t*)&sensorConfig)[i])
break;
if (i == size)
{
cliPortPrintF("NOTE: Uploaded Sensor Config was Identical to RAM Config.\n");
}
else
{
sensorConfig = sensorRam;
cliPortPrintF("Sensor RAM Config updated!\n");
cliPortPrintF("NOTE: Sensor 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
time = millis();
while (millis() - time < TIMEOUT)
if (cliPortAvailable())
cliPortRead();
validQuery = false;
break;
///////////////////////////
case 'h': // Clear Bad Sensor History Flag
cliPortPrintF("Clearing Bad Sensor History Flag.\n");
sensorConfig.CRCFlags &= ~CRC_HistoryBad;
validQuery = false;
break;
///////////////////////////
case 'v': // Reset Sensor EEPROM Parameters
cliPortPrint( "\nSensor EEPROM Parameters Reset....(not rebooting)\n" );
checkSensorEEPROM(true);
validQuery = false;
break;
///////////////////////////
case 'w': // Write to Sensor EEPROM
cliPortPrint("\nWriting Sensor EEPROM Parameters....\n");
writeSensorEEPROM();
validQuery = false;
break;
///////////////////////////
case 'x': // exit EEPROM CLI
cliPortPrint("\nExiting EEPROM CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // config struct data
c1 = systemConfig.CRCAtEnd[0];
zeroPIDstates();
c2 = crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig]
(uint32_t*)(&systemConfig.CRCAtEnd));
cliPortPrintF("System EEPROM structure information:\n");
cliPortPrintF("Version : %d\n", systemConfig.version);
cliPortPrintF("Size : %d\n", sizeof(systemConfig));
cliPortPrintF("CRC on last read : %08X\n", c1);
cliPortPrintF("Current CRC : %08X\n", c2);
if ( c1 != c2 )
cliPortPrintF(" CRCs differ. Current SystemConfig has not yet been saved.\n");
cliPortPrintF("CRC Flags :\n");
cliPortPrintF(" History Bad : %s\n", systemConfig.CRCFlags & CRC_HistoryBad ? "true" : "false" );
validQuery = false;
break;
///////////////////////////
case 'B': // 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
zeroPIDstates();
cliPortPrintF("\n");
cliPrintSystemConfig();
cliPortPrintF("\n");
if (crcCheckVal != crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig CRC32B[systemConfig]]
(uint32_t*)(&systemConfig + 1)))
{
cliPortPrint("NOTE: in-memory system config CRC invalid; there have probably been\n");
cliPortPrint(" changes to system config since the last write to flash/eeprom.\n");
}
validQuery = false;
break;
///////////////////////////
case 'C': // Read System Config -> RAM
cliPortPrint("Re-reading System EEPROM.\n");
readSystemEEPROM();
validQuery = false;
break;
///////////////////////////
case 'D': // Read Console -> System RAM
charsEncountered = 0;
secondNibble = 0;
size = sizeof(systemConfig);
time = millis();
p = (uint8_t*)&systemRam;
end = (uint8_t*)(&systemRam + 1);
///////////////////////
cliPortPrintF("Ready to read in system config. Expecting %d (0x%03X) bytes as %d\n", size, size, size * 2);
cliPortPrintF("hexadecimal characters, optionally separated by [ \\n\\r_].\n");
cliPortPrintF("Times out if no character is received for %dms\n", TIMEOUT);
memset(p, 0, end - p);
while (p < end)
{
while (!cliPortAvailable() && millis() - time < TIMEOUT) {}
time = millis();
c = cliPortAvailable() ? cliPortRead() : '\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
charsEncountered++;
if (ignore)
continue;
if (hex == -1)
break;
*p |= secondNibble ? hex : hex << 4;
p += secondNibble;
secondNibble ^= 1;
}
if (c == 0)
{
cliPortPrintF("Did not receive enough hex chars! (got %d, expected %d)\n",
(p - (uint8_t*)&systemRam) * 2 + secondNibble, size * 2);
}
else if (p < end || secondNibble)
{
cliPortPrintF("Invalid character found at position %d: '%c' (0x%02x)",
charsEncountered, c, c);
}
// HJI else if (crcCheckVal != crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig CRC32B[systemConfig]]
// HJI (uint32_t*)(&systemConfig + 1)))
// HJI {
// HJI cliPortPrintF("CRC mismatch! Not writing to in-memory config.\n");
// HJI cliPortPrintF("Here's what was received:\n\n");
// HJI cliPrintSystemConfig();
// HJI }
else
{
// check to see if the newly received sytem config
// actually differs from what's in-memory
zeroPIDstates();
for (i = 0; i < size; i++)
if (((uint8_t*)&systemRam)[i] != ((uint8_t*)&systemConfig)[i])
break;
if (i == size)
{
cliPortPrintF("NOTE: Uploaded System Config was Identical to RAM Config.\n");
}
else
{
systemConfig = systemRam;
cliPortPrintF("System RAM Config updated!\n");
cliPortPrintF("NOTE: System 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
time = millis();
while (millis() - time < TIMEOUT)
if (cliPortAvailable())
cliPortRead();
validQuery = false;
break;
///////////////////////////
case 'H': // Clear Bad System History Flag
cliPortPrintF("Clearing Bad System History Flag.\n");
systemConfig.CRCFlags &= ~CRC_HistoryBad;
validQuery = false;
break;
///////////////////////////
case 'V': // Reset System EEPROM Parameters
cliPortPrint( "\nSystem EEPROM Parameters Reset....(not rebooting)\n" );
checkSystemEEPROM(true);
validQuery = false;
break;
///////////////////////////
case 'W': // Write out to System EEPROM
cliPortPrint("\nWriting System EEPROM Parameters....\n");
writeSystemEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
// 0 1 2 3 4 5 6 7
// 01234567890123456789012345678901234567890123456789012345678901234567890123456789
cliPortPrintF("\n");
cliPortPrintF("'a' Display Sensor Config Information 'A' Display System Config Information\n");
cliPortPrintF("'b' Write Sensor Config -> Console 'B' Write System Config - > Console\n");
cliPortPrintF("'c' Read Sensor Config -> RAM 'C' Read System Config -> RAM\n");
cliPortPrintF("'d' Read Console -> Sensor RAM 'D' Read Console -> System RAM\n");
cliPortPrintF("'h' Clear Sensor CRC Bad History flag 'H' Clear System CRC Bad History flag\n");
cliPortPrintF("'v' Reset Sensor Config to Default 'V' Reset System Config to Default\n");
cliPortPrintF("'w' Write Sensor Config -> EEPROM 'W' Write System Config -> EEPROM\n");
cliPortPrintF("'x' Exit EEPROM CLI '?' Command Summary\n");
cliPortPrintF("\n");
break;
///////////////////////////
}
}
}
void receiverCLI()
{
char rcOrderString[9];
float tempFloat;
uint8_t index;
uint8_t receiverQuery = 'x';
uint8_t validQuery = false;
NVIC_InitTypeDef NVIC_InitStructure;
cliBusy = true;
cliPortPrint("\nEntering Receiver CLI....\n\n");
while(true)
{
cliPortPrint("Receiver CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
receiverQuery = cliPortRead();
cliPortPrint("\n");
switch(receiverQuery)
{
///////////////////////////
case 'a': // Receiver Configuration
cliPortPrint("\nReceiver Type: ");
switch(systemConfig.receiverType)
{
case PPM:
cliPortPrint("PPM\n");
break;
case SPEKTRUM:
cliPortPrint("Spektrum\n");
break;
}
cliPortPrint("Current RC Channel Assignment: ");
for (index = 0; index < 8; index++)
rcOrderString[systemConfig.rcMap[index]] = rcChannelLetters[index];
rcOrderString[index] = '\0';
cliPortPrint(rcOrderString); cliPortPrint("\n");
cliPortPrintF("Secondary Spektrum: ");
if ((systemConfig.slaveSpektrum == true) && false) // HJI Inhibit Slave Spektrum on Naze32 Pro
cliPortPrintF("Installed\n");
else
cliPortPrintF("Uninstalled\n");
cliPortPrintF("Mid Command: %4ld\n", (uint16_t)systemConfig.midCommand);
cliPortPrintF("Min Check: %4ld\n", (uint16_t)systemConfig.minCheck);
cliPortPrintF("Max Check: %4ld\n", (uint16_t)systemConfig.maxCheck);
cliPortPrintF("Min Throttle: %4ld\n", (uint16_t)systemConfig.minThrottle);
cliPortPrintF("Max Thottle: %4ld\n\n", (uint16_t)systemConfig.maxThrottle);
tempFloat = systemConfig.rollAndPitchRateScaling * 180000.0 / PI;
cliPortPrintF("Max Roll and Pitch Rate Cmd: %6.2f DPS\n", tempFloat);
tempFloat = systemConfig.yawRateScaling * 180000.0 / PI;
cliPortPrintF("Max Yaw Rate Cmd: %6.2f DPS\n\n", tempFloat);
cliPortPrintF("Roll Rate Cmd Tau: %6.2f\n", systemConfig.rollRateCmdLowPassTau);
cliPortPrintF("Pitch Rate Cmd Tau: %6.2f\n\n", systemConfig.pitchRateCmdLowPassTau);
tempFloat = systemConfig.attitudeScaling * 180000.0 / PI;
cliPortPrintF("Max Attitude Cmd: %6.2f Degrees\n\n", tempFloat);
cliPortPrintF("Roll Attitude Cmd Tau: %6.2f\n", systemConfig.rollAttCmdLowPassTau);
cliPortPrintF("Pitch Attitude Cmd Tau: %6.2f\n\n", systemConfig.pitchAttCmdLowPassTau);
cliPortPrintF("Arm Delay Count: %3d Frames\n", systemConfig.armCount);
cliPortPrintF("Disarm Delay Count: %3d Frames\n\n", systemConfig.disarmCount);
validQuery = false;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Receiver CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Toggle PPM/Spektrum Satellite Receiver
if (systemConfig.receiverType == PPM)
{
NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_ITConfig(TIM1, TIM_IT_CC1, DISABLE);
systemConfig.receiverType = SPEKTRUM;
spektrumInit();
}
else
{
NVIC_InitStructure.NVIC_IRQChannel = TIM1_TRG_COM_TIM17_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_ITConfig(TIM17, TIM_IT_Update, DISABLE);
systemConfig.receiverType = PPM;
ppmRxInit();
}
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read RC Control Order
readStringCLI( rcOrderString, 8 );
parseRcChannels( rcOrderString );
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // Toggle Slave Spektrum State
// HJI Inhibit Slave Spektrum on Naze32 Pro if (systemConfig.slaveSpektrum == true)
systemConfig.slaveSpektrum = false;
// HJI Inhibit Slave Spektrum on Naze32 Pro else
// HJI Inhibit Slave Spektrum on Naze32 Pro systemConfig.slaveSpektrum = true;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // Read RC Control Points
systemConfig.midCommand = readFloatCLI();
systemConfig.minCheck = readFloatCLI();
systemConfig.maxCheck = readFloatCLI();
systemConfig.minThrottle = readFloatCLI();
systemConfig.maxThrottle = readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // Read Arm/Disarm Counts
systemConfig.armCount = (uint8_t)readFloatCLI();
systemConfig.disarmCount = (uint8_t)readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read Max Rate Value
systemConfig.rollAndPitchRateScaling = readFloatCLI() / 180000.0f * PI;
systemConfig.yawRateScaling = readFloatCLI() / 180000.0f * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'G': // Read Max Attitude Value
systemConfig.attitudeScaling = readFloatCLI() / 180000.0f * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'H': // Read Rate Cmd Tau Value
systemConfig.rollRateCmdLowPassTau = readFloatCLI();
systemConfig.pitchRateCmdLowPassTau = readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'I': // Read Attitude Cmd Tau Value
systemConfig.rollAttCmdLowPassTau = readFloatCLI();
systemConfig.pitchAttCmdLowPassTau = readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write System EEPROM Parameters
cliPortPrint("\nWriting System EEPROM Parameters....\n\n");
writeSystemEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Receiver Configuration Data 'A' Toggle PPM/Spektrum Receiver\n");
cliPortPrint(" 'B' Set RC Control Order BTAER1234\n");
cliPortPrint(" 'C' Toggle Slave Spektrum State\n");
cliPortPrint(" 'D' Set RC Control Points DmidCmd;minChk;maxChk;minThrot;maxThrot\n");
cliPortPrint(" 'E' Set Arm/Disarm Counts EarmCount;disarmCount\n");
cliPortPrint(" 'F' Set Maximum Rate Commands FRP;Y RP = Roll/Pitch, Y = Yaw\n");
cliPortPrint(" 'G' Set Maximum Attitude Command\n");
cliPortPrint(" 'H' Set Roll/Pitch Rate Command Filters HROLL;PITCH\n");
cliPortPrint(" 'I' Set Roll/Pitch Att Command Filters IROLL;PITCH\n");
cliPortPrint(" 'W' Write System EEPROM Parameters\n");
cliPortPrint("'x' Exit Receiver CLI '?' Command Summary\n");
cliPortPrint("\n");
break;
///////////////////////////
}
}
}
void cliCom(void)
{
uint8_t i2cReadBuff; // TEMP
uint8_t index;
uint8_t numChannels = 8;
char mvlkToggleString[5] = { 0, 0, 0, 0, 0 };
if (eepromConfig.receiverType == PPM)
numChannels = eepromConfig.ppmChannels;
if ((cliPortAvailable() && !validCliCommand))
{
// Pull one character from buffer to find command
cliQuery = cliPortRead();
// Check to see if we should toggle MAVLink state (pound sign)
if (cliQuery == '#')
{
while (cliPortAvailable == false);
// Check to see if we have 4 pound signs
readStringCLI(mvlkToggleString, 5);
if ((mvlkToggleString[0] == '#') &&
(mvlkToggleString[1] == '#') &&
(mvlkToggleString[2] == '#') &&
(mvlkToggleString[3] == '#'))
{
// Toggle MAVLink
if (eepromConfig.mavlinkEnabled == false)
eepromConfig.mavlinkEnabled = true;
else
eepromConfig.mavlinkEnabled = false;
// Write EEPROM state if pounds were followed by W
if (mvlkToggleString[4] == 'W')
{
cliPortPrint("\nWriting EEPROM Parameters....\n");
writeEEPROM();
}
}
}
}
validCliCommand = false;
// If MAVLink is disabled and we aren't toggling MAVLink, assume CLI command
if ((eepromConfig.mavlinkEnabled == false) && (cliQuery != '#'))
{
switch (cliQuery)
{
///////////////////////////////
case 'a': // Rate PIDs
cliPortPrintF("\nRoll Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[ROLL_RATE_PID].P,
eepromConfig.PID[ROLL_RATE_PID].I,
eepromConfig.PID[ROLL_RATE_PID].D,
eepromConfig.PID[ROLL_RATE_PID].Limit);
cliPortPrintF( "Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[PITCH_RATE_PID].P,
eepromConfig.PID[PITCH_RATE_PID].I,
eepromConfig.PID[PITCH_RATE_PID].D,
eepromConfig.PID[PITCH_RATE_PID].Limit);
cliPortPrintF( "Yaw Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[YAW_RATE_PID].P,
eepromConfig.PID[YAW_RATE_PID].I,
eepromConfig.PID[YAW_RATE_PID].D,
eepromConfig.PID[YAW_RATE_PID].Limit);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'b': // Attitude PIDs
cliPortPrintF("\nRoll Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[ROLL_ATT_PID].P,
eepromConfig.PID[ROLL_ATT_PID].I,
eepromConfig.PID[ROLL_ATT_PID].D,
eepromConfig.PID[ROLL_ATT_PID].Limit);
cliPortPrintF( "Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[PITCH_ATT_PID].P,
eepromConfig.PID[PITCH_ATT_PID].I,
eepromConfig.PID[PITCH_ATT_PID].D,
eepromConfig.PID[PITCH_ATT_PID].Limit);
cliPortPrintF( "Heading PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[HEADING_PID].P,
eepromConfig.PID[HEADING_PID].I,
eepromConfig.PID[HEADING_PID].D,
eepromConfig.PID[HEADING_PID].Limit);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'c': // Velocity PIDs
cliPortPrintF("\nnDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[NDOT_PID].P,
eepromConfig.PID[NDOT_PID].I,
eepromConfig.PID[NDOT_PID].D,
eepromConfig.PID[NDOT_PID].Limit);
cliPortPrintF( "eDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[EDOT_PID].P,
eepromConfig.PID[EDOT_PID].I,
eepromConfig.PID[EDOT_PID].D,
eepromConfig.PID[EDOT_PID].Limit);
cliPortPrintF( "hDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[HDOT_PID].P,
eepromConfig.PID[HDOT_PID].I,
eepromConfig.PID[HDOT_PID].D,
eepromConfig.PID[HDOT_PID].Limit);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'd': // Position PIDs
cliPortPrintF("\nN PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[N_PID].P,
eepromConfig.PID[N_PID].I,
eepromConfig.PID[N_PID].D,
eepromConfig.PID[N_PID].Limit);
cliPortPrintF( "E PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[E_PID].P,
eepromConfig.PID[E_PID].I,
eepromConfig.PID[E_PID].D,
eepromConfig.PID[E_PID].Limit);
cliPortPrintF( "h PID: %8.4f, %8.4f, %8.4f, %8.4f\n", eepromConfig.PID[H_PID].P,
eepromConfig.PID[H_PID].I,
eepromConfig.PID[H_PID].D,
eepromConfig.PID[H_PID].Limit);
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'e': // Loop Delta Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz,
deltaTime500Hz,
deltaTime100Hz,
deltaTime50Hz,
deltaTime10Hz,
deltaTime5Hz,
deltaTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'f': // Loop Execution Times
cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz,
executionTime500Hz,
executionTime100Hz,
executionTime50Hz,
executionTime10Hz,
executionTime5Hz,
executionTime1Hz);
validCliCommand = false;
break;
///////////////////////////////
case 'g': // 100 Hz Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel100Hz[XAXIS],
sensors.accel100Hz[YAXIS],
sensors.accel100Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'h': // 100 hz Earth Axis Accels
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS],
earthAxisAccels[YAXIS],
earthAxisAccels[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'i': // 500 hz Gyros
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D,
sensors.gyro500Hz[PITCH] * R2D,
sensors.gyro500Hz[YAW ] * R2D,
mpu6000Temperature);
validCliCommand = false;
break;
///////////////////////////////
case 'j': // 10 Hz Mag Data
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS],
sensors.mag10Hz[YAXIS],
sensors.mag10Hz[ZAXIS]);
validCliCommand = false;
break;
///////////////////////////////
case 'k': // Vertical Axis Variables
cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld, %9.4f\n", earthAxisAccels[ZAXIS],
sensors.pressureAlt50Hz,
hDotEstimate,
hEstimate,
ms5611Temperature,
aglRead());
validCliCommand = false;
break;
///////////////////////////////
case 'l': // Attitudes
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D,
sensors.attitude500Hz[PITCH] * R2D,
sensors.attitude500Hz[YAW ] * R2D);
validCliCommand = false;
break;
///////////////////////////////
case 'm': // Axis PIDs
cliPortPrintF("%9.4f, %9.4f, %9.4f\n", ratePID[ROLL ],
ratePID[PITCH],
ratePID[YAW ]);
validCliCommand = false;
break;
///////////////////////////////
case 'n': // GPS Data
cliPortPrintF("ITOW:%12ld, LAT:%12ld, LONG:%12ld, HEAD:%12ld, HEIGHT:%12ld, HMSL:%12ld, FIX:%4d, NUMSAT:%4d\n",
gps.iTOW,
gps.latitude,
gps.longitude,
gps.hDop,
gps.height,
gps.hMSL,
gps.fix,
gps.numSats);
validCliCommand = false;
break;
///////////////////////////////
case 'o':
cliPortPrintF("%9.4f\n", batteryVoltage);
validCliCommand = false;
break;
///////////////////////////////
case 'p':
cameraCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'q':
adcCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////FLIGHT MODE
case 'r':
if (flightMode == RATE)
cliPortPrint("Flight Mode:RATE ");
else if (flightMode == ATTITUDE)
cliPortPrint("Flight Mode:ATTITUDE ");
else if (flightMode == GPS)
cliPortPrint("Flight Mode:GPS ");
if (headingHoldEngaged == true)
cliPortPrint("Heading Hold:ENGAGED ");
else
cliPortPrint("Heading Hold:DISENGAGED ");
switch (verticalModeState)
{
case ALT_DISENGAGED_THROTTLE_ACTIVE:
cliPortPrint("Alt:Disenaged Throttle Active ");
break;
case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
cliPortPrint("Alt:Hold Fixed at Engagement Alt ");
break;
case ALT_HOLD_AT_REFERENCE_ALTITUDE:
cliPortPrint("Alt:Hold at Reference Alt ");
break;
case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
cliPortPrint("Alt:Velocity Hold at Reference Vel ");
break;
case ALT_DISENGAGED_THROTTLE_INACTIVE:
cliPortPrint("Alt:Disengaged Throttle Inactive ");
break;
}
if (rxCommand[AUX3] > MIDCOMMAND)
cliPortPrint("Mode:Simple ");
else
cliPortPrint("Mode:Normal ");
if (rxCommand[AUX4] > MIDCOMMAND)
cliPortPrint("Emergency Bail:Active\n");
else
cliPortPrint("Emergency Bail:Inactive\n");
validCliCommand = false;
break;
///////////////////////////////
case 's': // Raw Receiver Commands
if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0))
{
for (index = 0; index < maxChannelNum - 1; index++)
cliPortPrintF("%4ld, ", spektrumBuf[index]);
cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]);
}
else if (eepromConfig.receiverType == SBUS)
{
for (index = 0; index < 7; index++)
cliPortPrintF("%4ld, ", sBusRead(index));
cliPortPrintF("%4ld\n", sBusRead(7));
}
else
{
for (index = 0; index < numChannels - 1; index++)
cliPortPrintF("%4i, ", Inputs[index].pulseWidth);
cliPortPrintF("%4i\n", Inputs[numChannels - 1].pulseWidth);
}
validCliCommand = false;
break;
///////////////////////////////
case 't': // Processed Receiver Commands
for (index = 0; index < numChannels - 1; index++)
cliPortPrintF("%8.2f, ", rxCommand[index]);
cliPortPrintF("%8.2f\n", rxCommand[numChannels - 1]);
validCliCommand = false;
break;
///////////////////////////////
case 'u': // Command in Detent Discretes
cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false");
cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false");
cliPortPrintF("%s\n", commandInDetent[YAW ] ? " true" : "false");
validCliCommand = false;
break;
///////////////////////////////
case 'v': // ESC PWM Outputs
cliPortPrintF("%4ld, ", TIM8->CCR4);
cliPortPrintF("%4ld, ", TIM8->CCR3);
cliPortPrintF("%4ld, ", TIM8->CCR2);
cliPortPrintF("%4ld, ", TIM8->CCR1);
cliPortPrintF("%4ld, ", TIM2->CCR1);
cliPortPrintF("%4ld, ", TIM2->CCR2);
cliPortPrintF("%4ld, ", TIM3->CCR1);
cliPortPrintF("%4ld\n", TIM3->CCR2);
validCliCommand = false;
break;
///////////////////////////////
case 'w': // Servo PWM Outputs
cliPortPrintF("%4ld, ", TIM5->CCR3);
cliPortPrintF("%4ld, ", TIM5->CCR2);
cliPortPrintF("%4ld\n", TIM5->CCR1);
validCliCommand = false;
break;
///////////////////////////////
case 'x':
validCliCommand = false;
break;
///////////////////////////////
case 'y': // ESC Calibration
escCalibration();
cliQuery = 'x';
break;
///////////////////////////////
case 'z': // ADC readings
cliPortPrintF("%8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %8.4f\n", adcValue(1),
adcValue(2),
adcValue(3),
adcValue(4),
adcValue(5),
adcValue(6),
adcValue(7));
break;
///////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////
case 'A': // Read Roll Rate PID Values
readCliPID(ROLL_RATE_PID);
cliPortPrint( "\nRoll Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'B': // Read Pitch Rate PID Values
readCliPID(PITCH_RATE_PID);
cliPortPrint( "\nPitch Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'C': // Read Yaw Rate PID Values
readCliPID(YAW_RATE_PID);
cliPortPrint( "\nYaw Rate PID Received....\n" );
cliQuery = 'a';
validCliCommand = false;
break;
///////////////////////////////
case 'D': // Read Roll Attitude PID Values
readCliPID(ROLL_ATT_PID);
cliPortPrint( "\nRoll Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'E': // Read Pitch Attitude PID Values
readCliPID(PITCH_ATT_PID);
cliPortPrint( "\nPitch Attitude PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'F': // Read Heading Hold PID Values
readCliPID(HEADING_PID);
cliPortPrint( "\nHeading PID Received....\n" );
cliQuery = 'b';
validCliCommand = false;
break;
///////////////////////////////
case 'G': // Read nDot PID Values
readCliPID(NDOT_PID);
cliPortPrint( "\nnDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'H': // Read eDot PID Values
readCliPID(EDOT_PID);
cliPortPrint( "\neDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'I': // Read hDot PID Values
readCliPID(HDOT_PID);
cliPortPrint( "\nhDot PID Received....\n" );
cliQuery = 'c';
validCliCommand = false;
break;
///////////////////////////////
case 'J': // Read n PID Values
readCliPID(N_PID);
cliPortPrint( "\nn PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'K': // Read e PID Values
readCliPID(E_PID);
cliPortPrint( "\ne PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'L': // Read h PID Values
readCliPID(H_PID);
cliPortPrint( "\nh PID Received....\n" );
cliQuery = 'd';
validCliCommand = false;
break;
///////////////////////////////
case 'M': // MAX7456 CLI
max7456CLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'N': // Mixer CLI
mixerCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'O': // Receiver CLI
receiverCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'P': // Sensor CLI
sensorCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Q': // GPS Type Selection
cliBusy = true;
eepromConfig.gpsType = (uint8_t)readFloatCLI();
switch(eepromConfig.gpsType) {
case NO_GPS:
cliPortPrint("GPS Module Disabled\n");
break;
case MEDIATEK_3329_BINARY:
cliPortPrint("GPS Module set to MEDIATEK3329 (Binary)\n");
break;
case MEDIATEK_3329_NMEA:
cliPortPrint("GPS Module set to MEDIATEK3329 (NMEA)\n");
break;
case UBLOX:
cliPortPrint("GPS Module set to UBLOX\n");
break;
default:
cliPortPrint("Invalid GPS module type. Use 0-3 (NONE, MEDIATEK BINARY, MEDIATEK NMEA, UBLOX\n");
break;
}
initGPS();
cliBusy = false;
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'R': // Reset to Bootloader
cliPortPrint("Entering Bootloader....\n\n");
delay(100);
systemReset(true);
break;
///////////////////////////////
case 'S': // Reset System
cliPortPrint("\nSystem Reseting....\n\n");
delay(100);
systemReset(false);
break;
///////////////////////////////
case 'T': // Telemetry CLI
telemetryCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'U': // EEPROM CLI
eepromCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'V': // Reset EEPROM Parameters
cliPortPrint( "\nEEPROM Parameters Reset....\n" );
checkFirstTime(true);
cliPortPrint("\nSystem Resetting....\n\n");
delay(100);
systemReset(false);
break;
///////////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n");
writeEEPROM();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'X': // Environmental Sensor Bus CLI
esbCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Y': // ADC CLI
adcCLI();
cliQuery = 'x';
validCliCommand = false;
break;
///////////////////////////////
case 'Z': // Not Used
computeGeoMagElements();
cliQuery = 'x';
break;
///////////////////////////////
case '?': // Command Summary
cliBusy = true;
cliPortPrint("\n");
cliPortPrint("'a' Rate PIDs 'A' Set Roll Rate PID Data AP;I;D;N\n");
cliPortPrint("'b' Attitude PIDs 'B' Set Pitch Rate PID Data BP;I;D;N\n");
cliPortPrint("'c' Velocity PIDs 'C' Set Yaw Rate PID Data CP;I;D;N\n");
cliPortPrint("'d' Position PIDs 'D' Set Roll Att PID Data DP;I;D;N\n");
cliPortPrint("'e' Loop Delta Times 'E' Set Pitch Att PID Data EP;I;D;N\n");
cliPortPrint("'f' Loop Execution Times 'F' Set Hdg Hold PID Data FP;I;D;N\n");
cliPortPrint("'g' 500 Hz Accels 'G' Set nDot PID Data GP;I;D;N\n");
cliPortPrint("'h' 100 Hz Earth Axis Accels 'H' Set eDot PID Data HP;I;D;N\n");
cliPortPrint("'i' 500 Hz Gyros 'I' Set hDot PID Data IP;I;D;N\n");
cliPortPrint("'j' 10 hz Mag Data 'J' Set n PID Data JP;I;D;N\n");
cliPortPrint("'k' Vertical Axis Variable 'K' Set e PID Data KP;I;D;N\n");
cliPortPrint("'l' Attitudes 'L' Set h PID Data LP;I;D;N\n");
cliPortPrint("'m' Axis PIDs 'M' MAX7456 CLI\n");
cliPortPrint("'n' GPS Data 'N' Mixer CLI\n");
cliPortPrint("'o' Battery Voltage 'O' Receiver CLI\n");
cliPortPrint("'p' Camera CLI 'P' Sensor CLI\n");
cliPortPrint("'q' ADC CLI 'Q' GPS Data Selection\n");
cliPortPrint("'r' Mode States 'R' Reset and Enter Bootloader\n");
cliPortPrint("'s' Raw Receiver Commands 'S' Reset\n");
cliPortPrint("'t' Processed Receiver Commands 'T' Telemetry CLI\n");
cliPortPrint("'u' Command In Detent Discretes 'U' EEPROM CLI\n");
cliPortPrint("'v' Motor PWM Outputs 'V' Reset EEPROM Parameters\n");
cliPortPrint("'w' Servo PWM Outputs 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Terminate Serial Communication 'X' ESB CLI\n");
cliPortPrint("'y' ESC Calibration/Motor Verification 'Y' ADC CLI\n");
cliPortPrint("'z' ADC Values 'Z' WMM Test\n");
cliPortPrint(" '?' Command Summary\n");
cliPortPrint("\n");
cliQuery = 'x';
cliBusy = false;
break;
///////////////////////////////
}
}
}
void communication_receive(void)
{
mavlink_message_t msg;
mavlink_status_t status;
// COMMUNICATION THROUGH EXTERNAL UART PORT (XBee serial)
while(cliPortAvailable())
{
uint8_t c = cliPortRead();
// Try to get a new message
if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
// Handle message
switch(msg.msgid)
{
//COMMANDS
case MAVLINK_MSG_ID_COMMAND_LONG:
{
mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(&msg, &cmd);
switch (cmd.command) {
/*CAMERA CONTROL COMMAND*/
case MAV_CMD_VIDEO_START_CAPTURE:
/*Enable camera*/
cameraEnable(true);
break;
case MAV_CMD_VIDEO_STOP_CAPTURE:
cameraEnable(false);
break;
case MAV_CMD_DO_DIGICAM_CONTROL:
if (((int)(cmd.param1)) == 1){
cameraFlip(true);
}
else if (((int)(cmd.param1)) == 2){
cameraFlip(false);
}
else if (((int)(cmd.param3)) == 1){
cameraZoomIn();
}
else if (((int)(cmd.param3)) == -1){
cameraZoomOut();
}
else if (((int)(cmd.param3)) == 2){
cameraStopZoom();
}
else if (((int)(cmd.param7)) == 1){
cameraEnableOSD(true);
}
else if (((int)(cmd.param7)) == -1){
cameraEnableOSD(false);
}
break;
/*OSD CONTROL COMMAND
***UNCOMMENT WHEN THIS FUNCTIONALITY IS READY****
* case MAV_CMD_OSD_CONTROL:
* if ((int)(cmd.param1)) == 0){
* //Disable component 1
* }
* else if((int)(cmd.param1)) == 1){
* //Enable component 1
* }
* if((int)(cmd.param2)) == 0){
* //Disable component 2
* }
* else if((int)(cmd.param2)) == 0{
* //Enable component 2
* }
* //Continue statements for each OSD component we wish to enable or disable
* break;
*/
default:
//mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, MAV_RESULT_UNSUPPORTED);
break;
}
}
break;
default:
//Do nothing
break;
}
}
// And get the next one
}
// Update global packet drops counter
packet_drops += status.packet_rx_drop_count;
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPortPrint("Sensor CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliPortRead();
cliPortPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
cliPortPrintF("\n");
cliPortPrintF("External HMC5883 in use: %s\n", eepromConfig.externalHMC5883 ? "Yes" : "No");
cliPortPrintF("External MS5611 in use: %s\n", eepromConfig.externalMS5611 ? "Yes" : "No");
cliPortPrintF("MXR9150 Accel in use: %s\n", eepromConfig.useMXR9150 ? "Yes" : "No");
cliPortPrintF("\n");
if (eepromConfig.useMXR9150 == true)
{
cliPortPrintF("MXR Accel Bias: %9.3f, %9.3f, %9.3f\n", eepromConfig.accelBiasMXR[XAXIS],
eepromConfig.accelBiasMXR[YAXIS],
eepromConfig.accelBiasMXR[ZAXIS]);
cliPortPrintF("MXR Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", eepromConfig.accelScaleFactorMXR[XAXIS],
eepromConfig.accelScaleFactorMXR[YAXIS],
eepromConfig.accelScaleFactorMXR[ZAXIS]);
}
else
{
cliPortPrintF("MPU Accel Bias: %9.3f, %9.3f, %9.3f\n", eepromConfig.accelBiasMPU[XAXIS],
eepromConfig.accelBiasMPU[YAXIS],
eepromConfig.accelBiasMPU[ZAXIS]);
cliPortPrintF("MPU Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", eepromConfig.accelScaleFactorMPU[XAXIS],
eepromConfig.accelScaleFactorMPU[YAXIS],
eepromConfig.accelScaleFactorMPU[ZAXIS]);
}
cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPortPrintF("Internal Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPortPrintF("External Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS + 3],
eepromConfig.magBias[YAXIS + 3],
eepromConfig.magBias[ZAXIS + 3]);
cliPortPrintF("Accel One G: %9.4f\n", accelOneG);
cliPortPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPortPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPortPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc);
cliPortPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPortPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag);
cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB);
cliPortPrint("MPU6000 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPortPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPortPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPortPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPortPrint("42 Hz\n");
break;
}
cliPortPrint("Sensor Orientation: ");
switch(eepromConfig.sensorOrientation)
{
case 1:
cliPortPrint("Normal\n");
break;
case 2:
cliPortPrint("Rotated 90 Degrees CW\n");
break;
case 3:
cliPortPrint("Rotated 180 Degrees\n");
break;
case 4:
cliPortPrint("Rotated 90 Degrees CCW\n");
break;
default:
cliPortPrint("Normal\n");
}
if (eepromConfig.verticalVelocityHoldOnly)
cliPortPrint("Vertical Velocity Hold Only\n\n");
else
cliPortPrint("Vertical Velocity and Altitude Hold\n\n");
cliPortPrintF("Voltage Monitor Scale: %9.4f\n", eepromConfig.voltageMonitorScale);
cliPortPrintF("Voltage Monitor Bias: %9.4f\n", eepromConfig.voltageMonitorBias);
cliPortPrintF("Number of Battery Cells: %1d\n\n", eepromConfig.batteryCells);
cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", eepromConfig.batteryLow);
cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", eepromConfig.batteryVeryLow);
cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", eepromConfig.batteryMaxLow);
validQuery = false;
break;
///////////////////////////
case 'b': // MPU6000 Calibration
mpu6000Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'd': // Accel Bias and Scale Factor Calibration
if (eepromConfig.useMXR9150 == true)
accelCalibrationMXR();
else
accelCalibrationMPU();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'e': // Toggle External HMC5883 Use
if (eepromConfig.externalHMC5883 == 0)
eepromConfig.externalHMC5883 = 3;
else
eepromConfig.externalHMC5883 = 0;
initMag();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'f': // Toggle External MS5611 Use
if (eepromConfig.externalMS5611)
eepromConfig.externalMS5611 = false;
else
eepromConfig.externalMS5611 = true;
initPressure();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'g': // Toggle MXR9150 Use
if (eepromConfig.useMXR9150)
eepromConfig.useMXR9150 = false;
else
eepromConfig.useMXR9150 = true;
computeMPU6000RTData();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'h': // MXR Bias
eepromConfig.accelBiasMXR[XAXIS] = readFloatCLI();
eepromConfig.accelBiasMXR[YAXIS] = readFloatCLI();
eepromConfig.accelBiasMXR[ZAXIS] = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6000 Digital Low Pass Filter
tempInt = (uint8_t)readFloatCLI();
switch(tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock (within 20 +/- 10%)
GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
spiTransfer(MPU6000_SPI, MPU6000_CONFIG);
spiTransfer(MPU6000_SPI, eepromConfig.dlpfSetting);
GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN);
setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%)
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc, kiAcc
eepromConfig.KpAcc = readFloatCLI();
eepromConfig.KiAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag, kiMag
eepromConfig.KpMag = readFloatCLI();
eepromConfig.KiMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Sensor Orientation
eepromConfig.sensorOrientation = (uint8_t)readFloatCLI();
orientSensors();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'N': // Set Voltage Monitor Trip Points
eepromConfig.batteryLow = readFloatCLI();
eepromConfig.batteryVeryLow = readFloatCLI();
eepromConfig.batteryMaxLow = readFloatCLI();
thresholds[BATTERY_LOW].value = eepromConfig.batteryLow;
thresholds[BATTERY_VERY_LOW].value = eepromConfig.batteryVeryLow;
thresholds[BATTRY_MAX_LOW].value = eepromConfig.batteryMaxLow;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'V': // Set Voltage Monitor Parameters
eepromConfig.voltageMonitorScale = readFloatCLI();
eepromConfig.voltageMonitorBias = readFloatCLI();
eepromConfig.batteryCells = (uint8_t)readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3, see aq32Plus.h\n");
cliPortPrint("'b' MPU6000 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n");
cliPortPrint("'d' Accel Bias and SF Calibration 'D' Set kpMag/kiMag DkpMag;kiMag\n");
cliPortPrint("'e' Toggle External HMC5883 State 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPortPrint("'f' Toggle External MS5611 State 'F' Set Sensor Orientation F1 thru 4\n");
cliPortPrint("'g' Toggle MXR9150 Use\n");
cliPortPrint(" 'N' Set Voltage Monitor Trip Points Nlow;veryLow;maxLow\n");
cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n");
break;
///////////////////////////
}
}
}
void sensorCLI()
{
uint8_t sensorQuery = 'x';
uint8_t tempInt;
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Sensor CLI....\n\n");
while(true)
{
cliPortPrint("Sensor CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
sensorQuery = cliPortRead();
cliPortPrint("\n");
switch(sensorQuery)
{
///////////////////////////
case 'a': // Sensor Data
if (eepromConfig.useMpu6050 == true)
{
cliPortPrint("\nUsing MPU6050....\n\n");
cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS],
eepromConfig.accelTCBiasSlope[YAXIS],
eepromConfig.accelTCBiasSlope[ZAXIS]);
cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS],
eepromConfig.accelTCBiasIntercept[YAXIS],
eepromConfig.accelTCBiasIntercept[ZAXIS]);
}
else
{
cliPortPrint("\nUsing ADXL345/MPU3050....\n\n");
}
cliPortPrintF("Accel Scale Factor: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelScaleFactor[XAXIS],
eepromConfig.accelScaleFactor[YAXIS],
eepromConfig.accelScaleFactor[ZAXIS]);
cliPortPrintF("Accel Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelBias[XAXIS],
eepromConfig.accelBias[YAXIS],
eepromConfig.accelBias[ZAXIS]);
cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ],
eepromConfig.gyroTCBiasSlope[PITCH],
eepromConfig.gyroTCBiasSlope[YAW ]);
cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ],
eepromConfig.gyroTCBiasIntercept[PITCH],
eepromConfig.gyroTCBiasIntercept[YAW ]);
cliPortPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS],
eepromConfig.magBias[YAXIS],
eepromConfig.magBias[ZAXIS]);
cliPortPrintF("Accel One G: %9.4f\n", accelOneG);
cliPortPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff);
cliPortPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc);
cliPortPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag);
cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA);
cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB);
if (eepromConfig.useMpu6050 == true)
{
cliPortPrint("MPU6050 DLPF: ");
switch(eepromConfig.dlpfSetting)
{
case DLPF_256HZ:
cliPortPrint("256 Hz\n");
break;
case DLPF_188HZ:
cliPortPrint("188 Hz\n");
break;
case DLPF_98HZ:
cliPortPrint("98 Hz\n");
break;
case DLPF_42HZ:
cliPortPrint("42 Hz\n");
break;
}
}
if (eepromConfig.useMs5611 == true)
cliPortPrint("\nUsing MS5611....\n\n");
else
cliPortPrint("\nUsing BMP085....\n\n");
if (eepromConfig.verticalVelocityHoldOnly == true)
cliPortPrint("Vertical Velocity Hold Only\n\n");
else
cliPortPrint("Vertical Velocity and Altitude Hold\n\n");
cliPortPrintF("Voltage Monitor Scale: %9.4f\n", eepromConfig.voltageMonitorScale);
cliPortPrintF("Voltage Monitor Bias: %9.4f\n", eepromConfig.voltageMonitorBias);
cliPortPrintF("Number of Battery Cells: %1d\n\n", eepromConfig.batteryCells);
cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", eepromConfig.batteryLow);
cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", eepromConfig.batteryVeryLow);
cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", eepromConfig.batteryMaxLow);
validQuery = false;
break;
///////////////////////////
case 'b': // MPU Calibration
if (eepromConfig.useMpu6050 == true)
mpu6050Calibration();
else
mpu3050Calibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Magnetometer Calibration
magCalibration();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'd': // Accel Calibration
if (eepromConfig.useMpu6050 == false)
accelCalibrationADXL345();
else
accelCalibrationMPU();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'm': // Toggle MPU3050/MPU6050
if (eepromConfig.useMpu6050)
{
eepromConfig.useMpu6050 = false;
initAdxl345();
initMpu3050();
}
else
{
eepromConfig.useMpu6050 = true;
initMpu6050();
}
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'p': // Toggle BMP085/MS5611
if (eepromConfig.useMs5611)
{
eepromConfig.useMs5611 = false;
initBmp085();
}
else
{
eepromConfig.useMs5611 = true;
initMs5611();
}
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'v': // Toggle Vertical Velocity Hold Only
if (eepromConfig.verticalVelocityHoldOnly)
eepromConfig.verticalVelocityHoldOnly = false;
else
eepromConfig.verticalVelocityHoldOnly = true;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Sensor CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Set MPU6050 Digital Low Pass Filter
if (eepromConfig.useMpu6050 == true)
{
tempInt = (uint8_t)readFloatCLI();
switch (tempInt)
{
case DLPF_256HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ;
break;
case DLPF_188HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ;
break;
case DLPF_98HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ;
break;
case DLPF_42HZ:
eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ;
break;
}
i2cWrite(I2C2, MPU6050_ADDRESS, MPU6050_CONFIG, eepromConfig.dlpfSetting); // Accel and Gyro DLPF Setting
sensorQuery = 'a';
validQuery = true;
}
break;
///////////////////////////
case 'B': // Accel Cutoff
eepromConfig.accelCutoff = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'C': // kpAcc
eepromConfig.KpAcc = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // kpMag
eepromConfig.KpMag = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'E': // h dot est/h est Comp Filter A/B
eepromConfig.compFilterA = readFloatCLI();
eepromConfig.compFilterB = readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'N': // Set Voltage Monitor Trip Points
eepromConfig.batteryLow = readFloatCLI();
eepromConfig.batteryVeryLow = readFloatCLI();
eepromConfig.batteryMaxLow = readFloatCLI();
thresholds[BATTERY_LOW].value = eepromConfig.batteryLow;
thresholds[BATTERY_VERY_LOW].value = eepromConfig.batteryVeryLow;
thresholds[BATTRY_MAX_LOW].value = eepromConfig.batteryMaxLow;
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'V': // Set Voltage Monitor Parameters
eepromConfig.voltageMonitorScale = readFloatCLI();
eepromConfig.voltageMonitorBias = readFloatCLI();
eepromConfig.batteryCells = (uint8_t)readFloatCLI();
sensorQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
validQuery = false;
writeEEPROM();
break;
///////////////////////////
case '?':
cliPortPrint("\n");
if (eepromConfig.useMpu6050 == true)
cliPortPrint("'a' Display Sensor Data 'A' Set MPU6050 DLPF A0 thru 3\n");
else
cliPortPrint("'a' Display Sensor Data\n");
cliPortPrint("'b' MPU Calibration 'B' Set Accel Cutoff BAccelCutoff\n");
cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc CKpAcc\n");
cliPortPrint("'d' Accel Calibration 'D' Set kpMag DKpMag\n");
cliPortPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n");
cliPortPrint("'m' Toggle MPU3050/MPU6050\n");
cliPortPrint(" 'N' Set Voltage Monitor Trip Points Nlow;veryLow;maxLow\n");
cliPortPrint("'p' Toggle BMP085/MS5611\n");
cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n");
cliPortPrint("\n");
break;
///////////////////////////
}
}
}
void receiverCLI()
{
char rcOrderString[9];
float tempFloat;
uint8_t index;
uint8_t receiverQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Receiver CLI....\n\n");
while(true)
{
cliPortPrint("Receiver CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
receiverQuery = cliPortRead();
cliPortPrint("\n");
switch(receiverQuery)
{
///////////////////////////
case 'a': // Receiver Configuration
cliPortPrint("\nReceiver Type: ");
switch(eepromConfig.receiverType)
{
case PPM:
cliPortPrint("PPM\n");
break;
case SPEKTRUM:
cliPortPrint("Spektrum\n");
break;
}
cliPortPrint("Current RC Channel Assignment: ");
for (index = 0; index < 8; index++)
rcOrderString[eepromConfig.rcMap[index]] = rcChannelLetters[index];
rcOrderString[index] = '\0';
cliPortPrint(rcOrderString); cliPortPrint("\n");
#if defined(STM32F40XX)
cliPortPrintF("Secondary Spektrum: ");
if (eepromConfig.slaveSpektrum == true)
cliPortPrintF("Installed\n");
else
cliPortPrintF("Uninstalled\n");
#endif
cliPortPrintF("Mid Command: %4ld\n", (uint16_t)eepromConfig.midCommand);
cliPortPrintF("Min Check: %4ld\n", (uint16_t)eepromConfig.minCheck);
cliPortPrintF("Max Check: %4ld\n", (uint16_t)eepromConfig.maxCheck);
cliPortPrintF("Min Throttle: %4ld\n", (uint16_t)eepromConfig.minThrottle);
cliPortPrintF("Max Thottle: %4ld\n\n", (uint16_t)eepromConfig.maxThrottle);
tempFloat = eepromConfig.rollAndPitchRateScaling * 180000.0 / PI;
cliPortPrintF("Max Roll and Pitch Rate Cmd: %6.2f DPS\n", tempFloat);
tempFloat = eepromConfig.yawRateScaling * 180000.0 / PI;
cliPortPrintF("Max Yaw Rate Cmd: %6.2f DPS\n\n", tempFloat);
tempFloat = eepromConfig.attitudeScaling * 180000.0 / PI;
cliPortPrintF("Max Attitude Cmd: %6.2f Degrees\n\n", tempFloat);
cliPortPrintF("Arm Delay Count: %3d Frames\n", eepromConfig.armCount);
cliPortPrintF("Disarm Delay Count: %3d Frames\n\n", eepromConfig.disarmCount);
validQuery = false;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Receiver CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Toggle PPM/Spektrum Satellite Receiver
if (eepromConfig.receiverType == PPM)
{
#if defined(STM32F30X)
TIM_ITConfig(TIM1, TIM_IT_CC1, DISABLE);
#endif
#if defined(STM32F40XX)
TIM_ITConfig(TIM1, TIM_IT_CC4, DISABLE);
#endif
eepromConfig.receiverType = SPEKTRUM;
spektrumInit();
}
else
{
#if defined(STM32F30X)
TIM_ITConfig(TIM17, TIM_IT_Update, DISABLE);
#endif
#if defined(STM32F40XX)
TIM_ITConfig(TIM6, TIM_IT_Update, DISABLE);
#endif
eepromConfig.receiverType = PPM;
#if defined(STM32F30X)
ppmRxInit();
#endif
#if defined(STM32F40XX)
agl_ppmRxInit();
#endif
}
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'B': // Read RC Control Order
readStringCLI( rcOrderString, 8 );
parseRcChannels( rcOrderString );
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
#if defined(STM32F40XX)
case 'C': // Toggle Slave Spektrum State
if (eepromConfig.slaveSpektrum == true)
eepromConfig.slaveSpektrum = false;
else
eepromConfig.slaveSpektrum = true;
receiverQuery = 'a';
validQuery = true;
break;
#endif
///////////////////////////
case 'D': // 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 'E': // Read Arm/Disarm Counts
eepromConfig.armCount = (uint8_t)readFloatCLI();
eepromConfig.disarmCount = (uint8_t)readFloatCLI();
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read Max Rate Value
eepromConfig.rollAndPitchRateScaling = readFloatCLI() / 180000.0f * PI;
eepromConfig.yawRateScaling = readFloatCLI() / 180000.0f * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'G': // Read Max Attitude Value
eepromConfig.attitudeScaling = readFloatCLI() / 180000.0f * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Receiver Configuration Data 'A' Toggle PPM/Spektrum Receiver\n");
cliPortPrint(" 'B' Set RC Control Order BTAER1234\n");
#if defined(STM32F40XX)
cliPortPrint(" 'C' Toggle Slave Spektrum State\n");
#endif
cliPortPrint(" 'D' Set RC Control Points DmidCmd;minChk;maxChk;minThrot;maxThrot\n");
cliPortPrint(" 'E' Set Arm/Disarm Counts EarmCount;disarmCount\n");
cliPortPrint(" 'F' Set Maximum Rate Commands FRP;Y RP = Roll/Pitch, Y = Yaw\n");
cliPortPrint(" 'G' Set Maximum Attitude Command\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Receiver CLI '?' Command Summary\n");
cliPortPrint("\n");
break;
///////////////////////////
}
}
}
void eepromCLI()
{
uint32_t c1, c2;
uint8_t eepromQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering EEPROM CLI....\n\n");
while(true)
{
cliPortPrint("EEPROM CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
eepromQuery = cliPortRead();
cliPortPrint("\n");
switch(eepromQuery)
{
// 'a' is the standard "print all the information" character
case 'a': // config struct data
c1 = eepromConfig.CRCAtEnd[0];
zeroPIDstates();
c2 = crc32bEEPROM(&eepromConfig, false);
cliPortPrintF("Config structure information:\n");
cliPortPrintF("Version : %d\n", eepromConfig.version );
cliPortPrintF("Size : %d\n", sizeof(eepromConfig) );
cliPortPrintF("CRC on last read : %08X\n", c1 );
cliPortPrintF("Current CRC : %08X\n", c2 );
if ( c1 != c2 )
cliPortPrintF(" CRCs differ. Current Config has not yet been saved.\n");
cliPortPrintF("CRC Flags :\n");
cliPortPrintF(" 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
zeroPIDstates();
cliPortPrintF("\n");
cliPrintEEPROM(&eepromConfig);
cliPortPrintF("\n");
if (crcCheckVal != crc32bEEPROM(&eepromConfig, true))
{
cliPortPrint("NOTE: in-memory config CRC invalid; there have probably been changes to\n");
cliPortPrint(" eepromConfig since the last write to flash/eeprom.\n");
}
validQuery = false;
break;
///////////////////////////
case 'H': // clear bad history flag
cliPortPrintF("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;
cliPortPrintF("Ready to read in config. Expecting %d (0x%03X) bytes as %d\n",
sz, sz, sz * 2);
cliPortPrintF("hexadecimal characters, optionally separated by [ \\n\\r_].\n");
cliPortPrintF("Times out if no character is received for %dms\n", Timeout);
memset(p, 0, end - p);
while (p < end)
{
while (!cliPortAvailable() && millis() - t < Timeout) {}
t = millis();
c = cliPortAvailable() ? cliPortRead() : '\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)
{
cliPortPrintF("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)
{
cliPortPrintF("Invalid character found at position %d: '%c' (0x%02x)",
chars_encountered, c, c);
}
// HJI else if (crcCheckVal != crc32bEEPROM(&e, true))
// HJI {
// HJI cliPortPrintF("CRC mismatch! Not writing to in-memory config.\n");
// HJI cliPortPrintF("Here's what was received:\n\n");
// HJI cliPrintEEPROM(&e);
// HJI }
else
{
// check to see if the newly received eeprom config
// actually differs from what's in-memory
zeroPIDstates();
int i;
for (i = 0; i < sz; i++)
if (((uint8_t*)&e)[i] != ((uint8_t*)&eepromConfig)[i])
break;
if (i == sz)
{
cliPortPrintF("NOTE: uploaded config was identical to in-memory config.\n");
}
else
{
eepromConfig = e;
cliPortPrintF("In-memory config updated!\n");
cliPortPrintF("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 (cliPortAvailable())
cliPortRead();
validQuery = false;
break;
///////////////////////////
case 'E': // Read in from EEPROM. (EEPROM -> RAM)
cliPortPrint("Re-reading EEPROM.\n");
readEEPROM();
validQuery = false;
break;
///////////////////////////
case 'x': // exit EEPROM CLI
cliPortPrint("\nExiting EEPROM CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'W':
case 'e': // Write out to EEPROM. (RAM -> EEPROM)
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
validQuery = false;
writeEEPROM();
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
cliPortPrint( "\nEEPROM Parameters Reset....(not rebooting)\n" );
checkFirstTime(true);
validQuery = false;
break;
///////////////////////////
case '?':
// 0 1 2 3 4 5 6 7
// 01234567890123456789012345678901234567890123456789012345678901234567890123456789
cliPortPrintF("\n");
cliPortPrintF("'a' Display in-RAM config information\n");
cliPortPrintF("'c' Write in-RAM -> Console (as Hex) 'C' Read Console (as Hex) -> in-RAM\n");
cliPortPrintF("'e' Write in-RAM -> EEPROM 'E' Read EEPROM -> in-RAM\n");
cliPortPrintF("'f' Write in-RAM -> sd FILE (Not yet imp) 'F' Read sd FILE -> in-RAM (Not imp)\n");
cliPortPrintF(" 'H' Clear CRC Bad History flag\n");
cliPortPrintF(" 'V' Reset in-RAM config to default.\n");
cliPortPrintF("'x' Exit EEPROM CLI '?' Command Summary\n");
cliPortPrintF("\n");
cliPortPrintF("For compatability: 'W' Write in-RAM -> EEPROM\n");
cliPortPrintF("\n");
break;
///////////////////////////
}
}
}
void mixerCLI()
{
float tempFloat;
uint8_t index;
uint8_t rows, columns;
uint8_t mixerQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Mixer CLI....\n\n");
while(true)
{
cliPortPrint("Mixer CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
mixerQuery = cliPortRead();
cliPortPrint("\n");
switch(mixerQuery)
{
///////////////////////////
case 'a': // Mixer Configuration
cliPortPrint("\nMixer Configuration: ");
switch (eepromConfig.mixerConfiguration)
{
case MIXERTYPE_TRI:
cliPortPrint(" MIXERTYPE TRI\n");
break;
case MIXERTYPE_QUADX:
cliPortPrint("MIXERTYPE QUAD X\n");
break;
case MIXERTYPE_HEX6X:
cliPortPrint(" MIXERTYPE HEX X\n");
break;
case MIXERTYPE_FREE:
cliPortPrint(" MIXERTYPE FREE\n");
break;
}
cliPortPrintF("Number of Motors: %1d\n", numberMotor);
cliPortPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate);
cliPortPrintF("Servo PWM Rate: %3ld\n", eepromConfig.servoPwmRate);
if (eepromConfig.yawDirection == 1.0f)
cliPortPrintF("Yaw Direction: Normal\n\n");
else if (eepromConfig.yawDirection == -1.0f)
cliPortPrintF("Yaw Direction: Reverse\n\n");
else
cliPortPrintF("Yaw Direction: Undefined\n\n");
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
cliPortPrintF("TriCopter Yaw Servo PWM Rate: %3ld\n", eepromConfig.triYawServoPwmRate);
cliPortPrintF("TriCopter Yaw Servo Min PWM: %4ld\n", (uint16_t)eepromConfig.triYawServoMin);
cliPortPrintF("TriCopter Yaw Servo Mid PWM: %4ld\n", (uint16_t)eepromConfig.triYawServoMid);
cliPortPrintF("TriCopter Yaw Servo Max PWM: %4ld\n\n", (uint16_t)eepromConfig.triYawServoMax);
cliPortPrintF("Tricopter Yaw Cmd Time Constant: %5.3f\n\n", eepromConfig.triCopterYawCmd500HzLowPassTau);
}
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
cliPortPrintF("\nNumber of Free Mixer Motors: %1d\n Roll Pitch Yaw\n\n", eepromConfig.freeMixMotors);
for ( index = 0; index < eepromConfig.freeMixMotors; index++ )
{
cliPortPrintF("Motor%1d %6.3f %6.3f %6.3f\n", index,
eepromConfig.freeMix[index][ROLL ],
eepromConfig.freeMix[index][PITCH],
eepromConfig.freeMix[index][YAW ]);
}
cliPortPrint("\n");
}
validQuery = false;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Mixer CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read Mixer Configuration
eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI();
initMixer();
pwmEscInit();
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();
pwmServoInit();
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'D': // 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 'E': // Read TriCopter Servo PWM Rate
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoPwmRate = (uint16_t)readFloatCLI();
pwmEscInit();
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'F': // Read TriCopter Servo Min Point
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoMin = readFloatCLI();
pwmEscWrite(7, (uint16_t)eepromConfig.triYawServoMin);
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'G': // Read TriCopter Servo Mid Point
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoMid = readFloatCLI();
pwmEscWrite(7, (uint16_t)eepromConfig.triYawServoMid);
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'H': // Read TriCopter Servo Max Point
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triYawServoMax = readFloatCLI();
pwmEscWrite(7, (uint16_t)eepromConfig.triYawServoMax);
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'I': // Read TriCopter Yaw Command Time Constant
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
eepromConfig.triCopterYawCmd500HzLowPassTau = readFloatCLI();
initFirstOrderFilter();
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nTriCopter Mixing not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'J': // Read Free Mix Motor Number
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
eepromConfig.freeMixMotors = (uint8_t)readFloatCLI();
initMixer();
}
else
{
tempFloat = readFloatCLI();
cliPortPrintF("\nFree Mix not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'K': // Read Free Mix Matrix Element
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
rows = (uint8_t)readFloatCLI() - 1;
columns = (uint8_t)readFloatCLI() - 1;
eepromConfig.freeMix[rows][columns] = readFloatCLI();
}
else
{
tempFloat = readFloatCLI();
tempFloat = readFloatCLI();
tempFloat = readFloatCLI();
cliPortPrintF("\nFree Mix not Selected....\n\n");
}
mixerQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A0 thru 3, see aq32Plus.h\n");
cliPortPrint(" 'B' Set PWM Rates BESC;Servo\n");
cliPortPrint(" 'D' Set Yaw Direction D1 or D-1\n");
if (eepromConfig.mixerConfiguration == MIXERTYPE_TRI)
{
cliPortPrint(" 'E' Set TriCopter Servo PWM Rate ERate\n");
cliPortPrint(" 'F' Set TriCopter Servo Min Point FMin\n");
cliPortPrint(" 'G' Set TriCopter Servo Mid Point GMid\n");
cliPortPrint(" 'H' Set TriCopter Servo Max Point HMax\n");
cliPortPrint(" 'I' Set TriCopter Yaw Cmd Time Constant ITimeConstant\n");
}
if (eepromConfig.mixerConfiguration == MIXERTYPE_FREE)
{
cliPortPrint(" 'J' Set Number of FreeMix Motors JNumb\n");
cliPortPrint(" 'K' Set FreeMix Matrix Element KRow;Col;Value\n");
}
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Mixer CLI '?' Command Summary\n\n");
break;
///////////////////////////
}
}
}
void receiverCLI()
{
char rcOrderString[13];
float tempFloat;
uint8_t tempChannels = 0;
uint16_t tempMax = 0;
uint16_t tempMin = 0;
uint8_t tempPin = 0;
uint8_t tempWarn = 0;
uint8_t index;
uint8_t receiverQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Receiver CLI....\n\n");
while(true)
{
cliPortPrint("Receiver CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
receiverQuery = cliPortRead();
cliPortPrint("\n");
switch(receiverQuery)
{
///////////////////////////
case 'a': // Receiver Configuration
cliPortPrint("\nReceiver Type: ");
if (eepromConfig.receiverType == PPM)
cliPortPrint(" PPM\n");
else if (eepromConfig.receiverType == PWM)
cliPortPrint(" PWM\n");
else if (eepromConfig.receiverType == SPEKTRUM)
cliPortPrint("Spektrum\n");
else
cliPortPrint("Error...\n");
if (eepromConfig.receiverType == PPM)
tempChannels = eepromConfig.ppmChannels;
else
tempChannels = 8;
for (index = 0; index < tempChannels; index++)
rcOrderString[eepromConfig.rcMap[index]] = rcChannelLetters[index];
rcOrderString[index] = '\0';
cliPortPrintF("Current RC Channel Assignment: %12s\n", rcOrderString);
if (eepromConfig.receiverType == PPM)
cliPortPrintF("Number of serial PWM channels %2d\n", eepromConfig.ppmChannels);
cliPortPrintF("Mid Command: %4ld\n", (uint16_t)eepromConfig.midCommand);
cliPortPrintF("Min Check: %4ld\n", (uint16_t)eepromConfig.minCheck);
cliPortPrintF("Max Check: %4ld\n", (uint16_t)eepromConfig.maxCheck);
cliPortPrintF("Min Throttle: %4ld\n", (uint16_t)eepromConfig.minThrottle);
cliPortPrintF("Max Thottle: %4ld\n\n", (uint16_t)eepromConfig.maxThrottle);
tempFloat = eepromConfig.rollAndPitchRateScaling * 180000.0 / PI;
cliPortPrintF("Max Roll and Pitch Rate Cmd: %6.2f DPS\n", tempFloat);
tempFloat = eepromConfig.yawRateScaling * 180000.0 / PI;
cliPortPrintF("Max Yaw Rate Cmd: %6.2f DPS\n", tempFloat);
tempFloat = eepromConfig.attitudeScaling * 180000.0 / PI;
cliPortPrintF("Max Attitude Cmd: %6.2f Degrees\n\n", tempFloat);
cliPortPrintF("Arm Delay Count: %3d Frames\n", eepromConfig.armCount);
cliPortPrintF("Disarm Delay Count: %3d Frames\n\n", eepromConfig.disarmCount);
cliPortPrintF("RSSI via PPM or ADC: %s", eepromConfig.rssiPPM ? "PPM\n" : "ADC\n");
if (eepromConfig.rssiPPM == true)
cliPortPrintF("RSSI PPM Channel: %1d\n", eepromConfig.rssiPin);
else
cliPortPrintF("RSSI ADC Pin: %1d\n", eepromConfig.rssiPin);
cliPortPrintF("RSSI Min: %4d\n", eepromConfig.rssiMin);
cliPortPrintF("RSSI Max: %4d\n", eepromConfig.rssiMax);
cliPortPrintF("RSSI Warning %%: %2d\n\n", eepromConfig.rssiWarning);
validQuery = false;
break;
///////////////////////////
case 'b': // Read Max Rate Values
eepromConfig.rollAndPitchRateScaling = readFloatCLI() / 180000.0f * PI;
eepromConfig.yawRateScaling = readFloatCLI() / 180000.0f * PI;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Read Max Attitude Value
eepromConfig.attitudeScaling = readFloatCLI() / 180000 * PI;
receiverQuery = 'a';
validQuery = true;
break;
case 'r': // Toggle RSSI between ADC and PPM
eepromConfig.rssiPPM = !eepromConfig.rssiPPM;
if (eepromConfig.rssiPPM)
{ // automatically adjust the settings
eepromConfig.rssiPin = 9;
eepromConfig.rssiMin = eepromConfig.minCheck;
eepromConfig.rssiMax = eepromConfig.maxCheck;
}
else
{
eepromConfig.rssiPin = 5; // default from config.c
eepromConfig.rssiMin = 10;
eepromConfig.rssiMax = 3450;
}
eepromConfig.rssiWarning = 25;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Receiver CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'A': // Read RX Input Type
eepromConfig.receiverType = (uint8_t)readFloatCLI();
cliPortPrint( "\nReceiver Type Changed....\n");
cliPortPrint("\nSystem Resetting....\n");
delay(100);
writeEEPROM();
systemReset(false);
break;
///////////////////////////
case 'B': // Read RC Control Order
readStringCLI( rcOrderString, 12 );
parseRcChannels( rcOrderString );
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 'G': // Read number of PPM Channels
tempChannels = (uint8_t)readFloatCLI();
if ((tempChannels < 8) || (tempChannels > 12))
{
cliPortPrintF("\nValid number of channels are 8 to 12\n");
cliPortPrintF("You entered %2d\n\n", tempChannels);
}
else
eepromConfig.ppmChannels = tempChannels;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'R': // RSSI pin/min/max/warning
tempPin = readFloatCLI();
tempMin = readFloatCLI();
tempMax = readFloatCLI();
tempWarn = readFloatCLI();
if (eepromConfig.rssiPPM)
{
if ((tempPin < 0) || (tempPin > eepromConfig.ppmChannels))
{
cliPortPrintF("Invalid RSSI PPM channel number, valid numbers are 0-%2d\n", eepromConfig.ppmChannels);
cliPortPrintF("You entered %2d, please try again\n", tempPin);
receiverQuery = '?';
validQuery = false;
break;
}
}
else
{
if ((tempPin < 1) || (tempPin > 6))
{
cliPortPrintF("Invalid RSSI Pin number, valid numbers are 1-6\n");
cliPortPrintF("You entered %2d, please try again\n", tempPin);
receiverQuery = '?';
validQuery = false;
break;
}
}
eepromConfig.rssiPin = tempPin;
eepromConfig.rssiMin = tempMin;
eepromConfig.rssiMax = tempMax;
eepromConfig.rssiWarning = tempWarn;
receiverQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Receiver Configuration Data 'A' Set RX Input Type AX, 0=PPM, 1=PWM, 2=Spektrum\n");
cliPortPrint("'b' Set Maximum Rate Commands 'B' Set RC Control Order BTAER12345678\n");
cliPortPrint("'c' Set Maximum Attitude Command\n");
cliPortPrint(" 'E' Set RC Control Points EmidCmd;minChk;maxChk;minThrot;maxThrot\n");
cliPortPrint(" 'F' Set Arm/Disarm Counts FarmCount;disarmCount\n");
cliPortPrint(" 'G' Set number of serial PWM channels GnumChannels\n");
cliPortPrint("'r' Toggle RSSI between PPM/ADC 'R' Set RSSI Config RPin;Min;Max;Warning\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Receiver CLI '?' Command Summary\n\n");
break;
///////////////////////////
}
}
}
void telemetryCLI()
{
uint8_t telemetryQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Telemetry CLI....\n\n");
while(true)
{
cliPortPrint("Telemetry CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
telemetryQuery = cliPortRead();
cliPortPrint("\n");
switch(telemetryQuery)
{
///////////////////////////
case 'a': // Telemetry Configuration
cliPortPrint("\nTelemetry Configuration:\n");
cliPortPrint(" Telemetry Set 1: ");
cliPortPrintF("%s\n", eepromConfig.activeTelemetry == 1 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 2: ");
cliPortPrintF("%s\n", eepromConfig.activeTelemetry == 2 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 3: ");
cliPortPrintF("%s\n", eepromConfig.activeTelemetry == 4 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 4: ");
cliPortPrintF("%s\n", eepromConfig.activeTelemetry == 8 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 5: ");
cliPortPrintF("%s\n", eepromConfig.activeTelemetry == 16 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 6: ");
cliPortPrintF("%s\n", eepromConfig.activeTelemetry == 32 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 7: ");
cliPortPrintF("%s\n", eepromConfig.activeTelemetry == 64 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 8: ");
cliPortPrintF("%s\n", eepromConfig.activeTelemetry == 128 ? " Active" : "Inactive");
cliPortPrint(" MavLink: ");
cliPortPrintF("%s\n", eepromConfig.mavlinkEnabled == true ? " Enabled\n" : "Disabled\n");
validQuery = false;
break;
///////////////////////////
case '0': // Turn all Telemetry Off
eepromConfig.activeTelemetry = 0;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case '1': // Toggle Telemetry Set 1 State
eepromConfig.activeTelemetry = 1;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case '2': // Toggle Telemetry Set 2 State
eepromConfig.activeTelemetry = 2;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case '3': // Toggle Telemetry Set 3 State
eepromConfig.activeTelemetry = 4;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case '4': // Toggle Telemetry Set 4 State
eepromConfig.activeTelemetry = 8;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case '5': // Toggle Telemetry Set 5 State
eepromConfig.activeTelemetry = 16;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case '6': // Toggle Telemetry Set 6 State
eepromConfig.activeTelemetry = 32;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case '7': // Toggle Telemetry Set 7 State
eepromConfig.activeTelemetry = 64;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case '8': // Toggle Telemetry Set 8 State
eepromConfig.activeTelemetry = 128;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Telemetry CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'W': // Write EEPROM Parameters
cliPortPrint("\nWriting EEPROM Parameters....\n\n");
writeEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Telemetry Configuration Data\n");
cliPortPrint("'0' Turn all Telemetry Off\n");
cliPortPrint("'1' Toggle Telemetry Set 1 State\n");
cliPortPrint("'2' Toggle Telemetry Set 2 State\n");
cliPortPrint("'3' Toggle Telemetry Set 3 State\n");
cliPortPrint("'4' Toggle Telemetry Set 4 State\n");
cliPortPrint("'5' Toggle Telemetry Set 5 State\n");
cliPortPrint("'6' Toggle Telemetry Set 6 State\n");
cliPortPrint("'7' Toggle Telemetry Set 7 State\n");
cliPortPrint("'8' Toggle Telemetry Set 8 State\n");
cliPortPrint(" 'W' Write EEPROM Parameters\n");
cliPortPrint("'x' Exit Telemetry CLI '?' Command Summary\n\n");
break;
///////////////////////////
}
}
}
void telemetryCLI()
{
uint8_t telemetryQuery = 'x';
uint8_t validQuery = false;
cliBusy = true;
cliPortPrint("\nEntering Telemetry CLI....\n\n");
while(true)
{
cliPortPrint("Telemetry CLI -> ");
while ((cliPortAvailable() == false) && (validQuery == false));
if (validQuery == false)
telemetryQuery = cliPortRead();
cliPortPrint("\n");
switch(telemetryQuery)
{
///////////////////////////
case 'a': // Telemetry Configuration
cliPortPrint("\nTelemetry Configuration:\n");
cliPortPrint(" Telemetry Set 1: ");
cliPortPrintF("%s\n", systemConfig.activeTelemetry == 1 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 2: ");
cliPortPrintF("%s\n", systemConfig.activeTelemetry == 2 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 3: ");
cliPortPrintF("%s\n", systemConfig.activeTelemetry == 4 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 4: ");
cliPortPrintF("%s\n", systemConfig.activeTelemetry == 8 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 5: ");
cliPortPrintF("%s\n", systemConfig.activeTelemetry == 16 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 6: ");
cliPortPrintF("%s\n", systemConfig.activeTelemetry == 32 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 7: ");
cliPortPrintF("%s\n", systemConfig.activeTelemetry == 64 ? " Active" : "Inactive");
cliPortPrint(" Telemetry Set 8: ");
cliPortPrintF("%s\n", systemConfig.activeTelemetry == 128 ? " Active" : "Inactive");
cliPortPrint(" MavLink: ");
cliPortPrintF("%s\n", systemConfig.mavlinkEnabled == true ? " Enabled\n" : "Disabled\n");
validQuery = false;
break;
///////////////////////////
case 'b': // Turn all Telemetry Off
systemConfig.activeTelemetry = 0;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'c': // Toggle Telemetry Set 1 State
systemConfig.activeTelemetry = 1;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'd': // Toggle Telemetry Set 2 State
systemConfig.activeTelemetry = 2;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'e': // Toggle Telemetry Set 3 State
systemConfig.activeTelemetry = 4;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'f': // Toggle Telemetry Set 4 State
systemConfig.activeTelemetry = 8;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'g': // Toggle Telemetry Set 5 State
systemConfig.activeTelemetry = 16;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'h': // Toggle Telemetry Set 6 State
systemConfig.activeTelemetry = 32;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'i': // Toggle Telemetry Set 7 State
systemConfig.activeTelemetry = 64;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'j': // Toggle Telemetry Set 8 State
systemConfig.activeTelemetry = 128;
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'k': // Toggle MavLink Enable/Disable
if (systemConfig.mavlinkEnabled == false)
{
systemConfig.mavlinkEnabled = true;
systemConfig.activeTelemetry = 0x0000;
}
else
{
systemConfig.mavlinkEnabled = false;
}
telemetryQuery = 'a';
validQuery = true;
break;
///////////////////////////
case 'x':
cliPortPrint("\nExiting Telemetry CLI....\n\n");
cliBusy = false;
return;
break;
///////////////////////////
case 'W': // Write System EEPROM Parameters
cliPortPrint("\nWriting System EEPROM Parameters....\n\n");
writeSystemEEPROM();
validQuery = false;
break;
///////////////////////////
case '?':
cliPortPrint("\n");
cliPortPrint("'a' Telemetry Configuration Data\n");
cliPortPrint("'b' Turn all Telemetry Off\n");
cliPortPrint("'c' Toggle Telemetry Set 1 State\n");
cliPortPrint("'d' Toggle Telemetry Set 2 State\n");
cliPortPrint("'e' Toggle Telemetry Set 3 State\n");
cliPortPrint("'f' Toggle Telemetry Set 4 State\n");
cliPortPrint("'g' Toggle Telemetry Set 5 State\n");
cliPortPrint("'h' Toggle Telemetry Set 6 State\n");
cliPortPrint("'i' Toggle Telemetry Set 7 State\n");
cliPortPrint("'j' Toggle Telemetry Set 8 State\n");
cliPortPrint("'k' Toggle MavLink Enabled/Disabled\n");
cliPortPrint(" 'W' Write System EEPROM Parameters\n");
cliPortPrint("'x' Exit Telemetry CLI '?' Command Summary\n");
cliPortPrint("\n");
break;
///////////////////////////
}
}
}
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;
cliPortPrint("\nMXR9150 Accelerometer Calibration:\n\n");
///////////////////////////////////
cliPortPrint("Place accelerometer right side up\n");
cliPortPrint(" Send a character when ready to proceed\n\n");
while (cliPortAvailable() == false);
cliPortRead();
cliPortPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
rightSideUp += mxr9150ZAxis();
xBias += mxr9150XAxis();
yBias += mxr9150YAxis();
delayMicroseconds(1000);
}
rightSideUp /= 5000.0f;
cliPortPrint("Place accelerometer up side down\n");
cliPortPrint(" Send a character when ready to proceed\n\n");
while (cliPortAvailable() == false);
cliPortRead();
cliPortPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
upSideDown += mxr9150ZAxis();
xBias += mxr9150XAxis();
yBias += mxr9150YAxis();
delayMicroseconds(1000);
}
upSideDown /= 5000.0f;
///////////////////////////////////
cliPortPrint("Place accelerometer left edge down\n");
cliPortPrint(" Send a character when ready to proceed\n\n");
while (cliPortAvailable() == false);
cliPortRead();
cliPortPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
leftWingDown += mxr9150YAxis();
zBias += mxr9150ZAxis();
delayMicroseconds(1000);
}
leftWingDown /= 5000.0f;
cliPortPrint("Place accelerometer right edge down\n");
cliPortPrint(" Send a character when ready to proceed\n\n");
while (cliPortAvailable() == false);
cliPortRead();
cliPortPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
rightWingDown += mxr9150YAxis();
zBias += mxr9150ZAxis();
delayMicroseconds(1000);
}
rightWingDown /= 5000.0f;
///////////////////////////////////
cliPortPrint("Place accelerometer rear edge down\n");
cliPortPrint(" Send a character when ready to proceed\n\n");
while (cliPortAvailable() == false);
cliPortRead();
cliPortPrint(" Gathering Data...\n\n");
for (index = 0; index < 5000; index++)
{
noseUp += mxr9150XAxis();
zBias += mxr9150ZAxis();
delayMicroseconds(1000);
}
noseUp /= 5000.0f;
cliPortPrint("Place accelerometer front edge down\n");
cliPortPrint(" Send a character when ready to proceed\n\n");
while (cliPortAvailable() == false);
cliPortRead();
cliPortPrint(" 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;
}