void sensorCLI() { uint8_t sensorQuery = 'x'; uint8_t validQuery = false; cliBusy = true; cliPrint("\nEntering Sensor CLI....\n\n"); while(true) { cliPrint("Sensor CLI -> "); while ((cliAvailable() == false) && (validQuery == false)); if (validQuery == false) sensorQuery = cliRead(); cliPrint("\n"); switch(sensorQuery) { /////////////////////////// case 'a': // Sensor Data cliPrintF("\nAccel Scale Factor: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelScaleFactor[XAXIS], eepromConfig.accelScaleFactor[YAXIS], eepromConfig.accelScaleFactor[ZAXIS]); cliPrintF("Accel Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelBias[XAXIS], eepromConfig.accelBias[YAXIS], eepromConfig.accelBias[ZAXIS]); cliPrintF("Gyro TC Bias Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ], eepromConfig.gyroTCBiasSlope[PITCH], eepromConfig.gyroTCBiasSlope[YAW ]); cliPrintF("Gyro TC Bias Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ], eepromConfig.gyroTCBiasIntercept[PITCH], eepromConfig.gyroTCBiasIntercept[YAW ]); cliPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS], eepromConfig.magBias[YAXIS], eepromConfig.magBias[ZAXIS]); cliPrintF("Accel One G: %9.4f\n", accelOneG); cliPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff); cliPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc); cliPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc); cliPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag); cliPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag); cliPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA); cliPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB); cliPrint("Magnetic Variation: "); if (eepromConfig.magVar >= 0.0f) cliPrintF("E%6.4f\n", eepromConfig.magVar * R2D); else cliPrintF("W%6.4f\n", -eepromConfig.magVar * R2D); cliPrintF("Battery Voltage Divider: %9.4f\n\n", eepromConfig.batteryVoltageDivider); validQuery = false; break; /////////////////////////// case 'b': // Accel Calibration accelCalibration(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'c': // Magnetometer Calibration magCalibration(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'x': cliPrint("\nExiting Sensor CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'B': // Accel Cutoff eepromConfig.accelCutoff = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // kpAcc, kiAcc eepromConfig.KpAcc = readFloatCLI(); eepromConfig.KiAcc = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // kpMag, kiMag eepromConfig.KpMag = readFloatCLI(); eepromConfig.KiMag = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'E': // h dot est/h est Comp Filter A/B eepromConfig.compFilterA = readFloatCLI(); eepromConfig.compFilterB = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'M': // Magnetic Variation eepromConfig.magVar = readFloatCLI() * D2R; sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'V': // Set Battery Voltage Divider eepromConfig.batteryVoltageDivider = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write EEPROM Parameters cliPrint("\nWriting EEPROM Parameters....\n\n"); writeEEPROM(); break; /////////////////////////// case '?': cliPrint("\n"); cliPrint("'a' Display Sensor Data\n"); cliPrint("'b' Accel Calibration 'B' Set Accel Cutoff BAccelCutoff\n"); cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CKpAcc;KiAcc\n"); cliPrint(" 'D' Set kpMag/kiMag DKpMag;KiMag\n"); cliPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n"); cliPrint(" 'M' Set Mag Variation (+ East, - West) MMagVar\n"); cliPrint(" 'V' Set Battery Voltage Divider VbatVoltDivider\n"); cliPrint(" 'W' Write EEPROM Parameters\n"); cliPrint("'x' Exit Sensor CLI '?' Command Summary\n"); cliPrint("\n"); break; /////////////////////////// } } }
void mixerCLI() { float tempFloat; uint8_t index; uint8_t rows, columns; uint8_t mixerQuery = 'x'; uint8_t validQuery = false; cliBusy = true; cliPrint("\nEntering Mixer CLI....\n\n"); while(true) { cliPrint("Mixer CLI -> "); while ((cliAvailable() == false) && (validQuery == false)); if (validQuery == false) mixerQuery = cliRead(); cliPrint("\n"); switch(mixerQuery) { /////////////////////////// case 'a': // Mixer Configuration cliPrint("\nMixer Configuration: "); switch (eepromConfig.mixerConfiguration) { case MIXERTYPE_GIMBAL: cliPrint("MIXERTYPE GIMBAL\n"); break; /////////////////////// case MIXERTYPE_FLYING_WING: cliPrint("MIXERTYPE FLYING WING\n"); break; /////////////////////// case MIXERTYPE_BI: cliPrint("MIXERTYPE BICOPTER\n"); break; /////////////////////// case MIXERTYPE_TRI: cliPrint("MIXERTYPE TRICOPTER\n"); break; /////////////////////// case MIXERTYPE_QUADP: cliPrint("MIXERTYPE QUAD PLUS\n"); break; case MIXERTYPE_QUADX: cliPrint("MIXERTYPE QUAD X\n"); break; case MIXERTYPE_VTAIL4_NO_COMP: cliPrint("MULTITYPE VTAIL NO COMP\n"); break; case MIXERTYPE_VTAIL4_Y_COMP: cliPrint("MULTITYPE VTAIL Y COMP\n"); break; case MIXERTYPE_VTAIL4_RY_COMP: cliPrint("MULTITYPE VTAIL RY COMP\n"); break; case MIXERTYPE_VTAIL4_PY_COMP: cliPrint("MULTITYPE VTAIL PY COMP\n"); break; case MIXERTYPE_VTAIL4_RP_COMP: cliPrint("MULTITYPE VTAIL RP COMP\n"); break; case MIXERTYPE_VTAIL4_RPY_COMP: cliPrint("MULTITYPE VTAIL RPY COMP\n"); break; case MIXERTYPE_Y4: cliPrint("MIXERTYPE Y4\n"); break; /////////////////////// case MIXERTYPE_HEX6P: cliPrint("MIXERTYPE HEX PLUS\n"); break; case MIXERTYPE_HEX6X: cliPrint("MIXERTYPE HEX X\n"); break; case MIXERTYPE_Y6: cliPrint("MIXERTYPE Y6\n"); break; /////////////////////// case MIXERTYPE_OCTOF8P: cliPrint("MIXERTYPE FLAT OCTO PLUS\n"); break; case MIXERTYPE_OCTOF8X: cliPrint("MIXERTYPE FLAT OCTO X\n"); break; case MIXERTYPE_OCTOX8P: cliPrint("MIXERTYPE COAXIAL OCTO PLUS\n"); break; case MIXERTYPE_OCTOX8X: cliPrint("MIXERTYPE COAXIAL OCTO X\n"); break; /////////////////////// case MIXERTYPE_FREEMIX: cliPrint("MIXERTYPE FREE MIX\n"); break; } cliPrintF("Number of Motors: %1d\n", numberMotor); cliPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate); cliPrintF("Servo PWM Rate: %3ld\n", eepromConfig.servoPwmRate); if ( eepromConfig.mixerConfiguration == MIXERTYPE_BI ) { cliPrintF("BiCopter Left Servo Min: %4ld\n", (uint16_t)eepromConfig.biLeftServoMin); cliPrintF("BiCopter Left Servo Mid: %4ld\n", (uint16_t)eepromConfig.biLeftServoMid); cliPrintF("BiCopter Left Servo Max: %4ld\n", (uint16_t)eepromConfig.biLeftServoMax); cliPrintF("BiCopter Right Servo Min: %4ld\n", (uint16_t)eepromConfig.biRightServoMin); cliPrintF("BiCopter Right Servo Mid: %4ld\n", (uint16_t)eepromConfig.biRightServoMid); cliPrintF("BiCopter Right Servo Max: %4ld\n", (uint16_t)eepromConfig.biRightServoMax); } if ( eepromConfig.mixerConfiguration == MIXERTYPE_FLYING_WING ) { cliPrintF("Roll Direction Left: %4ld\n", (uint16_t)eepromConfig.rollDirectionLeft); cliPrintF("Roll Direction Right: %4ld\n", (uint16_t)eepromConfig.rollDirectionRight); cliPrintF("Pitch Direction Left: %4ld\n", (uint16_t)eepromConfig.pitchDirectionLeft); cliPrintF("Pitch Direction Right: %4ld\n", (uint16_t)eepromConfig.pitchDirectionRight); cliPrintF("Wing Left Minimum: %4ld\n", (uint16_t)eepromConfig.wingLeftMinimum); cliPrintF("Wing Left Maximum: %4ld\n", (uint16_t)eepromConfig.wingLeftMaximum); cliPrintF("Wing Right Minimum: %4ld\n", (uint16_t)eepromConfig.wingRightMinimum); cliPrintF("Wing Right Maximum: %4ld\n", (uint16_t)eepromConfig.wingRightMaximum); } if ( eepromConfig.mixerConfiguration == MIXERTYPE_GIMBAL ) { cliPrintF("Gimbal Roll Servo Min: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMin); cliPrintF("Gimbal Roll Servo Mid: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMid); cliPrintF("Gimbal Roll Servo Max: %4ld\n", (uint16_t)eepromConfig.gimbalRollServoMax); cliPrintF("Gimbal Roll Servo Gain: %7.3f\n", eepromConfig.gimbalRollServoGain); cliPrintF("Gimbal Pitch Servo Min: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMin); cliPrintF("Gimbal Pitch Servo Mid: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMid); cliPrintF("Gimbal Pitch Servo Max: %4ld\n", (uint16_t)eepromConfig.gimbalPitchServoMax); cliPrintF("Gimbal Pitch Servo Gain: %7.3f\n", eepromConfig.gimbalPitchServoGain); } if ( eepromConfig.mixerConfiguration == MIXERTYPE_TRI ) { cliPrintF("TriCopter Yaw Servo Min: %4ld\n", (uint16_t)eepromConfig.triYawServoMin); cliPrintF("TriCopter Yaw Servo Mid: %4ld\n", (uint16_t)eepromConfig.triYawServoMid); cliPrintF("TriCopter Yaw Servo Max: %4ld\n", (uint16_t)eepromConfig.triYawServoMax); } if (eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_Y_COMP || eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RY_COMP || eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_PY_COMP || eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RP_COMP || eepromConfig.mixerConfiguration == MIXERTYPE_VTAIL4_RPY_COMP) { cliPrintF("V Tail Angle %6.2f\n", eepromConfig.vTailAngle); } cliPrintF("Yaw Direction: %2d\n\n", (uint16_t)eepromConfig.yawDirection); validQuery = false; break; /////////////////////////// case 'b': // Free Mix Matrix cliPrintF("\nNumber of Free Mixer Motors: %1d\n Roll Pitch Yaw\n", eepromConfig.freeMixMotors); for ( index = 0; index < eepromConfig.freeMixMotors; index++ ) { cliPrintF("Motor%1d %6.3f %6.3f %6.3f\n", index, eepromConfig.freeMix[index][ROLL ], eepromConfig.freeMix[index][PITCH], eepromConfig.freeMix[index][YAW ]); } cliPrint("\n"); validQuery = false; break; /////////////////////////// case 'x': cliPrint("\nExiting Mixer CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Read Mixer Configuration eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI(); initMixer(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'B': // Read ESC and Servo PWM Update Rates eepromConfig.escPwmRate = (uint16_t)readFloatCLI(); eepromConfig.servoPwmRate = (uint16_t)readFloatCLI(); pwmEscInit(eepromConfig.escPwmRate); pwmServoInit(eepromConfig.servoPwmRate); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // Read BiCopter Left Servo Parameters eepromConfig.biLeftServoMin = readFloatCLI(); eepromConfig.biLeftServoMid = readFloatCLI(); eepromConfig.biLeftServoMax = readFloatCLI(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // Read BiCopter Right Servo Parameters eepromConfig.biRightServoMin = readFloatCLI(); eepromConfig.biRightServoMid = readFloatCLI(); eepromConfig.biRightServoMax = readFloatCLI(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'E': // Read Flying Wing Servo Directions eepromConfig.rollDirectionLeft = readFloatCLI(); eepromConfig.rollDirectionRight = readFloatCLI(); eepromConfig.pitchDirectionLeft = readFloatCLI(); eepromConfig.pitchDirectionRight = readFloatCLI(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'F': // Read Flying Wing Servo Limits eepromConfig.wingLeftMinimum = readFloatCLI(); eepromConfig.wingLeftMaximum = readFloatCLI(); eepromConfig.wingRightMinimum = readFloatCLI(); eepromConfig.wingRightMaximum = readFloatCLI(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'G': // Read Free Mix Motor Number eepromConfig.freeMixMotors = (uint8_t)readFloatCLI(); initMixer(); mixerQuery = 'b'; validQuery = true; break; /////////////////////////// case 'H': // Read Free Mix Matrix Element rows = (uint8_t)readFloatCLI(); columns = (uint8_t)readFloatCLI(); eepromConfig.freeMix[rows][columns] = readFloatCLI(); mixerQuery = 'b'; validQuery = true; break; /////////////////////////// case 'I': // Read Gimbal Roll Servo Parameters eepromConfig.gimbalRollServoMin = readFloatCLI(); eepromConfig.gimbalRollServoMid = readFloatCLI(); eepromConfig.gimbalRollServoMax = readFloatCLI(); eepromConfig.gimbalRollServoGain = readFloatCLI(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'J': // Read Gimbal Pitch Servo Parameters eepromConfig.gimbalPitchServoMin = readFloatCLI(); eepromConfig.gimbalPitchServoMid = readFloatCLI(); eepromConfig.gimbalPitchServoMax = readFloatCLI(); eepromConfig.gimbalPitchServoGain = readFloatCLI(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'K': // Read TriCopter YawServo Parameters eepromConfig.triYawServoMin = readFloatCLI(); eepromConfig.triYawServoMid = readFloatCLI(); eepromConfig.triYawServoMax = readFloatCLI(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'L': // Read V Tail Angle eepromConfig.vTailAngle = readFloatCLI(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'M': // Read yaw direction tempFloat = readFloatCLI(); if (tempFloat >= 0.0) tempFloat = 1.0; else tempFloat = -1.0; eepromConfig.yawDirection = tempFloat; mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write EEPROM Parameters cliPrint("\nWriting EEPROM Parameters....\n\n"); writeEEPROM(); break; /////////////////////////// case '?': cliPrint("\n"); cliPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A1 thru 21, see aq32Plus.h\n"); cliPrint("'b' Free Mixer Configuration 'B' Set PWM Rates BESC;Servo\n"); cliPrint(" 'C' Set BiCopter Left Servo Parameters CMin;Mid;Max\n"); cliPrint(" 'D' Set BiCopter Right Servo Parameters DMin;Mid;Max\n"); cliPrint(" 'E' Set Flying Wing Servo Directions ERollLeft;RollRight;PitchLeft;PitchRight\n"); cliPrint(" 'F' Set Flying Wing Servo Limits FLeftMin;LeftMax;RightMin;RightMax\n"); cliPrint(" 'G' Set Number of FreeMix Motors GNumber\n"); cliPrint(" 'H' Set FreeMix Matrix Element HRow;Column;Element\n"); cliPrint(" 'I' Set Gimbal Roll Servo Parameters IMin;Mid;Max;Gain\n"); cliPrint(" 'J' Set Gimbal Pitch Servo Parameters JMin;Mid;Max;Gain\n"); cliPrint(" 'K' Set TriCopter Servo Parameters KMin;Mid;Max\n"); cliPrint(" 'L' Set V Tail Angle LAngle\n"); cliPrint(" 'M' Set Yaw Direction M1 or M-1\n"); cliPrint(" 'W' Write EEPROM Parameters\n"); cliPrint("'x' Exit Sensor CLI '?' Command Summary\n"); cliPrint("\n"); break; /////////////////////////// } } }
void receiverCLI() { char rcOrderString[9]; float tempFloat; uint8_t index; uint8_t receiverQuery = 'x'; uint8_t validQuery = false; cliBusy = true; cliPrint("\nEntering Receiver CLI....\n\n"); while(true) { cliPrint("Receiver CLI -> "); while ((cliAvailable() == false) && (validQuery == false)); if (validQuery == false) receiverQuery = cliRead(); cliPrint("\n"); switch(receiverQuery) { /////////////////////////// case 'a': // Receiver Configuration cliPrint("\nReceiver Type: "); switch(eepromConfig.receiverType) { case PARALLEL_PWM: cliPrint("Parallel\n"); break; case SERIAL_PWM: cliPrint("Serial\n"); break; case SPEKTRUM: cliPrint("Spektrum\n"); break; } cliPrint("Current RC Channel Assignment: "); for (index = 0; index < 8; index++) rcOrderString[eepromConfig.rcMap[index]] = rcChannelLetters[index]; rcOrderString[index] = '\0'; cliPrint(rcOrderString); cliPrint("\n"); cliPrintF("Spektrum Resolution: %s\n", eepromConfig.spektrumHires ? "11 Bit Mode" : "10 Bit Mode"); cliPrintF("Number of Spektrum Channels: %2d\n", eepromConfig.spektrumChannels); cliPrintF("Mid Command: %4ld\n", (uint16_t)eepromConfig.midCommand); cliPrintF("Min Check: %4ld\n", (uint16_t)eepromConfig.minCheck); cliPrintF("Max Check: %4ld\n", (uint16_t)eepromConfig.maxCheck); cliPrintF("Min Throttle: %4ld\n", (uint16_t)eepromConfig.minThrottle); cliPrintF("Max Thottle: %4ld\n\n", (uint16_t)eepromConfig.maxThrottle); tempFloat = eepromConfig.rateScaling * 180000.0 / PI; cliPrintF("Max Rate Command: %6.2f DPS\n", tempFloat); tempFloat = eepromConfig.attitudeScaling * 180000.0 / PI; cliPrintF("Max Attitude Command: %6.2f Degrees\n\n", tempFloat); cliPrintF("Arm Delay Count: %3d Frames\n", eepromConfig.armCount); cliPrintF("Disarm Delay Count: %3d Frames\n\n", eepromConfig.disarmCount); validQuery = false; break; /////////////////////////// case 'b': // Read Max Rate Value eepromConfig.rateScaling = readFloatCLI() / 180000 * PI; receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'c': // Read Max Attitude Value eepromConfig.attitudeScaling = readFloatCLI() / 180000 * PI; receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'x': cliPrint("\nExiting Receiver CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Read RX Input Type eepromConfig.receiverType = (uint8_t)readFloatCLI(); cliPrint( "\nReceiver Type Changed....\n"); cliPrint("\nSystem Resetting....\n"); delay(100); writeEEPROM(); systemReset(false); break; /////////////////////////// case 'B': // Read RC Control Order readStringCLI( rcOrderString, 8 ); parseRcChannels( rcOrderString ); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // Read Spektrum Resolution eepromConfig.spektrumHires = (uint8_t)readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // Read Number of Spektrum Channels eepromConfig.spektrumChannels = (uint8_t)readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'E': // Read RC Control Points eepromConfig.midCommand = readFloatCLI(); eepromConfig.minCheck = readFloatCLI(); eepromConfig.maxCheck = readFloatCLI(); eepromConfig.minThrottle = readFloatCLI(); eepromConfig.maxThrottle = readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'F': // Read Arm/Disarm Counts eepromConfig.armCount = (uint8_t)readFloatCLI(); eepromConfig.disarmCount = (uint8_t)readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write EEPROM Parameters cliPrint("\nWriting EEPROM Parameters....\n\n"); writeEEPROM(); break; /////////////////////////// case '?': cliPrint("\n"); cliPrint("'a' Receiver Configuration Data 'A' Set RX Input Type AX, 1=Parallel, 2=Serial, 3=Spektrum\n"); cliPrint("'b' Set Maximum Rate Command 'B' Set RC Control Order BTAER1234\n"); cliPrint("'c' Set Maximum Attitude Command 'C' Set Spektrum Resolution C0 or C1\n"); cliPrint(" 'D' Set Number of Spektrum Channels D6 thru D12\n"); cliPrint(" 'E' Set RC Control Points EmidCmd;minChk;maxChk;minThrot;maxThrot\n"); cliPrint(" 'F' Set Arm/Disarm Counts FarmCount;disarmCount\n"); cliPrint(" 'W' Write EEPROM Parameters\n"); cliPrint("'x' Exit Receiver CLI '?' Command Summary\n"); cliPrint("\n"); break; /////////////////////////// } } }
void 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, %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("\nnDot PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[NDOT_PID].B, eepromConfig.PID[NDOT_PID].P, eepromConfig.PID[NDOT_PID].I, eepromConfig.PID[NDOT_PID].D, eepromConfig.PID[NDOT_PID].windupGuard, eepromConfig.PID[NDOT_PID].dErrorCalc ? "Error" : "State"); cliPortPrintF("eDot PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[EDOT_PID].B, eepromConfig.PID[EDOT_PID].P, eepromConfig.PID[EDOT_PID].I, eepromConfig.PID[EDOT_PID].D, eepromConfig.PID[EDOT_PID].windupGuard, eepromConfig.PID[EDOT_PID].dErrorCalc ? "Error" : "State"); cliPortPrintF("hDot 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("\nN PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[N_PID].B, eepromConfig.PID[N_PID].P, eepromConfig.PID[N_PID].I, eepromConfig.PID[N_PID].D, eepromConfig.PID[N_PID].windupGuard, eepromConfig.PID[N_PID].dErrorCalc ? "Error" : "State"); cliPortPrintF("E PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[E_PID].B, eepromConfig.PID[E_PID].P, eepromConfig.PID[E_PID].I, eepromConfig.PID[E_PID].D, eepromConfig.PID[E_PID].windupGuard, eepromConfig.PID[E_PID].dErrorCalc ? "Error" : "State"); cliPortPrintF("h 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': // 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", axisPID[ROLL ], axisPID[PITCH], axisPID[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': // Not Used cliQuery = 'x'; 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 ((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, ", sBusChannel[index]); cliPortPrintF("%4ld\n", sBusChannel[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; /////////////////////////////// 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' 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 gpsCLI() { USART_InitTypeDef USART_InitStructure; uint8_t gpsQuery = 'x'; uint8_t validQuery = false; cliBusy = true; cliPrint("\nEntering GPS CLI....\n\n"); while(true) { cliPrint("GPS CLI -> "); while ((cliAvailable() == false) && (validQuery == false)); if (validQuery == false) gpsQuery = cliRead(); cliPrint("\n"); switch(gpsQuery) { /////////////////////////// case 'a': // GPS Installation Data cliPrint("\n"); switch(eepromConfig.gpsType) { /////////////// case NO_GPS: cliPrint("No GPS Installed....\n\n"); break; /////////////// case MEDIATEK_3329_BINARY: cliPrint("MediaTek 3329 GPS installed, Binary Mode....\n\n"); break; /////////////// case MEDIATEK_3329_NMEA: cliPrint("MediaTek 3329 GPS Installed, NMEA Mode....\n\n"); break; /////////////// case UBLOX: cliPrint("UBLOX GPS Installed, Binary Mode....\n\n"); break; /////////////// } cliPrintF("GPS Baud Rate: %6ld\n\n", eepromConfig.gpsBaudRate); validQuery = false; break; /////////////////////////// case 'x': cliPrint("\nExiting GPS CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Set GPS Installed State to False eepromConfig.gpsType = NO_GPS; gpsQuery = 'a'; validQuery = true; break; /////////////////////////// case 'B': // Set GPS Type to MediaTek 3329 Binary eepromConfig.gpsType = MEDIATEK_3329_BINARY; initGPS(); gpsQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // Set GPS Type to MediaTek 3329 NMEA eepromConfig.gpsType = MEDIATEK_3329_NMEA; initGPS(); gpsQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // Set GPS Type to UBLOX Binary eepromConfig.gpsType = UBLOX; initGPS(); gpsQuery = 'a'; validQuery = true; break; /////////////////////////// case 'S': // Read GPS Baud Rate eepromConfig.gpsBaudRate = (uint16_t)readFloatCLI(); USART_StructInit(&USART_InitStructure); USART_InitStructure.USART_BaudRate = eepromConfig.gpsBaudRate; USART_Init(USART2, &USART_InitStructure); gpsQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write EEPROM Parameters cliPrint("\nWriting EEPROM Parameters....\n\n"); writeEEPROM(); break; /////////////////////////// case '?': cliPrint("\n"); cliPrint("'a' Display GPS Installation Data 'A' Set GPS Type to No GPS\n"); cliPrint(" 'B' Set GPS Type to MediaTek 3329 Binary\n"); cliPrint(" 'C' Set GPS Type to MediaTek 3329 NMEA\n"); cliPrint(" 'D' Set GPS Type to UBLOX\n"); cliPrint(" 'S' Set GPS Baud Rate\n"); cliPrint(" 'W' Write EEPROM Parameters\n"); cliPrint("'x' Exit GPS CLI '?' Command Summary\n"); cliPrint("\n"); break; /////////////////////////// } } }
void 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 sensorCLI() { uint8_t sensorQuery = 'x'; uint8_t tempInt; uint8_t validQuery = false; cliBusy = true; cliPortPrint("\nEntering Sensor CLI....\n\n"); while(true) { cliPortPrint("Sensor CLI -> "); while ((cliPortAvailable() == false) && (validQuery == false)); if (validQuery == false) sensorQuery = cliPortRead(); cliPortPrint("\n"); switch(sensorQuery) { /////////////////////////// case 'a': // Sensor Data if (eepromConfig.useMpu6050 == true) { cliPortPrint("\nUsing MPU6050....\n\n"); cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS], eepromConfig.accelTCBiasSlope[YAXIS], eepromConfig.accelTCBiasSlope[ZAXIS]); cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS], eepromConfig.accelTCBiasIntercept[YAXIS], eepromConfig.accelTCBiasIntercept[ZAXIS]); } else { cliPortPrint("\nUsing ADXL345/MPU3050....\n\n"); } cliPortPrintF("Accel Scale Factor: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelScaleFactor[XAXIS], eepromConfig.accelScaleFactor[YAXIS], eepromConfig.accelScaleFactor[ZAXIS]); cliPortPrintF("Accel Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelBias[XAXIS], eepromConfig.accelBias[YAXIS], eepromConfig.accelBias[ZAXIS]); cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ], eepromConfig.gyroTCBiasSlope[PITCH], eepromConfig.gyroTCBiasSlope[YAW ]); cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ], eepromConfig.gyroTCBiasIntercept[PITCH], eepromConfig.gyroTCBiasIntercept[YAW ]); cliPortPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS], eepromConfig.magBias[YAXIS], eepromConfig.magBias[ZAXIS]); cliPortPrintF("Accel One G: %9.4f\n", accelOneG); cliPortPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff); cliPortPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc); cliPortPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag); cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", eepromConfig.compFilterA); cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", eepromConfig.compFilterB); if (eepromConfig.useMpu6050 == true) { cliPortPrint("MPU6050 DLPF: "); switch(eepromConfig.dlpfSetting) { case DLPF_256HZ: cliPortPrint("256 Hz\n"); break; case DLPF_188HZ: cliPortPrint("188 Hz\n"); break; case DLPF_98HZ: cliPortPrint("98 Hz\n"); break; case DLPF_42HZ: cliPortPrint("42 Hz\n"); break; } } if (eepromConfig.useMs5611 == true) cliPortPrint("\nUsing MS5611....\n\n"); else cliPortPrint("\nUsing BMP085....\n\n"); if (eepromConfig.verticalVelocityHoldOnly == true) cliPortPrint("Vertical Velocity Hold Only\n\n"); else cliPortPrint("Vertical Velocity and Altitude Hold\n\n"); cliPortPrintF("Voltage Monitor Scale: %9.4f\n", eepromConfig.voltageMonitorScale); cliPortPrintF("Voltage Monitor Bias: %9.4f\n", eepromConfig.voltageMonitorBias); cliPortPrintF("Number of Battery Cells: %1d\n\n", eepromConfig.batteryCells); cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", eepromConfig.batteryLow); cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", eepromConfig.batteryVeryLow); cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", eepromConfig.batteryMaxLow); validQuery = false; break; /////////////////////////// case 'b': // MPU Calibration if (eepromConfig.useMpu6050 == true) mpu6050Calibration(); else mpu3050Calibration(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'c': // Magnetometer Calibration magCalibration(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'd': // Accel Calibration if (eepromConfig.useMpu6050 == false) accelCalibrationADXL345(); else accelCalibrationMPU(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'm': // Toggle MPU3050/MPU6050 if (eepromConfig.useMpu6050) { eepromConfig.useMpu6050 = false; initAdxl345(); initMpu3050(); } else { eepromConfig.useMpu6050 = true; initMpu6050(); } sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'p': // Toggle BMP085/MS5611 if (eepromConfig.useMs5611) { eepromConfig.useMs5611 = false; initBmp085(); } else { eepromConfig.useMs5611 = true; initMs5611(); } sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'v': // Toggle Vertical Velocity Hold Only if (eepromConfig.verticalVelocityHoldOnly) eepromConfig.verticalVelocityHoldOnly = false; else eepromConfig.verticalVelocityHoldOnly = true; sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'x': cliPortPrint("\nExiting Sensor CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Set MPU6050 Digital Low Pass Filter if (eepromConfig.useMpu6050 == true) { tempInt = (uint8_t)readFloatCLI(); switch (tempInt) { case DLPF_256HZ: eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ; break; case DLPF_188HZ: eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ; break; case DLPF_98HZ: eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ; break; case DLPF_42HZ: eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ; break; } i2cWrite(I2C2, MPU6050_ADDRESS, MPU6050_CONFIG, eepromConfig.dlpfSetting); // Accel and Gyro DLPF Setting sensorQuery = 'a'; validQuery = true; } break; /////////////////////////// case 'B': // Accel Cutoff eepromConfig.accelCutoff = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // kpAcc eepromConfig.KpAcc = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // kpMag eepromConfig.KpMag = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'E': // h dot est/h est Comp Filter A/B eepromConfig.compFilterA = readFloatCLI(); eepromConfig.compFilterB = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'N': // Set Voltage Monitor Trip Points eepromConfig.batteryLow = readFloatCLI(); eepromConfig.batteryVeryLow = readFloatCLI(); eepromConfig.batteryMaxLow = readFloatCLI(); thresholds[BATTERY_LOW].value = eepromConfig.batteryLow; thresholds[BATTERY_VERY_LOW].value = eepromConfig.batteryVeryLow; thresholds[BATTRY_MAX_LOW].value = eepromConfig.batteryMaxLow; sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'V': // Set Voltage Monitor Parameters eepromConfig.voltageMonitorScale = readFloatCLI(); eepromConfig.voltageMonitorBias = readFloatCLI(); eepromConfig.batteryCells = (uint8_t)readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write EEPROM Parameters cliPortPrint("\nWriting EEPROM Parameters....\n\n"); validQuery = false; writeEEPROM(); break; /////////////////////////// case '?': cliPortPrint("\n"); if (eepromConfig.useMpu6050 == true) cliPortPrint("'a' Display Sensor Data 'A' Set MPU6050 DLPF A0 thru 3\n"); else cliPortPrint("'a' Display Sensor Data\n"); cliPortPrint("'b' MPU Calibration 'B' Set Accel Cutoff BAccelCutoff\n"); cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc CKpAcc\n"); cliPortPrint("'d' Accel Calibration 'D' Set kpMag DKpMag\n"); cliPortPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n"); cliPortPrint("'m' Toggle MPU3050/MPU6050\n"); cliPortPrint(" 'N' Set Voltage Monitor Trip Points Nlow;veryLow;maxLow\n"); cliPortPrint("'p' Toggle BMP085/MS5611\n"); cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n"); cliPortPrint(" 'W' Write EEPROM Parameters\n"); cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n"); cliPortPrint("\n"); break; /////////////////////////// } } }
void sensorCLI() { uint8_t sensorQuery = 'x'; uint8_t tempInt; uint8_t validQuery = false; cliBusy = true; cliPortPrint("\nEntering Sensor CLI....\n\n"); while(true) { cliPortPrint("Sensor CLI -> "); while ((cliPortAvailable() == false) && (validQuery == false)); if (validQuery == false) sensorQuery = cliPortRead(); cliPortPrint("\n"); switch(sensorQuery) { /////////////////////////// case 'a': // Sensor Data cliPortPrintF("MPU Accel Bias: %9.3f, %9.3f, %9.3f\n", sensorConfig.accelBiasMPU[XAXIS], sensorConfig.accelBiasMPU[YAXIS], sensorConfig.accelBiasMPU[ZAXIS]); cliPortPrintF("MPU Accel Scale Factor: %9.7f, %9.7f, %9.7f\n", sensorConfig.accelScaleFactorMPU[XAXIS], sensorConfig.accelScaleFactorMPU[YAXIS], sensorConfig.accelScaleFactorMPU[ZAXIS]); cliPortPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", sensorConfig.accelTCBiasSlope[XAXIS], sensorConfig.accelTCBiasSlope[YAXIS], sensorConfig.accelTCBiasSlope[ZAXIS]); cliPortPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", sensorConfig.accelTCBiasIntercept[XAXIS], sensorConfig.accelTCBiasIntercept[YAXIS], sensorConfig.accelTCBiasIntercept[ZAXIS]); cliPortPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", sensorConfig.gyroTCBiasSlope[ROLL ], sensorConfig.gyroTCBiasSlope[PITCH], sensorConfig.gyroTCBiasSlope[YAW ]); cliPortPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", sensorConfig.gyroTCBiasIntercept[ROLL ], sensorConfig.gyroTCBiasIntercept[PITCH], sensorConfig.gyroTCBiasIntercept[YAW ]); cliPortPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", sensorConfig.magBias[XAXIS], sensorConfig.magBias[YAXIS], sensorConfig.magBias[ZAXIS]); cliPortPrintF("Accel One G: %9.4f\n", accelOneG); cliPortPrintF("Accel Cutoff: %9.4f\n", sensorConfig.accelCutoff); cliPortPrintF("KpAcc (MARG): %9.4f\n", sensorConfig.KpAcc); cliPortPrintF("KiAcc (MARG): %9.4f\n", sensorConfig.KiAcc); cliPortPrintF("KpMag (MARG): %9.4f\n", sensorConfig.KpMag); cliPortPrintF("KiMag (MARG): %9.4f\n", sensorConfig.KiMag); cliPortPrintF("hdot est/h est Comp Fil A: %9.4f\n", sensorConfig.compFilterA); cliPortPrintF("hdot est/h est Comp Fil B: %9.4f\n", sensorConfig.compFilterB); cliPortPrint("MPU6000 DLPF: "); switch(sensorConfig.dlpfSetting) { case DLPF_256HZ: cliPortPrint("256 Hz\n"); break; case DLPF_188HZ: cliPortPrint("188 Hz\n"); break; case DLPF_98HZ: cliPortPrint("98 Hz\n"); break; case DLPF_42HZ: cliPortPrint("42 Hz\n"); break; } cliPortPrintF("Voltage Monitor Scale: %9.4f\n\n", sensorConfig.voltageMonitorScale); cliPortPrintF("Voltage Monitor Bias: %9.4f\n", sensorConfig.voltageMonitorBias); cliPortPrintF("Number of Battery Cells: %1d\n\n", sensorConfig.batteryCells); cliPortPrintF("Battery Low Setpoint: %4.2f volts\n", sensorConfig.batteryLow); cliPortPrintF("Battery Very Low Setpoint: %4.2f volts\n", sensorConfig.batteryVeryLow); cliPortPrintF("Battery Max Low Setpoint: %4.2f volts\n\n", sensorConfig.batteryMaxLow); if (sensorConfig.gpsVelocityHoldOnly) cliPortPrint("GPS Velocity Hold Only\n"); else cliPortPrint("GPS Velocity and Position Hold\n"); if (sensorConfig.verticalVelocityHoldOnly) cliPortPrint("Vertical Velocity Hold Only\n\n"); else cliPortPrint("Vertical Velocity and Altitude Hold\n\n"); validQuery = false; break; /////////////////////////// case 'b': // MPU6000 Calibration mpu6000Calibration(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'c': // Magnetometer Calibration magCalibration(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'd': // Accel Bias and Scale Factor Calibration accelCalibrationMPU(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'g': // Toggle GPS Velocity Hold Only if (sensorConfig.gpsVelocityHoldOnly) sensorConfig.gpsVelocityHoldOnly = false; else sensorConfig.gpsVelocityHoldOnly = true; sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'v': // Toggle Vertical Velocity Hold Only if (sensorConfig.verticalVelocityHoldOnly) sensorConfig.verticalVelocityHoldOnly = false; else sensorConfig.verticalVelocityHoldOnly = true; sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'x': cliPortPrint("\nExiting Sensor CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Set MPU6000 Digital Low Pass Filter tempInt = (uint8_t)readFloatCLI(); switch(tempInt) { case DLPF_256HZ: sensorConfig.dlpfSetting = BITS_DLPF_CFG_256HZ; break; case DLPF_188HZ: sensorConfig.dlpfSetting = BITS_DLPF_CFG_188HZ; break; case DLPF_98HZ: sensorConfig.dlpfSetting = BITS_DLPF_CFG_98HZ; break; case DLPF_42HZ: sensorConfig.dlpfSetting = BITS_DLPF_CFG_42HZ; break; } setSPIdivisor(MPU6000_SPI, 64); // 0.65625 MHz SPI clock GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN); spiTransfer(MPU6000_SPI, MPU6000_CONFIG); spiTransfer(MPU6000_SPI, sensorConfig.dlpfSetting); GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN); setSPIdivisor(MPU6000_SPI, 2); // 21 MHz SPI clock (within 20 +/- 10%) sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'B': // Accel Cutoff sensorConfig.accelCutoff = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // kpAcc, kiAcc sensorConfig.KpAcc = readFloatCLI(); sensorConfig.KiAcc = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // kpMag, kiMag sensorConfig.KpMag = readFloatCLI(); sensorConfig.KiMag = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'E': // h dot est/h est Comp Filter A/B sensorConfig.compFilterA = readFloatCLI(); sensorConfig.compFilterB = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'M': // Set Voltage Monitor Trip Points sensorConfig.batteryLow = readFloatCLI(); sensorConfig.batteryVeryLow = readFloatCLI(); sensorConfig.batteryMaxLow = readFloatCLI(); thresholds[BATTERY_LOW].value = sensorConfig.batteryLow; thresholds[BATTERY_VERY_LOW].value = sensorConfig.batteryVeryLow; thresholds[BATTRY_MAX_LOW].value = sensorConfig.batteryMaxLow; sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'V': // Set Voltage Monitor Parameters sensorConfig.voltageMonitorScale = readFloatCLI(); sensorConfig.voltageMonitorBias = readFloatCLI(); sensorConfig.batteryCells = (uint8_t)readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write Sensor EEPROM Parameters cliPortPrint("\nWriting Sensor EEPROM Parameters....\n\n"); writeSensorEEPROM(); validQuery = false; break; /////////////////////////// case '?': cliPortPrint("\n"); cliPortPrint("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3\n"); cliPortPrint("'b' MPU6000 Calibration 'B' Set Accel Cutoff BAccelCutoff\n"); cliPortPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CKpAcc;KiAcc\n"); cliPortPrint("'d' Accel Bias and SF Calibration 'D' Set kpMag/kiMag DKpMag;KiMag\n"); cliPortPrint(" 'E' Set h dot est/h est Comp Filter A/B EA;B\n"); cliPortPrint("'g' Toggle GPS Velocity Hold Only 'M' Set Voltage Monitor Trip Points Mlow;veryLow;maxLow\n"); cliPortPrint("'v' Toggle Vertical Velocity Hold Only 'V' Set Voltage Monitor Parameters Vscale;bias;cells\n"); cliPortPrint(" 'W' Write EEPROM Parameters\n"); cliPortPrint("'x' Exit Sensor CLI '?' Command Summary\n"); cliPortPrint("\n"); break; /////////////////////////// } } }
void sensorCLI() { uint8_t sensorQuery = 'x'; uint8_t tempInt; uint8_t validQuery = false; cliBusy = true; cliPrintF("\nEntering Sensor CLI....\n\n"); while (true) { cliPrintF("Sensor CLI -> "); while ((cliAvailable() == false) && (validQuery == false)); if (validQuery == false) sensorQuery = getChar(); //cliRead(sensorQuery, 1); cliPrintF("\n"); switch (sensorQuery) { /////////////////////////// case 'a': // Sensor Data cliPrintF("Accel Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasSlope[XAXIS], eepromConfig.accelTCBiasSlope[YAXIS], eepromConfig.accelTCBiasSlope[ZAXIS]); cliPrintF("Accel Temp Comp Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.accelTCBiasIntercept[XAXIS], eepromConfig.accelTCBiasIntercept[YAXIS], eepromConfig.accelTCBiasIntercept[ZAXIS]); cliPrintF("Gyro Temp Comp Slope: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasSlope[ROLL ], eepromConfig.gyroTCBiasSlope[PITCH], eepromConfig.gyroTCBiasSlope[YAW ]); cliPrintF("Gyro Temp Comp Intercept: %9.4f, %9.4f, %9.4f\n", eepromConfig.gyroTCBiasIntercept[ROLL ], eepromConfig.gyroTCBiasIntercept[PITCH], eepromConfig.gyroTCBiasIntercept[YAW ]); cliPrintF("Mag Bias: %9.4f, %9.4f, %9.4f\n", eepromConfig.magBias[XAXIS], eepromConfig.magBias[YAXIS], eepromConfig.magBias[ZAXIS]); cliPrintF("Accel One G: %9.4f\n", accelOneG); cliPrintF("Accel Cutoff: %9.4f\n", eepromConfig.accelCutoff); cliPrintF("KpAcc (MARG): %9.4f\n", eepromConfig.KpAcc); cliPrintF("KiAcc (MARG): %9.4f\n", eepromConfig.KiAcc); cliPrintF("KpMag (MARG): %9.4f\n", eepromConfig.KpMag); cliPrintF("KiMag (MARG): %9.4f\n", eepromConfig.KiMag); cliPrintF("MPU6000 DLPF: "); switch (eepromConfig.dlpfSetting) { case DLPF_256HZ: cliPrintF("256 Hz\n"); break; case DLPF_188HZ: cliPrintF("188 Hz\n"); break; case DLPF_98HZ: cliPrintF("98 Hz\n"); break; case DLPF_42HZ: cliPrintF("42 Hz\n"); break; } validQuery = false; break; /////////////////////////// case 'b': // MPU6050Calibration mpu6050Calibration(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// // HJI case 'c': // Magnetometer Calibration // HJI magCalibration(); // HJI sensorQuery = 'a'; // HJI validQuery = true; // HJI break; /////////////////////////// case 'x': cliPrintF("\nExiting Sensor CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Set MPU6000 Digital Low Pass Filter tempInt = (uint8_t)readFloatCLI(); switch (tempInt) { case DLPF_256HZ: eepromConfig.dlpfSetting = BITS_DLPF_CFG_256HZ; break; case DLPF_188HZ: eepromConfig.dlpfSetting = BITS_DLPF_CFG_188HZ; break; case DLPF_98HZ: eepromConfig.dlpfSetting = BITS_DLPF_CFG_98HZ; break; case DLPF_42HZ: eepromConfig.dlpfSetting = BITS_DLPF_CFG_42HZ; break; } i2cWrite(MPU6050_ADDRESS, MPU6050_CONFIG, eepromConfig.dlpfSetting); // Accel and Gyro DLPF Setting sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'B': // Accel Cutoff eepromConfig.accelCutoff = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // kpAcc, kiAcc eepromConfig.KpAcc = readFloatCLI(); eepromConfig.KiAcc = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // kpMag, kiMag eepromConfig.KpMag = readFloatCLI(); eepromConfig.KiMag = readFloatCLI(); sensorQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write EEPROM Parameters cliPrintF("\nWriting EEPROM Parameters....\n\n"); writeEEPROM(); break; /////////////////////////// case '?': cliPrintF("\n"); cliPrintF("'a' Display Sensor Data 'A' Set MPU6000 DLPF A0 thru 3\n"); cliPrintF("'b' MPU6050 Temp Calibration 'B' Set Accel Cutoff BAccelCutoff\n"); cliPrintF(" 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n"); // HJI cliPrint("'c' Magnetometer Calibration 'C' Set kpAcc/kiAcc CkpAcc;kiAcc\n"); cliPrintF(" 'D' Set kpMag/kiMag DkpMag;kiMag\n"); cliPrintF(" 'W' Write EEPROM Parameters\n"); cliPrintF("'x' Exit Sensor CLI '?' Command Summary\n\n"); break; /////////////////////////// } } }
void mixerCLI() { float tempFloat; uint8_t 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_Y6: cliPortPrint(" MIXERTYPE Y6\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("Number of Free Mixer Motors: %1d\n\n Roll Pitch Yaw Throttle\n\n", eepromConfig.freeMixMotors); for ( index = 0; index < eepromConfig.freeMixMotors; index++ ) { cliPortPrintF("Motor%1d %6.3f %6.3f %6.3f %6.3f\n", index + 1, eepromConfig.freeMix[index][ROLL ], eepromConfig.freeMix[index][PITCH ], eepromConfig.freeMix[index][YAW ], eepromConfig.freeMix[index][THROTTLE]); } cliPortPrint("\n"); } cliPortPrintF("Roll Att Alt Compensation Limit: %4.1f\n", eepromConfig.rollAttAltCompensationLimit * R2D); cliPortPrintF("Roll Att Alt Compensation Gain: %4.1f\n\n", eepromConfig.rollAttAltCompensationGain); cliPortPrintF("Pitch Att Alt Compensation Limit: %4.1f\n", eepromConfig.pitchAttAltCompensationLimit * R2D); cliPortPrintF("Pitch Att Alt Compensation Gain: %4.1f\n\n", eepromConfig.pitchAttAltCompensationGain); 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(); columns = (uint8_t)readFloatCLI(); if ((rows > 0) && (rows < 9) && (columns > 0) && (columns < 5)) eepromConfig.freeMix[rows - 1][columns - 1] = readFloatCLI(); else { tempFloat = readFloatCLI(); cliPortPrint("Index entry error!!!!\n\n"); } } else { tempFloat = readFloatCLI(); tempFloat = readFloatCLI(); tempFloat = readFloatCLI(); cliPortPrintF("\nFree Mix not Selected....\n\n"); } mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'U': // Roll Att Alt Compensation Limit and Gain eepromConfig.rollAttAltCompensationLimit = readFloatCLI() * D2R; eepromConfig.rollAttAltCompensationGain = readFloatCLI(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'V': // PitchAtt Alt Compensation Limit and Gain eepromConfig.pitchAttAltCompensationLimit = readFloatCLI() * D2R; eepromConfig.pitchAttAltCompensationGain = readFloatCLI(); 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 4, 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(" 'U' Roll Att Alt Comp Limit;Gain UrLimit;rGain\n"); cliPortPrint(" 'V' Pitch Att Alt Comp Limit;Gain VpLimit;pGain\n"); cliPortPrint(" 'W' Write EEPROM Parameters\n"); cliPortPrint("'x' Exit Mixer CLI '?' Command Summary\n\n"); break; /////////////////////////// } } }
void mixerCLI() { float tempFloat; uint8_t mixerQuery = 'x'; uint8_t validQuery = false; cliBusy = true; cliPrint("\nEntering Mixer CLI....\n\n"); while(true) { cliPrint("Mixer CLI -> "); while ((cliAvailable() == false) && (validQuery == false)); if (validQuery == false) mixerQuery = cliRead(); cliPrint("\n"); switch(mixerQuery) { /////////////////////////// case 'a': // Mixer Configuration cliPrint("\nMixer Configuration: "); switch (eepromConfig.mixerConfiguration) { case MIXERTYPE_QUADX: cliPrint("MIXERTYPE QUAD X\n"); break; case MIXERTYPE_HEX6X: cliPrint(" MIXERTYPE HEX X\n"); break; } cliPrintF("Number of Motors: %1d\n", numberMotor); cliPrintF("ESC PWM Rate: %3ld\n", eepromConfig.escPwmRate); cliPrintF("Servo PWM Rate: %3ld\n\n", eepromConfig.servoPwmRate); validQuery = false; break; /////////////////////////// case 'x': cliPrint("\nExiting Mixer CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Read Mixer Configuration eepromConfig.mixerConfiguration = (uint8_t)readFloatCLI(); initMixer(); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'B': // Read ESC and Servo PWM Update Rates eepromConfig.escPwmRate = (uint16_t)readFloatCLI(); eepromConfig.servoPwmRate = (uint16_t)readFloatCLI(); pwmEscInit(eepromConfig.escPwmRate); pwmServoInit(eepromConfig.servoPwmRate); mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'M': // Read yaw direction tempFloat = readFloatCLI(); if (tempFloat >= 0.0) tempFloat = 1.0; else tempFloat = -1.0; eepromConfig.yawDirection = tempFloat; mixerQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write EEPROM Parameters cliPrint("\nWriting EEPROM Parameters....\n\n"); writeEEPROM(); break; /////////////////////////// case '?': cliPrint("\n"); cliPrint("'a' Mixer Configuration Data 'A' Set Mixer Configuration A1 thru 21, see aq32Plus.h\n"); cliPrint("'b' Free Mixer Configuration 'B' Set PWM Rates BESC;Servo\n"); cliPrint(" 'C' Set BiCopter Left Servo Parameters CMin;Mid;Max\n"); cliPrint(" 'D' Set BiCopter Right Servo Parameters DMin;Mid;Max\n"); cliPrint(" 'E' Set Flying Wing Servo Directions ERollLeft;RollRight;PitchLeft;PitchRight\n"); cliPrint(" 'F' Set Flying Wing Servo Limits FLeftMin;LeftMax;RightMin;RightMax\n"); cliPrint(" 'G' Set Number of FreeMix Motors GNumber\n"); cliPrint(" 'H' Set FreeMix Matrix Element HRow;Column;Element\n"); cliPrint(" 'I' Set Gimbal Roll Servo Parameters IMin;Mid;Max;Gain\n"); cliPrint(" 'J' Set Gimbal Pitch Servo Parameters JMin;Mid;Max;Gain\n"); cliPrint(" 'K' Set TriCopter Servo Parameters KMin;Mid;Max\n"); cliPrint(" 'L' Set V Tail Angle LAngle\n"); cliPrint(" 'M' Set Yaw Direction M1 or M-1\n"); cliPrint(" 'W' Write EEPROM Parameters\n"); cliPrint("'x' Exit Sensor CLI '?' Command Summary\n"); cliPrint("\n"); break; /////////////////////////// } } }
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; /////////////////////////////// } } }