void telemetryCLI() { uint8_t telemetryQuery = 'x'; uint8_t validQuery = false; cliBusy = true; cliPortPrint("\nEntering Telemetry CLI....\n\n"); while(true) { cliPortPrint("Telemetry CLI -> "); while ((cliPortAvailable() == false) && (validQuery == false)); if (validQuery == false) telemetryQuery = cliPortRead(); cliPortPrint("\n"); switch(telemetryQuery) { /////////////////////////// case 'a': // Telemetry Configuration cliPortPrint("\nTelemetry Configuration:\n"); cliPortPrint(" Telemetry Set 1: "); cliPortPrintF("%s\n", systemConfig.activeTelemetry == 1 ? " Active" : "Inactive"); cliPortPrint(" Telemetry Set 2: "); cliPortPrintF("%s\n", systemConfig.activeTelemetry == 2 ? " Active" : "Inactive"); cliPortPrint(" Telemetry Set 3: "); cliPortPrintF("%s\n", systemConfig.activeTelemetry == 4 ? " Active" : "Inactive"); cliPortPrint(" Telemetry Set 4: "); cliPortPrintF("%s\n", systemConfig.activeTelemetry == 8 ? " Active" : "Inactive"); cliPortPrint(" Telemetry Set 5: "); cliPortPrintF("%s\n", systemConfig.activeTelemetry == 16 ? " Active" : "Inactive"); cliPortPrint(" Telemetry Set 6: "); cliPortPrintF("%s\n", systemConfig.activeTelemetry == 32 ? " Active" : "Inactive"); cliPortPrint(" Telemetry Set 7: "); cliPortPrintF("%s\n", systemConfig.activeTelemetry == 64 ? " Active" : "Inactive"); cliPortPrint(" Telemetry Set 8: "); cliPortPrintF("%s\n", systemConfig.activeTelemetry == 128 ? " Active" : "Inactive"); cliPortPrint(" MavLink: "); cliPortPrintF("%s\n", systemConfig.mavlinkEnabled == true ? " Enabled\n" : "Disabled\n"); validQuery = false; break; /////////////////////////// case 'b': // Turn all Telemetry Off systemConfig.activeTelemetry = 0; telemetryQuery = 'a'; validQuery = true; break; /////////////////////////// case 'c': // Toggle Telemetry Set 1 State systemConfig.activeTelemetry = 1; telemetryQuery = 'a'; validQuery = true; break; /////////////////////////// case 'd': // Toggle Telemetry Set 2 State systemConfig.activeTelemetry = 2; telemetryQuery = 'a'; validQuery = true; break; /////////////////////////// case 'e': // Toggle Telemetry Set 3 State systemConfig.activeTelemetry = 4; telemetryQuery = 'a'; validQuery = true; break; /////////////////////////// case 'f': // Toggle Telemetry Set 4 State systemConfig.activeTelemetry = 8; telemetryQuery = 'a'; validQuery = true; break; /////////////////////////// case 'g': // Toggle Telemetry Set 5 State systemConfig.activeTelemetry = 16; telemetryQuery = 'a'; validQuery = true; break; /////////////////////////// case 'h': // Toggle Telemetry Set 6 State systemConfig.activeTelemetry = 32; telemetryQuery = 'a'; validQuery = true; break; /////////////////////////// case 'i': // Toggle Telemetry Set 7 State systemConfig.activeTelemetry = 64; telemetryQuery = 'a'; validQuery = true; break; /////////////////////////// case 'j': // Toggle Telemetry Set 8 State systemConfig.activeTelemetry = 128; telemetryQuery = 'a'; validQuery = true; break; /////////////////////////// case 'k': // Toggle MavLink Enable/Disable if (systemConfig.mavlinkEnabled == false) { systemConfig.mavlinkEnabled = true; systemConfig.activeTelemetry = 0x0000; } else { systemConfig.mavlinkEnabled = false; } telemetryQuery = 'a'; validQuery = true; break; /////////////////////////// case 'x': cliPortPrint("\nExiting Telemetry CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'W': // Write System EEPROM Parameters cliPortPrint("\nWriting System EEPROM Parameters....\n\n"); writeSystemEEPROM(); validQuery = false; break; /////////////////////////// case '?': cliPortPrint("\n"); cliPortPrint("'a' Telemetry Configuration Data\n"); cliPortPrint("'b' Turn all Telemetry Off\n"); cliPortPrint("'c' Toggle Telemetry Set 1 State\n"); cliPortPrint("'d' Toggle Telemetry Set 2 State\n"); cliPortPrint("'e' Toggle Telemetry Set 3 State\n"); cliPortPrint("'f' Toggle Telemetry Set 4 State\n"); cliPortPrint("'g' Toggle Telemetry Set 5 State\n"); cliPortPrint("'h' Toggle Telemetry Set 6 State\n"); cliPortPrint("'i' Toggle Telemetry Set 7 State\n"); cliPortPrint("'j' Toggle Telemetry Set 8 State\n"); cliPortPrint("'k' Toggle MavLink Enabled/Disabled\n"); cliPortPrint(" 'W' Write System EEPROM Parameters\n"); cliPortPrint("'x' Exit Telemetry CLI '?' Command Summary\n"); cliPortPrint("\n"); break; /////////////////////////// } } }
void receiverCLI() { char rcOrderString[9]; float tempFloat; uint8_t index; uint8_t receiverQuery = 'x'; uint8_t validQuery = false; NVIC_InitTypeDef NVIC_InitStructure; cliBusy = true; cliPortPrint("\nEntering Receiver CLI....\n\n"); while(true) { cliPortPrint("Receiver CLI -> "); while ((cliPortAvailable() == false) && (validQuery == false)); if (validQuery == false) receiverQuery = cliPortRead(); cliPortPrint("\n"); switch(receiverQuery) { /////////////////////////// case 'a': // Receiver Configuration cliPortPrint("\nReceiver Type: "); switch(systemConfig.receiverType) { case PPM: cliPortPrint("PPM\n"); break; case SPEKTRUM: cliPortPrint("Spektrum\n"); break; } cliPortPrint("Current RC Channel Assignment: "); for (index = 0; index < 8; index++) rcOrderString[systemConfig.rcMap[index]] = rcChannelLetters[index]; rcOrderString[index] = '\0'; cliPortPrint(rcOrderString); cliPortPrint("\n"); cliPortPrintF("Secondary Spektrum: "); if ((systemConfig.slaveSpektrum == true) && false) // HJI Inhibit Slave Spektrum on Naze32 Pro cliPortPrintF("Installed\n"); else cliPortPrintF("Uninstalled\n"); cliPortPrintF("Mid Command: %4ld\n", (uint16_t)systemConfig.midCommand); cliPortPrintF("Min Check: %4ld\n", (uint16_t)systemConfig.minCheck); cliPortPrintF("Max Check: %4ld\n", (uint16_t)systemConfig.maxCheck); cliPortPrintF("Min Throttle: %4ld\n", (uint16_t)systemConfig.minThrottle); cliPortPrintF("Max Thottle: %4ld\n\n", (uint16_t)systemConfig.maxThrottle); tempFloat = systemConfig.rollAndPitchRateScaling * 180000.0 / PI; cliPortPrintF("Max Roll and Pitch Rate Cmd: %6.2f DPS\n", tempFloat); tempFloat = systemConfig.yawRateScaling * 180000.0 / PI; cliPortPrintF("Max Yaw Rate Cmd: %6.2f DPS\n\n", tempFloat); cliPortPrintF("Roll Rate Cmd Tau: %6.2f\n", systemConfig.rollRateCmdLowPassTau); cliPortPrintF("Pitch Rate Cmd Tau: %6.2f\n\n", systemConfig.pitchRateCmdLowPassTau); tempFloat = systemConfig.attitudeScaling * 180000.0 / PI; cliPortPrintF("Max Attitude Cmd: %6.2f Degrees\n\n", tempFloat); cliPortPrintF("Roll Attitude Cmd Tau: %6.2f\n", systemConfig.rollAttCmdLowPassTau); cliPortPrintF("Pitch Attitude Cmd Tau: %6.2f\n\n", systemConfig.pitchAttCmdLowPassTau); cliPortPrintF("Arm Delay Count: %3d Frames\n", systemConfig.armCount); cliPortPrintF("Disarm Delay Count: %3d Frames\n\n", systemConfig.disarmCount); validQuery = false; break; /////////////////////////// case 'x': cliPortPrint("\nExiting Receiver CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // Toggle PPM/Spektrum Satellite Receiver if (systemConfig.receiverType == PPM) { NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; NVIC_Init(&NVIC_InitStructure); TIM_ITConfig(TIM1, TIM_IT_CC1, DISABLE); systemConfig.receiverType = SPEKTRUM; spektrumInit(); } else { NVIC_InitStructure.NVIC_IRQChannel = TIM1_TRG_COM_TIM17_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE; NVIC_Init(&NVIC_InitStructure); TIM_ITConfig(TIM17, TIM_IT_Update, DISABLE); systemConfig.receiverType = PPM; ppmRxInit(); } receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'B': // Read RC Control Order readStringCLI( rcOrderString, 8 ); parseRcChannels( rcOrderString ); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'C': // Toggle Slave Spektrum State // HJI Inhibit Slave Spektrum on Naze32 Pro if (systemConfig.slaveSpektrum == true) systemConfig.slaveSpektrum = false; // HJI Inhibit Slave Spektrum on Naze32 Pro else // HJI Inhibit Slave Spektrum on Naze32 Pro systemConfig.slaveSpektrum = true; receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'D': // Read RC Control Points systemConfig.midCommand = readFloatCLI(); systemConfig.minCheck = readFloatCLI(); systemConfig.maxCheck = readFloatCLI(); systemConfig.minThrottle = readFloatCLI(); systemConfig.maxThrottle = readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'E': // Read Arm/Disarm Counts systemConfig.armCount = (uint8_t)readFloatCLI(); systemConfig.disarmCount = (uint8_t)readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'F': // Read Max Rate Value systemConfig.rollAndPitchRateScaling = readFloatCLI() / 180000.0f * PI; systemConfig.yawRateScaling = readFloatCLI() / 180000.0f * PI; receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'G': // Read Max Attitude Value systemConfig.attitudeScaling = readFloatCLI() / 180000.0f * PI; receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'H': // Read Rate Cmd Tau Value systemConfig.rollRateCmdLowPassTau = readFloatCLI(); systemConfig.pitchRateCmdLowPassTau = readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'I': // Read Attitude Cmd Tau Value systemConfig.rollAttCmdLowPassTau = readFloatCLI(); systemConfig.pitchAttCmdLowPassTau = readFloatCLI(); receiverQuery = 'a'; validQuery = true; break; /////////////////////////// case 'W': // Write System EEPROM Parameters cliPortPrint("\nWriting System EEPROM Parameters....\n\n"); writeSystemEEPROM(); validQuery = false; break; /////////////////////////// case '?': cliPortPrint("\n"); cliPortPrint("'a' Receiver Configuration Data 'A' Toggle PPM/Spektrum Receiver\n"); cliPortPrint(" 'B' Set RC Control Order BTAER1234\n"); cliPortPrint(" 'C' Toggle Slave Spektrum State\n"); cliPortPrint(" 'D' Set RC Control Points DmidCmd;minChk;maxChk;minThrot;maxThrot\n"); cliPortPrint(" 'E' Set Arm/Disarm Counts EarmCount;disarmCount\n"); cliPortPrint(" 'F' Set Maximum Rate Commands FRP;Y RP = Roll/Pitch, Y = Yaw\n"); cliPortPrint(" 'G' Set Maximum Attitude Command\n"); cliPortPrint(" 'H' Set Roll/Pitch Rate Command Filters HROLL;PITCH\n"); cliPortPrint(" 'I' Set Roll/Pitch Att Command Filters IROLL;PITCH\n"); cliPortPrint(" 'W' Write System EEPROM Parameters\n"); cliPortPrint("'x' Exit Receiver CLI '?' Command Summary\n"); cliPortPrint("\n"); break; /////////////////////////// } } }
void eepromCLI() { char c; sensorConfig_t sensorRam; systemConfig_t systemRam; uint8_t eepromQuery = 'x'; uint8_t *p; uint8_t *end; uint8_t secondNibble; uint8_t validQuery = false; uint16_t i; uint32_t c1, c2; uint32_t size; uint32_t time; uint32_t charsEncountered; /////////////////////////////////////////////////////////////////////////////// cliBusy = true; cliPortPrint("\nEntering EEPROM CLI....\n\n"); while(true) { cliPortPrint("EEPROM CLI -> "); while ((cliPortAvailable() == false) && (validQuery == false)); if (validQuery == false) eepromQuery = cliPortRead(); cliPortPrint("\n"); switch(eepromQuery) { /////////////////////////// case 'a': // config struct data c1 = sensorConfig.CRCAtEnd[0]; c2 = crc32B((uint32_t*)(&sensorConfig), // CRC32B[sensorConfig] (uint32_t*)(&sensorConfig.CRCAtEnd)); cliPortPrintF("Sensor EEPROM structure information:\n"); cliPortPrintF("Version : %d\n", sensorConfig.version); cliPortPrintF("Size : %d\n", sizeof(sensorConfig)); cliPortPrintF("CRC on last read : %08X\n", c1); cliPortPrintF("Current CRC : %08X\n", c2); if ( c1 != c2 ) cliPortPrintF(" CRCs differ. Current Sensor Config has not yet been saved.\n"); cliPortPrintF("CRC Flags :\n"); cliPortPrintF(" History Bad : %s\n", sensorConfig.CRCFlags & CRC_HistoryBad ? "true" : "false" ); validQuery = false; break; /////////////////////////// case 'b': // Write out to Console in Hex. (RAM -> console) // we assume the flyer is not in the air, so that this is ok; cliPortPrintF("\n"); cliPrintSensorEEPROM(); cliPortPrintF("\n"); if (crcCheckVal != crc32B((uint32_t*)(&sensorConfig), // CRC32B[sensorConfig CRC32B[sensorConfig]] (uint32_t*)(&sensorConfig + 1))) { cliPortPrint("NOTE: in-memory sensor config CRC invalid; there have probably been\n"); cliPortPrint(" changes to sensor config since the last write to flash/eeprom.\n"); } validQuery = false; break; /////////////////////////// case 'c': // Read Sensor Config -> RAM cliPortPrint("Re-reading Sensor EEPROM.\n"); readSensorEEPROM(); validQuery = false; break; /////////////////////////// case 'd': // Read Console -> Sensor RAM charsEncountered = 0; secondNibble = 0; size = sizeof(sensorConfig); time = millis(); p = (uint8_t*)&sensorRam; end = (uint8_t*)(&sensorRam + 1); /////////////////////// cliPortPrintF("Ready to read in sensor config. Expecting %d (0x%03X) bytes as %d\n", size, size, size * 2); cliPortPrintF("hexadecimal characters, optionally separated by [ \\n\\r_].\n"); cliPortPrintF("Times out if no character is received for %dms\n", TIMEOUT); memset(p, 0, end - p); while (p < end) { while (!cliPortAvailable() && millis() - time < TIMEOUT) {} time = millis(); c = cliPortAvailable() ? cliPortRead() : '\0'; int8_t hex = parse_hex(c); int ignore = c == ' ' || c == '\n' || c == '\r' || c == '_' ? true : false; if (c != '\0') // assume the person isn't sending null chars charsEncountered++; if (ignore) continue; if (hex == -1) break; *p |= secondNibble ? hex : hex << 4; p += secondNibble; secondNibble ^= 1; } if (c == 0) { cliPortPrintF("Did not receive enough hex chars! (got %d, expected %d)\n", (p - (uint8_t*)&sensorRam) * 2 + secondNibble, size * 2); } else if (p < end || secondNibble) { cliPortPrintF("Invalid character found at position %d: '%c' (0x%02x)", charsEncountered, c, c); } // HJI else if (crcCheckVal != crc32B((uint32_t*)(&sensorConfig), // CRC32B[sensorConfig CRC32B[sensorConfig]] // HJI (uint32_t*)(&sensorConfig + 1))) // HJI { // HJI cliPortPrintF("CRC mismatch! Not writing to in-memory config.\n"); // HJI cliPortPrintF("Here's what was received:\n\n"); // HJI cliPrintSensorEEPROM(); // HJI } else { // check to see if the newly received sytem config // actually differs from what's in-memory for (i = 0; i < size; i++) if (((uint8_t*)&sensorRam)[i] != ((uint8_t*)&sensorConfig)[i]) break; if (i == size) { cliPortPrintF("NOTE: Uploaded Sensor Config was Identical to RAM Config.\n"); } else { sensorConfig = sensorRam; cliPortPrintF("Sensor RAM Config updated!\n"); cliPortPrintF("NOTE: Sensor Config not written to EEPROM; use 'w' to do so.\n"); } } // eat the next 100ms (or whatever Timeout is) of characters, // in case the person pasted too much by mistake or something time = millis(); while (millis() - time < TIMEOUT) if (cliPortAvailable()) cliPortRead(); validQuery = false; break; /////////////////////////// case 'h': // Clear Bad Sensor History Flag cliPortPrintF("Clearing Bad Sensor History Flag.\n"); sensorConfig.CRCFlags &= ~CRC_HistoryBad; validQuery = false; break; /////////////////////////// case 'v': // Reset Sensor EEPROM Parameters cliPortPrint( "\nSensor EEPROM Parameters Reset....(not rebooting)\n" ); checkSensorEEPROM(true); validQuery = false; break; /////////////////////////// case 'w': // Write to Sensor EEPROM cliPortPrint("\nWriting Sensor EEPROM Parameters....\n"); writeSensorEEPROM(); validQuery = false; break; /////////////////////////// case 'x': // exit EEPROM CLI cliPortPrint("\nExiting EEPROM CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'A': // config struct data c1 = systemConfig.CRCAtEnd[0]; zeroPIDstates(); c2 = crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig] (uint32_t*)(&systemConfig.CRCAtEnd)); cliPortPrintF("System EEPROM structure information:\n"); cliPortPrintF("Version : %d\n", systemConfig.version); cliPortPrintF("Size : %d\n", sizeof(systemConfig)); cliPortPrintF("CRC on last read : %08X\n", c1); cliPortPrintF("Current CRC : %08X\n", c2); if ( c1 != c2 ) cliPortPrintF(" CRCs differ. Current SystemConfig has not yet been saved.\n"); cliPortPrintF("CRC Flags :\n"); cliPortPrintF(" History Bad : %s\n", systemConfig.CRCFlags & CRC_HistoryBad ? "true" : "false" ); validQuery = false; break; /////////////////////////// case 'B': // Write out to Console in Hex. (RAM -> console) // we assume the flyer is not in the air, so that this is ok; // these change randomly when not in flight and can mistakenly // make one think that the in-memory eeprom struct has changed zeroPIDstates(); cliPortPrintF("\n"); cliPrintSystemConfig(); cliPortPrintF("\n"); if (crcCheckVal != crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig CRC32B[systemConfig]] (uint32_t*)(&systemConfig + 1))) { cliPortPrint("NOTE: in-memory system config CRC invalid; there have probably been\n"); cliPortPrint(" changes to system config since the last write to flash/eeprom.\n"); } validQuery = false; break; /////////////////////////// case 'C': // Read System Config -> RAM cliPortPrint("Re-reading System EEPROM.\n"); readSystemEEPROM(); validQuery = false; break; /////////////////////////// case 'D': // Read Console -> System RAM charsEncountered = 0; secondNibble = 0; size = sizeof(systemConfig); time = millis(); p = (uint8_t*)&systemRam; end = (uint8_t*)(&systemRam + 1); /////////////////////// cliPortPrintF("Ready to read in system config. Expecting %d (0x%03X) bytes as %d\n", size, size, size * 2); cliPortPrintF("hexadecimal characters, optionally separated by [ \\n\\r_].\n"); cliPortPrintF("Times out if no character is received for %dms\n", TIMEOUT); memset(p, 0, end - p); while (p < end) { while (!cliPortAvailable() && millis() - time < TIMEOUT) {} time = millis(); c = cliPortAvailable() ? cliPortRead() : '\0'; int8_t hex = parse_hex(c); int ignore = c == ' ' || c == '\n' || c == '\r' || c == '_' ? true : false; if (c != '\0') // assume the person isn't sending null chars charsEncountered++; if (ignore) continue; if (hex == -1) break; *p |= secondNibble ? hex : hex << 4; p += secondNibble; secondNibble ^= 1; } if (c == 0) { cliPortPrintF("Did not receive enough hex chars! (got %d, expected %d)\n", (p - (uint8_t*)&systemRam) * 2 + secondNibble, size * 2); } else if (p < end || secondNibble) { cliPortPrintF("Invalid character found at position %d: '%c' (0x%02x)", charsEncountered, c, c); } // HJI else if (crcCheckVal != crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig CRC32B[systemConfig]] // HJI (uint32_t*)(&systemConfig + 1))) // HJI { // HJI cliPortPrintF("CRC mismatch! Not writing to in-memory config.\n"); // HJI cliPortPrintF("Here's what was received:\n\n"); // HJI cliPrintSystemConfig(); // HJI } else { // check to see if the newly received sytem config // actually differs from what's in-memory zeroPIDstates(); for (i = 0; i < size; i++) if (((uint8_t*)&systemRam)[i] != ((uint8_t*)&systemConfig)[i]) break; if (i == size) { cliPortPrintF("NOTE: Uploaded System Config was Identical to RAM Config.\n"); } else { systemConfig = systemRam; cliPortPrintF("System RAM Config updated!\n"); cliPortPrintF("NOTE: System Config not written to EEPROM; use 'W' to do so.\n"); } } // eat the next 100ms (or whatever Timeout is) of characters, // in case the person pasted too much by mistake or something time = millis(); while (millis() - time < TIMEOUT) if (cliPortAvailable()) cliPortRead(); validQuery = false; break; /////////////////////////// case 'H': // Clear Bad System History Flag cliPortPrintF("Clearing Bad System History Flag.\n"); systemConfig.CRCFlags &= ~CRC_HistoryBad; validQuery = false; break; /////////////////////////// case 'V': // Reset System EEPROM Parameters cliPortPrint( "\nSystem EEPROM Parameters Reset....(not rebooting)\n" ); checkSystemEEPROM(true); validQuery = false; break; /////////////////////////// case 'W': // Write out to System EEPROM cliPortPrint("\nWriting System EEPROM Parameters....\n"); writeSystemEEPROM(); validQuery = false; break; /////////////////////////// case '?': // 0 1 2 3 4 5 6 7 // 01234567890123456789012345678901234567890123456789012345678901234567890123456789 cliPortPrintF("\n"); cliPortPrintF("'a' Display Sensor Config Information 'A' Display System Config Information\n"); cliPortPrintF("'b' Write Sensor Config -> Console 'B' Write System Config - > Console\n"); cliPortPrintF("'c' Read Sensor Config -> RAM 'C' Read System Config -> RAM\n"); cliPortPrintF("'d' Read Console -> Sensor RAM 'D' Read Console -> System RAM\n"); cliPortPrintF("'h' Clear Sensor CRC Bad History flag 'H' Clear System CRC Bad History flag\n"); cliPortPrintF("'v' Reset Sensor Config to Default 'V' Reset System Config to Default\n"); cliPortPrintF("'w' Write Sensor Config -> EEPROM 'W' Write System Config -> EEPROM\n"); cliPortPrintF("'x' Exit EEPROM CLI '?' Command Summary\n"); cliPortPrintF("\n"); break; /////////////////////////// } } }
void 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 checkSystemEEPROM(bool eepromReset) { uint8_t version; systemConfigaddr.value = SYSTEM_EEPROM_ADDR; ENABLE_EEPROM; spiTransfer(EEPROM_SPI, READ_DATA); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[2]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[1]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[0]); version = spiTransfer(EEPROM_SPI, 0x00); DISABLE_EEPROM; delayMicroseconds(2); /////////////////////////////////// if (eepromReset || version != systemVersion) { // Default settings systemConfig.version = systemVersion; /////////////////////////////////// systemConfig.rollAndPitchRateScaling = 100.0 / 180000.0 * PI; // Stick to rate scaling for 100 DPS systemConfig.yawRateScaling = 300.0 / 180000.0 * PI; // Stick to rate scaling for 100 DPS systemConfig.rollRateCmdLowPassTau = 0.1f; systemConfig.pitchRateCmdLowPassTau = 0.1f; systemConfig.attitudeScaling = 60.0 / 180000.0 * PI; // Stick to att scaling for 60 degrees systemConfig.rollAttCmdLowPassTau = 0.1f; systemConfig.pitchAttCmdLowPassTau = 0.1f; systemConfig.nDotEdotScaling = 0.009f; // Stick to nDot/eDot scaling (9 mps)/(1000 RX PWM Steps) = 0.009 systemConfig.hDotScaling = 0.003f; // Stick to hDot scaling (3 mps)/(1000 RX PWM Steps) = 0.003 /////////////////////////////////// systemConfig.receiverType = SPEKTRUM; systemConfig.slaveSpektrum = false; parseRcChannels("TAER2134"); systemConfig.escPwmRate = 450; systemConfig.servoPwmRate = 50; /////////////////////////////////// systemConfig.mixerConfiguration = MIXERTYPE_TRI; systemConfig.yawDirection = 1.0f; systemConfig.triYawServoPwmRate = 50; systemConfig.triYawServoMin = 2000.0f; systemConfig.triYawServoMid = 3000.0f; systemConfig.triYawServoMax = 4000.0f; systemConfig.triCopterYawCmd500HzLowPassTau = 0.05f; // Free Mix Defaults to Quad X systemConfig.freeMixMotors = 4; systemConfig.freeMix[0][ROLL ] = 1.0f; systemConfig.freeMix[0][PITCH] = -1.0f; systemConfig.freeMix[0][YAW ] = -1.0f; systemConfig.freeMix[1][ROLL ] = -1.0f; systemConfig.freeMix[1][PITCH] = -1.0f; systemConfig.freeMix[1][YAW ] = 1.0f; systemConfig.freeMix[2][ROLL ] = -1.0f; systemConfig.freeMix[2][PITCH] = 1.0f; systemConfig.freeMix[2][YAW ] = -1.0f; systemConfig.freeMix[3][ROLL ] = 1.0f; systemConfig.freeMix[3][PITCH] = 1.0f; systemConfig.freeMix[3][YAW ] = 1.0f; systemConfig.freeMix[4][ROLL ] = 0.0f; systemConfig.freeMix[4][PITCH] = 0.0f; systemConfig.freeMix[4][YAW ] = 0.0f; systemConfig.freeMix[5][ROLL ] = 0.0f; systemConfig.freeMix[5][PITCH] = 0.0f; systemConfig.freeMix[5][YAW ] = 0.0f; /////////////////////////////// systemConfig.rollAttAltCompensationGain = 1.0f; systemConfig.rollAttAltCompensationLimit = 0.0f * D2R; systemConfig.pitchAttAltCompensationGain = 1.0f; systemConfig.pitchAttAltCompensationLimit = 0.0f * D2R; /////////////////////////////////// systemConfig.midCommand = 3000.0f; systemConfig.minCheck = (float)(MINCOMMAND + 200); systemConfig.maxCheck = (float)(MAXCOMMAND - 200); systemConfig.minThrottle = (float)(MINCOMMAND + 200); systemConfig.maxThrottle = (float)(MAXCOMMAND); /////////////////////////////// systemConfig.PID[ROLL_RATE_PID].P = 250.0f; systemConfig.PID[ROLL_RATE_PID].I = 100.0f; systemConfig.PID[ROLL_RATE_PID].D = 0.0f; systemConfig.PID[ROLL_RATE_PID].N = 100.0f; systemConfig.PID[ROLL_RATE_PID].integratorState = 0.0f; systemConfig.PID[ROLL_RATE_PID].filterState = 0.0f; systemConfig.PID[ROLL_RATE_PID].prevResetState = false; systemConfig.PID[PITCH_RATE_PID].P = 250.0f; systemConfig.PID[PITCH_RATE_PID].I = 100.0f; systemConfig.PID[PITCH_RATE_PID].D = 0.0f; systemConfig.PID[PITCH_RATE_PID].N = 100.0f; systemConfig.PID[PITCH_RATE_PID].integratorState = 0.0f; systemConfig.PID[PITCH_RATE_PID].filterState = 0.0f; systemConfig.PID[PITCH_RATE_PID].prevResetState = false; systemConfig.PID[YAW_RATE_PID].P = 350.0f; systemConfig.PID[YAW_RATE_PID].I = 100.0f; systemConfig.PID[YAW_RATE_PID].D = 0.0f; systemConfig.PID[YAW_RATE_PID].N = 100.0f; systemConfig.PID[YAW_RATE_PID].integratorState = 0.0f; systemConfig.PID[YAW_RATE_PID].filterState = 0.0f; systemConfig.PID[YAW_RATE_PID].prevResetState = false; systemConfig.PID[ROLL_ATT_PID].P = 2.0f; systemConfig.PID[ROLL_ATT_PID].I = 0.0f; systemConfig.PID[ROLL_ATT_PID].D = 0.0f; systemConfig.PID[ROLL_ATT_PID].N = 100.0f; systemConfig.PID[ROLL_ATT_PID].integratorState = 0.0f; systemConfig.PID[ROLL_ATT_PID].filterState = 0.0f; systemConfig.PID[ROLL_ATT_PID].prevResetState = false; systemConfig.PID[PITCH_ATT_PID].P = 2.0f; systemConfig.PID[PITCH_ATT_PID].I = 0.0f; systemConfig.PID[PITCH_ATT_PID].D = 0.0f; systemConfig.PID[PITCH_ATT_PID].N = 100.0f; systemConfig.PID[PITCH_ATT_PID].integratorState = 0.0f; systemConfig.PID[PITCH_ATT_PID].filterState = 0.0f; systemConfig.PID[PITCH_ATT_PID].prevResetState = false; systemConfig.PID[HEADING_PID].P = 3.0f; systemConfig.PID[HEADING_PID].I = 0.0f; systemConfig.PID[HEADING_PID].D = 0.0f; systemConfig.PID[HEADING_PID].N = 100.0f; systemConfig.PID[HEADING_PID].integratorState = 0.0f; systemConfig.PID[HEADING_PID].filterState = 0.0f; systemConfig.PID[HEADING_PID].prevResetState = false; systemConfig.PID[NDOT_PID].P = 3.0f; systemConfig.PID[NDOT_PID].I = 0.0f; systemConfig.PID[NDOT_PID].D = 0.0f; systemConfig.PID[NDOT_PID].N = 100.0f; systemConfig.PID[NDOT_PID].integratorState = 0.0f; systemConfig.PID[NDOT_PID].filterState = 0.0f; systemConfig.PID[NDOT_PID].prevResetState = false; systemConfig.PID[EDOT_PID].P = 3.0f; systemConfig.PID[EDOT_PID].I = 0.0f; systemConfig.PID[EDOT_PID].D = 0.0f; systemConfig.PID[EDOT_PID].N = 100.0f; systemConfig.PID[EDOT_PID].integratorState = 0.0f; systemConfig.PID[EDOT_PID].filterState = 0.0f; systemConfig.PID[EDOT_PID].prevResetState = false; systemConfig.PID[HDOT_PID].P = 2.0f; systemConfig.PID[HDOT_PID].I = 0.0f; systemConfig.PID[HDOT_PID].D = 0.0f; systemConfig.PID[HDOT_PID].N = 100.0f; systemConfig.PID[HDOT_PID].integratorState = 0.0f; systemConfig.PID[HDOT_PID].filterState = 0.0f; systemConfig.PID[HDOT_PID].prevResetState = false; systemConfig.PID[N_PID].P = 3.0f; systemConfig.PID[N_PID].I = 0.0f; systemConfig.PID[N_PID].D = 0.0f; systemConfig.PID[N_PID].N = 100.0f; systemConfig.PID[N_PID].integratorState = 0.0f; systemConfig.PID[N_PID].filterState = 0.0f; systemConfig.PID[N_PID].prevResetState = false; systemConfig.PID[E_PID].P = 3.0f; systemConfig.PID[E_PID].I = 0.0f; systemConfig.PID[E_PID].D = 0.0f; systemConfig.PID[E_PID].N = 100.0f; systemConfig.PID[E_PID].integratorState = 0.0f; systemConfig.PID[E_PID].filterState = 0.0f; systemConfig.PID[E_PID].prevResetState = false; systemConfig.PID[H_PID].P = 2.0f; systemConfig.PID[H_PID].I = 0.0f; systemConfig.PID[H_PID].D = 0.0f; systemConfig.PID[H_PID].N = 100.0f; systemConfig.PID[H_PID].integratorState = 0.0f; systemConfig.PID[H_PID].filterState = 0.0f; systemConfig.PID[H_PID].prevResetState = false; /////////////////////////////// systemConfig.armCount = 50; systemConfig.disarmCount = 0; /////////////////////////////////// systemConfig.activeTelemetry = 0; systemConfig.mavlinkEnabled = false; /////////////////////////////////// systemConfig.CRCFlags = 0; /////////////////////////////////// writeSystemEEPROM(); } }