EMOKIT_DECLSPEC int emokit_get_next_frame(emokit_device* s) { memset(s->raw_unenc_frame, 0, 32); if(emokit_get_next_raw(s) >= 0) { s->current_frame.F3 = get_level(s->raw_unenc_frame, F3_MASK); s->current_frame.FC6 = get_level(s->raw_unenc_frame, FC6_MASK); s->current_frame.P7 = get_level(s->raw_unenc_frame, P7_MASK); s->current_frame.T8 = get_level(s->raw_unenc_frame, T8_MASK); s->current_frame.F7 = get_level(s->raw_unenc_frame, F7_MASK); s->current_frame.F8 = get_level(s->raw_unenc_frame, F8_MASK); s->current_frame.T7 = get_level(s->raw_unenc_frame, T7_MASK); s->current_frame.P8 = get_level(s->raw_unenc_frame, P8_MASK); s->current_frame.AF4 = get_level(s->raw_unenc_frame, AF4_MASK); s->current_frame.F4 = get_level(s->raw_unenc_frame, F4_MASK); s->current_frame.AF3 = get_level(s->raw_unenc_frame, AF3_MASK); s->current_frame.O2 = get_level(s->raw_unenc_frame, O2_MASK); s->current_frame.O1 = get_level(s->raw_unenc_frame, O1_MASK); s->current_frame.FC5 = get_level(s->raw_unenc_frame, FC5_MASK); // testing code for signal levels... ignore for now if((s->raw_unenc_frame[0] >= 0 && s->raw_unenc_frame[0] <= 15)/* || (s->raw_unenc_frame[0] >= 64 && s->raw_unenc_frame[0] <= 80)*/) { unsigned char lvl = (unsigned char) get_level_normal(s->raw_unenc_frame, 112, 119); //printf("%d LVL: %d\n", s->raw_unenc_frame[0], lvl); // TODO: this can probably be optimized to simply use an offset, but for now, I don't give a f**k // TODO: verify that these actually correspond correctly to the sensors! I'm sure they don't... switch(s->raw_unenc_frame[0]) { case 0: s->current_frame.cq.F3 = lvl; break; case 1: s->current_frame.cq.FC6 = lvl; break; case 2: s->current_frame.cq.P7 = lvl; break; case 3: s->current_frame.cq.T8 = lvl; break; case 4: s->current_frame.cq.F7 = lvl; break; case 5: s->current_frame.cq.F8 = lvl; break; case 6: s->current_frame.cq.T7 = lvl; break; case 7: s->current_frame.cq.P8 = lvl; break; case 8: s->current_frame.cq.AF4 = lvl; break; case 9: s->current_frame.cq.F4 = lvl; break; case 10: s->current_frame.cq.AF3 = lvl; break; case 11: s->current_frame.cq.O2 = lvl; break; case 12: s->current_frame.cq.O1 = lvl; break; case 13: s->current_frame.cq.FC5 = lvl; break; } } s->current_frame.gyroX = s->raw_unenc_frame[29] - 102; s->current_frame.gyroY = s->raw_unenc_frame[30] - 104; if(s->raw_unenc_frame[0] & 0x80) { unsigned char b = s->raw_unenc_frame[0]; if(b >= 248) { battery = 100; } else { switch(b) { case 247: battery = 99; break; case 246: battery = 97; break; case 245: battery = 93; break; case 244: battery = 89; break; case 243: battery = 85; break; case 242: battery = 82; break; case 241: battery = 77; break; case 240: battery = 72; break; case 239: battery = 66; break; case 238: battery = 62; break; case 237: battery = 55; break; case 236: battery = 46; break; case 235: battery = 32; break; case 234: battery = 20; break; case 233: battery = 12; break; case 232: battery = 5; break; case 231: battery = 4; break; case 230: battery = 3; break; case 229: battery = 2; break; case 228: case 227: case 226: battery = 1; break; default: battery = 0; } } } s->current_frame.battery = battery; return 0; } return -1; }
EMOKIT_DECLSPEC int emokit_get_next_frame(emokit_device* s) { memset(s->raw_unenc_frame, 0, 32); emokit_get_next_raw(s); s->current_frame.F3 = get_level(s->raw_unenc_frame, F3_MASK); s->current_frame.FC6 = get_level(s->raw_unenc_frame, FC6_MASK); s->current_frame.P7 = get_level(s->raw_unenc_frame, P7_MASK); s->current_frame.T8 = get_level(s->raw_unenc_frame, T8_MASK); s->current_frame.F7 = get_level(s->raw_unenc_frame, F7_MASK); s->current_frame.F8 = get_level(s->raw_unenc_frame, F8_MASK); s->current_frame.T7 = get_level(s->raw_unenc_frame, T7_MASK); s->current_frame.P8 = get_level(s->raw_unenc_frame, P8_MASK); s->current_frame.AF4 = get_level(s->raw_unenc_frame, AF4_MASK); s->current_frame.F4 = get_level(s->raw_unenc_frame, F4_MASK); s->current_frame.AF3 = get_level(s->raw_unenc_frame, AF3_MASK); s->current_frame.O2 = get_level(s->raw_unenc_frame, O2_MASK); s->current_frame.O1 = get_level(s->raw_unenc_frame, O1_MASK); s->current_frame.FC5 = get_level(s->raw_unenc_frame, FC5_MASK); s->current_frame.gyroX = s->raw_unenc_frame[29] - 102; s->current_frame.gyroY = s->raw_unenc_frame[30] - 104; //Check if the current frame is a battery or a contact quality frame unsigned char f_byte = s->raw_unenc_frame[0]; if(f_byte<128) //This is a counter frame from 0-127 { //Update the contact quality if(f_byte<14) //note that only the 14 first are used because of the repeating pattern { s->contact_quality[f_byte] = get_level(s->raw_unenc_frame,LEV_MASK); } s->current_counter = f_byte; } else //This is a battery byte { unsigned char battery; if(f_byte & 0x80) { if(f_byte >= 248) { battery = 100; } else { switch(f_byte) { case 247: battery = 99; break; case 246: battery = 97; break; case 245: battery = 93; break; case 244: battery = 89; break; case 243: battery = 85; break; case 242: battery = 82; break; case 241: battery = 77; break; case 240: battery = 72; break; case 239: battery = 66; break; case 238: battery = 62; break; case 237: battery = 55; break; case 236: battery = 46; break; case 235: battery = 32; break; case 234: battery = 20; break; case 233: battery = 12; break; case 232: battery = 5; break; case 231: battery = 4; break; case 230: battery = 3; break; case 229: battery = 2; break; case 228: case 227: case 226: battery = 1; break; default: battery = 0; } } } s->battery = battery; s->current_counter = 128; } }