void motor_c_dir(MotorDirection dir)
{
dm_c.dir = dir << MOTOR_C_SHIFT;
dlcd_hide(LCD_C_LEFT);
dlcd_hide(LCD_C_RIGHT);
if (dir == fwd || dir == brake)
dlcd_show(LCD_C_RIGHT);
if (dir == rev || dir == brake)
dlcd_show(LCD_C_LEFT);
}
//! receive a byte, decoding LNP packets with a state machine.
void lnp_integrity_byte(unsigned char b) {
static unsigned char buffer[256+3];
static int bytesRead,endOfData;
static unsigned char chk;
if(lnp_integrity_state==LNPwaitHeader)
bytesRead=0;
buffer[bytesRead++]=b;
switch(lnp_integrity_state) {
case LNPwaitHeader:
// valid headers are 0xf0 .. 0xf7
//
if(((b & 0xf8) == 0xf0) || (b == 0x55)) {
#ifdef CONF_VIS
if (lnp_logical_range_is_far()) {
dlcd_show(LCD_IR_UPPER);
dlcd_show(LCD_IR_LOWER);
} else {
dlcd_hide(LCD_IR_UPPER);
dlcd_show(LCD_IR_LOWER);
}
#ifndef CONF_LCD_REFRESH
lcd_refresh();
#endif
#endif
// Init checksum
lnp_checksum_init( chk );
// switch on protocol header
if (b == 0x55) {
#if defined(CONF_RCX_PROTOCOL) || defined(CONF_RCX_MESSAGE)
// 0x55 is header for standard firmware message
lnp_integrity_state = LNPwaitRMH1;
#else
lnp_integrity_reset();
#endif
} else {
lnp_integrity_state++;
}
}
break;
case LNPwaitLength:
endOfData=b+2;
lnp_integrity_state++;
break;
case LNPwaitData:
if(bytesRead==endOfData)
lnp_integrity_state++;
break;
case LNPwaitCRC:
if(b==chk)
lnp_receive_packet(buffer);
lnp_integrity_reset();
break;
#if defined(CONF_RCX_PROTOCOL) || defined (CONF_RCX_MESSAGE)
// state machine to handle remote
case LNPwaitRMH1:
case LNPwaitRMH2:
// waiting for header bytes
if ( b == lnp_rcx_header[ lnp_integrity_state-LNPwaitRMH1 ] )
lnp_integrity_state++;
else
lnp_integrity_reset();
break;
case LNPwaitRMH3:
case LNPwaitRMH4:
if ( b == lnp_rcx_remote_op[ lnp_integrity_state-LNPwaitRMH3 ] )
lnp_integrity_state++;
#if defined(CONF_RCX_MESSAGE)
else if ( b == lnp_rcx_msg_op[ lnp_integrity_state-LNPwaitRMH3 ] )
lnp_integrity_state = LNPwaitMH4;
#endif
else
lnp_integrity_reset();
break;
case LNPwaitRB0:
lnp_rcx_temp0 = b;
lnp_integrity_state++;
break;
case LNPwaitRB0I:
if ( (unsigned char)~b == lnp_rcx_temp0 )
lnp_integrity_state++;
else
lnp_integrity_reset();
break;
case LNPwaitRB1:
lnp_rcx_temp1 = b;
lnp_integrity_state++;
break;
case LNPwaitRB1I:
if ( (unsigned char)~b == lnp_rcx_temp1 )
lnp_integrity_state++;
else
lnp_integrity_reset();
break;
case LNPwaitRC:
lnp_rcx_checksum = 0xd2 + lnp_rcx_temp0 + lnp_rcx_temp1;
if ( b == lnp_rcx_checksum )
lnp_integrity_state++;
else
lnp_integrity_reset();
break;
case LNPwaitRCI:
// if checksum valid and remote handler has been installed, call remote handler
if ( b == (unsigned char)~lnp_rcx_checksum) {
#if defined(CONF_RCX_MESSAGE)
// if a message, set message number and exit
if (lnp_rcx_temp1 & 0x07)
{
lnp_rcx_message = (lnp_rcx_temp1 > 2) ? 3 : lnp_rcx_temp1;
}
else
#endif
{
// Invoke remote handler if any
lnp_remote_handler_t rmth = lnp_remote_handler;
if (rmth)
rmth( (lnp_rcx_temp0<<8)+lnp_rcx_temp1 );
}
}
// reset state machine when done
lnp_integrity_reset();
break;
#endif
#if defined(CONF_RCX_MESSAGE)
// state machine to handle RCX protocol messages
case LNPwaitMH3:
case LNPwaitMH4:
if ( b == lnp_rcx_msg_op[ lnp_integrity_state-LNPwaitMH3 ] )
lnp_integrity_state++;
else
lnp_integrity_reset();
break;
case LNPwaitMN:
lnp_rcx_temp0 = b;
lnp_integrity_state++;
break;
case LNPwaitMNC:
if ( (unsigned char)~b == lnp_rcx_temp0 )
lnp_integrity_state++;
else
lnp_integrity_reset();
break;
case LNPwaitMC:
lnp_rcx_temp1 = 0xf7 + lnp_rcx_temp0;
if (b == lnp_rcx_temp1)
lnp_integrity_state++;
else
lnp_integrity_reset();
break;
case LNPwaitMCC:
// set message variable if it is valid message
if ( (unsigned char)~b == lnp_rcx_temp1 )
lnp_rcx_message = lnp_rcx_temp0;
// reset state machine
lnp_integrity_reset();
break;
#endif
}
// Accumulate checksum
lnp_checksum_step( chk, b );
}
