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;
}
