void handleRecv()
{
uint8_t i, a;
static uint8_t state=0, b=1;
uint16_t s0, s1;
Iserial *serial = ser_getSerial();
for (i=0; i<10; i++)
{
switch(state)
{
case 0: // look for sync
if(serial->receive(&a, 1)<1)
return;
if (a==0xff && b==0x00)
state = 1;
b = a;
break;
case 1: // read rest of data
if (serial->receiveLen()>=4)
{
serial->receive((uint8_t *)&s0, 2);
serial->receive((uint8_t *)&s1, 2);
//cprintf("servo %d %d\n", s0, s1);
rcs_setPos(0, s0);
rcs_setPos(1, s1);
state = 0;
}
break;
default:
state = 0;
break;
}
}
}
void handleRecv()
{
uint8_t i, a;
static uint16_t w=0xffff;
static uint8_t lastByte;
uint16_t s0, s1;
Iserial *serial = ser_getSerial();
for (i=0; i<10; i++)
{
switch(g_state)
{
case 0: // reset
lastByte = 0xff; // This is not part of any of the sync word most significant bytes
g_state = 1;
break;
case 1: // sync word
if(serial->receive(&a, 1))
{
w = lastByte << 8;
w |= a;
lastByte = a;
g_state = 2; // compare
}
break;
case 2: // receive data byte(s)
if (w==SYNC_SERVO)
{ // read rest of data
if (serial->receiveLen()>=4)
{
serial->receive((uint8_t *)&s0, 2);
serial->receive((uint8_t *)&s1, 2);
//cprintf("servo %d %d\n", s0, s1);
rcs_setPos(0, s0);
rcs_setPos(1, s1);
g_state = 0;
}
}
else if (w==SYNC_CAM_BRIGHTNESS)
{
if(serial->receive(&a, 1))
{
cam_setBrightness(a);
g_state = 0;
}
}
else if (w==SYNC_SET_LED)
{
if (serial->receiveLen()>=3)
{
uint8_t r, g, b;
serial->receive(&r, 1);
serial->receive(&g, 1);
serial->receive(&b, 1);
led_setRGB(r, g, b);
//cprintf("%x %x %x\n", r, g ,b);
g_ledSet = true; // it will stay true until the next power cycle
g_state = 0;
}
}
else
g_state = 1; // try another word, but read only a byte
break;
default:
g_state = 0; // try another whole word
break;
}
}
}
