/*****************************************************************************
*
* flash_test - System Flash ROM diagnostics
*
* A destructive Flash ROM Read/Write test. Note that the area of Flash
* which is tested changes based on whether the diagnostic is being invoked
* from the System code or from the Factory code (can't write over MON960).
*
* This test basically does a Longword Address test to the Flash area.
*
*/
void flash_test(MENU_ARG arg)
{
ADDR start_addr = (ADDR)FLASH_ADDR; /* Original */
int i;
unsigned long *f_ptr = (unsigned long *)FLASH_ADDR;
int bytes_written = 0;
unsigned long flash_data;
char answer[20];
/* 10/31/00 */
int status;
init_eeprom();
printf("***********************************\n");
printf("*** WARNING ***\n");
printf("*** This test is destructive to ***\n");
printf("*** all contents of the FLASH! ***\n");
printf("***********************************\n");
printf("\nDo you wish to continue? (y/n)\n");
sgets(answer);
printf("\n\n");
if ((answer[0] != 'y') && (answer[0] != 'Y'))
return;
printf ("FLASH begins at 0x%X\n", FLASH_ADDR);
printf ("Total FLASH size = 0x%X\n\n", eeprom_size);
printf ("Checking FLASH ...\n");
if (check_eeprom(NO_ADDR, 0) == OK)
printf("FLASH is erased\n\n");
else
printf("FLASH is programmed between 0x%X and 0x%X\n\n",
eeprom_prog_first, eeprom_prog_last);
printf ("\nClearing Block Lock Bits... \n");
if(clear_all_lock_bits(NO_ADDR)==OK)
printf("Done!\n\n");
else
printf("Error!\n\n");
printf ("Erasing FLASH...\n");
if (erase_eeprom(NO_ADDR, 0) != OK)
printf("Error on erase_eeprom()\n\n");
else
printf("Done Erasing FLASH!\n\n");
(ADDR)start_addr = (ADDR)FLASH_BLK4_BASE_ADDR;
printf ("Writing Longword Data to FLASH...\n");
/* write to all of available Flash ROM. Don't do this thousands of times
since the Flash has only 100,000 write cycles in its lifespan */
while (bytes_written < (eeprom_size - RESERVED_AREA_SIZE)) {
flash_data = (unsigned long)start_addr;
for (i=0; i<TEST_BUF_LONGS; i++) {
flash_buffer[i] = flash_data; /* put address in buffer */
flash_data += 4; /* increment address */
}
if (write_eeprom (start_addr, (void *)flash_buffer, TEST_BUF_CHARS) != OK) {
printf("Error on write_eeprom()\n");
goto finish;
}
start_addr = (unsigned long)start_addr + TEST_BUF_CHARS;
bytes_written += TEST_BUF_CHARS;
}
printf ("Write Complete, Verifying Data...\n");
bytes_written = 0;
f_ptr = (unsigned long *)FLASH_BLK4_BASE_ADDR;
while (bytes_written < (eeprom_size - RESERVED_AREA_SIZE)) {
if (*f_ptr != (unsigned long)f_ptr) {
printf ("Data verification error at 0x%X\n", (unsigned long)f_ptr);
printf ("Expected 0x%X Got 0x%X\n", (unsigned long)f_ptr, *f_ptr);
goto finish;
}
f_ptr++;
bytes_written += 4;
}
printf ("Done Verifying Longword Data!\n\n");
printf ("Checking FLASH...\n");
if (check_eeprom(NO_ADDR, 0) == OK)
printf("FLASH is erased\n\n");
else
printf("FLASH is programmed between 0x%X and 0x%X\n\n",
eeprom_prog_first, eeprom_prog_last);
printf ("Erasing FLASH...\n");
if (erase_eeprom(NO_ADDR, 0) != OK)
printf("Error on erase_eeprom()\n\n");
else
printf("Done Erasing FLASH!\n\n");
printf ("Checking FLASH...\n");
if (check_eeprom(NO_ADDR, 0) == OK)
printf("FLASH is erased\n\n");
else
printf("FLASH is programmed between 0x%X and 0x%X\n\n",
eeprom_prog_first, eeprom_prog_last);
/* reinitialize variables */
bytes_written = 0;
start_addr = (ADDR)FLASH_BLK4_BASE_ADDR;
f_ptr = (unsigned long *)FLASH_BLK4_BASE_ADDR;
printf ("Writing Inverted Longword Data to FLASH...\n");
/* write to all of available Flash ROM. Don't do this thousands of times
since the Flash has only 100,000 write cycles in its lifespan */
while (bytes_written < (eeprom_size - RESERVED_AREA_SIZE)) {
flash_data = (unsigned long)start_addr;
for (i=0; i<TEST_BUF_LONGS; i++) {
flash_buffer[i] = ~flash_data; /* put address BAR in buffer */
flash_data += 4; /* increment address */
}
if (write_eeprom (start_addr, (void *)flash_buffer, TEST_BUF_CHARS) != OK) {
printf("Error on write_eeprom()\n");
goto finish;
}
start_addr = (unsigned long)start_addr + TEST_BUF_CHARS;
bytes_written += TEST_BUF_CHARS;
}
printf ("Write Complete, Verifying Data...\n");
bytes_written = 0;
while (bytes_written < (eeprom_size - RESERVED_AREA_SIZE)) {
if (*f_ptr != (~(unsigned long)f_ptr)) {
printf ("Data verification error at 0x%X\n", (unsigned long)f_ptr);
printf ("Expected 0x%X Got 0x%X\n", (~(unsigned long)f_ptr), *f_ptr);
goto finish;
}
f_ptr++;
bytes_written += 4;
}
printf ("Done Verifying Inverted Longword Data!\n\n");
printf ("Checking FLASH...\n");
if (check_eeprom(NO_ADDR, 0) == OK)
printf("FLASH is erased\n\n");
else {
printf("FLASH is programmed between 0x%X and 0x%X\n\n",
eeprom_prog_first, eeprom_prog_last);
}
printf ("Erasing FLASH...\n");
if (erase_eeprom(NO_ADDR, 0) != OK)
printf("Error on erase_eeprom()\n\n");
else
printf("Done Erasing FLASH!\n\n");
printf ("Checking FLASH...\n");
if (check_eeprom(NO_ADDR, 0) == OK)
printf("FLASH is erased\n\n");
else
printf("FLASH is programmed between 0x%X and 0x%X\n\n",
eeprom_prog_first, eeprom_prog_last);
/* 11/02/00 */
printf ("Setting Lock Bits for Blocks 0-3... \n");
if( (status = set_all_lock_bits() ) == OK )
printf("Done!\n");
else
printf("Error! status =0x%x\n", status);
finish:
_flushICache();
printf ("\nHit <CR> to Continue...\n");
(void)hexIn();
return;
}
void run()
{
uint8_t key[2];
uint8_t key_itr = 0;
while(key_itr < 2)
key[++key_itr] = 0;
key_itr = 0;
moveflags = 0;
recordIter = 0;
speed = 127;
le.clear();
re.clear();
recordTime.stop();
recordTime.clear();
uint32_t nextPlay = 0;
uint32_t nextPlayBase = 0;
state = 0;
encoder_play_l.stop();
encoder_play_r.stop();
startTime = 0;
/*rs232.send("YuniRC program has started!\r\n"
"Controls: W,A,S,D - movement, Space - read sensor values,");
rs232.wait();
rs232.send(" R - reset encoders, Q - On/Off engine correction, 1 2 3 - speed \r\n");
rs232.wait();
rs232.send("Engine correction is disabled.\r\n"); */
char ch;
while(true)
{
if(state & STATE_ERASING)
continue;
// Move correction
if((state & STATE_CORRECTION) && (moveflags == MOVE_FORWARD || moveflags == MOVE_BACKWARD))
MovementCorrection();
if((state & STATE_PLAY) && (lastAdress == 0 || EventHappened(&lastRec, &nextPlayBase, &nextPlay)))
{
encoder_play_l.stop();
encoder_play_r.stop();
read_mem(&lastRec, lastAdress);
lastAdress += REC_SIZE;
if((lastRec.key[0] == 0 && lastRec.key[1] == 0 && lastRec.getBigNum() == 0) ||
lastAdress > 512)
{
state &= ~(STATE_PLAY);
rs232.send("Playback finished\r\n");
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
continue;
}
SetMovement(lastRec.key);
nextPlay = 0;
nextPlayBase = 0;
if(lastRec.end_event == EVENT_TIME)
{
nextPlayBase = getTickCount();
nextPlay = (uint32_t(lastRec.getBigNum())*10000) * JUNIOR_WAIT_MUL / JUNIOR_WAIT_DIV;
}
//Uncomment to set messure delay
/*else if(lastRec.end_event == EVENT_RANGE_MIDDLE_HIGHER || lastRec.end_event == EVENT_RANGE_MIDDLE_LOWER)
{
nextPlayBase = getTickCount();
nextPlay = (50000) * JUNIOR_WAIT_MUL / JUNIOR_WAIT_DIV;
}*/
else if(lastRec.end_event == EVENT_DISTANCE || lastRec.end_event == EVENT_DISTANCE_LEFT || lastRec.end_event == EVENT_DISTANCE_RIGHT)
{
encoder_play_r.clear();
encoder_play_l.clear();
encoder_play_l.start();
encoder_play_r.start();
}
++recordIter;
}
//Read command
if(!rs232.peek(ch))
continue;
key[key_itr] = uint8_t(ch);
++key_itr;
//key recieved
if(key_itr >= 2)
{
key_itr = 0;
// FIXME: ignore two or more keys at once
if((state & STATE_RECORD) && char(lastRec.key[1]) == 'd' && char(key[1]) != 'u' && char(key[0]) != 'C')
{
while(key_itr < 2)
key[++key_itr] = '0';
key_itr = 0;
continue;
}
bool down_only = SetMovement(key);
if(char(key[0]) == 'O' || char(key[0]) == 'P')
continue;
else if((state & STATE_RECORD) && char(key[0]) != 'C' &&
(!down_only || (down_only && char(key[1]) == 'd'))) // do not record down only keys
{
if(!recordTime.isRunning())
{
recordTime.clear();
recordTime.start();
}
if(recordIter > 0)
{
lastRec.end_event = EVENT_TIME;
lastRec.setBigNum(recordTime.getTime()/10000);
write_mem(&lastRec, lastAdress);
lastAdress+=REC_SIZE;
}
if(recordIter < MEM_SIZE-1)
{
while(key_itr < 2)
{
lastRec.key[key_itr] = key[key_itr];
++key_itr;
}
key_itr = 0;
recordTime.clear();
++recordIter;
}
else
{
key[0] = uint8_t('C');
key[1] = uint8_t('d');
rs232.send("Memory full\r\n");
continue;
}
}
}
// EEPROM Flash mode
else if(ch == 0x1C)
{
while(key_itr < 2)
key[++key_itr] = '0';
key_itr = 0;
erase_eeprom();
rs232.sendCharacter(0x1D);
for(lastAdress = 0; true; )
{
if(!rs232.peek(ch))
continue;
if(ch == 0x1E && lastAdress%5 == 0)
break;
write_byte(lastAdress, uint8_t(ch));
++lastAdress;
rs232.sendCharacter(0x1F);
}
lastAdress = 0;
}
// EEPROM read mode
else if(ch == 0x16)
{
while(key_itr < 2)
key[++key_itr] = '0';
key_itr = 0;
rs232.sendCharacter(0x17);
for(lastAdress = 0; lastAdress < 512; ++lastAdress)
{
rs232.wait();
rs232.sendCharacter(read_byte(lastAdress));
}
rs232.sendCharacter(0x18);
lastAdress = 0;
}
}
}
bool Usb3Camera::upload_firmware (const std::string& firmware_package,
const std::string& firmware,
std::function<void(int)> progress)
{
int r = -1;
std::vector<unsigned char> fw;
if (is_package_file(firmware_package))
{
fw = extract_file_from_package(firmware_package, firmware);
}
else
fw = load_file(firmware_package);
if (fw.empty())
{
throw std::runtime_error("Firmware File could not be loaded correctly");
}
auto map_progress = [] ( std::function<void(int)> progress, int begin, int end )
{
return [=]( int x )
{
progress( begin + x * (end-begin) / 100 );
};
};
int retry = 0;
while (retry++ < 5)
{
progress( 0 );
r = erase_eeprom( map_progress( progress, 0, 20 ) );
if ( r < 0 )
{
continue;
}
try
{
r = upload_firmware_file( fw, map_progress( progress, 20, 70 ) );
}
catch (std::runtime_error& err)
{
}
if ( r < 0 )
{
continue;
}
std::vector<unsigned char> buffer( fw.size() );
try
{
r = download_firmware( buffer, map_progress( progress, 70, 100 ) );
}
catch (std::runtime_error& err)
{
throw;
}
if ( r < 0 )
continue;
if ( fw == buffer )
{
progress( 100 );
return true;
}
}
progress( 0 );
return false;
}
bool SetMovement(uint8_t key[])
{
// Set Movement Flags
bool down = (key[1] == uint8_t('d'));
bool down_only = false;
// only down keys
if(down)
{
switch(char(key[0]))
{
//speed (1 2 3 on keyboard Oo)
case 'a': speed = 50; break;
case 'b': speed = 100; break;
case 'c': speed = 127; break;
case 'R': // reset encoders
re.clear();
le.clear();
break;
case 'Q': // on/off correction
rs232.send("Engine correction is ");
if(state & STATE_CORRECTION)
{
state &= ~(STATE_CORRECTION);
rs232.send("disabled \r\n");
}
else
{
state |= STATE_CORRECTION;
rs232.send("enabled \r\n");
}
break;
case 'C':
rs232.wait();
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
if(!(state & STATE_RECORD))
{
state |= STATE_RECORD;
recordIter = 0;
rs232.send("Erasing EEPROM...");
state |= STATE_ERASING;
erase_eeprom();
state &= ~(STATE_ERASING);
rs232.send("done\r\n");
lastAdress = 0;
}
else
{
lastRec.end_event = EVENT_TIME;
lastRec.setBigNum(recordTime.getTime()/10000);
write_mem(&lastRec, lastAdress);
recordTime.stop();
recordTime.clear();
recordIter = 0;
state &= ~(STATE_RECORD);
}
rs232.send("Trace recording is ");
if(state & STATE_RECORD){ rs232.send("enabled \r\n");}
else {rs232.send("disabled \r\n");}
break;
case 'P':
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
setMotorPower(0, 0);
if(!(state & STATE_PLAY))
{
recordTime.stop();
recordTime.clear();
rs232.send("Playing..\r\n");
recordIter = 0;
lastAdress = 0;
state |= STATE_PLAY;
state &= ~(STATE_RECORD);
}
else
{
rs232.send("Playback stopped\r\n");
state &= ~(STATE_PLAY);
}
break;
case 'O':
if(state & STATE_RECORD)
break;
rs232.send("Playback ");
if(state & STATE_PLAY)
{
rs232.send("unpaused\r\n");
state &= ~(STATE_PLAY);
}
else
{
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
rs232.send("paused\r\n");
state |= STATE_PLAY;
}
break;
}
}
// Movement
switch(char(key[0]))
{
case 'W':
if(!(moveflags & MOVE_BACKWARD))
{
if(down) moveflags |= MOVE_FORWARD;
else moveflags &= ~(MOVE_FORWARD);
}
break;
case 'S':
if(!(moveflags & MOVE_FORWARD))
{
if(down) moveflags |= MOVE_BACKWARD;
else moveflags &= ~(MOVE_BACKWARD);
}
break;
case 'A':
if(!(moveflags & MOVE_RIGHT))
{
if(down) moveflags |= MOVE_LEFT;
else moveflags &= ~(MOVE_LEFT);
}
break;
case 'D':
if(!(moveflags & MOVE_LEFT))
{
if(down) moveflags |= MOVE_RIGHT;
else moveflags &= ~(MOVE_RIGHT);
}
break;
default:
down_only = true;
break;
}
// Sensors
if(char(key[0]) == ' ' && down) // Space
{
rs232.wait();
rs232.send("\r\nSensors: ");
rs232.dumpNumber(getSensorValue(6));
rs232.sendNumber(getSensorValue(7)); // proud
/*rs232.send("\r\nSensors: \r\n");
rs232.send(" ");
rs232.sendNumber(getSensorValue(5));
rs232.send(" ");
rs232.sendNumber(getSensorValue(1));
rs232.wait();
rs232.send("\r\n");
rs232.sendNumber(getSensorValue(2));
rs232.send(" ");
rs232.sendNumber(getSensorValue(3));
rs232.wait(); */
rs232.send("\r\nEncoders: \r\n L: ");
rs232.sendNumber(le.get());
rs232.send(" R: ");
rs232.sendNumber(re.get());
rs232.send("\r\nRange \r\nL: ");
rs232.wait();
rs232.sendNumber(ReadRange(FINDER_LEFT));
rs232.send("cm M: ");
rs232.sendNumber(ReadRange(FINDER_MIDDLE));
rs232.send("cm R: ");
rs232.sendNumber(ReadRange(FINDER_RIGHT));
rs232.send("cm\r\n");
}
//Set motors
if(moveflags & MOVE_FORWARD)
{
if(moveflags & MOVE_LEFT)
setMotorPower(speed-TURN_VALUE, speed);
else if(moveflags & MOVE_RIGHT)
setMotorPower(speed, speed-TURN_VALUE);
else
{
le_cor.start();
re_cor.start();
le_cor.clear();
re_cor.clear();
setMotorPower(speed, speed);
state &= ~(STATE_CORRECTION2);
}
startTime = getTickCount();
}
else if(moveflags & MOVE_BACKWARD)
{
if(moveflags & MOVE_LEFT)
setMotorPower(-(speed-TURN_VALUE), -speed);
else if(moveflags & MOVE_RIGHT)
setMotorPower(-speed, -(speed-TURN_VALUE));
else
{
state &= ~(STATE_CORRECTION2);
le_cor.start();
re_cor.start();
le_cor.clear();
re_cor.clear();
setMotorPower(-speed, -speed);
}
startTime = getTickCount();
}
else if(moveflags & MOVE_LEFT)
{
setMotorPower(-speed, speed);
startTime = getTickCount();
}
else if(moveflags & MOVE_RIGHT)
{
setMotorPower(speed, -speed);
startTime = getTickCount();
}
else
{
startTime = getTickCount();
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
state &= ~(STATE_CORRECTION2);
}
return down_only;
}
