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);
}
/*! \param mask the mask to display
*/
void cputc_native_4(char mask)
{
dlcd_hide(LCD_4_DOT);
bit_load(mask, 0x2);
dlcd_store(LCD_4_TOP);
bit_load(mask, 0x0);
dlcd_store(LCD_4_MID);
bit_load(mask, 0x5);
dlcd_store(LCD_4_BOT);
bit_load(mask, 0x1);
dlcd_store(LCD_4_TOPR);
bit_load(mask, 0x6);
dlcd_store(LCD_4_BOTR);
bit_load(mask, 0x3);
dlcd_store(LCD_4_TOPL);
bit_load(mask, 0x4);
dlcd_store(LCD_4_BOTL);
}
void lnp_integrity_reset(void) {
#else
HANDLER_WRAPPER("lnp_integrity_reset","lnp_integrity_reset_core");
void lnp_integrity_reset_core(void) {
#endif
#ifndef CONF_HOST
if(tx_state>TX_IDLE) {
txend_handler();
tx_state=TX_COLL;
} else
#endif
if(lnp_integrity_state!=LNPwaitHeader) {
lnp_integrity_state=LNPwaitHeader;
#ifdef CONF_VIS
dlcd_hide(LCD_IR_LOWER);
dlcd_hide(LCD_IR_UPPER);
#ifndef CONF_LCD_REFRESH
lcd_refresh();
#endif
#endif
}
}
//! return whether a packet is currently being received
/*! \return 1 if yes, else zero
*/
int lnp_integrity_active(void) {
return lnp_integrity_state!=LNPwaitHeader;
}
//! reset the inter-byte timeout counter.
#if defined(CONF_RCX_COMPILER) || defined(CONF_HOST)
void lnp_timeout_reset(void) {
#else
HANDLER_WRAPPER("lnp_timeout_reset","lnp_timeout_reset_core");
void lnp_timeout_reset_core(void) {
#endif
lnp_timeout_counter=lnp_timeout;
}
//! set the inter-byte timeout and reset the timeout counter to that value.
/*! \param timeout the new timeout value
*/
void lnp_timeout_set(unsigned short timeout) {
lnp_timeout_counter=lnp_timeout=timeout;
}
//! Initialise protocol handlers
/*! Adressing port 0 is reserved for the program handler.
*/
void lnp_init(void) {
int k;
for(k=1; k<=LNP_PORTMASK; k++)
lnp_addressing_handler[k]=LNP_DUMMY_ADDRESSING;
lnp_integrity_handler=LNP_DUMMY_INTEGRITY;
#if defined(CONF_RCX_PROTOCOL)
lnp_remote_handler=LNP_DUMMY_REMOTE;
#endif
#if defined(CONF_RCX_MESSAGE)
clear_msg();
#endif
}
//! 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 );
}
