void tlvReceiveFSM(TLV_FSM *fsm, TLV_Buffer *tlvBuf, uint8 *ptr)
{
ptr[fsm->i] = uartGetByte();
switch(fsm->state)
{
case WAIT_FOR_TYPE:
if(ptr[fsm->i] == PROGRAMMING_MODE)
{
setProgrammingMode();
// while(Busy1USART()); //for debugging purpose
resetTarget = 1;
}
else if(ptr[fsm->i] == START_RUNNING)
{
setStartRunningMode();
resetTarget = 1;
}
else
fsm->state = WAIT_FOR_LENGTH;
break;
case WAIT_FOR_LENGTH:
fsm->length = ptr[fsm->i];
fsm->state = WAIT_FOR_VALUE;
break;
case WAIT_FOR_VALUE:
if((fsm->i - 1) < fsm->length)
fsm->state = WAIT_FOR_VALUE;
else
{
fsm->state = WAIT_FOR_TYPE;
if(!verifyCheckSum(ptr))
{
if(!verifyType(ptr))
uartSendByte(ERR_WRONG_TYPE);
else
uartSendByte(ERR_WRONG_CHECKSUM);
}
else
setTLVframe(tlvBuf, ptr);
}
break;
default:
break;
}
if(fsm->state == WAIT_FOR_TYPE)
fsm->i = 0;
else
fsm->i++;
}
bool InstructionSet::Instruction::execute( Processor* dev, const std::vector<char>& p, std::vector<char>& r) const
{
printCall( std::cerr, (const char*)&p[0] );
bool ret=false;
// Ein Befehl ist immer genau 4 byte lang
// CODE, P1, P2, XOR-CHKSUM
// die Antwort kann auch länger sein, niemals aber länger
// als als max( n*(X+Y) + 4 ) = 256*16+4 = 4096+4 = 4100;
// für normale (nicht Schleifen) Befehle aber nur 8 byte
static char buffer[4+8]; // max antwort + 4 bytes command
std::fill( buffer, buffer+12, 0x00 );
Device *d = static_cast<Device*>(dev);
// instruction codes des Scanners sind immer ein byte lang
buffer[0] = code()[0];
buffer[1] = paramLength()>0 ? p[0] : 0;
buffer[2] = paramLength()>1 ? p[1] : 0;
calcCheckSum(buffer, 4);
// calc return length
// 0 -> 1
// 2 -> 4
// 4 -> 8
unsigned int ret_length = (returnLength()==0) ? 1 : (returnLength()<<1);
// send
// Processor::usleep( 1000 );
if( d->sendCommand( buffer, 4 ) )
if( d->receiveAnswer( buffer+4, ret_length, _millies ) )
{
// if( ret_length > 1 )
// buffer[4] = buffer[0];
if( !verifyCheckSum(buffer+4, ret_length) )
std::cerr << "\t\t\t\tWARNING CRC failed!" << std::endl;
{
// Hier müssen wir insgesamt returnLength() Werte zurück liefern
// 0: __
// 2: __ XX XX __
// 4: __ XX XX __ __ XX XX __ __ XX XX __
// 0 1 2 2 3 4 5
// 1 1 1 3 3 5 5
// 1 2 3-> 5 6 9 10
unsigned offset=1;
unsigned index =0;
//std::cerr << " returns " << std::hex;
while( index < returnLength() )
{
assert( offset+index < ret_length );
//std::cerr << (unsigned(buffer[4+index+offset])&255) << " ";
r.push_back( buffer[4+offset+index] );
index++;
if( (offset+index)&3 == 3 )
offset+=2;
}
//******************
//std::cerr << " -------> answer was: ";
//for( index = 0; index<ret_length; index++ )
//std::cerr << unsigned((buffer[4+index]>>4)&15) << unsigned(buffer[4+index]&15) << " ";
//*************************************
ret=true;
}
}
//std::cerr << std::dec << std::endl;
return ret;
}
bool InstructionSet::SequenceInstruction::execute( Processor* dev, const char *p, char *r) const
{
printCall( std::cerr, p ) << std::endl;
// Answer length = max( n*(X+Y) + 4 ) = 256*16+4 = 4096+4 = 4100;
static char buffer[4104];
Device *d = static_cast<Device*>(dev);
// complete length of sequence to receive
unsigned int sequence_length=0;
// answer length to be returned (use data)
unsigned int answer_length =0;
// sequence and answer length of single command inside loop
unsigned int seq1_length =0;
unsigned int seq2_length =0;
unsigned int ans1_length =0;
unsigned int ans2_length =0;
// additional bytes to receive
unsigned int suffix_length = 0;
switch( code()[0] )
{
case REPT: // set loop count and send REPT command
_counter = *(reinterpret_cast<const unsigned int*>(p));
return Instruction::execute( dev, p, r );
case SEQUEX: // Sequence 1
{
const ::InstructionSet::Instruction* ins1=0;
const ::InstructionSet::Instruction* ins2=0;
ins1 = d->instructionSet()->instruction(unsigned(p[0]));
ins2 = d->instructionSet()->instruction(unsigned(p[1]));
ans1_length = ins1 ? ins1->returnLength() : 0;
seq1_length = ans1_length ? ans1_length<<1 : 1;
ans2_length = ins2 ? ins2->returnLength() : 0;
seq2_length = ans2_length ? ans2_length<<1 : 1;
suffix_length = 4;
}
break;
case SEQ2:
case SEQ3:
seq1_length = 5;
ans1_length = 4;
seq2_length = 0;
ans2_length = 0;
suffix_length = 0;
break;
case LEDCHK:
_counter = 9;
seq1_length = 4;
ans1_length = 2;
seq2_length = 0;
ans2_length = 0;
suffix_length = 0;
default:
return false;
}
sequence_length = (seq1_length+seq2_length)*_counter;
answer_length = (ans1_length+ans2_length)*_counter;
// commando aufbauen
// instruction codes des Scanners sind immer ein byte lang
buffer[0] = code()[0];
buffer[1] = paramLength()>0 ? p[0] : 0;
buffer[2] = paramLength()>1 ? p[1] : 0;
calcCheckSum(buffer, 4);
// calc return length
// 0 -> 1
// 2 -> 4
// 4 -> 8
// send
if( d->sendCommand( buffer, 4 ) )
{
char *t = r;
char *s = buffer+4;
unsigned int cnt = sequence_length;
while( _counter > 0 )
{
if( seq1_length > 0 &&
!d->receiveAnswer( s, seq1_length, _millies ))
break;
if( seq2_length > 0 &&
!d->receiveAnswer( s+seq1_length, seq2_length, _millies ))
break;
if( seq1_length > 0 && verifyCheckSum(s, seq1_length) )
{
unsigned i = 0;
unsigned o = (seq1_length-ans1_length)>>1;
while( i < ans1_length ) {
*t++ = s[i+o];
i++;
}
answer_length -= ans1_length;
}
if( seq2_length > 0 && verifyCheckSum(s+seq1_length, seq2_length) )
{
unsigned i = 0;
unsigned o = seq1_length + (seq2_length-ans2_length)>>1;
while( i < ans2_length ) {
*t++ = s[i+o];
i++;
}
answer_length -= ans2_length;
}
/**
* This method tries to read all lines contained in the receive buffer. A line
* is always terminated by a newline and is taken over in the receiver queue,
* if the checksum is valid and the GPS identifier is requested.
*/
void GpsClient::readSentenceFromBuffer()
{
char *start = databuffer;
char *end = 0;
while(strlen(start))
{
// Search for a newline in the receiver buffer.
// That is the normal end of a GPS sentence.
if( ! (end = strchr( start, '\n' )) )
{
// No newline in the receiver buffer, wait for more characters
return;
}
if( start == end )
{
// skip newline and start at next position with a new search
start++;
continue;
}
// found a complete record in the buffer, it will be extracted now
char *record = (char *) malloc( end-start + 2 );
memset( record, 0, end-start + 2 );
strncpy( record, start, end-start + 1);
if( verifyCheckSum( record ) == true )
{
// Forward sentence to the server, if checksum is ok and
// processing is desired.
if( checkGpsMessageFilter( record ) == true && forwardGpsData == true )
{
QByteArray ba;
ba.append( MSG_GPS_DATA );
ba.append( ' ' );
ba.append( record );
writeForwardMsg( ba.data() );
}
}
#ifdef DEBUG_NMEA
qDebug() << "GpsClient::read():" << record;
#endif
free(record);
record = 0;
// remove queued record from receive buffer
memmove( databuffer, end+1,
databuffer + sizeof(databuffer)-1 - end );
datapointer -= (end+1 - databuffer);
dbsize -= ( end+1 - databuffer);
start = databuffer;
end = 0;
}
}