int writeEEPROM(void) { FLASH_Status status; int32_t i; eepromConfig_t *src = &eepromConfig; uint32_t *dst = (uint32_t*)FLASH_WRITE_EEPROM_ADDR; // there's no reason to write these values to EEPROM, they'll just be noise zeroPIDstates(); if ( src->CRCFlags & CRC_HistoryBad ) evrPush(EVR_ConfigBadHistory,0); src->CRCAtEnd[0] = crc32B( (uint32_t*)&src[0], src->CRCAtEnd); FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); status = FLASH_EraseSector(FLASH_Sector_1, VoltageRange_3); /////////////////////////////////// i = -1; while ( FLASH_COMPLETE == status && i++ < eepromConfigNUMWORD ) status = FLASH_ProgramWord((uint32_t)&dst[i], ((uint32_t*)src)[i]); if ( FLASH_COMPLETE != status ) evrPush( -1 == i ? EVR_FlashEraseFail : EVR_FlashProgramFail, status); /////////////////////////////////// FLASH_Lock(); readEEPROM(); return status; }
uint8_t writeEEPROM(void) { // there's no reason to write these values to EEPROM, they'll just be noise zeroPIDintegralError(); zeroPIDstates(); FLASH_Status status; int i; uint32_t *dst = (uint32_t*)FLASH_WRITE_EEPROM_ADDR; eepromConfig_t *src = &eepromConfig; if ( src->CRCFlags & CRC_HistoryBad ) evrPush(EVR_ConfigBadHistory,0); src->CRCAtEnd[0] = crc32B( (uint32_t*)&src[0], src->CRCAtEnd); FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR); i = -1; status = FLASH_ErasePage(FLASH_WRITE_EEPROM_ADDR); while ( status == FLASH_COMPLETE && i++ < eepromConfigNUMWORD ) status = FLASH_ProgramWord((uint32_t)&dst[i], ((uint32_t*)src)[i]); if ( status != FLASH_COMPLETE ) evrPush( -1 == i ? EVR_FlashEraseFail : EVR_FlashProgramFail, status); FLASH_Lock(); readEEPROM(); return status; }
void eepromCLI() { uint32_t c1, c2; uint8_t eepromQuery = 'x'; uint8_t validQuery = false; cliBusy = true; cliPortPrint("\nEntering EEPROM CLI....\n\n"); while(true) { cliPortPrint("EEPROM CLI -> "); while ((cliPortAvailable() == false) && (validQuery == false)); if (validQuery == false) eepromQuery = cliPortRead(); cliPortPrint("\n"); switch(eepromQuery) { // 'a' is the standard "print all the information" character case 'a': // config struct data c1 = eepromConfig.CRCAtEnd[0]; zeroPIDstates(); c2 = crc32bEEPROM(&eepromConfig, false); cliPortPrintF("Config structure information:\n"); cliPortPrintF("Version : %d\n", eepromConfig.version ); cliPortPrintF("Size : %d\n", sizeof(eepromConfig) ); cliPortPrintF("CRC on last read : %08X\n", c1 ); cliPortPrintF("Current CRC : %08X\n", c2 ); if ( c1 != c2 ) cliPortPrintF(" CRCs differ. Current Config has not yet been saved.\n"); cliPortPrintF("CRC Flags :\n"); cliPortPrintF(" History Bad : %s\n", eepromConfig.CRCFlags & CRC_HistoryBad ? "true" : "false" ); validQuery = false; break; /////////////////////////// case 'c': // Write out to Console in Hex. (RAM -> console) // we assume the flyer is not in the air, so that this is ok; // these change randomly when not in flight and can mistakenly // make one think that the in-memory eeprom struct has changed zeroPIDstates(); cliPortPrintF("\n"); cliPrintEEPROM(&eepromConfig); cliPortPrintF("\n"); if (crcCheckVal != crc32bEEPROM(&eepromConfig, true)) { cliPortPrint("NOTE: in-memory config CRC invalid; there have probably been changes to\n"); cliPortPrint(" eepromConfig since the last write to flash/eeprom.\n"); } validQuery = false; break; /////////////////////////// case 'H': // clear bad history flag cliPortPrintF("Clearing Bad History flag.\n"); eepromConfig.CRCFlags &= ~CRC_HistoryBad; validQuery = false; break; /////////////////////////// case 'C': // Read in from Console in hex. Console -> RAM ; uint32_t sz = sizeof(eepromConfig); eepromConfig_t e; uint8_t *p = (uint8_t*)&e; uint8_t *end = (uint8_t*)(&e + 1); uint32_t t = millis(); enum { Timeout = 100 }; // timeout is in ms int second_nibble = 0; // 0 or 1 char c; uint32_t chars_encountered = 0; cliPortPrintF("Ready to read in config. Expecting %d (0x%03X) bytes as %d\n", sz, sz, sz * 2); cliPortPrintF("hexadecimal characters, optionally separated by [ \\n\\r_].\n"); cliPortPrintF("Times out if no character is received for %dms\n", Timeout); memset(p, 0, end - p); while (p < end) { while (!cliPortAvailable() && millis() - t < Timeout) {} t = millis(); c = cliPortAvailable() ? cliPortRead() : '\0'; int8_t hex = parse_hex(c); int ignore = c == ' ' || c == '\n' || c == '\r' || c == '_' ? true : false; if (c != '\0') // assume the person isn't sending null chars chars_encountered++; if (ignore) continue; if (hex == -1) break; *p |= second_nibble ? hex : hex << 4; p += second_nibble; second_nibble ^= 1; } if (c == 0) { cliPortPrintF("Did not receive enough hex chars! (got %d, expected %d)\n", (p - (uint8_t*)&e) * 2 + second_nibble, sz * 2); } else if (p < end || second_nibble) { cliPortPrintF("Invalid character found at position %d: '%c' (0x%02x)", chars_encountered, c, c); } // HJI else if (crcCheckVal != crc32bEEPROM(&e, true)) // HJI { // HJI cliPortPrintF("CRC mismatch! Not writing to in-memory config.\n"); // HJI cliPortPrintF("Here's what was received:\n\n"); // HJI cliPrintEEPROM(&e); // HJI } else { // check to see if the newly received eeprom config // actually differs from what's in-memory zeroPIDstates(); int i; for (i = 0; i < sz; i++) if (((uint8_t*)&e)[i] != ((uint8_t*)&eepromConfig)[i]) break; if (i == sz) { cliPortPrintF("NOTE: uploaded config was identical to in-memory config.\n"); } else { eepromConfig = e; cliPortPrintF("In-memory config updated!\n"); cliPortPrintF("NOTE: config not written to EEPROM; use 'W' to do so.\n"); } } // eat the next 100ms (or whatever Timeout is) of characters, // in case the person pasted too much by mistake or something t = millis(); while (millis() - t < Timeout) if (cliPortAvailable()) cliPortRead(); validQuery = false; break; /////////////////////////// case 'E': // Read in from EEPROM. (EEPROM -> RAM) cliPortPrint("Re-reading EEPROM.\n"); readEEPROM(); validQuery = false; break; /////////////////////////// case 'x': // exit EEPROM CLI cliPortPrint("\nExiting EEPROM CLI....\n\n"); cliBusy = false; return; break; /////////////////////////// case 'W': case 'e': // Write out to EEPROM. (RAM -> EEPROM) cliPortPrint("\nWriting EEPROM Parameters....\n\n"); validQuery = false; writeEEPROM(); break; /////////////////////////// case 'f': // Write out to sdCard FILE. (RAM -> FILE) validQuery = false; break; /////////////////////////// case 'F': // Read in from sdCard FILE. (FILE -> RAM) validQuery = false; break; /////////////////////////// case 'V': // Reset EEPROM Parameters cliPortPrint( "\nEEPROM Parameters Reset....(not rebooting)\n" ); checkFirstTime(true); validQuery = false; break; /////////////////////////// case '?': // 0 1 2 3 4 5 6 7 // 01234567890123456789012345678901234567890123456789012345678901234567890123456789 cliPortPrintF("\n"); cliPortPrintF("'a' Display in-RAM config information\n"); cliPortPrintF("'c' Write in-RAM -> Console (as Hex) 'C' Read Console (as Hex) -> in-RAM\n"); cliPortPrintF("'e' Write in-RAM -> EEPROM 'E' Read EEPROM -> in-RAM\n"); cliPortPrintF("'f' Write in-RAM -> sd FILE (Not yet imp) 'F' Read sd FILE -> in-RAM (Not imp)\n"); cliPortPrintF(" 'H' Clear CRC Bad History flag\n"); cliPortPrintF(" 'V' Reset in-RAM config to default.\n"); cliPortPrintF("'x' Exit EEPROM CLI '?' Command Summary\n"); cliPortPrintF("\n"); cliPortPrintF("For compatability: 'W' Write in-RAM -> EEPROM\n"); cliPortPrintF("\n"); break; /////////////////////////// } } }
void processFlightCommands(void) { uint8_t channel; uint8_t channelsToRead = 8; float hdgDelta, simpleX, simpleY; if ( rcActive == true ) { // Read receiver commands if (eepromConfig.receiverType == PPM) channelsToRead = eepromConfig.ppmChannels; for (channel = 0; channel < channelsToRead; channel++) { if (eepromConfig.receiverType == SPEKTRUM) rxCommand[channel] = spektrumRead(eepromConfig.rcMap[channel]); else if (eepromConfig.receiverType == SBUS) rxCommand[channel] = sBusRead(eepromConfig.rcMap[channel]); else rxCommand[channel] = rxRead(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 for (channel = 3; channel < channelsToRead; channel++) rxCommand[channel] -= eepromConfig.midCommand - MIDCOMMAND; // 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) ) { disarmingTimer++; if (disarmingTimer > eepromConfig.disarmCount) { 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) { zeroPIDstates(); armed = true; armingTimer = 0; } } else { armingTimer = 0; } } /////////////////////////////////// // Check for armed true and throttle command > minThrottle if ((armed == true) && (rxCommand[THROTTLE] > eepromConfig.minThrottle)) pidReset = false; else pidReset = true; /////////////////////////////////// // Check yaw in detent and flight mode to determine hdg hold engaged state if ((commandInDetent[YAW] == true) && (flightMode == ATTITUDE) && (headingHoldEngaged == false)) { headingHoldEngaged = true; setPIDstates(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; } } /////////////////////////////////////////////// // Need to have AUX channels update modes // based on change, to allow for both external // remote commanding from serial port and with // transmitter switches // // Conditions --------------------------------- // A switch can actuate multiple modes // Mode enables are defined by channel ranges // A mode is only enabled/disabled when a // channel range changes, this allows remote // commands via serial to be sent /////////////////////////////////////////////// // Search through each AUX channel int ch; for (ch=AUX1; ch<LASTCHANNEL; ch++) { // Only make update if channel value changed if (fabs(previousRxCommand[ch] - rxCommand[ch]) > CHANGE_RANGE) { // Search through each mode slot int slot; for (slot=1; slot < MODE_SLOTS; slot++) { // If mode slot uses current rx channel, update if mode is on/off if (eepromConfig.mode[slot].channel == ch) { // Only change the mode state if the rx channels are in range int chValue = constrain(rxCommand[ch]/2, 1000, 2000); if ((chValue >= eepromConfig.mode[slot].minChannelValue) && (chValue <= eepromConfig.mode[slot].maxChannelValue)) { switch(eepromConfig.mode[slot].modeType) { case MODE_NONE: flightMode = ATTITUDE; verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; autoNavMode = MODE_NONE; break; case MODE_ATTITUDE: autoNavMode = MODE_NONE; if (eepromConfig.mode[slot].state) { flightMode = ATTITUDE; setPIDstates(ROLL_ATT_PID, 0.0f); setPIDstates(PITCH_ATT_PID, 0.0f); setPIDstates(HEADING_PID, 0.0f); } else { // if OFF and no other mode set, default to rate mode flightMode = RATE; setPIDstates(ROLL_RATE_PID, 0.0f); setPIDstates(PITCH_RATE_PID, 0.0f); setPIDstates(YAW_RATE_PID, 0.0f); } break; case MODE_RATE: autoNavMode = MODE_NONE; if (eepromConfig.mode[slot].state) { flightMode = RATE; setPIDstates(ROLL_RATE_PID, 0.0f); setPIDstates(PITCH_RATE_PID, 0.0f); setPIDstates(YAW_RATE_PID, 0.0f); } else { // if OFF and no other mode set, default to attitude mode flightMode = ATTITUDE; setPIDstates(ROLL_ATT_PID, 0.0f); setPIDstates(PITCH_ATT_PID, 0.0f); setPIDstates(HEADING_PID, 0.0f); } break; case MODE_SIMPLE: autoNavMode = MODE_NONE; if (eepromConfig.mode[slot].state) { flightMode = MODE_SIMPLE; hdgDelta = sensors.attitude500Hz[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; } else { // if OFF and no other mode set, default to attitude mode flightMode = ATTITUDE; setPIDstates(ROLL_ATT_PID, 0.0f); setPIDstates(PITCH_ATT_PID, 0.0f); setPIDstates(HEADING_PID, 0.0f); } break; case MODE_AUTONAV: if (eepromConfig.mode[slot].state) { flightMode = ATTITUDE; //verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT; autoNavMode = MODE_AUTONAV; setAutoNavState(AUTONAV_ENABLED); } else { flightMode = ATTITUDE; //verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; autoNavMode = MODE_NONE; setAutoNavState(AUTONAV_DISABLED); } break; case MODE_POSITIONHOLD: if (eepromConfig.mode[slot].state) { flightMode = ATTITUDE; //verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT; autoNavMode = MODE_POSITIONHOLD; } else { flightMode = ATTITUDE; //verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; autoNavMode = MODE_NONE; } break; case MODE_RETURNTOHOME: if (eepromConfig.mode[slot].state) { flightMode = ATTITUDE; //verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT; autoNavMode = MODE_RETURNTOHOME; } else { flightMode = ATTITUDE; //verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; autoNavMode = MODE_NONE; } break; case MODE_ALTHOLD: if (eepromConfig.mode[slot].state) { if (verticalModeState == ALT_DISENGAGED_THROTTLE_ACTIVE) { verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT; setPIDstates(HDOT_PID, 0.0f); setPIDstates(H_PID, 0.0f); altitudeHoldReference = hEstimate; throttleReference = rxCommand[THROTTLE]; } else if (verticalModeState == ALT_DISENGAGED_THROTTLE_INACTIVE) verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT; } else if (verticalModeState == VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY) { verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; altitudeHoldReference = hEstimate; } else verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE; break; case MODE_PANIC: if (eepromConfig.mode[slot].state) { flightMode = ATTITUDE; verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; autoNavMode = MODE_PANIC; } break; } } } } } previousRxCommand[ch] = rxCommand[ch]; } /////////////////////////////////// // AutoNavigation State Machine switch (autoNavMode) { case MODE_NONE: autoNavPitchAxisCorrection = 0.0; autoNavRollAxisCorrection = 0.0; autoNavYawAxisCorrection = 0.0; break; case MODE_AUTONAV: processAutoNavigation(); break; case MODE_POSITIONHOLD: processPositionHold(); break; case MODE_RETURNTOHOME: processReturnToHome(); break; } /////////////////////////////////// // Vertical Mode State Machine switch (verticalModeState) { case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT: if ((vertRefCmdInDetent == true) || eepromConfig.verticalVelocityHoldOnly) verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE; break; case ALT_DISENGAGED_THROTTLE_ACTIVE: break; case ALT_HOLD_AT_REFERENCE_ALTITUDE: if ((vertRefCmdInDetent == false) || eepromConfig.verticalVelocityHoldOnly) verticalModeState = VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY; break; case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY: if ((vertRefCmdInDetent == true) && !eepromConfig.verticalVelocityHoldOnly) { verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE; altitudeHoldReference = hEstimate; } break; case ALT_DISENGAGED_THROTTLE_INACTIVE: // This mode verifies throttle is at center when disengaging alt hold if (((rxCommand[THROTTLE] < throttleCmd + THROTTLE_WINDOW) && (rxCommand[THROTTLE] > throttleCmd - THROTTLE_WINDOW)) || eepromConfig.verticalVelocityHoldOnly) verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE; } }
void processFlightCommands(void) { uint8_t channel; if ( rcActive == true ) { // Read receiver commands for (channel = 0; channel < 8; channel++) rxCommand[channel] = (float)rxRead(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 ), will disarm immediately if ( (rxCommand[YAW] < (eepromConfig.minCheck - MIDCOMMAND)) && (armed == true) ) { armed = false; zeroPIDintegralError(); zeroPIDstates(); } // 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), must be present for 1 sec before arming if ((rxCommand[YAW] > (eepromConfig.maxCheck - MIDCOMMAND) ) && (armed == false) && (execUp == true)) { armingTimer++; if ( armingTimer > 50 ) { 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); } /////////////////////////////////// if ((commandInDetent[YAW] == true) && (flightMode == ATTITUDE)) headingHoldEngaged = true; else headingHoldEngaged = false; /////////////////////////////////// // Check AUX2 for altitude hold mode (2 Position Switch) if ((rxCommand[AUX2] > MIDCOMMAND) && (previousAUX2State <= MIDCOMMAND)) // Rising edge detection { altitudeHoldState = ENGAGED; altitudeHoldThrottleValue = rxCommand[THROTTLE]; } else if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // Falling edge detection { altitudeHoldState = DISENGAGED; } previousAUX2State = rxCommand[AUX2]; /////////////////////////////////// }
uint8_t writeEEPROM(void) { FLASH_Status status; int32_t i; /////////////////////////////////// if (eepromConfig.receiverType == SPEKTRUM) { USART_Cmd(USART6, DISABLE); TIM_Cmd(TIM6, DISABLE); if (eepromConfig.slaveSpektrum == true) USART_Cmd(UART4, DISABLE); } /////////////////////////////////// eepromConfig_t *src = &eepromConfig; uint32_t *dst = (uint32_t*)FLASH_WRITE_EEPROM_ADDR; // there's no reason to write these values to EEPROM, they'll just be noise zeroPIDintegralError(); zeroPIDstates(); if ( src->CRCFlags & CRC_HistoryBad ) evrPush(EVR_ConfigBadHistory,0); src->CRCAtEnd[0] = crc32B( (uint32_t*)&src[0], src->CRCAtEnd); FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); status = FLASH_EraseSector(FLASH_Sector_1, VoltageRange_3); /////////////////////////////////// i = -1; while ( FLASH_COMPLETE == status && i++ < eepromConfigNUMWORD ) status = FLASH_ProgramWord((uint32_t)&dst[i], ((uint32_t*)src)[i]); if ( FLASH_COMPLETE != status ) evrPush( -1 == i ? EVR_FlashEraseFail : EVR_FlashProgramFail, status); /////////////////////////////////// FLASH_Lock(); readEEPROM(); if (eepromConfig.receiverType == SPEKTRUM) { primarySpektrumState.reSync = 1; TIM_Cmd(TIM6, ENABLE); USART_Cmd(USART6, ENABLE); if (eepromConfig.slaveSpektrum == true) { slaveSpektrumState.reSync = 1; USART_Cmd(UART4, ENABLE); } } /////////////////////////////////// return status; }
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]; /////////////////////////////////// }
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 writeSystemEEPROM(void) { uint16_t byteCount; uint8_t pageIndex; uint8_t* start = (uint8_t*)(&systemConfig); uint8_t* end = (uint8_t*)(&systemConfig + 1); /////////////////////////////////// // there's no reason to write these values to EEPROM, they'll just be noise zeroPIDstates(); if (systemConfig.CRCFlags & CRC_HistoryBad) evrPush(EVR_ConfigBadSystemHistory,0); systemConfig.CRCAtEnd[0] = crc32B((uint32_t*)(&systemConfig), // CRC32B[systemConfig] (uint32_t*)(&systemConfig.CRCAtEnd)); /////////////////////////////////// setSPIdivisor(EEPROM_SPI, 2); // 18 MHz SPI clock systemConfigaddr.value = SYSTEM_EEPROM_ADDR; /////////////////////////////////// // Sector Erase writeEnable(); ENABLE_EEPROM; spiTransfer(EEPROM_SPI, SECTOR_ERASE_64KB); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[2]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[1]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[0]); DISABLE_EEPROM; delayMicroseconds(2); eepromBusy(); /////////////////////////////////// // Program Page(s) for (pageIndex = 0; pageIndex < ((sizeof(systemConfig) / 256) + 1); pageIndex++) { writeEnable(); ENABLE_EEPROM; spiTransfer(EEPROM_SPI, PAGE_PROGRAM_256_BYTES); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[2]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[1]); spiTransfer(EEPROM_SPI, systemConfigaddr.bytes[0]); for (byteCount = 0; byteCount < 256; byteCount++) { spiTransfer(EEPROM_SPI, *start++); if (start >= end) break; } DISABLE_EEPROM; delayMicroseconds(2); eepromBusy(); systemConfigaddr.value += 0x0100; } readSystemEEPROM(); }
uint8_t writeEEPROM(void) { FLASH_Status status; int32_t i; /////////////////////////////////// if (eepromConfig.receiverType == SPEKTRUM) { USART_Cmd(USART2, DISABLE); TIM_Cmd(TIM2, DISABLE); } /////////////////////////////////// eepromConfig_t *src = &eepromConfig; uint32_t *dst = (uint32_t*)FLASH_WRITE_EEPROM_ADDR; // there's no reason to write these values to EEPROM, they'll just be noise zeroPIDstates(); if (src->CRCFlags & CRC_HistoryBad) evrPush(EVR_ConfigBadHistory,0); src->CRCAtEnd[0] = crc32B( (uint32_t*)&src[0], src->CRCAtEnd); FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); status = FLASH_ErasePage(FLASH_WRITE_EEPROM_ADDR); /////////////////////////////////// i = -1; while (status == FLASH_COMPLETE && i++ < eepromConfigNUMWORD) status = FLASH_ProgramWord((uint32_t)&dst[i], ((uint32_t*)src)[i]); if (status != FLASH_COMPLETE) evrPush( i == -1 ? EVR_FlashEraseFail : EVR_FlashProgramFail, status); /////////////////////////////////// FLASH_Lock(); readEEPROM(); /////////////////////////////////// if (eepromConfig.receiverType == SPEKTRUM) { primarySpektrumState.reSync = 1; TIM_Cmd(TIM2, ENABLE); USART_Cmd(USART2, ENABLE); } /////////////////////////////////// return status; }