From 74662206d2031c6dcbdb1a1969985defb71adf21 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 8 Dec 2016 19:40:50 -0500 Subject: Update code to fix timing issues. --- src/survive_data.c | 446 +++++++++------------------------------------ tools/avr_codegen/test.c | 16 +- tools/avr_codegen/test.elf | Bin 7040 -> 6960 bytes tools/avr_codegen/test.hex | 20 +- 4 files changed, 110 insertions(+), 372 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index a2e4209..f5d0e6b 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -18,35 +18,6 @@ #define POP2 (*(((uint16_t*)((readdata+=2)-2)))) #define POP4 (*(((uint32_t*)((readdata+=4)-4)))) -//Pass pointer to end-of-string -int ARCANEPOP( uint8_t ** end, uint8_t * start ) -{ - //78 - //a8 03 - uint8_t tnum; - uint32_t accumulator; - - tnum = (*end)[0]; - accumulator = (tnum&0x7f); - (*end)--; - if( ( tnum & 0x80 ) || ( *end == start ) ) return accumulator; - - tnum = (*end)[0]; - accumulator = (accumulator<<7)|(tnum&0x7f); - (*end)--; - if( ( tnum & 0x80 ) || ( *end == start ) ) return accumulator; - - tnum = (*end)[0]; - (*end)--; - accumulator = (accumulator<<7)|(tnum&0x7f); - if( ( tnum & 0x80 ) || ( *end == start ) ) return accumulator; - - tnum = (*end)[0]; - (*end)--; - accumulator = (accumulator<<7)|(tnum&0x7f); - return accumulator; -} - struct LightcapElement { @@ -198,47 +169,9 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) qty++; readdata--; *readdata = type; - //Put type back on stack. It might have changed - //from above. - - -#if 0 //Doesn't work!!!! - int reads = qty/6; - - int leds[6]; - leds[0] = type; - for( i = 1; i < reads; i++ ) - { - leds[i] = POP1; - } - for( i = 0; i < reads; i++ ) - { - printf( "%02x: ", leds[i] ); - for( i = 0; i < 5; i++ ) - { - printf( "%02x ", POP1 ); - } - printf( "\n" ); - } -#endif - - //What does post data look like? - - //1) code, code, code, PAAYYLOOAAADDD - //"code" typically has 2 or fewer bits in word set. - // code = 0x00 = expect 5 bytes - // code = 0x08 = expect 5 bytes - // code = 0x10 = expect 5 bytes - // code = 0x5c = ???? ... 12? - // code = 0x91 = ???? ... 25 bytes? - #if 1 - static int lasttime; // good good maybe? probably wrong - int mytimex = (time1<<24)|(time2<<16)|(readdata[4]<<8)|(readdata[5]); - int diff = mytimex - lasttime; - lasttime = mytimex; printf( "POST %d: %4d (%02x%02x) - ", propset, qty, time1, time2 ); for( i = 0; i < qty + 4; i++ ) { @@ -246,322 +179,115 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) } printf("\n"); #endif - //XXX XXX XXX This code is awful. Rejigger to make go fast. - uint8_t * end = &readdata[qty-4]; - uint8_t * start = readdata; - - //XXX TODO: Do something clever here to get the MSB for the timecode. Can do by - //looking at the difference of the msb of the wrong packet and last OR difference from this - //and IMU timecode. - uint32_t imutime = (time1<<24) | (time2<<16); - uint32_t mytime = (end[1] << 0)|(end[2] << 8)|(end[3] << 16); - mytime |= (time1<<24); - int32_t tdiff = mytime - imutime; - if( tdiff > 0x7fffff ) { mytime -= 0x01000000; } - if( tdiff < -0x7fffff ) { mytime += 0x01000000; } - - uint32_t parameters[25]; - int parplace = 0; - - while( end != start ) - { - uint32_t acpop = ARCANEPOP( &end, start ); - parameters[parplace++] = acpop; - int remain = end - start + 1; - if( (parplace+1) / 2 >= remain ) break; - } - uint32_t lights[10]; - int lightno = 0; + uint8_t * end = &readdata[qty]; + uint32_t mytime = (end[1] << 0)|(end[2] << 8)|(end[3] << 16); - while( end >= start ) - { - lights[lightno++] = end[0]; - end--; - } + uint32_t times[20]; + uint32_t time; + const int nrtime = sizeof(times)/sizeof(uint32_t); + int timecount = 0; + int leds; + int parameters; + int fault = 0; - if( (parplace+1)/2 != lightno ) + //First, pull off the times, starting with the current time, then all the delta times going backwards. { - struct SurviveContext * ctx = w->ctx; - SV_INFO( "Watchman code count doesn't match" ); - return; - } -#if 1 - //This is getting very close. + uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to advance. + uint32_t imutime = (time1<<24) | (time2<<16); + time = mptr[3]<<16 | mptr[2]<<8 | mptr[1]; + time |= (time1<<24); + int32_t tdiff = time - imutime; + if( tdiff > 0x7fffff ) { time -= 0x01000000; } + if( tdiff < -0x7fffff ) { time += 0x01000000; } - const int M_LEDS = 32; - uint8_t onleds[M_LEDS]; - uint32_t offtimes[M_LEDS]; - memset( onleds, 0, sizeof( onleds ) ); - int k = 0; - for( i = lightno-1; i >= 0; i-- ) - { - int led = lights[i]>>3; - int ledtime = lights[i]&0x07; - int deltaA = k?parameters[k*2-1]:0; - int deltaB = parameters[k*2+0]; - k++; + times[timecount++] = time; - onleds[led] = ledtime+1; - offtimes[led] = mytime; - printf( "%d %d %d %d\n", led, ledtime, deltaA, deltaB ); - - if( deltaA ) //XXX TODO: This should just check to see if k != 0 + while( mptr - readdata > (timecount>>1) ) { - mytime -= deltaA; - - for( j = 0; j < M_LEDS; j++ ) + uint32_t arcane_value = 0; + //ArcanePop (Pop off values from the back, forward, checking if the MSB is set) + do { - if( onleds[j] ) - { - onleds[j]--; - if( !onleds[j] ) - { - //Got a light event. - struct LightcapElement le; - le.type = 0xfe; - le.sensor_id = j; - le.timestamp = mytime; - if( offtimes[j] - mytime > 65535 ) - { - printf( "OFLOW: LED %d @ %d, len %d\n", j, mytime, offtimes[j] - mytime ); - } - else - { - le.length = offtimes[j] - mytime; - handle_lightcap( w, &le ); - printf( "Light Event: LED %d @ %d, len %d\n", j, mytime, le.length ); - } - } - } - } + uint8_t ap = *(mptr--); + arcane_value |= (ap&0x7f); + if( ap & 0x80 ) break; + arcane_value <<= 7; + } while(1); + times[timecount++] = (time -= arcane_value); } - mytime -= deltaB; - for( j = 0; j < M_LEDS; j++ ) - { - if( onleds[j] ) - { - onleds[j]--; - if( !onleds[j] ) - { - //Got a light event. - struct LightcapElement le; - le.type = 0xfe; - le.sensor_id = j; - le.timestamp = mytime; - if( offtimes[j] - mytime > 65535 ) - { - printf( "OFLOW: LED %d @ %d, len %d\n", j, mytime, offtimes[j] - mytime ); - } - else - { - le.length = offtimes[j] - mytime; - handle_lightcap( w, &le ); - printf( "Light Event: LED %d @ %d, len %d\n", j, mytime, le.length ); - } - } - } - } - } - for( j = 0; j < M_LEDS; j++ ) - { - if( onleds[j] ) - { - fprintf(stderr, "ERROR: Too many LEDs found (%d = %d)\n", j, onleds[j] ); - } + leds = timecount>>1; + //Check that the # of sensors at the beginning match the # of parameters we would expect. + if( timecount & 1 ) { fault = 1; goto end; } //Inordinal LED count + if( leds != mptr - readdata + 1 ) { fault = 2; goto end; } //LED Count does not line up with parameters } -#endif + struct LightcapElement les[10]; + int lese = 0; //les's end -/* - static int olddel = 0; - int lightmask = lights[0]; - //Operting on mytime - printf( "Events (%10d): ", mytime ); - for( i = 0; i < lightno; i++ ) + //Second, go through all LEDs and extract the lightevent from them. { - int deltaA = parameters[parplace-i*2-1]; - int deltaB = -1; - if( parplace-i*2-2 >= 0 ) - deltaB = parameters[parplace-i*2-2]; - - printf( "%02x/(%5d/%5d)", lightmask, deltaA, deltaB ); - if( lightno-i-2 >= 0 ) - lightmask = lights[i+1]; - } - printf( "\n"); - -*/ - //What I think I know: - // 2 pulses: mytime pulse? deltime? deltime? pulse? deltime? - // 3 pulses: mytime pulse? deltime? deltime? pulse? deltime? deltime? pulse deltime? - // Maybe pulse codes are change in masks? - //One light on then off. Events (1358180290): 50/( 112/ 1714)50/( 107/ -1) - // LED 50 / LED 60 - // 5a/(7 >< 90)60/( 7 <> 1719)5a/(7/90)60/(7/-1) - // 50 then - // 7 later) 60 on - // 90 later, 60 only - // 7 later, 60 off. - // 1719 later, 50 on - // 7 later, 60 on. - // 90 later, 50 off - // 7 later, 60 off. - - //Notes: All LEDs are divisble by 8. - //That leaves us 3 bits earlier in the message. - // - // - - //Assumptions: Bit 0 1 -//XXX DISCARD BETWEEN HERE - //More details. Example: - // 40 only / 40 and 48 / 48 only - // 49/( 139/ 102)41/( 130/ -1) - // 48 only / 48 and 40 / 40 only - // 41/( 129/ 102)49/( 132/ -1) - // - // 00 then 08+00 then 08 only - // 09/( 133/ 100)01/( 135/ -1) - // 08 then 08+00 then 00 only - // 01/( 133/ 102)09/( 133/ -1) - // - // 10 then 10+00+08 then 00+08 - // 0a/( 107/ 20)73/( 93/ 17)11/( 132/ -1) - // 08+00 then 10+08+00 then 10 - // 6a/( 34/ 35)12/( 86/ 125)0a/( 20/ -1) - // - // 08 then 10+08 then 10 - // 11/( 136/ 97)09/( 136/ -1) - - // 08 then nothing then 10 - // 10/( 104/ 161)08/( 102/ -1) - // 10/( 105/ 425)08/( 102/ -1) (LONGER DELAY BETWEEN) - // 10/( 369/ 423)08/( 102/ -1) (LONGER FIRST PULSE) - // 40 -> 48 -> 50 50/( 523/ 2260)48/( 520/ 2260)40/( 873/ -1) -//AND HERE!!! XXX DISCARD - - - - //I think the format is: - // mytime = start of events - // mask = LEDs on. - // parameter = delta - // mask = Led mask switching -/* - static int olddel = 0; - int lightmask = lights[0]; - - // - // - // - - //Operting on mytime - printf( "Events (%10d): ", mytime ); - for( i = 0; i < lightno; i++ ) - { - int deltaA = parameters[parplace-i*2-1]; - int deltaB = -1; - if( parplace-i*2-2 >= 0 ) - deltaB = parameters[parplace-i*2-2]; - - printf( "%02x/(%5d/%5d)", lightmask, deltaA, deltaB ); - if( lightno-i-2 >= 0 ) - lightmask = lights[i+1]; + uint8_t marked[nrtime]; + memset( marked, 0, sizeof( marked ) ); + int i, parpl = 0; + timecount--; + int timepl = 0; + + //This works, but usually returns the values in reverse end-time order. + for( i = 0; i < leds; i++ ) + { + int led = readdata[i]; + int adv = led & 0x07; + led >>= 3; + + while( marked[timepl] ) timepl++; + if( timepl > timecount ) { fault = 3; goto end; } //Ran off max of list. + uint32_t endtime = times[timepl++]; + int end = timepl + adv; + if( end > timecount ) { fault = 4; goto end; } //end referencing off list + if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used. + uint32_t starttime = times[end]; + marked[end] = 1; + //printf( "LED: %d (adv %d) / st: %d en: %d len: %d / TC: %d\n", led, adv, starttime, endtime, endtime-starttime, end ); + + + //Insert all lighting things into a sorted list. This list will be + //reverse sorted, but that is to minimize operations. To read it + //in sorted order simply read it back backwards. + //Use insertion sort, since we should most of the time, be in order. + struct LightcapElement * le = &les[lese++]; + le->sensor_id = led; + le->type = 0xfe; + + if( (uint32_t)(endtime - starttime) > 65535 ) { fault = 6; goto end; } //Length of pulse dumb. + le->length = endtime - starttime; + le->timestamp = starttime; + int swap = lese-2; + while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp ) + { + struct LightcapElement l; + memcpy( &l, &les[swap], sizeof( l ) ); + memcpy( &les[swap], &les[swap+1], sizeof( l ) ); + memcpy( &les[swap+1], &l, sizeof( l ) ); + swap--; + } + } } - printf( "\n"); -*/ - //Three distinct motions. - // 40 nothing (long wait) 48 nothing 50 nothing (long pulse) - // 40/( 524/ 2262)48/( 522/ 570)50/( 874/ -1) - // 40, 40+48, 48 only. - // 41/( 529/ 517)49/( 875/ -1) - // 48, 48+40, 40 only - // 49/( 525/ 518)41/( 877/ -1) - // 48 then 48+40 then 48 only //XXX SERIOUSLY???? - // 40/( 526/ 524)4a/( 869/ -1) - // 48 then 48+50 then 50 only. - // 49/( 528/ 518)51/( 881/ -1) - // 50 then 48+50 then 48 only. - // 51/( 528/ 520)49/( 523/ -1) - // 48 then 48+50 then 50 only. - // 49/( 527/ 518)51/( 532/ -1) - // 48 then 48+50 then 48 only //INCORRECT BELOW HERE XXX DO NOT TRUST!!! - // 50/( 527/ 524)4a/( 515/ -1) //No, really... wat? - // 50 then 48+50 then 50 only - // 48/( 529/ 521)52/( 524/ -1) // oh come on! - // 50 then 48+50 then 50 only then 50+40 then 50 only. - // 40/( 5364/ 2292)50/( 5363/283869)48/( 526/ 521)52/( 522/ -1) - - - - - -/* - for( i = 0; i < lightno; i++ ) + int i; + for( i = lese-1; i >= 0; i-- ) { - struct LightcapElement le; - le.sensor_id = lights[lightno-i-1]; - le.type = 0xfe; - le.length = parameters[parplace-i*2-1]; - le.timestamp = mytime; - handle_lightcap( w, &le ); - - int delta = le.timestamp - olddel; - olddel = le.timestamp; - - printf( "LIGHTCAP: %02x %3d %10lu %d\n", le.sensor_id, le.length, le.timestamp, delta ); - - int pl = parplace-i*2-2; - if( pl>=0 ) - mytime += parameters[parplace-i*2-2]; + printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp ); } -*/ - -#if 0 - printf( " SRR: " ); - for( i = 0; i < 29; i++ ) - { - printf( "%02x ", startread[i] ); - } - printf( "\n" ); -#endif + return; + end: + printf( "FAULT: %d\n", fault ); } - - return; - //NO, seriously, there is something wacky going on here. -// else if( type == 0xe8 && sensor_id == 15 ) -// { -// printf( "IMU\n" ); -// } - -/* - { - printf( "PSIDPIN:%3d ", qty ); - w->charging = readdata[0]>>7; - w->charge = readdata[0]&0x7f; - w->ison = 1; - printf( "%02x/%02x/%02x/%02x: ", type, qty, time1, time2 ); - for( i = 0; i < 25; i++ ) - { - printf( "%02x ", readdata[i] ); - } - printf("\n"); - -/* - //WHAT IS THIS??? - printf( "%02x/%02x/%02x/%02x: ", type, sensor_id, time1, time2 ); - for( i = 0; i < 25; i++ ) - { - printf( "%02x ", readdata[i] ); - } - printf("\n");*/ } diff --git a/tools/avr_codegen/test.c b/tools/avr_codegen/test.c index d245e16..89829a2 100644 --- a/tools/avr_codegen/test.c +++ b/tools/avr_codegen/test.c @@ -71,7 +71,7 @@ int main( ) #define DO_MARKER( time, LEDS ) \ marker = time; do { PORTB = LEDS; marker--; PORTB = 0; } while( marker ); -#if 1 //this breaks it? +#if 0 //this breaks it? DO_MARKER(20, LED50); DO_MARKER(20, LED50|LED48); DO_MARKER(20, LED48); @@ -87,6 +87,20 @@ int main( ) DO_MARKER(20, LED40); #endif +#if 1 +DO_MARKER(30, LED48); +DO_MARKER(20, LED48|LED40|LED50); +DO_MARKER(20, LED48|LED40); +DO_MARKER(30, LED48); +DO_MARKER(20, LED48|LED40); +DO_MARKER(20, LED48|LED50); +DO_MARKER(30, LED48); +DO_MARKER(20, LED48|LED40); +DO_MARKER(20, LED48|LED40|LED50); +DO_MARKER(30, LED48); + +#endif + #if 0 DO_MARKER(10, LED50); DO_MARKER(50, LED50|LED48); diff --git a/tools/avr_codegen/test.elf b/tools/avr_codegen/test.elf index 3d55c28..56a580f 100755 Binary files a/tools/avr_codegen/test.elf and b/tools/avr_codegen/test.elf differ diff --git a/tools/avr_codegen/test.hex b/tools/avr_codegen/test.hex index 2038204..c89f46a 100644 --- a/tools/avr_codegen/test.hex +++ b/tools/avr_codegen/test.hex @@ -1,18 +1,16 @@ :100000000EC015C014C013C012C011C010C00FC064 :100010000EC00DC00CC00BC00AC009C008C011241E -:100020001FBECFE5D2E0DEBFCDBF1AD06EC0E8CF95 +:100020001FBECFE5D2E0DEBFCDBF1AD05EC0E8CFA5 :100030000F931F9300E010E0980106171707280799 :10004000390759F0EFECF7E03197F1F700C0000005 :100050000F5F1F4F2F4F3F4FF0CF1F910F9108950C :10006000F89480E886BD16BC82E481BF8AE187BB34 -:1000700032E04AE058E088E190E124E138BB18BA68 -:100080002150E1F724E148BB18BA2150E1F724E1FF -:1000900058BB18BA2150E1F724E188BB18BA2150A7 -:1000A000E1F724E198BB18BA2150E1F724E188BBBD -:1000B00018BA2150E1F724E198BB18BA2150E1F7B2 -:1000C00024E188BB18BA2150E1F724E198BB18BAA3 -:1000D0002150E1F724E188BB18BA2150E1F724E16F -:1000E00098BB18BA2150E1F724E188BB18BA215017 -:1000F000E1F724E198BB18BA2150E1F7EFEFF9EFEF -:0E0100003197F1F700C00000B8CFF894FFCFA0 +:1000700088E03AE198E14AE02EE188BB18BA2150C5 +:10008000E1F724E138BB18BA2150E1F724E198BB2D +:1000900018BA2150E1F72EE188BB18BA2150E1F7D8 +:1000A00024E198BB18BA2150E1F724E148BB18BA03 +:1000B0002150E1F72EE188BB18BA2150E1F724E185 +:1000C00098BB18BA2150E1F724E138BB18BA215087 +:1000D000E1F72EE188BB18BA2150E1F7EFEFF9EF15 +:0E00E0003197F1F700C00000C7CFF894FFCFB2 :00000001FF -- cgit v1.2.3