////////////////////////////////////////////////////////////
// LOAD PORT INFO FROM EEPROM
void loadPortInfo(byte forceReload)
{
byte cs = 0;
for(int i=0; i<NUM_PORTS; ++i)
{
PORT_INFO *p = port[i];
memset(p, 0, sizeof(PORT_INFO));
int eepromAddr = 16 * i;
p->cfg.triggerChannel = eeprom_read(eepromAddr + 0);
p->cfg.triggerNote = eeprom_read(eepromAddr + 1);
p->cfg.durationMax = (int)eeprom_read(eepromAddr + 2) << 8;
p->cfg.durationMax |= (int)eeprom_read(eepromAddr + 3);
p->cfg.durationModulator = eeprom_read(eepromAddr + 4);
p->cfg.dutyMax = eeprom_read(eepromAddr + 5);
p->cfg.dutyModulator = eeprom_read(eepromAddr + 6);
byte flags = eeprom_read(eepromAddr + 7);
p->cfg.invert = !!(flags & EEPROM_FLAG_INVERT);
// default to full duration/duty
p->status.duration = p->cfg.durationMax;
p->status.duty = p->cfg.dutyMax;
calcCheckSum((byte*)&p->cfg, sizeof(PORT_CONFIG), &cs);
}
if(forceReload || eeprom_read(EEPROM_ADDR_CHECKSUM) != cs)
{
// failed checksum (possibly no previous stored config)
flashLed(10);
initPortInfo();
savePortInfo();
}
}
AREXPORT bool ArNetPacket::verifyCheckSum(void)
{
ArTypes::Byte2 chksum;
ArTypes::Byte2 calcedChksum;
unsigned char c1, c2;
int length;
if (myLength - 2 < myHeaderLength)
return false;
c2 = myBuf[myLength-2];
c1 = myBuf[myLength-1];
chksum = (c1 & 0xff) | (c2 << 8);
length = myLength;
myLength = myLength - 2;
calcedChksum = calcCheckSum();
myLength = length;
//printf("%d %d\n", chksum, calcedChksum);
if (chksum == calcedChksum) {
return true;
} else {
return false;
}
}
// Sends a NMEA sentence to the GPS. Check sum will be calculated by
// this routine. Don't add an asterix at the end of the sentence. It
// will be part of the check sum.
int GpsClient::writeGpsData( const char *sentence )
{
// don't try to send anything if there is no valid connection
// available
if( fd < 0 )
{
return -1;
}
uchar csum = calcCheckSum( sentence );
QString check;
check.sprintf ("*%02X\r\n", csum);
QString cmd (sentence + check);
#ifdef DEBUG_NMEA
qDebug() << "GpsClient::write():" << cmd;
#endif
// write sentence to gps device
int result = write (fd, cmd.toLatin1().data(), cmd.length());
if (result != -1 && result != cmd.length())
{
qWarning() << "GpsClient::writeGpsData Only"
<< result
<< "characters were written:"
<< cmd;
}
return result;
}
AREXPORT void ArNetPacket::finalizePacket(void)
{
insertHeader();
int chkSum = calcCheckSum();
byteToBuf((chkSum >> 8) & 0xff );
byteToBuf(chkSum & 0xff );
myAddedFooter = true;
//log();
//printf("%d %d %d\n", myLength ,myCommand, chkSum);
}
/**
* Verify the checksum of the passed sentences.
*
* @returns true (success) or false (error occurred)
*/
bool GpsClient::verifyCheckSum( const char *sentence )
{
// Filter out wrong data messages read in from the GPS port. Known messages
// do start with a dollar sign or an exclamation mark.
// Note: Flarm sends several debug text messages after a restart not starting
// with a dollar sign or an exclamation mark.
if( sentence[0] != '$' && sentence[0] != '!' )
{
qWarning() << "GpsClient::CheckSumError:" << sentence;
badSentences++;
return false;
}
badSentences = 0;
for( int i = strlen(sentence) - 1; i >= 0; i-- )
{
if( sentence[i] == '*' )
{
if( (strlen(sentence) - 1 - i) < 2 )
{
// too less characters
return false;
}
char checkBytes[3];
checkBytes[0] = sentence[i+1];
checkBytes[1] = sentence[i+2];
checkBytes[2] = '\0';
uchar checkSum = (uchar) QString( checkBytes ).toUShort( 0, 16 );
if( checkSum == calcCheckSum( sentence ) )
{
return true;
}
else
{
return false;
}
}
}
return false;
}
AREXPORT bool ArRobotPacket::verifyCheckSum(void)
{
ArTypes::Byte2 chksum;
unsigned char c1, c2;
if (myLength - 2 < myHeaderLength)
return false;
c2 = myBuf[myLength - 2];
c1 = myBuf[myLength - 1];
chksum = (c1 & 0xff) | (c2 << 8);
if (chksum == calcCheckSum()) {
return true;
} else {
return false;
}
}
AREXPORT void ArRobotPacket::finalizePacket(void)
{
int len = myLength;
int chkSum;
myLength = 0;
uByteToBuf(mySync1);
uByteToBuf(mySync2);
uByteToBuf(len - getHeaderLength() + 3);
myLength = len;
chkSum = calcCheckSum();
byteToBuf((chkSum >> 8) & 0xff );
byteToBuf(chkSum & 0xff );
/* Put this in if you want to see the packets being outputted
printf("Output(%3d) ", getID());
printHex();
*/
// or put this in if you just want to see the type
//printf("Output %d\n", getID());
}
////////////////////////////////////////////////////////////
// SAVE PORT INFO TO EEPROM
void savePortInfo()
{
byte cs = 0;
for(int i=0; i<NUM_PORTS; ++i)
{
PORT_INFO *p = port[i];
calcCheckSum((byte*)&p->cfg, sizeof(PORT_CONFIG), &cs); // include checksum
int eepromAddr = 16 * i; // allocate 16 bytes for each port config
eeprom_write(eepromAddr+0, p->cfg.triggerChannel);
eeprom_write(eepromAddr+1, p->cfg.triggerNote);
eeprom_write(eepromAddr+2, (p->cfg.durationMax >> 8) & 0xFF);
eeprom_write(eepromAddr+3, p->cfg.durationMax & 0xFF);
eeprom_write(eepromAddr+4, p->cfg.durationModulator);
eeprom_write(eepromAddr+5, p->cfg.dutyMax);
eeprom_write(eepromAddr+6, p->cfg.dutyModulator);
byte flags = 0;
if(p->cfg.invert) flags |= EEPROM_FLAG_INVERT;
eeprom_write(eepromAddr+7, flags);
}
eeprom_write(EEPROM_ADDR_CHECKSUM, cs);
}
int main( int argc , char ** argv)
{
unsigned char msg[6];
unsigned int device = 2;
msg[0] = 0xf7;
msg[1] = (unsigned char)(device); // I've read this is bad form but I will fix it later. I don't even know if it'll work correctly
msg[2] = 0x50;
msg[3] = 0xa0;
msg[4] = 0x10;
//I really need to wrie a checksum function
//msg[11] = checksum(msg[1] through msg[10]);
//how about this:
msg[5] = calcCheckSum ( & msg[1], 4);
printf("checksum = %x\n", msg[5]);
return 0;
}
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;
}
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;
}
//------------------------------------------------------------
//------------------------------------------------------------
static int l_FrameStripLoadBin(lua_State * lState)
{
int nArgC = lua_gettop(lState);
FrameStrip * * ppStrip = checkFrameStrip(lState, 1);
const char * szFName = luaL_checkstring(lState, 2);
int nLoop, nFrames = 1;
unsigned int nSize, nWanted, nFrameSize;
// check parameters
if((NULL == ppStrip) || (NULL == *ppStrip))
{
return luaL_error(lState, "Invalid FrameStrip object");
}
// get optional param
if(nArgC > 2)
{
nFrames = luaL_checkint(lState, 3);
if(nFrames <= 0)
{
return luaL_error(lState, "Number of frames has to be at least 1");
}
}
// open palette file in binary mode
FILE * dsfImg = fopen(szFName, "rb");
if(NULL == dsfImg)
{
return luaL_error(lState, "Failed to open image file '%s'", szFName);
}
// load image into memroy
nSize = getFileSize(dsfImg);
nFrameSize = (*ppStrip)->getFrameSize();
nWanted = nFrames * nFrameSize;
// make sure we have enough frame data
if(nSize < nWanted)
{
fclose(dsfImg);
return luaL_error(lState, "Image file contains less than %d frames of sprite", nWanted);
}
// allocate memory to read frame data
// char *cbTemp = ( char* )malloc( nFrameSize );
AlignedMemory * amMem = new AlignedMemory(5, nFrameSize);
char * cbTemp = (char *)(amMem->vpAlignedMem);
#ifdef DEBUG
char szTemp[64];
sprintf(szTemp, "F:%p (%p)", cbTemp, amMem->vpRealMem);
SimpleConsolePrintStringCR(SCREEN_TOP, szTemp);
#endif // DEBUG
// make sure we have a valid pointer
if(NULL == cbTemp)
{
fclose(dsfImg);
return luaL_error(lState, "Unable to allocate %d bytes for sprite image data", nFrameSize);
}
// now loop, and read in all frames
size_t nRead;
for(nLoop = 0; nLoop < nFrames; ++nLoop)
{
// read in a single frame of bin data
// memset( cbTemp, 0, nFrameSize );
nRead = fread(cbTemp, 1, nFrameSize, dsfImg);
// make sure we read in something
if(nRead != nFrameSize)
{
// free( cbTemp );
delete (amMem);
fclose(dsfImg);
return luaL_error(lState, "Can only read in %d bytes of sprite data for frame %d", nRead, nLoop);
}
// add frame to strip
#ifdef DEBUG
u32 nSum1;
char szDbgTemp[128];
nSum1 = calcCheckSum(cbTemp, nFrameSize);
sprintf(szDbgTemp, "PreAddFrame: %x", nSum1);
SimpleConsolePrintStringCR(SCREEN_TOP, szDbgTemp);
#endif
waitUntilDMAFree(3);
// waitVBLDelay( 10 );
(*ppStrip)->addFrame(cbTemp);
waitUntilDMAFree(3);
// waitVBLDelay( 10 );
#ifdef DEBUG
nSum1 = calcCheckSum(cbTemp, nFrameSize);
sprintf(szDbgTemp, "PostAddFrame: %x", nSum1);
SimpleConsolePrintStringCR(SCREEN_TOP, szDbgTemp);
#endif
}
// free mem, close the file when we are done
// free( cbTemp );
// delete [] cbTemp;
delete (amMem);
fclose(dsfImg);
return 0;
}
void ReceiveDataClass::RunMain()
{
TCPClass TCPdata(atoi(TCPDATA_PORT));
// save pointers for later use in closing scanner
TCPDataClassPtr = (TCPClass *)&TCPdata;
int TCP_initialized;
TCP_initialized = TCPdata.initTCPclient(SCANNER_ID);
if(!TCP_initialized)
{
printf("TCP RECEIVE DATA SOCKET OPENING PROBLEM ON CLIENT, port %d\n", TCPdata.TCPport);
Sleep(1000);
return;
}
printf("TCP RECEIVE DATA SOCKET INITIALIZED, port %d. ReceiveDataClass has started.\n", TCPdata.TCPport);
int nrecd=0, totalrecd = 0, len;
SRIPacket *pktptr;
//flush input
pktptr = &Sptr->s_SRIlinePackets[Sptr->s_SRIdataPacketcount+1];
while(nrecd = TCPDataClassPtr->receiveTCP((char *)&pktptr->lineHdr, sizeof(SRIPacketHdr), 100)); //timeout of 0 waits until packet Hdr recd
while(1)
{
if(Sptr->s_SRIdataPacketcount == NLINESMAX-1)
pktptr = &Sptr->s_SRIlinePackets[0];
else
pktptr = &Sptr->s_SRIlinePackets[Sptr->s_SRIdataPacketcount+1];
// Receive messages
//printf("Getting Header.\n");
nrecd = TCPDataClassPtr->receiveTCP((char *)&pktptr->lineHdr, sizeof(SRIPacketHdr), 0); //timeout of 0 waits until packet Hdr recd
if(nrecd != sizeof(SRIPacketHdr))
{ //problem: need to reconnect
continue;
}
//printf("%d\n",Sptr->s_SRIdataPacketcount);
SetEvent(Sptr->scanPacketRecvdEvent);
unsigned short cksum = calcCheckSum((unsigned char *)&pktptr->lineHdr, sizeof(SRIPacketHdr) - sizeof(short)); //dont include checksum
if (pktptr->lineHdr.checksum != cksum)
{
//This can occur if the client is too busy or not fast enough to keep up with the scan data stream.
printf("Header checksum did not match.\n");
// continue;
}
Sptr->s_status = pktptr->lineHdr.status;
//if this data is part of a scan, keep the data. If it is just a response to a command, while not sending data, dont keep.
//packets are always a fixed length, but scan line data may be broken across packets and scan lines are variable length
//get the line of points. If not in a scan we should get the entire packet
switch(Sptr->s_status)
{
case OK_IN_FIRST_SCAN:
case OK_IN_LAST_SCAN:
Sptr->scanInProgress = 1;
case OK_SCAN_FINISHED: //extra header sent at end of scan. samplesPerLine will say how much to read to finish out packet
Sptr->scanInProgress = 0;
len = pktptr->lineHdr.samplesPerLine*sizeof(scanDataPerSample);
scannum = pktptr->lineHdr.scanNumber;
if(pktptr->lineHdr.lineNumber != Sptr->s_lineNumber_old+1)
{
printf("Header line count mismatch.\n");
// continue;
}
Sptr->s_lineNumber_old = pktptr->lineHdr.lineNumber;
if(Sptr->s_status == OK_SCAN_FINISHED)
Sptr->s_lineNumber_old = 0;
break;
case OK_SLEEPING:
case OK_READY:
case OK_STARTING_SCAN:
case OK_WAITING_ELEV_MOTOR_SPEED:
case OK_WAITING_AZ_MOTOR:
default:
len = BUFLENBYTES-sizeof(SRIPacketHdr);
break;
}
if(len)
{
nrecd = TCPDataClassPtr->receiveTCP((char *)&pktptr->lineData, len, 0);
if(nrecd != len)
{ //problem: need to reconnect
continue;
}
}
//Will have valid log data if it was a log report command if(pktptr->lineHdr.ack.commandID == LogCommandID)
Sptr->log = (ScannerLog *)pktptr->lineData;
//if there is a command ack in the header, extract it.
if(pktptr->lineHdr.ack.commandID)
{
Sptr->ack = pktptr->lineHdr.ack;
ackRecd++;
SetEvent(Sptr->ackReceivedEvent); //ack was received: for timeout check and display
printf("Ack received.\n");
}
if(Sptr->s_status == OK_SCAN_FINISHED)
{
printf("Scan Finished.\n");
#if CHECKAZIMUTH
int i,j;
//test that azimuth psoition is not generating spurious values. Only valid at low turret speed (1 az count per line or less)
for(i=0;i<=Sptr->s_SRIdataPacketcount;i++)
{
for(j=1;j<Sptr->s_SRIlinePackets[i].lineHdr.samplesPerLine;j++)
{
if(Sptr->s_SRIlinePackets[i].lineData[j].azimuthEncoder > lastAz+1 || Sptr->s_SRIlinePackets[i].lineData[j].azimuthEncoder < lastAz-1)
printf("Az error: Line %d sample %d: Last:%d This:%d\n", i, j, lastAz, Sptr->s_SRIlinePackets[i].lineData[j].azimuthEncoder);
lastAz = Sptr->s_SRIlinePackets[i].lineData[j].azimuthEncoder;
}
}
#endif
#if BINARYLOG
if(Sptr->s_DataFile)
{
fclose(Sptr->s_DataFile);
Sptr->s_DataFile = NULL;
printf("Binary Logging Completed.\n");
#if CONVERTTOASCII
printf("Converting binary file to ASCII.\n");
Sptr->CalibrateAndConvertToAscii();
printf("Done converting.\n");
#endif
}
#endif
//get ready for next scan
Sptr->s_SRIdataPacketcount = NLINESMAX-1; // initialize buffer data counter so first filled will be 0
}
//if in a scan this will contain point data
if(Sptr->s_status == OK_IN_FIRST_SCAN || Sptr->s_status == OK_IN_LAST_SCAN)
{
// g_theGlobals.g_globalsMutex->Lock(); //no longer used
Sptr->s_SRIdataPacketcount++;
if(Sptr->s_SRIdataPacketcount == NLINESMAX)
Sptr->s_SRIdataPacketcount = 0;
// g_theGlobals.g_globalsMutex->Unlock();
#if BINARYLOG
Sptr->logBinaryScanLine(&Sptr->s_SRIlinePackets[Sptr->s_SRIdataPacketcount], 0);
#endif
}
} //while (1)
return;
}