void processFlightCommands(void) { uint8_t channel, subChannel; float hdgDelta, simpleX, simpleY; if (rcActive == true) { // Read receiver commands for (channel = 0; channel < 8; channel++) { subChannel = channel; if (channel >= VERTICAL) subChannel++; if (eepromConfig.receiverType == SPEKTRUM) rxCommand[subChannel] = (float)spektrumRead(eepromConfig.rcMap[channel]); else rxCommand[subChannel] = (float)ppmRxRead(eepromConfig.rcMap[channel]); } rxCommand[ROLL] -= eepromConfig.midCommand; // Roll Range -1000:1000 rxCommand[PITCH] -= eepromConfig.midCommand; // Pitch Range -1000:1000 rxCommand[YAW] -= eepromConfig.midCommand; // Yaw Range -1000:1000 rxCommand[VERTICAL] = rxCommand[THROTTLE] - eepromConfig.midCommand; // Vertical Range -1000:1000 rxCommand[THROTTLE] -= eepromConfig.midCommand - MIDCOMMAND; // Throttle Range 2000:4000 rxCommand[AUX1] -= eepromConfig.midCommand - MIDCOMMAND; // Aux1 Range 2000:4000 rxCommand[AUX2] -= eepromConfig.midCommand - MIDCOMMAND; // Aux2 Range 2000:4000 rxCommand[AUX3] -= eepromConfig.midCommand - MIDCOMMAND; // Aux3 Range 2000:4000 rxCommand[AUX4] -= eepromConfig.midCommand - MIDCOMMAND; // Aux4 Range 2000:4000 } // Set past command in detent values for (channel = 0; channel < 4; channel++) previousCommandInDetent[channel] = commandInDetent[channel]; // Apply deadbands and set detent discretes' for (channel = 0; channel < 4; channel++) { if ((rxCommand[channel] <= DEADBAND) && (rxCommand[channel] >= -DEADBAND)) { rxCommand[channel] = 0; commandInDetent[channel] = true; } else { commandInDetent[channel] = false; if (rxCommand[channel] > 0) { rxCommand[channel] = (rxCommand[channel] - DEADBAND) * DEADBAND_SLOPE; } else { rxCommand[channel] = (rxCommand[channel] + DEADBAND) * DEADBAND_SLOPE; } } } /////////////////////////////////// // Check for low throttle if ( rxCommand[THROTTLE] < eepromConfig.minCheck ) { // Check for disarm command ( low throttle, left yaw ) if (((rxCommand[YAW] < (eepromConfig.minCheck - MIDCOMMAND)) && (armed == true)) && (verticalModeState == ALT_DISENGAGED_THROTTLE_ACTIVE)) { disarmingTimer++; if (disarmingTimer > eepromConfig.disarmCount) { armed = false; disarmingTimer = 0; } } else { disarmingTimer = 0; } // Check for gyro bias command ( low throttle, left yaw, aft pitch, right roll ) if ( (rxCommand[YAW ] < (eepromConfig.minCheck - MIDCOMMAND)) && (rxCommand[ROLL ] > (eepromConfig.maxCheck - MIDCOMMAND)) && (rxCommand[PITCH] < (eepromConfig.minCheck - MIDCOMMAND)) ) { gyroRTBias[ROLL ] = 0.0f; gyroRTBias[PITCH] = 0.0f; gyroRTBias[YAW ] = 0.0f; gyroRunTimeCalCount = 2520; pulseMotors(3); } // Check for arm command ( low throttle, right yaw) if ((rxCommand[YAW] > (eepromConfig.maxCheck - MIDCOMMAND) ) && (armed == false) && (systemOperational == true) && (gyroRunTimeCalCount == 0)) { armingTimer++; if (armingTimer > eepromConfig.armCount) { armed = true; armingTimer = 0; } } else { armingTimer = 0; } } /////////////////////////////////// // Check for armed true and throttle command > minThrottle if ((armed == true) && (rxCommand[THROTTLE] > eepromConfig.minThrottle)) tasks500Hz_U.resetPIDs = false; else tasks500Hz_U.resetPIDs = true; /////////////////////////////////// // Simple Mode Command Processing if (rxCommand[AUX3] > MIDCOMMAND) { hdgDelta = tasks500Hz_Y.attitudes[YAW] - homeData.magHeading; hdgDelta = standardRadianFormat(hdgDelta); simpleX = cosf(hdgDelta) * rxCommand[PITCH] + sinf(hdgDelta) * rxCommand[ROLL ]; simpleY = cosf(hdgDelta) * rxCommand[ROLL ] - sinf(hdgDelta) * rxCommand[PITCH]; rxCommand[ROLL ] = simpleY; rxCommand[PITCH] = simpleX; } /////////////////////////////////// // Check AUX1 for rate, attitude, or GPS mode (3 Position Switch) NOT COMPLETE YET.... if (rxCommand[AUX1] > MIDCOMMAND) { flightMode = ATTITUDE; tasks500Hz_U.rateModes[ROLL ] = OUTER_LOOP_INPUT; tasks500Hz_U.rateModes[PITCH] = OUTER_LOOP_INPUT; tasks500Hz_U.attModes[ROLL ] = COMMAND_INPUT; tasks500Hz_U.attModes[PITCH] = COMMAND_INPUT; tasks500Hz_U.velModes[XAXIS] = STATE_INPUT; tasks500Hz_U.velModes[YAXIS] = STATE_INPUT; tasks500Hz_U.posModes[XAXIS] = STATE_INPUT; tasks500Hz_U.posModes[YAXIS] = STATE_INPUT; tasks500Hz_U.attCmds[ROLL ] = rxCommand[ROLL ] * eepromConfig.attitudeScaling; tasks500Hz_U.attCmds[PITCH] = rxCommand[PITCH] * eepromConfig.attitudeScaling; } else if (rxCommand[AUX1] <= MIDCOMMAND) { flightMode = RATE; tasks500Hz_U.rateModes[ROLL ] = COMMAND_INPUT; tasks500Hz_U.rateModes[PITCH] = COMMAND_INPUT; tasks500Hz_U.attModes[ROLL ] = STATE_INPUT; tasks500Hz_U.attModes[PITCH] = STATE_INPUT; tasks500Hz_U.velModes[XAXIS] = STATE_INPUT; tasks500Hz_U.velModes[YAXIS] = STATE_INPUT; tasks500Hz_U.posModes[XAXIS] = STATE_INPUT; tasks500Hz_U.posModes[YAXIS] = STATE_INPUT; tasks500Hz_U.rateCmds[ROLL ] = rxCommand[ROLL ] * eepromConfig.rollAndPitchRateScaling; tasks500Hz_U.rateCmds[PITCH] = rxCommand[PITCH] * eepromConfig.rollAndPitchRateScaling; } /////////////////////////////////// // Check yaw in detent and flight mode to determine hdg hold engaged state if ((commandInDetent[YAW] == true) && (flightMode == ATTITUDE) && (headingHoldEngaged == false) && (abs(tasks500Hz_U.gyro[YAW]) < (5.0f * D2R))) { headingHoldEngaged = true; tasks500Hz_U.rateModes[YAW] = OUTER_LOOP_INPUT; tasks500Hz_U.attModes[YAW] = COMMAND_INPUT; tasks500Hz_U.attCmds[YAW] = tasks500Hz_Y.attitudes[YAW]; } if ((commandInDetent[YAW] == false) || (flightMode != ATTITUDE)) { headingHoldEngaged = false; tasks500Hz_U.rateModes[YAW] = COMMAND_INPUT; tasks500Hz_U.attModes[YAW] = STATE_INPUT; tasks500Hz_U.rateCmds[YAW] = rxCommand[YAW] * eepromConfig.yawRateScaling; } /////////////////////////////////// // Vertical Mode State Machine switch (verticalModeState) { case ALT_DISENGAGED_THROTTLE_ACTIVE: tasks500Hz_U.velModes[ZAXIS] = STATE_INPUT; tasks500Hz_U.posModes[ZAXIS] = STATE_INPUT; tasks500Hz_U.velCmds[ZAXIS] = rxCommand[THROTTLE]; if ((rxCommand[AUX2] > MIDCOMMAND) && (previousAUX2State <= MIDCOMMAND)) // AUX2 Rising edge detection { verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT; tasks500Hz_U.posCmds[ZAXIS] = tasks500Hz_U.positions[ZAXIS]; } break; /////////////////////////////// case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT: tasks500Hz_U.velModes[ZAXIS] = OUTER_LOOP_INPUT; tasks500Hz_U.posModes[ZAXIS] = COMMAND_INPUT; if ((commandInDetent[VERTICAL] == true) || eepromConfig.verticalVelocityHoldOnly) verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE; if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; break; /////////////////////////////// case ALT_HOLD_AT_REFERENCE_ALTITUDE: tasks500Hz_U.velModes[ZAXIS] = OUTER_LOOP_INPUT; tasks500Hz_U.posModes[ZAXIS] = COMMAND_INPUT; if ((commandInDetent[VERTICAL] == false) || eepromConfig.verticalVelocityHoldOnly) verticalModeState = VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY; if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; break; /////////////////////////////// case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY: tasks500Hz_U.velModes[ZAXIS] = COMMAND_INPUT; tasks500Hz_U.posModes[ZAXIS] = STATE_INPUT; tasks500Hz_U.velCmds[ZAXIS] = rxCommand[VERTICAL] * eepromConfig.hDotScaling; if ((commandInDetent[VERTICAL] == true) && !eepromConfig.verticalVelocityHoldOnly) { verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE; tasks500Hz_U.posCmds[ZAXIS] = tasks500Hz_U.positions[ZAXIS]; } if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection { verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; tasks500Hz_U.posCmds[ZAXIS] = tasks500Hz_U.positions[ZAXIS]; } if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; break; /////////////////////////////// case ALT_DISENGAGED_THROTTLE_INACTIVE: if (((rxCommand[THROTTLE] < tasks500Hz_Y.axisCmds[VERTICAL] + THROTTLE_WINDOW) && (rxCommand[THROTTLE] > tasks500Hz_Y.axisCmds[VERTICAL] - THROTTLE_WINDOW)) || eepromConfig.verticalVelocityHoldOnly) verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; if ((rxCommand[AUX2] > MIDCOMMAND) && (previousAUX2State <= MIDCOMMAND)) // AUX2 Rising edge detection verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT; if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; break; } previousAUX2State = rxCommand[AUX2]; previousAUX4State = rxCommand[AUX4]; /////////////////////////////////// }
void cliCom(void) { uint8_t index; char mvlkToggleString[5] = { 0, 0, 0, 0, 0 }; if ((cliPortAvailable() && !validCliCommand)) { cliQuery = cliPortRead(); if (cliQuery == '#') // Check to see if we should toggle mavlink msg state { while (cliPortAvailable == false); readStringCLI(mvlkToggleString, 5); if ((mvlkToggleString[0] == '#') && (mvlkToggleString[1] == '#') && (mvlkToggleString[2] == '#') && (mvlkToggleString[3] == '#')) { if (eepromConfig.mavlinkEnabled == false) { eepromConfig.mavlinkEnabled = true; eepromConfig.activeTelemetry = 0x0000; } else { eepromConfig.mavlinkEnabled = false; } if (mvlkToggleString[4] == 'W') { cliPortPrint("\nWriting EEPROM Parameters....\n"); writeEEPROM(); } } } } validCliCommand = false; if ((eepromConfig.mavlinkEnabled == false) && (cliQuery != '#')) { switch (cliQuery) { /////////////////////////////// case 'a': // Rate PIDs cliPortPrintF("\nRoll Rate PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[ROLL_RATE_PID].B, eepromConfig.PID[ROLL_RATE_PID].P, eepromConfig.PID[ROLL_RATE_PID].I, eepromConfig.PID[ROLL_RATE_PID].D, eepromConfig.PID[ROLL_RATE_PID].windupGuard, eepromConfig.PID[ROLL_RATE_PID].dErrorCalc ? "Error" : "State"); cliPortPrintF("Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[PITCH_RATE_PID].B, eepromConfig.PID[PITCH_RATE_PID].P, eepromConfig.PID[PITCH_RATE_PID].I, eepromConfig.PID[PITCH_RATE_PID].D, eepromConfig.PID[PITCH_RATE_PID].windupGuard, eepromConfig.PID[PITCH_RATE_PID].dErrorCalc ? "Error" : "State"); cliPortPrintF("Yaw Rate PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[YAW_RATE_PID].B, eepromConfig.PID[YAW_RATE_PID].P, eepromConfig.PID[YAW_RATE_PID].I, eepromConfig.PID[YAW_RATE_PID].D, eepromConfig.PID[YAW_RATE_PID].windupGuard, eepromConfig.PID[YAW_RATE_PID].dErrorCalc ? "Error" : "State"); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'b': // Attitude PIDs cliPortPrintF("\nRoll Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[ROLL_ATT_PID].B, eepromConfig.PID[ROLL_ATT_PID].P, eepromConfig.PID[ROLL_ATT_PID].I, eepromConfig.PID[ROLL_ATT_PID].D, eepromConfig.PID[ROLL_ATT_PID].windupGuard, eepromConfig.PID[ROLL_ATT_PID].dErrorCalc ? "Error" : "State"); cliPortPrintF("Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[PITCH_ATT_PID].B, eepromConfig.PID[PITCH_ATT_PID].P, eepromConfig.PID[PITCH_ATT_PID].I, eepromConfig.PID[PITCH_ATT_PID].D, eepromConfig.PID[PITCH_ATT_PID].windupGuard, eepromConfig.PID[PITCH_ATT_PID].dErrorCalc ? "Error" : "State"); cliPortPrintF("Heading PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[HEADING_PID].B, eepromConfig.PID[HEADING_PID].P, eepromConfig.PID[HEADING_PID].I, eepromConfig.PID[HEADING_PID].D, eepromConfig.PID[HEADING_PID].windupGuard, eepromConfig.PID[HEADING_PID].dErrorCalc ? "Error" : "State"); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'c': // Velocity PIDs cliPortPrintF("\nhDot PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[HDOT_PID].B, eepromConfig.PID[HDOT_PID].P, eepromConfig.PID[HDOT_PID].I, eepromConfig.PID[HDOT_PID].D, eepromConfig.PID[HDOT_PID].windupGuard, eepromConfig.PID[HDOT_PID].dErrorCalc ? "Error" : "State"); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'd': // Position PIDs cliPortPrintF("\nh PID: %8.4f, %8.4f, %8.4f, %8.4f, %8.4f, %s\n", eepromConfig.PID[H_PID].B, eepromConfig.PID[H_PID].P, eepromConfig.PID[H_PID].I, eepromConfig.PID[H_PID].D, eepromConfig.PID[H_PID].windupGuard, eepromConfig.PID[H_PID].dErrorCalc ? "Error" : "State"); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'e': // Loop Delta Times cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz, deltaTime500Hz, deltaTime100Hz, deltaTime50Hz, deltaTime10Hz, deltaTime5Hz, deltaTime1Hz); validCliCommand = false; break; /////////////////////////////// case 'f': // Loop Execution Times cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz, executionTime500Hz, executionTime100Hz, executionTime50Hz, executionTime10Hz, executionTime5Hz, executionTime1Hz); validCliCommand = false; break; /////////////////////////////// case 'g': // 500 Hz Accels cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS]); validCliCommand = false; break; /////////////////////////////// case 'h': // 100 hz Earth Axis Accels cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS], earthAxisAccels[YAXIS], earthAxisAccels[ZAXIS]); validCliCommand = false; break; /////////////////////////////// case 'i': // 500 hz Gyros cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D, sensors.gyro500Hz[PITCH] * R2D, sensors.gyro500Hz[YAW ] * R2D, mpuTemperature); validCliCommand = false; break; /////////////////////////////// case 'j': // 10 Hz Mag Data cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS]); validCliCommand = false; break; /////////////////////////////// case 'k': // Vertical Axis Variables cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", earthAxisAccels[ZAXIS], sensors.pressureAlt50Hz, hDotEstimate, hEstimate); validCliCommand = false; break; /////////////////////////////// case 'l': // Attitudes cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D, sensors.attitude500Hz[PITCH] * R2D, sensors.attitude500Hz[YAW ] * R2D); validCliCommand = false; break; /////////////////////////////// case 'm': // Axis PIDs cliPortPrintF("%9.4f, %9.4f, %9.4f\n", axisPID[ROLL ], axisPID[PITCH], axisPID[YAW ]); validCliCommand = false; break; /////////////////////////////// case 'o': cliPortPrintF("%9.4f\n", batteryVoltage); validCliCommand = false; break; /////////////////////////////// case 'p': // Primary Spektrum Raw Data cliPortPrintF("%04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X\n", primarySpektrumState.lostFrameCnt, primarySpektrumState.rcAvailable, primarySpektrumState.values[0], primarySpektrumState.values[1], primarySpektrumState.values[2], primarySpektrumState.values[3], primarySpektrumState.values[4], primarySpektrumState.values[5], primarySpektrumState.values[6], primarySpektrumState.values[7]); validCliCommand = false; break; /////////////////////////////// case 'q': // Not Used cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'r': if (flightMode == RATE) cliPortPrint("Flight Mode = RATE "); else if (flightMode == ATTITUDE) cliPortPrint("Flight Mode = ATTITUDE "); if (headingHoldEngaged == true) cliPortPrint("Heading Hold = ENGAGED "); else cliPortPrint("Heading Hold = DISENGAGED "); cliPortPrint("Alt Hold = "); switch (verticalModeState) { case ALT_DISENGAGED_THROTTLE_ACTIVE: cliPortPrint("Alt Disenaged Throttle Active\n"); break; case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT: cliPortPrint("Alt Hold Fixed at Engagement Alt\n"); break; case ALT_HOLD_AT_REFERENCE_ALTITUDE: cliPortPrint("Alt Hold at Reference Alt\n"); break; case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY: cliPortPrint("V Velocity Hold at Reference Vel\n"); break; case ALT_DISENGAGED_THROTTLE_INACTIVE: cliPortPrint("Alt Disengaged Throttle Inactive\n"); break; } validCliCommand = false; break; /////////////////////////////// case 's': // Raw Receiver Commands if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0)) { for (index = 0; index < maxChannelNum - 1; index++) cliPortPrintF("%4ld, ", spektrumBuf[index]); cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]); } else if ((eepromConfig.receiverType == SPEKTRUM) && (maxChannelNum == 0)) cliPortPrint("Invalid Number of Spektrum Channels....\n"); else { for (index = 0; index < 7; index++) cliPortPrintF("%4i, ", ppmRxRead(index)); cliPortPrintF("%4i\n", ppmRxRead(7)); } validCliCommand = false; break; /////////////////////////////// case 't': // Processed Receiver Commands for (index = 0; index < 7; index++) cliPortPrintF("%8.2f, ", rxCommand[index]); cliPortPrintF("%8.2f\n", rxCommand[7]); validCliCommand = false; break; /////////////////////////////// case 'u': // Command in Detent Discretes cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false"); cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false"); cliPortPrintF("%s\n", commandInDetent[YAW ] ? " true" : "false"); validCliCommand = false; break; /////////////////////////////// case 'v': // ESC PWM Outputs cliPortPrintF("%4ld, ", TIM4->CCR4); cliPortPrintF("%4ld, ", TIM4->CCR3); cliPortPrintF("%4ld, ", TIM4->CCR2); cliPortPrintF("%4ld, ", TIM4->CCR1); cliPortPrintF("%4ld, ", TIM1->CCR4); cliPortPrintF("%4ld\n", TIM1->CCR1); validCliCommand = false; break; /////////////////////////////// case 'x': validCliCommand = false; break; /////////////////////////////// case 'y': // ESC Calibration escCalibration(); cliQuery = 'x'; break; /////////////////////////////// case 'z': // Voltage monitor ADC, Battery voltage cliPortPrintF("%7.2f, %5.2f\n", voltageMonitor(), batteryVoltage); break; /////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////// case 'A': // Read Roll Rate PID Values readCliPID(ROLL_RATE_PID); cliPortPrint( "\nRoll Rate PID Received....\n" ); cliQuery = 'a'; validCliCommand = false; break; /////////////////////////////// case 'B': // Read Pitch Rate PID Values readCliPID(PITCH_RATE_PID); cliPortPrint( "\nPitch Rate PID Received....\n" ); cliQuery = 'a'; validCliCommand = false; break; /////////////////////////////// case 'C': // Read Yaw Rate PID Values readCliPID(YAW_RATE_PID); cliPortPrint( "\nYaw Rate PID Received....\n" ); cliQuery = 'a'; validCliCommand = false; break; /////////////////////////////// case 'D': // Read Roll Attitude PID Values readCliPID(ROLL_ATT_PID); cliPortPrint( "\nRoll Attitude PID Received....\n" ); cliQuery = 'b'; validCliCommand = false; break; /////////////////////////////// case 'E': // Read Pitch Attitude PID Values readCliPID(PITCH_ATT_PID); cliPortPrint( "\nPitch Attitude PID Received....\n" ); cliQuery = 'b'; validCliCommand = false; break; /////////////////////////////// case 'F': // Read Heading Hold PID Values readCliPID(HEADING_PID); cliPortPrint( "\nHeading PID Received....\n" ); cliQuery = 'b'; validCliCommand = false; break; /////////////////////////////// case 'I': // Read hDot PID Values readCliPID(HDOT_PID); cliPortPrint( "\nhDot PID Received....\n" ); cliQuery = 'c'; validCliCommand = false; break; /////////////////////////////// case 'L': // Read h PID Values readCliPID(H_PID); cliPortPrint( "\nh PID Received....\n" ); cliQuery = 'd'; validCliCommand = false; break; /////////////////////////////// case 'N': // Mixer CLI mixerCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'O': // Receiver CLI receiverCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'P': // Sensor CLI sensorCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'R': // Reset to Bootloader cliPortPrint("Entering Bootloader....\n\n"); delay(100); systemReset(true); break; /////////////////////////////// case 'S': // Reset System cliPortPrint("\nSystem Reseting....\n\n"); delay(100); systemReset(false); break; /////////////////////////////// case 'T': // Telemetry CLI telemetryCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'U': // EEPROM CLI eepromCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'V': // Reset EEPROM Parameters cliPortPrint( "\nEEPROM Parameters Reset....\n" ); checkFirstTime(true); cliPortPrint("\nSystem Resetting....\n\n"); delay(100); systemReset(false); break; /////////////////////////////// case 'W': // Write EEPROM Parameters cliPortPrint("\nWriting EEPROM Parameters....\n"); writeEEPROM(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'X': // Not Used cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'Y': // Not Used cliQuery = 'x'; break; /////////////////////////////// case 'Z': // Not Used cliQuery = 'x'; break; /////////////////////////////// case '?': // Command Summary cliBusy = true; cliPortPrint("\n"); cliPortPrint("'a' Rate PIDs 'A' Set Roll Rate PID Data AB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'b' Attitude PIDs 'B' Set Pitch Rate PID Data BB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'c' Velocity PIDs 'C' Set Yaw Rate PID Data CB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'d' Position PIDs 'D' Set Roll Att PID Data DB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'e' Loop Delta Times 'E' Set Pitch Att PID Data EB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'f' Loop Execution Times 'F' Set Hdg Hold PID Data FB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'g' 500 Hz Accels 'G' Not Used\n"); cliPortPrint("'h' 100 Hz Earth Axis Accels 'H' Not Used\n"); cliPortPrint("'i' 500 Hz Gyros 'I' Set hDot PID Data IB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'j' 10 hz Mag Data 'J' Not Used\n"); cliPortPrint("'k' Vertical Axis Variable 'K' Not Used\n"); cliPortPrint("'l' Attitudes 'L' Set h PID Data LB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("\n"); cliPortPrint("Press space bar for more, or enter a command....\n"); while (cliPortAvailable() == false); cliQuery = cliPortRead(); if (cliQuery != ' ') { validCliCommand = true; cliBusy = false; return; } cliPortPrint("\n"); cliPortPrint("'m' Axis PIDs 'M' Not Used\n"); cliPortPrint("'n' Not Used 'N' Mixer CLI\n"); cliPortPrint("'o' Battery Voltage 'O' Receiver CLI\n"); cliPortPrint("'p' Not Used 'P' Sensor CLI\n"); cliPortPrint("'q' Primary Spektrum Raw Data 'Q' Not Used\n"); cliPortPrint("'r' Mode States 'R' Reset and Enter Bootloader\n"); cliPortPrint("'s' Raw Receiver Commands 'S' Reset\n"); cliPortPrint("'t' Processed Receiver Commands 'T' Telemetry CLI\n"); cliPortPrint("'u' Command In Detent Discretes 'U' EEPROM CLI\n"); cliPortPrint("'v' Motor PWM Outputs 'V' Reset EEPROM Parameters\n"); cliPortPrint("'w' Not Used 'W' Write EEPROM Parameters\n"); cliPortPrint("'x' Terminate Serial Communication 'X' Not Used\n"); cliPortPrint("\n"); cliPortPrint("Press space bar for more, or enter a command....\n"); while (cliPortAvailable() == false); cliQuery = cliPortRead(); if (cliQuery != ' ') { validCliCommand = true; cliBusy = false; return; } cliPortPrint("\n"); cliPortPrint("'y' ESC Calibration 'Y' Not Used\n"); cliPortPrint("'z' ADC Values 'Z' Not Used\n"); cliPortPrint("'#####' Toggle MavLink Msg State '?' Command Summary\n"); cliPortPrint("\n"); cliQuery = 'x'; cliBusy = false; break; /////////////////////////////// } } }
void cliCom(void) { uint16_t index; char mvlkToggleString[5] = { 0, 0, 0, 0, 0 }; if ((cliPortAvailable() && !validCliCommand)) { cliQuery = cliPortRead(); if (cliQuery == '#') // Check to see if we should toggle mavlink msg state { while (cliPortAvailable == false); readStringCLI(mvlkToggleString, 5); if ((mvlkToggleString[0] == '#') && (mvlkToggleString[1] == '#') && (mvlkToggleString[2] == '#') && (mvlkToggleString[3] == '#')) { if (systemConfig.mavlinkEnabled == false) { systemConfig.mavlinkEnabled = true; systemConfig.activeTelemetry = 0x0000; } else { systemConfig.mavlinkEnabled = false; } if (mvlkToggleString[4] == 'W') { cliPortPrint("\nWriting EEPROM Parameters....\n"); writeSystemEEPROM(); } } } } validCliCommand = false; if ((systemConfig.mavlinkEnabled == false) && (cliQuery != '#')) { switch (cliQuery) { /////////////////////////////// case 'a': // Rate PIDs cliPortPrintF("\nRoll Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[ROLL_RATE_PID].P, systemConfig.PID[ROLL_RATE_PID].I, systemConfig.PID[ROLL_RATE_PID].D, systemConfig.PID[ROLL_RATE_PID].N); cliPortPrintF( "Pitch Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[PITCH_RATE_PID].P, systemConfig.PID[PITCH_RATE_PID].I, systemConfig.PID[PITCH_RATE_PID].D, systemConfig.PID[PITCH_RATE_PID].N); cliPortPrintF( "Yaw Rate PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[YAW_RATE_PID].P, systemConfig.PID[YAW_RATE_PID].I, systemConfig.PID[YAW_RATE_PID].D, systemConfig.PID[YAW_RATE_PID].N); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'b': // Attitude PIDs cliPortPrintF("\nRoll Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[ROLL_ATT_PID].P, systemConfig.PID[ROLL_ATT_PID].I, systemConfig.PID[ROLL_ATT_PID].D, systemConfig.PID[ROLL_ATT_PID].N); cliPortPrintF( "Pitch Attitude PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[PITCH_ATT_PID].P, systemConfig.PID[PITCH_ATT_PID].I, systemConfig.PID[PITCH_ATT_PID].D, systemConfig.PID[PITCH_ATT_PID].N); cliPortPrintF( "Heading PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[HEADING_PID].P, systemConfig.PID[HEADING_PID].I, systemConfig.PID[HEADING_PID].D, systemConfig.PID[HEADING_PID].N); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'c': // Velocity PIDs cliPortPrintF("\nnDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[NDOT_PID].P, systemConfig.PID[NDOT_PID].I, systemConfig.PID[NDOT_PID].D, systemConfig.PID[NDOT_PID].N); cliPortPrintF( "eDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[EDOT_PID].P, systemConfig.PID[EDOT_PID].I, systemConfig.PID[EDOT_PID].D, systemConfig.PID[EDOT_PID].N); cliPortPrintF( "hDot PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[HDOT_PID].P, systemConfig.PID[HDOT_PID].I, systemConfig.PID[HDOT_PID].D, systemConfig.PID[HDOT_PID].N); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'd': // Position PIDs cliPortPrintF("\nN PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[N_PID].P, systemConfig.PID[N_PID].I, systemConfig.PID[N_PID].D, systemConfig.PID[N_PID].N); cliPortPrintF( "E PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[E_PID].P, systemConfig.PID[E_PID].I, systemConfig.PID[E_PID].D, systemConfig.PID[E_PID].N); cliPortPrintF( "h PID: %8.4f, %8.4f, %8.4f, %8.4f\n", systemConfig.PID[H_PID].P, systemConfig.PID[H_PID].I, systemConfig.PID[H_PID].D, systemConfig.PID[H_PID].N); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'e': // Loop Delta Times cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", deltaTime1000Hz, deltaTime500Hz, deltaTime100Hz, deltaTime50Hz, deltaTime10Hz, deltaTime5Hz, deltaTime1Hz); validCliCommand = false; break; /////////////////////////////// case 'f': // Loop Execution Times cliPortPrintF("%7ld, %7ld, %7ld, %7ld, %7ld, %7ld, %7ld\n", executionTime1000Hz, executionTime500Hz, executionTime100Hz, executionTime50Hz, executionTime10Hz, executionTime5Hz, executionTime1Hz); validCliCommand = false; break; /////////////////////////////// case 'g': // 500 Hz Accels cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS]); validCliCommand = false; break; /////////////////////////////// case 'h': // 100 hz Earth Axis Accels cliPortPrintF("%9.4f, %9.4f, %9.4f\n", earthAxisAccels[XAXIS], earthAxisAccels[YAXIS], earthAxisAccels[ZAXIS]); validCliCommand = false; break; /////////////////////////////// case 'i': // 500 hz Gyros cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f\n", sensors.gyro500Hz[ROLL ] * R2D, sensors.gyro500Hz[PITCH] * R2D, sensors.gyro500Hz[YAW ] * R2D, mpu6000Temperature); validCliCommand = false; break; /////////////////////////////// case 'j': // 10 Hz Mag Data cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS]); validCliCommand = false; break; /////////////////////////////// case 'k': // Vertical Axis Variables cliPortPrintF("%9.4f, %9.4f, %9.4f, %9.4f, %4ld, %9.4f\n", earthAxisAccels[ZAXIS], sensors.pressureAlt50Hz, hDotEstimate, hEstimate, ms5611Temperature, aglRead()); validCliCommand = false; break; /////////////////////////////// case 'l': // Attitudes cliPortPrintF("%9.4f, %9.4f, %9.4f\n", sensors.attitude500Hz[ROLL ] * R2D, sensors.attitude500Hz[PITCH] * R2D, sensors.attitude500Hz[YAW ] * R2D); validCliCommand = false; break; /////////////////////////////// case 'm': // Axis PIDs cliPortPrintF("%9.4f, %9.4f, %9.4f\n", ratePID[ROLL ], ratePID[PITCH], ratePID[YAW ]); validCliCommand = false; break; /////////////////////////////// case 'n': // GPS Data switch (gpsDataType) { /////////////////////// case 0: cliPortPrintF("%12ld, %12ld, %12ld, %12ld, %12ld, %12ld, %4d, %4d\n", gps.latitude, gps.longitude, gps.hMSL, gps.velN, gps.velE, gps.velD, gps.fix, gps.numSats); break; /////////////////////// case 1: cliPortPrintF("%3d: ", gps.numCh); for (index = 0; index < gps.numCh; index++) cliPortPrintF("%3d ", gps.chn[index]); cliPortPrint("\n"); break; /////////////////////// case 2: cliPortPrintF("%3d: ", gps.numCh); for (index = 0; index < gps.numCh; index++) cliPortPrintF("%3d ", gps.svid[index]); cliPortPrint("\n"); break; /////////////////////// case 3: cliPortPrintF("%3d: ", gps.numCh); for (index = 0; index < gps.numCh; index++) cliPortPrintF("%3d ", gps.cno[index]); cliPortPrint("\n"); break; /////////////////////// } validCliCommand = false; break; /////////////////////////////// case 'o': cliPortPrintF("%9.4f\n", batteryVoltage); validCliCommand = false; break; /////////////////////////////// case 'p': // Primary Spektrum Raw Data cliPortPrintF("%04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X, %04X\n", primarySpektrumState.lostFrameCnt, primarySpektrumState.rcAvailable, primarySpektrumState.values[0], primarySpektrumState.values[1], primarySpektrumState.values[2], primarySpektrumState.values[3], primarySpektrumState.values[4], primarySpektrumState.values[5], primarySpektrumState.values[6], primarySpektrumState.values[7]); validCliCommand = false; break; /////////////////////////////// case 'q': // Not Used cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'r': if (flightMode == RATE) cliPortPrint("Flight Mode:RATE "); else if (flightMode == ATTITUDE) cliPortPrint("Flight Mode:ATTITUDE "); else if (flightMode == GPS) cliPortPrint("Flight Mode:GPS "); if (headingHoldEngaged == true) cliPortPrint("Heading Hold:ENGAGED "); else cliPortPrint("Heading Hold:DISENGAGED "); switch (verticalModeState) { case ALT_DISENGAGED_THROTTLE_ACTIVE: cliPortPrint("Alt:Disenaged Throttle Active "); break; case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT: cliPortPrint("Alt:Hold Fixed at Engagement Alt "); break; case ALT_HOLD_AT_REFERENCE_ALTITUDE: cliPortPrint("Alt:Hold at Reference Alt "); break; case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY: cliPortPrint("Alt:Velocity Hold at Reference Vel "); break; case ALT_DISENGAGED_THROTTLE_INACTIVE: cliPortPrint("Alt:Disengaged Throttle Inactive "); break; } if (rxCommand[AUX3] > MIDCOMMAND) cliPortPrint("Mode:Simple "); else cliPortPrint("Mode:Normal "); if (rxCommand[AUX4] > MIDCOMMAND) cliPortPrint("Emergency Bail:Active\n"); else cliPortPrint("Emergency Bail:Inactive\n"); validCliCommand = false; break; /////////////////////////////// case 's': // Raw Receiver Commands if ((systemConfig.receiverType == SPEKTRUM) && (maxChannelNum > 0)) { for (index = 0; index < maxChannelNum - 1; index++) cliPortPrintF("%4ld, ", spektrumBuf[index]); cliPortPrintF("%4ld\n", spektrumBuf[maxChannelNum - 1]); } else if ((systemConfig.receiverType == SPEKTRUM) && (maxChannelNum == 0)) cliPortPrint("Invalid Number of Spektrum Channels....\n"); else { for (index = 0; index < 7; index++) cliPortPrintF("%4i, ", ppmRxRead(index)); cliPortPrintF("%4i\n", ppmRxRead(7)); } validCliCommand = false; break; /////////////////////////////// case 't': // Processed Receiver Commands for (index = 0; index < 7; index++) cliPortPrintF("%8.2f, ", rxCommand[index]); cliPortPrintF("%8.2f\n", rxCommand[7]); validCliCommand = false; break; /////////////////////////////// case 'u': // Command in Detent Discretes cliPortPrintF("%s, ", commandInDetent[ROLL ] ? " true" : "false"); cliPortPrintF("%s, ", commandInDetent[PITCH] ? " true" : "false"); cliPortPrintF("%s\n", commandInDetent[YAW ] ? " true" : "false"); validCliCommand = false; break; /////////////////////////////// case 'v': // ESC PWM Outputs cliPortPrintF("%4ld, ", TIM2->CCR1 ); cliPortPrintF("%4ld, ", TIM2->CCR2 ); cliPortPrintF("%4ld, ", TIM15->CCR1); cliPortPrintF("%4ld, ", TIM15->CCR2); cliPortPrintF("%4ld, ", TIM3->CCR1 ); cliPortPrintF("%4ld\n", TIM3->CCR2 ); validCliCommand = false; break; /////////////////////////////// case 'w': // Servo PWM Outputs cliPortPrintF("%4ld, ", TIM4->CCR1); cliPortPrintF("%4ld, ", TIM4->CCR2); cliPortPrintF("%4ld, ", TIM4->CCR3); cliPortPrintF("%4ld\n", TIM4->CCR4); validCliCommand = false; break; /////////////////////////////// case 'x': validCliCommand = false; break; /////////////////////////////// case 'y': // ESC Calibration escCalibration(); cliQuery = 'x'; break; /////////////////////////////// case 'z': cliPortPrintF("%5.2f, %5.2f\n", voltageMonitor(), adcChannel()); break; /////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////// /////////////////////////////// case 'A': // Read Roll Rate PID Values readCliPID(ROLL_RATE_PID); cliPortPrint( "\nRoll Rate PID Received....\n" ); cliQuery = 'a'; validCliCommand = false; break; /////////////////////////////// case 'B': // Read Pitch Rate PID Values readCliPID(PITCH_RATE_PID); cliPortPrint( "\nPitch Rate PID Received....\n" ); cliQuery = 'a'; validCliCommand = false; break; /////////////////////////////// case 'C': // Read Yaw Rate PID Values readCliPID(YAW_RATE_PID); cliPortPrint( "\nYaw Rate PID Received....\n" ); cliQuery = 'a'; validCliCommand = false; break; /////////////////////////////// case 'D': // Read Roll Attitude PID Values readCliPID(ROLL_ATT_PID); cliPortPrint( "\nRoll Attitude PID Received....\n" ); cliQuery = 'b'; validCliCommand = false; break; /////////////////////////////// case 'E': // Read Pitch Attitude PID Values readCliPID(PITCH_ATT_PID); cliPortPrint( "\nPitch Attitude PID Received....\n" ); cliQuery = 'b'; validCliCommand = false; break; /////////////////////////////// case 'F': // Read Heading Hold PID Values readCliPID(HEADING_PID); cliPortPrint( "\nHeading PID Received....\n" ); cliQuery = 'b'; validCliCommand = false; break; /////////////////////////////// case 'G': // Read nDot PID Values readCliPID(NDOT_PID); cliPortPrint( "\nnDot PID Received....\n" ); cliQuery = 'c'; validCliCommand = false; break; /////////////////////////////// case 'H': // Read eDot PID Values readCliPID(EDOT_PID); cliPortPrint( "\neDot PID Received....\n" ); cliQuery = 'c'; validCliCommand = false; break; /////////////////////////////// case 'I': // Read hDot PID Values readCliPID(HDOT_PID); cliPortPrint( "\nhDot PID Received....\n" ); cliQuery = 'c'; validCliCommand = false; break; /////////////////////////////// case 'J': // Read n PID Values readCliPID(N_PID); cliPortPrint( "\nn PID Received....\n" ); cliQuery = 'd'; validCliCommand = false; break; /////////////////////////////// case 'K': // Read e PID Values readCliPID(E_PID); cliPortPrint( "\ne PID Received....\n" ); cliQuery = 'd'; validCliCommand = false; break; /////////////////////////////// case 'L': // Read h PID Values readCliPID(H_PID); cliPortPrint( "\nh PID Received....\n" ); cliQuery = 'd'; validCliCommand = false; break; /////////////////////////////// case 'N': // Mixer CLI mixerCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'O': // Receiver CLI receiverCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'P': // Sensor CLI sensorCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'Q': // GPS Data Selection gpsDataType = (uint8_t)readFloatCLI(); cliPortPrint("\n"); cliQuery = 'n'; validCliCommand = false; break; /////////////////////////////// case 'R': // Reset to Bootloader cliPortPrint("Entering Bootloader....\n\n"); delay(100); systemReset(true); break; /////////////////////////////// case 'S': // Reset System cliPortPrint("\nSystem Reseting....\n\n"); delay(100); systemReset(false); break; /////////////////////////////// case 'T': // Telemetry CLI telemetryCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'U': // EEPROM CLI eepromCLI(); cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'V': // Write Sensor EEPROM Parameters cliPortPrint("\nWriting Sensor EEPROM Parameters....\n\n"); cliBusy = true; writeSensorEEPROM(); cliBusy = false; cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'W': // Write System EEPROM Parameters cliPortPrint("\nWriting System EEPROM Parameters....\n\n"); cliBusy = true; writeSystemEEPROM(); cliBusy = false; cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'X': // Not Used cliQuery = 'x'; validCliCommand = false; break; /////////////////////////////// case 'Y': // Not Used computeGeoMagElements(); cliQuery = 'x'; break; /////////////////////////////// case 'Z': // Not Used if (usbIsConfigured() == true) cliPortPrint("\nUSB Configured TRUE\n"); else cliPortPrint("\nUSB Configured FALSE\n"); if (usbIsConnected() == true) cliPortPrint("\nUSB Connected TRUE\n"); else cliPortPrint("\nUSB Connected FALSE\n"); cliQuery = 'x'; break; /////////////////////////////// case '?': // Command Summary cliBusy = true; cliPortPrint("\n"); cliPortPrint("'a' Rate PIDs 'A' Set Roll Rate PID Data AB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'b' Attitude PIDs 'B' Set Pitch Rate PID Data BB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'c' Velocity PIDs 'C' Set Yaw Rate PID Data CB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'d' Position PIDs 'D' Set Roll Att PID Data DB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'e' Loop Delta Times 'E' Set Pitch Att PID Data EB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'f' Loop Execution Times 'F' Set Hdg Hold PID Data FB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'g' 500 Hz Accels 'G' Set nDot PID Data GB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'h' 100 Hz Earth Axis Accels 'H' Set eDot PID Data HB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'i' 500 Hz Gyros 'I' Set hDot PID Data IB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'j' 10 hz Mag Data 'J' Set n PID Data JB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'k' Vertical Axis Variable 'K' Set e PID Data KB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("'l' Attitudes 'L' Set h PID Data LB;P;I;D;windupGuard;dErrorCalc\n"); cliPortPrint("\n"); cliPortPrint("Press space bar for more, or enter a command....\n"); while (cliPortAvailable() == false); cliQuery = cliPortRead(); if (cliQuery != ' ') { validCliCommand = true; cliBusy = false; return; } cliPortPrint("\n"); cliPortPrint("'m' Axis PIDs 'M' Not Used\n"); cliPortPrint("'n' GPS Data 'N' Mixer CLI\n"); cliPortPrint("'o' Battery Voltage 'O' Receiver CLI\n"); cliPortPrint("'p' Primary Spektrum Raw Data 'P' Sensor CLI\n"); cliPortPrint("'q' Not Used 'Q' GPS Data Selection\n"); cliPortPrint("'r' Mode States 'R' Reset and Enter Bootloader\n"); cliPortPrint("'s' Raw Receiver Commands 'S' Reset\n"); cliPortPrint("'t' Processed Receiver Commands 'T' Telemetry CLI\n"); cliPortPrint("'u' Command In Detent Discretes 'U' EEPROM CLI\n"); cliPortPrint("'v' Motor PWM Outputs 'V' Write Sensor EEPROM Parameters\n"); cliPortPrint("'w' Servo PWM Outputs 'W' Write System EEPROM Parameters\n"); cliPortPrint("'x' Terminate Serial Communication 'X' Not Used\n"); cliPortPrint("\n"); cliPortPrint("Press space bar for more, or enter a command....\n"); while (cliPortAvailable() == false); cliQuery = cliPortRead(); if (cliQuery != ' ') { validCliCommand = true; cliBusy = false; return; } cliPortPrint("\n"); cliPortPrint("'y' ESC Calibration 'Y' Not Used\n"); cliPortPrint("'z' ADC Values 'Z' Not Used\n"); cliPortPrint(" '?' Command Summary\n"); cliPortPrint("\n"); cliQuery = 'x'; cliBusy = false; break; /////////////////////////////// } } }
void processFlightCommands(void) { uint8_t channel; if (rcActive == true) { // Read receiver commands for (channel = 0; channel < 8; channel++) { if (eepromConfig.receiverType == SPEKTRUM) rxCommand[channel] = (float)spektrumRead(eepromConfig.rcMap[channel]); else rxCommand[channel] = (float)ppmRxRead(eepromConfig.rcMap[channel]); } rxCommand[ROLL] -= eepromConfig.midCommand; // Roll Range -1000:1000 rxCommand[PITCH] -= eepromConfig.midCommand; // Pitch Range -1000:1000 rxCommand[YAW] -= eepromConfig.midCommand; // Yaw Range -1000:1000 rxCommand[THROTTLE] -= eepromConfig.midCommand - MIDCOMMAND; // Throttle Range 2000:4000 rxCommand[AUX1] -= eepromConfig.midCommand - MIDCOMMAND; // Aux1 Range 2000:4000 rxCommand[AUX2] -= eepromConfig.midCommand - MIDCOMMAND; // Aux2 Range 2000:4000 rxCommand[AUX3] -= eepromConfig.midCommand - MIDCOMMAND; // Aux3 Range 2000:4000 rxCommand[AUX4] -= eepromConfig.midCommand - MIDCOMMAND; // Aux4 Range 2000:4000 } // Set past command in detent values for (channel = 0; channel < 3; channel++) previousCommandInDetent[channel] = commandInDetent[channel]; // Apply deadbands and set detent discretes' for (channel = 0; channel < 3; channel++) { if ((rxCommand[channel] <= DEADBAND) && (rxCommand[channel] >= -DEADBAND)) { rxCommand[channel] = 0; commandInDetent[channel] = true; } else { commandInDetent[channel] = false; if (rxCommand[channel] > 0) { rxCommand[channel] = (rxCommand[channel] - DEADBAND) * DEADBAND_SLOPE; } else { rxCommand[channel] = (rxCommand[channel] + DEADBAND) * DEADBAND_SLOPE; } } } /////////////////////////////////// // Check for low throttle if ( rxCommand[THROTTLE] < eepromConfig.minCheck ) { // Check for disarm command ( low throttle, left yaw ) if (((rxCommand[YAW] < (eepromConfig.minCheck - MIDCOMMAND)) && (armed == true)) && (verticalModeState == ALT_DISENGAGED_THROTTLE_ACTIVE)) { disarmingTimer++; if (disarmingTimer > eepromConfig.disarmCount) { zeroPIDintegralError(); zeroPIDstates(); armed = false; disarmingTimer = 0; } } else { disarmingTimer = 0; } // Check for gyro bias command ( low throttle, left yaw, aft pitch, right roll ) if ( (rxCommand[YAW ] < (eepromConfig.minCheck - MIDCOMMAND)) && (rxCommand[ROLL ] > (eepromConfig.maxCheck - MIDCOMMAND)) && (rxCommand[PITCH] < (eepromConfig.minCheck - MIDCOMMAND)) ) { computeMPU6000RTData(); pulseMotors(3); } // Check for arm command ( low throttle, right yaw) if ((rxCommand[YAW] > (eepromConfig.maxCheck - MIDCOMMAND) ) && (armed == false) && (execUp == true)) { armingTimer++; if (armingTimer > eepromConfig.armCount) { zeroPIDintegralError(); zeroPIDstates(); armed = true; armingTimer = 0; } } else { armingTimer = 0; } } /////////////////////////////////// // Check for armed true and throttle command > minThrottle if ((armed == true) && (rxCommand[THROTTLE] > eepromConfig.minThrottle)) holdIntegrators = false; else holdIntegrators = true; /////////////////////////////////// // Check AUX1 for rate, attitude, or GPS mode (3 Position Switch) NOT COMPLETE YET.... if ((rxCommand[AUX1] > MIDCOMMAND) && (flightMode == RATE)) { flightMode = ATTITUDE; setPIDintegralError(ROLL_ATT_PID, 0.0f); setPIDintegralError(PITCH_ATT_PID, 0.0f); setPIDintegralError(HEADING_PID, 0.0f); setPIDstates(ROLL_ATT_PID, 0.0f); setPIDstates(PITCH_ATT_PID, 0.0f); setPIDstates(HEADING_PID, 0.0f); } else if ((rxCommand[AUX1] <= MIDCOMMAND) && (flightMode == ATTITUDE)) { flightMode = RATE; setPIDintegralError(ROLL_RATE_PID, 0.0f); setPIDintegralError(PITCH_RATE_PID, 0.0f); setPIDintegralError(YAW_RATE_PID, 0.0f); setPIDstates(ROLL_RATE_PID, 0.0f); setPIDstates(PITCH_RATE_PID, 0.0f); setPIDstates(YAW_RATE_PID, 0.0f); } /////////////////////////////////// // Check yaw in detent and flight mode to determine hdg hold engaged state if ((commandInDetent[YAW] == true) && (flightMode == ATTITUDE) && (headingHoldEngaged == false)) { headingHoldEngaged = true; setPIDintegralError(HEADING_PID, 0.0f); setPIDstates(YAW_RATE_PID, 0.0f); headingReference = heading.mag; } if (((commandInDetent[YAW] == false) || (flightMode != ATTITUDE)) && (headingHoldEngaged == true)) { headingHoldEngaged = false; } /////////////////////////////////// // Vertical Mode Command Processing verticalReferenceCommand = rxCommand[THROTTLE] - eepromConfig.midCommand; // Set past altitude reference in detent value previousVertRefCmdInDetent = vertRefCmdInDetent; // Apply deadband and set detent discrete' if ((verticalReferenceCommand <= ALT_DEADBAND) && (verticalReferenceCommand >= -ALT_DEADBAND)) { verticalReferenceCommand = 0; vertRefCmdInDetent = true; } else { vertRefCmdInDetent = false; if (verticalReferenceCommand > 0) { verticalReferenceCommand = (verticalReferenceCommand - ALT_DEADBAND) * ALT_DEADBAND_SLOPE; } else { verticalReferenceCommand = (verticalReferenceCommand + ALT_DEADBAND) * ALT_DEADBAND_SLOPE; } } /////////////////////////////////// // Vertical Mode State Machine switch (verticalModeState) { case ALT_DISENGAGED_THROTTLE_ACTIVE: if ((rxCommand[AUX2] > MIDCOMMAND) && (previousAUX2State <= MIDCOMMAND)) // AUX2 Rising edge detection { verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT; setPIDintegralError(HDOT_PID, 0.0f); setPIDintegralError(H_PID, 0.0f); setPIDstates(HDOT_PID, 0.0f); setPIDstates(H_PID, 0.0f); altitudeHoldReference = hEstimate; throttleReference = rxCommand[THROTTLE]; } break; /////////////////////////////// case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT: if ((vertRefCmdInDetent == true) || eepromConfig.verticalVelocityHoldOnly) verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE; if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; break; /////////////////////////////// case ALT_HOLD_AT_REFERENCE_ALTITUDE: if ((vertRefCmdInDetent == false) || eepromConfig.verticalVelocityHoldOnly) verticalModeState = VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY; if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; break; /////////////////////////////// case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY: if ((vertRefCmdInDetent == true) && !eepromConfig.verticalVelocityHoldOnly) { verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE; altitudeHoldReference = hEstimate; } if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection { verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; altitudeHoldReference = hEstimate; } if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; break; /////////////////////////////// case ALT_DISENGAGED_THROTTLE_INACTIVE: if (((rxCommand[THROTTLE] < throttleCmd + THROTTLE_WINDOW) && (rxCommand[THROTTLE] > throttleCmd - THROTTLE_WINDOW)) || eepromConfig.verticalVelocityHoldOnly) verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; if ((rxCommand[AUX2] > MIDCOMMAND) && (previousAUX2State <= MIDCOMMAND)) // AUX2 Rising edge detection verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT; if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; break; } previousAUX2State = rxCommand[AUX2]; previousAUX4State = rxCommand[AUX4]; /////////////////////////////////// }