void cameraTest(SmartCar * smartCar) {
AIData data[2];
int16_t pos[2];
while (1) {
AIData_init(&data[0], &smartCar->camera[0]);
AIData_init(&data[1], &smartCar->camera[1]);
binarization(&data[0]);
binarization(&data[1]);
dumpData(data[0].arr, &smartCar->barLED[0]);
dumpData(data[1].arr, &smartCar->barLED[1]);
pos[0] = findIndexRL(&data[0]);
pos[1] = findIndexLR(&data[1]);
Segment_print(&smartCar->segment[0], (smartCar->camera[1].average + smartCar->camera[0].average)/2);
Segment_print(&smartCar->segment[1], handling(pos[0], pos[1],smartCar->motor.targetSpeed));
Segment_print(&smartCar->segment[2], Camera_getInterval() / 100);
switch (board.button.check()) {
case 4:
Segment_print(&smartCar->segment[0], 0);
Segment_print(&smartCar->segment[1], 0);
Segment_print(&smartCar->segment[2], 0);
return;
}
}
}
void test1() {
initDataHeader();
Data *d1 = (Data*)malloc(sizeof(Data));
d1->next = NULL;
addData(d1);
Data *d2 = (Data*)malloc(sizeof(Data));
d2->next = NULL;
addData(d2);
Data *d3 = (Data*)malloc(sizeof(Data));
d3->next = NULL;
addData(d3);
Data *d4 = (Data*)malloc(sizeof(Data));
d4->next = NULL;
addData(d4);
dumpData("After add 4 Data!");
removeData(2);
removeData(6);
dumpData("Remove 2 Data!");
Data *d = getData(20);
printf("Get Data Num = %d\n", d->num);
dispose();
}
void SqlDatabase::open(const QString &password)
{
if( m_db.isOpen() )
return;
if( m_path.isEmpty() || m_name.isEmpty() ) {
qCritical() << "Unable to open DB with empty path or file:" << m_path << m_name;
return;
}
QDir(m_path).mkpath(".");
QString db_path = m_path + "/" + m_name;
qDebug() << "Open SQL DB:" << db_path;
m_db = QSqlDatabase::addDatabase(new QSQLCipherDriver(), db_path);
m_db.setDatabaseName(db_path);
m_db.setPassword(password);
if( ! m_db.open() ) {
qCritical() << m_db.lastError();
qFatal("Unable to open sql database");
}
// TODO: remove DEBUG dump schema & data
qDebug() << dumpSchema();
qDebug() << dumpData();
}
void listMeshDataFunctionObject::write()
{
Info << "Content of mesh data " << obr().name()
<< endl;
dumpData();
}
void DumpJitCodeStr( const char *buffer, unsigned len, void (*output)( char ) )
{
jit_code_str *header;
header = (jit_code_str *)buffer;
switch( header->arch ) {
case JIT_ARCH_I86:
case JIT_ARCH_SPARC:
myPrintf( output, "arch: %d (%s)\n", header->arch, archTypes[ header->arch ] );
dumpSection( output, "code", &header->code );
dumpSection( output, "data", &header->data );
dumpSection( output, "eh_table", &header->eh_table );
dumpSection( output, "pdata", &header->pdata );
dumpSection( output, "relocs", &header->relocs );
myPrintf( output, "code:\n" );
dumpCode( output, header );
myPrintf( output, "data:\n" );
dumpData( output, header );
myPrintf( output, "eh_table:\n" );
dumpEhTable( output, header );
myPrintf( output, "pdata:\n" );
dumpPData( output, header );
myPrintf( output, "relocs:\n" );
dumpRelocs( output, header );
break;
default:
myPrintf( output, "not a valid code string: arch = %d\n", header->arch );
}
}
void fs_dumpAllData() {
int i;
int j;
printf("\n filesystem[]:\n");
dumpData(FS_SIZE, filesystem);
}
/* returns id */
int fs_addFile(char* name, int fileSize, int processes, int* processStart, int* processSize, int* data) {
int i;
int id;
int fileStart;
if (FS_DEBUG) {
heavyLine();
printf("fs_addFile(\"%s\", %d, ..., ..., ...)\n", name, fileSize);
if (FS_VERBOSE) {
printf("processStart: ");
for (i = 0; i < MAXPRO; i++)
printf("%d ", processStart[i]);
printf("\nprocessSize: ");
for (i = 0; i < MAXPRO; i++)
printf("%d ", processSize[i]);
dumpData(fileSize, data);
}
heavyLine();
}
if (!fs_dataIsValid(fileSize, data))
return -1;
fileStart = fs_addData(fileSize, data);
id = fs_addINode(name, fileStart, fileSize, processes, processStart, processSize);
if (FS_VERBOSE)
fs_dump();
return id;
}
bool CRootFolder::dumpDirTable(TCHAR *aFileName)
{
ofstream file(aFileName,ios::binary | ios::out);
bool ret = dumpData(&file);
file.close();
return ret;
}
void dumpValues(void)
{
dprintk(150, "BuffersStart %d, BufferEnd %d, len %d\n", RCVBufferStart, RCVBufferEnd, getLen(-1));
if (RCVBufferStart != RCVBufferEnd)
if (paramDebug >= 50)
dumpData();
}
bool listMeshDataFunctionObject::start()
{
Info << "Content of mesh data " << obr().name()
<< " at start" << endl;
dumpData();
return true;
}
HS_PUBLIC_API
hs_error_t hs_scan_vector(const hs_database_t *db, const char * const * data,
const unsigned int *length, unsigned int count,
UNUSED unsigned int flags, hs_scratch_t *scratch,
match_event_handler onEvent, void *context) {
if (unlikely(!scratch || !data || !length)) {
return HS_INVALID;
}
hs_error_t err = validDatabase(db);
if (unlikely(err != HS_SUCCESS)) {
return err;
}
const struct RoseEngine *rose = hs_get_bytecode(db);
if (unlikely(!ISALIGNED_16(rose))) {
return HS_INVALID;
}
if (unlikely(rose->mode != HS_MODE_VECTORED)) {
return HS_DB_MODE_ERROR;
}
if (unlikely(!validScratch(rose, scratch))) {
return HS_INVALID;
}
hs_stream_t *id = (hs_stream_t *)(scratch->bstate);
init_stream(id, rose); /* open stream */
for (u32 i = 0; i < count; i++) {
DEBUG_PRINTF("block %u/%u offset=%llu len=%u\n", i, count, id->offset,
length[i]);
#ifdef DEBUG
dumpData(data[i], length[i]);
#endif
hs_error_t ret
= hs_scan_stream_internal(id, data[i], length[i], 0, scratch,
onEvent, context);
if (ret != HS_SUCCESS) {
return ret;
}
}
/* close stream */
if (onEvent) {
report_eod_matches(id, scratch, onEvent, context);
if (told_to_stop_matching(scratch)) {
return HS_SCAN_TERMINATED;
}
}
return HS_SUCCESS;
}
void saveFull( MultiBlock2D& multiBlock, FileName fName, IndexOrdering::OrderingT ordering )
{
global::profiler().start("io");
SparseBlockStructure2D blockStructure(multiBlock.getBoundingBox());
Box2D bbox = multiBlock.getBoundingBox();
if (ordering==IndexOrdering::forward) {
plint nBlocks = std::min(bbox.getNx(), (plint)global::mpi().getSize());
std::vector<std::pair<plint,plint> > ranges;
util::linearRepartition(bbox.x0, bbox.x1, nBlocks, ranges);
for (pluint iRange=0; iRange<ranges.size(); ++iRange) {
blockStructure.addBlock (
Box2D( ranges[iRange].first, ranges[iRange].second,
bbox.y0, bbox.y1 ),
iRange );
}
}
else if (ordering==IndexOrdering::backward) {
plint nBlocks = std::min(bbox.getNy(), (plint)global::mpi().getSize());
std::vector<std::pair<plint,plint> > ranges;
util::linearRepartition(bbox.y0, bbox.y1, nBlocks, ranges);
for (pluint iRange=0; iRange<ranges.size(); ++iRange) {
blockStructure.addBlock (
Box2D( bbox.x0, bbox.x1, ranges[iRange].first, ranges[iRange].second ),
iRange );
}
}
else {
// Sparse ordering not defined.
PLB_ASSERT( false );
}
plint envelopeWidth=1;
MultiBlockManagement2D adjacentMultiBlockManagement (
blockStructure, new OneToOneThreadAttribution, envelopeWidth );
MultiBlock2D* multiAdjacentBlock = multiBlock.clone(adjacentMultiBlockManagement);
std::vector<plint> offset;
std::vector<plint> myBlockIds;
std::vector<std::vector<char> > data;
bool dynamicContent = false;
dumpData(*multiAdjacentBlock, dynamicContent, offset, myBlockIds, data);
if (ordering==IndexOrdering::backward && myBlockIds.size()==1) {
PLB_ASSERT( data.size()==1 );
Box2D domain;
blockStructure.getBulk(myBlockIds[0], domain);
plint sizeOfCell = multiAdjacentBlock->sizeOfCell();
PLB_ASSERT( domain.nCells()*sizeOfCell == (plint)data[0].size() );
transposeToBackward( sizeOfCell, domain, data[0] );
}
plint totalSize = offset[offset.size()-1];
writeOneBlockXmlSpec(*multiAdjacentBlock, fName, totalSize, ordering);
writeRawData(fName, myBlockIds, offset, data);
delete multiAdjacentBlock;
global::profiler().stop("io");
}
void Context::Attr::dump() const
{
Debug::dump(" Attr{type=%x, size=%d, value=", int(AttributeType), int(AttributeLength));
switch (AttributeType & CSSM_ATTRIBUTE_TYPE_MASK) {
case CSSM_ATTRIBUTE_DATA_UINT32:
Debug::dump("%ld", long(Attribute.Uint32)); break;
case CSSM_ATTRIBUTE_DATA_STRING:
Debug::dump("%s@%p", Attribute.String, Attribute.String); break;
case CSSM_ATTRIBUTE_DATA_CSSM_DATA:
dumpData(Attribute.Data);
break;
case CSSM_ATTRIBUTE_DATA_CRYPTO_DATA:
dumpData(&Attribute.CryptoData->Param);
break;
default:
Debug::dump("%p", Attribute.String); break; // (slightly unclean)
};
Debug::dump("}\n");
}
void FieldSector::newTurn()
{
m_isEmpty = checkIsEmpty();
if(m_isEmpty)
return;
for(int i = 0; i < g_grassSectorSize*g_grassSectorSize; i++)
{
if(m_fieldMap[i].stadium >= 0 && m_fieldMap[i].stadium < 4)
m_fieldMap[i].stadium++;
}
dumpData();
}
void save( MultiBlock2D& multiBlock, FileName fName, bool dynamicContent )
{
global::profiler().start("io");
std::vector<plint> offset;
std::vector<plint> myBlockIds;
std::vector<std::vector<char> > data;
dumpData(multiBlock, dynamicContent, offset, myBlockIds, data);
writeXmlSpec(multiBlock, fName, offset, dynamicContent);
writeRawData(fName, myBlockIds, offset, data);
global::profiler().stop("io");
}
/* Open season logger device */
void openSeasonLogger(void)
{
int i_baudrate; /* Communication parameter */
int i_framesize;
char i_parity;
char c_parity;
char s_parity[5];
printf("==SEASON LOGGER==\n\r");
printf("Note: This is the integrated season-logger tool of the chipcardlab software.\n\r");
printf(" To return back to main menue end the program by pressing STRG+C and restart\n\r");
printf(" it. Now unplug the Chipcardlab and plug in your season logger card!\n\r");
printf("\n\r");
printf("Baudrate (e.g. 9600) ==> ");
improvedScanf("%i", &i_baudrate);
printf("Frame size (7 or 8) ==> ");
improvedScanf("%i", &i_framesize);
printf("Parity (O=odd, E=even, N=none) ==> ");
improvedScanf("%c", &c_parity);
c_parity = toupper(c_parity);
switch(c_parity)
{
case 'O': i_parity = ODD;
strcpy(s_parity, "odd\0");
break;
case 'E': i_parity = EVEN;
strcpy(s_parity, "even\0");
break;
default: i_parity = NONE;
strcpy(s_parity, "no\0");
}
printf("opening season-logger on %s with %i baud, %i bit framesize and %s parity...\n\r",configfilePortValue, i_baudrate, i_framesize, s_parity);
ttyInitProf(configfilePortValue, i_baudrate, i_framesize, i_parity);
printf("\n\r");
dumpData(DUMP_SOURCE_SERIAL);
return;
}
int fs_import(char* filename, char* name) {
int i;
int j;
int id;
int fileSize;
int processes;
int processStart[MAXPRO];
int processSize[MAXPRO];
FILE* fp;
if (FS_DEBUG) {
heavyLine();
printf("fs_import(%s, %s)\n", filename, name);
heavyLine();
}
fp = fopen(filename, "r");
// discard string: processes
free((char*)getString(fp));
processes = getInt(fp);
// discard string: size
free((char*)getString(fp));
fileSize = 0;
for (i = 0; i < processes; i++) {
processStart[i] = fileSize;
processSize[i] = getInt(fp) + 1;
fileSize += processSize[i];
}
int data[fileSize];
for (i = 0; i < fileSize; i++)
data[i] = getInt(fp);
if (FS_VERBOSE)
dumpData(fileSize, data);
id = fs_addFile(name, fileSize, processes, processStart, processSize, data);
fclose(fp);
return id;
}
int
main(int argc, char* argv[])
{
char input[MAX_INPUT_SIZE];
#ifdef INPUT_FROM_FILE
FILE* fp = NULL;
if ((fp = fopen("input.txt", "r")) == NULL) {
fprintf(stderr, "Cannot open file, program exit\n");
exit(1);
}
int num = atoi(fgets(input, sizeof(input), fp));
int* array = new int[num];
if (!array) {
fprintf(stderr, "Error. Cannot allocate memory\n");
exit(1);
}
readData(array, num, fp);
//dumpData(array, num);
int exchangeNum = selectionSort(array, num);
dumpData(array, num);
printf("%d\n", exchangeNum);
if (!array) {
delete [] array;
array = NULL;
}
if (fp != NULL) {
fclose(fp);
fp = NULL;
}
#else
while (fgets(input, sizeof(input), stdin) != NULL) {
}
#endif
return 0;
}
/*
Function that manages the second run and dumping the commands , variables and data to memoryDump struct
Input:
RunStatus struct
MemoryDump struct
Output: Number of errors during the pass
*/
int SecondReadManager(RunStatus *runStatus, MemoryDump *memStatus)
{
int i = 0;
/*Validation that all extern labels exists in the code*/
checkEntryLabels(runStatus);
/*We are dumping the command lines first*/
for (i = 0; i < runStatus -> lineCount; i++)
{
dumpLine(i, memStatus, runStatus);
}
/* After finishing with commands we are dumping the data*/
for (i = 0; i < runStatus -> dataCount; i++)
{
dumpData(runStatus -> dataArray[i], memStatus);
}
return runStatus -> errNum;
}
/* returns start position */
int fs_addData(int fileSize, int* data) {
int i;
int fileStart;
int count;
if (FS_DEBUG) {
lightLine();
printf("fs_addData(%d, ...)\n", fileSize);
if (FS_VERBOSE)
dumpData(fileSize, data);
lightLine();
}
i = 0;
count = 0;
while (i < FS_SIZE && count < fileSize) {
if (filesystem[i] == FS_NULL)
count++;
else
count = 0;
i++;
}
/* not enough room */
if (count < fileSize) {
printf("insufficient disk space\n");
return -1;
}
fileStart = i - fileSize;
for (i = 0; i < fileSize; i++)
filesystem[fileStart + i] = data[i];
return fileStart;
}
void handleConnection(int c) {
FILE *tmp;
char buf[1024];
int r;
shutdown(c,SHUT_RD);
tmp=tmpfile();
dumpData(tmp);
rewind(tmp);
if (fork()) {
fclose(tmp);
close(c);
} else {
while(!feof(tmp)) {
r=fread((char *)&buf,1,1024,tmp);
write(c,(char *)&buf,r);
}
close(c);
fclose(tmp);
exit(0);
}
}
int main(int ac, char **av) {
dumpData(stdout);
return(0);
}
static void processResponse(void)
{
int len, i;
dumpData();
len = getLen(-1);
if (len < cMinimumSize)
return;
dumpData();
if (expectEventId)
{
/* DATA_BTN_EVENT can be wrapped to start */
int index = (RCVBufferEnd + DATA_BTN_EVENT) % BUFFERSIZE;
expectEventData = RCVBuffer[index];
expectEventId = 0;
}
dprintk(100, "event 0x%02x %d %d\n", expectEventData, RCVBufferStart, RCVBufferEnd);
if (expectEventData)
{
switch (expectEventData)
{
case EVENT_BTN:
{
/* no longkeypress for frontpanel buttons! */
len = getLen(cPackageSizeFP);
if (len == 0)
goto out_switch;
if (len < cPackageSizeFP)
goto out_switch;
dprintk(1, "EVENT_BTN complete\n");
if (paramDebug >= 50)
dumpData();
/* copy data */
for (i = 0; i < cPackageSizeFP; i++)
{
int from, to;
from = (RCVBufferEnd + i) % BUFFERSIZE;
to = KeyBufferStart % BUFFERSIZE;
KeyBuffer[to] = RCVBuffer[from];
KeyBufferStart = (KeyBufferStart + 1) % BUFFERSIZE;
}
wake_up_interruptible(&wq);
RCVBufferEnd = (RCVBufferEnd + cPackageSizeFP) % BUFFERSIZE;
}
break;
case EVENT_RC:
{
len = getLen(cPackageSizeRC);
if (len == 0)
goto out_switch;
if (len < cPackageSizeRC)
goto out_switch;
dprintk(1, "EVENT_RC complete %d %d\n", RCVBufferStart, RCVBufferEnd);
if (paramDebug >= 50)
dumpData();
/* copy data */
for (i = 0; i < cPackageSizeRC; i++)
{
int from, to;
from = (RCVBufferEnd + i) % BUFFERSIZE;
to = KeyBufferStart % BUFFERSIZE;
KeyBuffer[to] = RCVBuffer[from];
KeyBufferStart = (KeyBufferStart + 1) % BUFFERSIZE;
}
wake_up_interruptible(&wq);
RCVBufferEnd = (RCVBufferEnd + cPackageSizeRC) % BUFFERSIZE;
}
break;
case EVENT_ANSWER_GETTIME:
len = getLen(cGetTimeSize);
if (len == 0)
goto out_switch;
if (len < cGetTimeSize)
goto out_switch;
handleCopyData(len);
dprintk(20, "Pos. response received\n");
errorOccured = 0;
ack_sem_up();
RCVBufferEnd = (RCVBufferEnd + cGetTimeSize) % BUFFERSIZE;
break;
case EVENT_ANSWER_WAKEUP_REASON:
len = getLen(cGetWakeupReasonSize);
if (len == 0)
goto out_switch;
if (len < cGetWakeupReasonSize)
goto out_switch;
handleCopyData(len);
dprintk(1, "Pos. response received\n");
errorOccured = 0;
ack_sem_up();
RCVBufferEnd = (RCVBufferEnd + cGetWakeupReasonSize) % BUFFERSIZE;
break;
case EVENT_ANSWER_FRONTINFO:
case EVENT_ANSWER_GETIRCODE:
case EVENT_ANSWER_GETPORT:
default: // Ignore Response
dprintk(1, "Invalid Response %02x\n", expectEventData);
dprintk(1, "start %d end %d\n", RCVBufferStart, RCVBufferEnd);
dumpData();
/* discard all data, because this happens currently
* sometimes. dont know the problem here.
*/
RCVBufferEnd = RCVBufferStart;
break;
}
}
out_switch:
expectEventId = 1;
expectEventData = 0;
}
/**
* Runs the pedometer by polling the step counter.
*/
void PedometerLpExample(unsigned short platform,
unsigned short accelid,
unsigned short compassid,
int *handles,
int numHandles)
{
unsigned long lastStep = 0;
unsigned long lastWalkTime = 0;
unsigned int state = PED_POWER_LOW;
unsigned int pre_freeze_state = state;
int mode;
int exit = FALSE;
tMLError result;
struct mpuirq_data **data;
lastStep = 0;
stepCount = 0;
walkTime = 0;
MPL_LOGI("\n\nPerforming Pedometer\n\n");
MPL_LOGI("You should see:\n"
"\t"
"pedometer step count incrementing as steps are taken\n"
"\n");
MPL_LOGI("Loading example...\n");
MPL_LOGI("Select sensitivity:\n");
MPL_LOGI(" 0) Low (less steps)\n");
MPL_LOGI(" 1) Normal (more steps)\n");
MPL_LOGI(" 2) High (most steps)\n");
scanf("%d", &mode);
PrintPedometerMenu();
InitPedLowPower();
if (mode == 0) {
params.threshold = 30000000L;
params.minUpTime = 320; // 3.125 Hz maximum
params.maxUpTime = 1200; // 0.833 Hz minimum
params.minSteps = 5;
params.minEnergy = 0x10000000L;
params.maxStepBufferTime = 2000;
params.clipThreshold = 0x06000000L;
} else if (mode == 1) {
params.threshold = 25000000L;
params.minUpTime = 320; // 3.125 Hz maximum
params.maxUpTime = 1200; // 0.833 Hz minimum
params.minSteps = 5;
params.minEnergy = 0x0d000000L;
params.maxStepBufferTime = 2500;
params.clipThreshold = 0x06000000L;
} else {
params.threshold = 20000000L;
params.minUpTime = 200; // 5.00 Hz maximum
params.maxUpTime = 1500; // 0.66 Hz minimum
params.minSteps = 5;
params.minEnergy = 0x0a000000L;
params.maxStepBufferTime = 3000;
params.clipThreshold = 0x06000000L;
}
CHECK_WARNING(MLPedometerStandAloneSetParams(¶ms));
MPL_LOGI("Example Loaded\n");
MPL_LOGI("\n");
CHECK_WARNING(MLPedometerStandAloneSetNumOfSteps(0));
CHECK_WARNING(MLDmpPedometerStandAloneStart());
MPL_LOGI("\n");
MPL_LOGI("**** MPU LOW POWER ****\n");
state = PED_POWER_LOW;
pre_freeze_state = state;
//Loop until a key is hit
while (!exit) {
if (state == PED_POWER_LOW) {
CHECK_WARNING(MLPedometerStandAloneGetWalkTime(&walkTime));
result = MLPedometerStandAloneGetNumOfSteps(&stepCount);
if (result == ML_SUCCESS) {
if ((walkTime != lastWalkTime) || (stepCount != lastStep)) {
lastStep = stepCount;
lastWalkTime = walkTime;
MPL_LOGI("Step %6lu, Time %8.3f s\n",
lastStep,
(float)walkTime / 1000.);
}
}
// No Motion tracking
if (0) {
unsigned char data[16];
CHECK_WARNING(MLSLSerialRead(MLSerialGetHandle(),
0x68, MPUREG_23_RSVD, 6, data));
MPL_LOGI("%02x%02x, %02x%02x, %02x%02x\n",
data[0], data[1],
data[2], data[3],
data[4], data[5]);
}
}
data = InterruptPoll(handles, numHandles, 2, 50000);
/* handle system suspend/resume */
#ifdef LINUX
if (data[4]->interruptcount) {
unsigned long power_state = *((unsigned long *)data[4]->data);
if (power_state == MPU_PM_EVENT_SUSPEND_PREPARE &&
state == PED_POWER_FULL) {
pre_freeze_state = PED_POWER_FULL;
TransitionToLowPower(handles, numHandles);
} else if (power_state == MPU_PM_EVENT_POST_SUSPEND &&
pre_freeze_state == PED_POWER_FULL) {
TransitionToFullPower(handles, numHandles);
}
ioctl(handles[4], MPU_PM_EVENT_HANDLED, 0);
}
#endif
InterruptPollDone(data);
if (state == PED_POWER_SLEEP && MLDLGetIntTrigger(INTSRC_AUX1)) {
MPL_LOGI("\n");
MPL_LOGI("**** MPU LOW POWER ****\n");
CHECK_WARNING(MLPedometerStandAloneSetNumOfSteps(stepCount));
CHECK_WARNING(MLPedometerStandAloneSetWalkTime(walkTime));
CHECK_WARNING(MLDmpPedometerStandAloneStart());
MLDLClearIntTrigger(INTSRC_AUX1);
MLDLClearIntTrigger(INTSRC_MPU);
state = PED_POWER_LOW;
pre_freeze_state = state;
}
if (state == PED_POWER_LOW && MLDLGetIntTrigger(INTSRC_AUX1)) {
CHECK_WARNING(MLPedometerStandAloneGetWalkTime(&walkTime));
CHECK_WARNING(MLPedometerStandAloneGetNumOfSteps(&stepCount));
MPL_LOGI("\n");
MPL_LOGI("**** MPU SLEEP POWER ****\n");
CHECK_WARNING(MLDmpPedometerStandAloneStop());
MLDLClearIntTrigger(INTSRC_AUX1);
MLDLClearIntTrigger(INTSRC_MPU);
state = PED_POWER_SLEEP;
pre_freeze_state = state;
}
if (state == PED_POWER_LOW && MLDLGetIntTrigger(INTSRC_MPU)) {
CHECK_WARNING(MLUpdateData());
DumpDataLowPower();
MLDLClearIntTrigger(INTSRC_MPU);
}
if (state == PED_POWER_FULL &&
(MLDLGetIntTrigger(INTSRC_MPU) || MLDLGetIntTrigger(INTSRC_AUX1))) {
CHECK_WARNING(MLUpdateData());
dumpData();
MLDLClearIntTrigger(INTSRC_AUX1);
MLDLClearIntTrigger(INTSRC_MPU);
}
if(ConsoleKbhit()) {
char ch;
switch (state) {
case PED_POWER_SLEEP:
case PED_POWER_LOW:
/* Transition to full power */
ch = ConsoleGetChar();
switch (ch) {
case 'q':
exit = TRUE;
break;
case 'h':
PrintPedometerMenu();
break;
case 'g':
minSteps += 2;
case 'G':
--minSteps;
if (minSteps < 0)
minSteps = 0;
result = MLPedometerStandAloneSetStepBuffer(minSteps);
MPL_LOGI("MLPedometerStandAloneSetStepBuffer(%d)\n",
minSteps);
break;
case 'v':
minStepsTime += 400;
case 'V':
minStepsTime -= 200;
if (minStepsTime < 0)
minStepsTime = 0;
MLPedometerStandAloneSetStepBufferResetTime(minStepsTime);
MPL_LOGI(
"MLPedometerStandAloneSetStepBufferResetTime(%d)\n",
minStepsTime);
break;
case 'b':
flag ^= 0x20;
if (flag & 0x20) {
FIFOSendAccel(ML_ELEMENT_1 |
ML_ELEMENT_2 |
ML_ELEMENT_3,
ML_32_BIT);
MLSetFifoInterrupt(TRUE);
} else {
FIFOSendAccel(ML_ELEMENT_1 |
ML_ELEMENT_2 |
ML_ELEMENT_3,
0);
MLSetFifoInterrupt(FALSE);
}
break;
case '\n':
case '\r':
/* Ignore carrage return and line feeds */
break;
default:
TransitionToFullPower(handles,numHandles);
state = PED_POWER_FULL;
pre_freeze_state = state;
break;
};
break;
case PED_POWER_FULL:
default:
/* Transition to low power */
exit = ProcessKbhit();
if (exit) {
TransitionToLowPower(handles, numHandles);
state = PED_POWER_LOW;
pre_freeze_state = state;
exit = FALSE;
}
break;
};
}
}
if (state == PED_POWER_LOW) {
CHECK_WARNING(MLDmpPedometerStandAloneClose());
} else if (PED_POWER_FULL == state) {
CHECK_WARNING(MLDmpClose());
}
}
void spline::dumpRawData()
{
dumpData(rawPoints, "raw_data.dat");
}
void spline::dumpSpline()
{
dumpData(points, "spline.dat");
}
static void processResponse(void)
{
int len, i;
if (paramDebug >= 100)
dumpData();
if (expectEventId)
{
/* DATA_BTN_EVENT can be wrapped to start */
int index = (RCVBufferEnd + DATA_BTN_EVENT) % BUFFERSIZE;
expectEventData = RCVBuffer[index];
expectEventId = 0;
}
dprintk(100, "event 0x%02x\n", expectEventData);
if (expectEventData)
{
switch (expectEventData)
{
case EVENT_BTN:
{
len = getLen(cPackageSize);
if (len == 0)
goto out_switch;
if (len < cPackageSize)
goto out_switch;
dprintk(1, "EVENT_BTN complete\n");
if (paramDebug >= 50)
dumpData();
/* copy data */
for (i = 0; i < cPackageSize; i++)
{
int from, to;
from = (RCVBufferEnd + i) % BUFFERSIZE;
to = KeyBufferStart % BUFFERSIZE;
KeyBuffer[to] = RCVBuffer[from];
KeyBufferStart = (KeyBufferStart + 1) % BUFFERSIZE;
}
wake_up_interruptible(&wq);
RCVBufferEnd = (RCVBufferEnd + cPackageSize) % BUFFERSIZE;
}
break;
case EVENT_RC:
{
len = getLen(cPackageSize);
if (len == 0)
goto out_switch;
if (len < cPackageSize)
goto out_switch;
dprintk(1, "EVENT_RC complete\n");
dprintk(1, "start %d end %d\n", RCVBufferStart, RCVBufferEnd);
if (paramDebug >= 50)
dumpData();
/* copy data */
for (i = 0; i < cPackageSize; i++)
{
int from, to;
from = (RCVBufferEnd + i) % BUFFERSIZE;
to = KeyBufferStart % BUFFERSIZE;
KeyBuffer[to] = RCVBuffer[from];
KeyBufferStart = (KeyBufferStart + 1) % BUFFERSIZE;
}
wake_up_interruptible(&wq);
RCVBufferEnd = (RCVBufferEnd + cPackageSize) % BUFFERSIZE;
}
break;
case EVENT_ERR:
{
len = getLen(-1);
if (len == 0)
goto out_switch;
dprintk(1, "Neg. response received\n");
/* if there is a waiter for an acknowledge ... */
errorOccured = 1;
ack_sem_up();
/* discard all data */
RCVBufferEnd = (RCVBufferEnd + len) % BUFFERSIZE;
}
break;
case EVENT_OK1:
case EVENT_OK2:
{
len = getLen(-1);
if (len == 0)
goto out_switch;
dprintk(20, "EVENT_OK1/2: Pos. response received\n");
/* if there is a waiter for an acknowledge ... */
errorOccured = 0;
ack_sem_up();
RCVBufferEnd = (RCVBufferEnd + len) % BUFFERSIZE;
}
break;
case EVENT_ANSWER_GETTIME:
len = getLen(cGetTimeSize);
if (len == 0)
goto out_switch;
if (len < cGetTimeSize)
goto out_switch;
handleCopyData(len);
/* if there is a waiter for an acknowledge ... */
dprintk(20, "EVENT_ANSWER_GETTIME: Pos. response received\n");
errorOccured = 0;
ack_sem_up();
RCVBufferEnd = (RCVBufferEnd + cGetTimeSize) % BUFFERSIZE;
break;
case EVENT_ANSWER_WAKEUP_REASON:
len = getLen(cGetWakeupReasonSize);
if (len == 0)
goto out_switch;
if (len < cGetWakeupReasonSize)
goto out_switch;
handleCopyData(len);
/* if there is a waiter for an acknowledge ... */
dprintk(1, "EVENT_ANSWER_WAKEUP_REASON: Pos. response received\n");
errorOccured = 0;
ack_sem_up();
RCVBufferEnd = (RCVBufferEnd + cGetWakeupReasonSize) % BUFFERSIZE;
break;
case EVENT_ANSWER_VERSION:
len = getLen(cGetVersionSize);
if (len == 0)
goto out_switch;
if (len < cGetVersionSize)
goto out_switch;
handleCopyData(len);
/* if there is a waiter for an acknowledge ... */
dprintk(1, "EVENT_ANSWER_VERSION: Pos. response received\n");
errorOccured = 0;
ack_sem_up();
RCVBufferEnd = (RCVBufferEnd + cGetVersionSize) % BUFFERSIZE;
break;
default: // Ignore Response
dprintk(1, "Invalid Response %02x\n", expectEventData);
dprintk(1, "start %d end %d\n", RCVBufferStart, RCVBufferEnd);
dumpData();
/* discard all data, because this happens currently
* sometimes. dont know the problem here.
*/
RCVBufferEnd = RCVBufferStart;
break;
}
}
out_switch:
expectEventId = 1;
expectEventData = 0;
}
static void processResponse(void)
{
int len, i;
//dumpData();
len = getLen(-1);
if (len < cMinimumSize)
return;
//dumpData();
if (expectEventId)
{
expectEventData = RCVBuffer[RCVBufferEnd];
expectEventId = 0;
}
dprintk(100, "event 0x%02x %d %d\n", expectEventData, RCVBufferStart, RCVBufferEnd);
if (expectEventData)
{
switch (expectEventData)
{
case _MCU_KEYIN:
{
len = getLen(cPackageSizeKeyIn);
if (len == 0)
goto out_switch;
if (len < cPackageSizeKeyIn)
goto out_switch;
dprintk(1, "_MCU_KEYIN complete\n");
if (paramDebug >= 50)
dumpData();
/* copy data */
for (i = 0; i < cPackageSizeKeyIn; i++)
{
int from, to;
from = (RCVBufferEnd + i) % BUFFERSIZE;
to = KeyBufferStart % BUFFERSIZE;
KeyBuffer[to] = RCVBuffer[from];
KeyBufferStart = (KeyBufferStart + 1) % BUFFERSIZE;
}
//printk("_MCU_KEYIN complete - %02x\n", RCVBuffer[(RCVBufferEnd+2)%BUFFERSIZE]);
wake_up_interruptible(&wq);
RCVBufferEnd = (RCVBufferEnd + cPackageSizeKeyIn) % BUFFERSIZE;
}
break;
case _MCU_VERSION:
len = getLen(cGetVersionSize);
if (len == 0)
goto out_switch;
if (len < cGetVersionSize)
goto out_switch;
handleCopyData(len);
dprintk(20, "Pos. response received\n");
errorOccured = 0;
ack_sem_up();
RCVBufferEnd = (RCVBufferEnd + cGetVersionSize) % BUFFERSIZE;
break;
case _MCU_TIME:
len = getLen(cGetTimeSize);
if (len == 0)
goto out_switch;
if (len < cGetTimeSize)
goto out_switch;
handleCopyData(len);
dprintk(20, "Pos. response received\n");
errorOccured = 0;
ack_sem_up();
RCVBufferEnd = (RCVBufferEnd + cGetTimeSize) % BUFFERSIZE;
break;
case _MCU_WAKEUPREASON:
len = getLen(cGetWakeupReasonSize);
if (len == 0)
goto out_switch;
if (len < cGetWakeupReasonSize)
goto out_switch;
handleCopyData(len);
dprintk(1, "Pos. response received\n");
errorOccured = 0;
ack_sem_up();
RCVBufferEnd = (RCVBufferEnd + cGetWakeupReasonSize) % BUFFERSIZE;
break;
case _MCU_RESPONSE:
len = getLen(cGetResponseSize);
if (len == 0)
goto out_switch;
if (len < cGetResponseSize)
goto out_switch;
handleCopyData(len);
dprintk(1, "Pos. response received\n");
errorOccured = 0;
ack_sem_up();
RCVBufferEnd = (RCVBufferEnd + cGetResponseSize) % BUFFERSIZE;
break;
default: // Ignore Response
dprintk(1, "Invalid Response %02x\n", expectEventData);
dprintk(1, "start %d end %d\n", RCVBufferStart, RCVBufferEnd);
dumpData();
/* discard all data, because this happens currently
* sometimes. dont know the problem here.
*/
RCVBufferEnd = RCVBufferStart;
break;
}
}
out_switch:
expectEventId = 1;
expectEventData = 0;
}
int main(int argc, char **argv)
{
MLan *mlan=NULL;
uchar serial[MLAN_SERIAL_SIZE];
char *serial_in=NULL;
int ch;
int flag=NO_FLAG;
char *cmd=NULL, *filename=NULL;
/* save the command */
cmd=argv[0];
while( (ch=getopt(argc, argv, "rwde")) != -1) {
switch(ch) {
case 'r':
if(flag!=NO_FLAG) {
fprintf(stderr, "Flag already given.\n");
usage(cmd);
}
flag=READ_FLAG;
break;
case 'w':
if(flag!=NO_FLAG) {
fprintf(stderr, "Flag already given.\n");
usage(cmd);
}
flag=WRITE_FLAG;
break;
case 'd':
if(flag!=NO_FLAG) {
fprintf(stderr, "Flag already given.\n");
usage(cmd);
}
flag=DUMP_FLAG;
break;
case 'e':
if(flag!=NO_FLAG) {
fprintf(stderr, "Flag already given.\n");
usage(cmd);
}
flag=ERASE_FLAG;
break;
case '?':
usage(cmd);
}
}
/* Adjust the argv and argc */
argc-=optind;
argv+=optind;
if(flag==NO_FLAG) {
fprintf(stderr, "No flag given given.\n");
usage(cmd);
}
if(argc<1) {
fprintf(stderr, "Need a serial number.\n");
exit(1);
}
serial_in=argv[0];
/* Get the right stuff if we're doing a read or write operation */
if(flag == READ_FLAG || flag == WRITE_FLAG) {
if(argc<2) {
fprintf(stderr, "Need a filename.\n");
usage(cmd);
}
filename=argv[1];
}
/* mlan=mlan_init(dev, PARMSET_9600); */
mlan=mlan_init(mlan_get_port(), PARMSET_115200);
assert(mlan);
mlan->debug=0;
mlan->parseSerial(mlan, serial_in, serial);
if(mlan->ds2480detect(mlan)!=TRUE) {
fprintf(stderr, "Found no DS2480\n");
exit(-1);
}
/* Was there a file argument? */
if(flag==WRITE_FLAG) {
printf("Writing...");
fflush(stdout);
putFile(mlan, serial, filename);
printf("done.\n");
} else if(flag==READ_FLAG) {
printf("Reading...");
fflush(stdout);
getFile(mlan, serial, filename);
printf("done.\n");
} else if(flag==DUMP_FLAG) {
dumpData(mlan, serial);
} else if(flag==ERASE_FLAG) {
printf("Erasing...");
fflush(stdout);
erase(mlan, serial);
printf("done.\n");
} else {
printf("How did you get here?\n");
}
mlan->destroy(mlan);
exit(0);
}
bool GlobFit::solve(std::vector<RelationEdge>& vecRelationEdge, RelationEdge::RelationEdgeType currentStage, const std::string& stageName)
{
// dump data to file for debugging in matlab
dumpData(vecRelationEdge, stageName);
size_t nConstraintNum = vecRelationEdge.size();
std::string optimization;
if (currentStage < RelationEdge::RET_COAXIAL) {
optimization = "OptimizeNormal";
std::cout << "Optimize Normal..." << std::endl;
} else if (currentStage < RelationEdge::RET_COPLANAR) {
optimization = "OptimizePoint";
std::cout << "Optimize Point..." << std::endl;
} else if (currentStage < RelationEdge::RET_EQUAL_RADIUS) {
optimization = "OptimizeDistance";
std::cout << "Optimize Distance..." << std::endl;
} else {
optimization = "OptimizeRadius";
std::cout << "Optimize Radius..." << std::endl;
}
if (nConstraintNum == 0)
{
std::cout << "Empty constraint set." << std::endl;
return true;
}
size_t numPrimitives = _vecPrimitive.size();
mxArray* inputParameters = mxCreateDoubleMatrix(numPrimitives, Primitive::getNumParameter(), mxREAL);
double* pInputParameters = mxGetPr(inputParameters);
for (size_t i = 0; i < numPrimitives; ++i) {
Primitive* pPrimitive = _vecPrimitive[i];
pPrimitive->prepareParameters();
for (size_t j = 0; j < Primitive::getNumParameter(); ++ j) {
pInputParameters[j*numPrimitives+i] = pPrimitive->getParameter(j);
}
}
engPutVariable(matlabEngine, "inputParameters", inputParameters);
mxArray* constraints = mxCreateNumericMatrix(nConstraintNum, RelationEdge::getNumParameter(), mxINT32_CLASS, mxREAL);
int* pConstraints = (int*)mxGetData(constraints);
for (size_t i = 0; i < nConstraintNum; ++i) {
vecRelationEdge[i].dumpData(pConstraints, nConstraintNum, i);
}
engPutVariable(matlabEngine, "constraints", constraints);
std::string path = boost::filesystem::current_path().string();
path = "cd "+path+"/matlab;";
engEvalString(matlabEngine, path.c_str());
size_t szOutputBuffer = 65536;
char* matlabOutputBuffer = new char[szOutputBuffer];
engOutputBuffer(matlabEngine, matlabOutputBuffer, szOutputBuffer);
std::string output = "[outputParameters, initialFittingError, exitFittingError, exitFlag]";
std::string input = "(inputParameters, maxIterNum, numVertices, primitiveType, coordX, coordY, coordZ, normalX, normalY, normalZ, confVertices, constraints);";
std::string command = output+"="+optimization+input;
engEvalString(matlabEngine, command.c_str());
matlabOutputBuffer[szOutputBuffer - 1] = '\0';
printf("%s\n", matlabOutputBuffer);
engOutputBuffer(matlabEngine, NULL, 0);
delete[] matlabOutputBuffer;
mxArray* outputParameters = engGetVariable(matlabEngine, "outputParameters");
double *pOutputParameters = mxGetPr(outputParameters);
mxArray* initialFittingError = engGetVariable(matlabEngine, "initialFittingError");
double *pInitialFittingError = mxGetPr(initialFittingError);
mxArray* exitFittingError = engGetVariable(matlabEngine, "exitFittingError");
double *pExitFittingError = mxGetPr(exitFittingError);
mxArray* exitFlag = engGetVariable(matlabEngine, "exitFlag");
double *pExitFlag = mxGetPr(exitFlag);
bool bValidOptimization = (*pExitFlag >= 0);
// posterior check: consider invalid if fitting error increased too much
// however, if the threshold is very big, the fitting error may increase a lot
// so, be careful with this
bValidOptimization &= (*pExitFittingError < 10*(*pInitialFittingError));
if (!bValidOptimization) {
mxDestroyArray(constraints);
mxDestroyArray(inputParameters);
mxDestroyArray(outputParameters);
mxDestroyArray(initialFittingError);
mxDestroyArray(exitFittingError);
mxDestroyArray(exitFlag);
std::cout << "No feasible solution found." << std::endl;
return false;
}
// update primitives
for (size_t i = 0; i < numPrimitives; ++ i) {
Primitive* pPrimitive = _vecPrimitive[i];
for (size_t j = 0; j < Primitive::getNumParameter(); ++ j) {
pPrimitive->setParameter(j, pOutputParameters[j*numPrimitives+i]);
}
pPrimitive->applyParameters();
}
// destroy matrix
mxDestroyArray(constraints);
mxDestroyArray(inputParameters);
mxDestroyArray(outputParameters);
mxDestroyArray(initialFittingError);
mxDestroyArray(exitFittingError);
mxDestroyArray(exitFlag);
return true;
}
