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