int readExtension (int& transparent)
{
uint8 type;
if (input.read (&type, 1) != 1)
return false;
uint8 b [260];
int n = 0;
if (type == 0xf9)
{
n = readDataBlock (b);
if (n < 0)
return 1;
if ((b[0] & 1) != 0)
transparent = b[3];
}
do
{
n = readDataBlock (b);
}
while (n > 0);
return n >= 0;
}
byte pollStatus()
{
if(busWriteByte(BUS_CMD_BOARDSTATUS, SLAVE_ID_POWERBOARD) != 0)
{
showString("STA FAIL ", 1);
return 255;
}
byte len = readDataBlock(SLAVE_ID_POWERBOARD);
if(len!=1)
{
showString("STA FAIL ", 1);
return 255;
}
rxBuf[0] &= 0xFD; /* Clear the reported kill switch bit */
/* We report the kill switch our own way */
if(IN_KS == 1)
rxBuf[0] |= 0x02;
return rxBuf[0];
}
ErrorCode readInodeData(FileSystem* fs, Inode* inode, BYTE* buf, size_type off, size_type read_size, size_type* readbyte){
size_type bytesread = 0;
size_type start = off;
size_type size =read_size;
if(start+read_size>inode->fileSize)
size = inode->fileSize-start;
while(bytesread < size){
size_type b_id;
size_type b_offset;
size_type len;
ErrorCode err = Potato_bmap(fs, inode, &start, &b_id, &b_offset);
if(err == Success && b_id > 0){
len = size - bytesread < BLOCK_SIZE-b_offset? size-bytesread:BLOCK_SIZE-b_offset;
printf("[readInodeData] write block %ld, offset %ld, len %ld\n", b_id, b_offset, len);
err = readDataBlock(fs, b_id, buf+bytesread, b_offset, len);
if(err != Success){
return err;
}
}else{
printf("[readInodeData] bmap failed.\n");
return err;
}
printf("bytesread %d\n", bytesread);
bytesread += len;
start+= len;
}
*readbyte = bytesread;
return Success;
}
CacheData *readBlock(int dev, int block) {
CacheData *cacheData = getCacheData(dev, block);
if (cacheData->isUptoDate) {
return cacheData;
} else {
readDataBlock(cacheData);
cacheData->isUptoDate = true;
}
return cacheData;
}
void Connection::checkNext() {
if (m_expectingDataSize == -1) {
if (m_socket->canReadLine()) {
readLine();
checkNext();
}
} else {
if (m_socket->bytesAvailable() >= m_expectingDataSize) {
readDataBlock();
checkNext();
}
}
}
void CommandParser::checkNext() {
if (m_expectingDataSize == -1) {
if (m_device->canReadLine()) {
readLine();
checkNext();
}
} else {
if (m_device->bytesAvailable() >= m_expectingDataSize) {
readDataBlock();
checkNext();
}
}
}
bool MdlFileFormat::readSdfFile(QIODevice *iodev, chemkit::MoleculeFile *file)
{
while(!iodev->atEnd()){
// read molecule
readMolFile(iodev, file);
// read data block
chemkit::Molecule *molecule = file->molecules().back();
readDataBlock(iodev, molecule, file);
}
// return false if we failed to read any molecules
if(file->moleculeCount() == 0){
return false;
}
return true;
}
void showIdent()
{
byte i=0;
long j=0;
unsigned char tmp[16];
while(!(pollStatus() & 0x80));
for(i=0; i<NUM_SLAVES; i++)
{
sprintf(tmp, "Ident IRQ %d: ", i);
showString(tmp, 0);
/* Don't mix the strings */
for(j=0; j<17; j++)
rxBuf[j]=0;
if(busWriteByte(BUS_CMD_ID, i) != 0)
{
showString("<Write Fail> ", 1);
} else
{
byte len = readDataBlock(i);
if(len > 0)
{
for(j=len; j<16; j++)
rxBuf[j]=32;
showString(rxBuf, 1);
} else
{
showString("<Read Fail> ", 1);
}
}
while(pollStatus() & 0x80);
while(!(pollStatus() & 0x80));
}
showString("Diagnostic Mode ", 0);
}
byte pollThrusterState()
{
if(busWriteByte(BUS_CMD_THRUSTER_STATE, SLAVE_ID_THRUSTERS) != 0)
{
showString("TSTA FAIL ", 1);
return 0;
}
byte len = readDataBlock(SLAVE_ID_THRUSTERS);
if(len!=1)
{
showString("TSTA FAIL ", 1);
return 0;
}
if(IN_KS == 1)
rxBuf[0] |= 0x10;
return rxBuf[0];
}
int getCode (const int codeSize_, const bool shouldInitialise)
{
if (shouldInitialise)
{
currentBit = 0;
lastBit = 0;
finished = false;
return 0;
}
if ((currentBit + codeSize_) >= lastBit)
{
if (finished)
return -1;
buffer[0] = buffer [lastByteIndex - 2];
buffer[1] = buffer [lastByteIndex - 1];
const int n = readDataBlock (buffer + 2);
if (n == 0)
finished = true;
lastByteIndex = 2 + n;
currentBit = (currentBit - lastBit) + 16;
lastBit = (2 + n) * 8 ;
}
int result = 0;
int i = currentBit;
for (int j = 0; j < codeSize_; ++j)
{
result |= ((buffer[i >> 3] & (1 << (i & 7))) != 0) << j;
++i;
}
currentBit += codeSize_;
return result;
}
int readLZWByte()
{
if (fresh)
{
fresh = false;
for (;;)
{
firstcode = oldcode = getCode (codeSize, false);
if (firstcode != clearCode)
return firstcode;
}
}
if (sp > stack)
return *--sp;
int code;
while ((code = getCode (codeSize, false)) >= 0)
{
if (code == clearCode)
{
clearTable();
codeSize = setCodeSize + 1;
maxCodeSize = 2 * clearCode;
maxCode = clearCode + 2;
sp = stack;
firstcode = oldcode = getCode (codeSize, false);
return firstcode;
}
else if (code == endCode)
{
if (dataBlockIsZero)
return -2;
uint8 buf [260];
int n;
while ((n = readDataBlock (buf)) > 0)
{}
if (n != 0)
return -2;
}
const int incode = code;
if (code >= maxCode)
{
*sp++ = firstcode;
code = oldcode;
}
while (code >= clearCode)
{
*sp++ = table[1][code];
if (code == table[0][code])
return -2;
code = table[0][code];
}
*sp++ = firstcode = table[1][code];
if ((code = maxCode) < maxGifCode)
{
table[0][code] = oldcode;
table[1][code] = firstcode;
++maxCode;
if (maxCode >= maxCodeSize && maxCodeSize < maxGifCode)
{
maxCodeSize <<= 1;
++codeSize;
}
}
oldcode = incode;
if (sp > stack)
return *--sp;
}
return code;
}
void ABoxDlg::event(iONode node) {
char* s = NodeOp.base.toString(node);
TraceOp.trc( "aboxdlg", TRCLEVEL_INFO, __LINE__, 9999, "event: %.256s", s );
StrOp.free(s);
if( wDataReq.getcmd(node) == wDataReq.abox_addlink ) {
const char* uid = wDataReq.getid(node);
iONode direntry = wDataReq.getdirentry(node);
if( direntry != NULL && wDirEntry.getfileentry(direntry) != NULL ) {
iONode fileentry = wDirEntry.getfileentry(direntry);
TraceOp.trc( "aboxdlg", TRCLEVEL_INFO, __LINE__, 9999, "uid=%s for [%s] added=[%s]", uid, wFileEntry.getfname(fileentry), m_AddedFilename );
/*
char* s = StrOp.fmt("%s:\nUID=%s\n%s=%s", wxGetApp().getCMsg("upload"), uid, wxGetApp().getCMsg("file"), wFileEntry.getfname(fileentry) );
int action = wxMessageDialog( this, wxString(s,wxConvUTF8), _T("Rocrail"), wxOK ).ShowModal();
StrOp.free(s);
*/
if( StrOp.equals( wFileEntry.getfname(fileentry), m_AddedFilename ) ) {
TraceOp.trc( "aboxdlg", TRCLEVEL_INFO, __LINE__, 9999, "start upload of [%s]...", wFileEntry.getfname(fileentry) );
StrOp.copy( m_AddedUID, uid );
iONode cmd = NodeOp.inst( wDataReq.name(), NULL, ELEMENT_NODE );
wDataReq.setcmd( cmd, wDataReq.abox_filedata );
wDataReq.setid( cmd, uid );
wDataReq.setcategory(cmd, wFileEntry.getcategory(fileentry));
wDataReq.setfilename(cmd, FileOp.ripPath(m_AddedFilename));
wDataReq.setdatapart(cmd, 0);
if( readDataBlock(wFileEntry.getfname(fileentry), cmd, 0) ) {
m_AddedFilename[0] = '\0';
m_AddedUID[0] = '\0';
EnableDlg(true);
}
wxGetApp().sendToRocrail( cmd );
cmd->base.del(cmd);
}
}
}
else if( wDataReq.getcmd(node) == wDataReq.abox_getdata ) {
TraceOp.trc( "aboxdlg", TRCLEVEL_INFO, __LINE__, 9999, "getdata..." );
/*
* <datareq cmd="11" id="20150816093859345" category="Zomaar" filename="paspoort-2014.jpeg" datapart="0" controlcode="" server="infw5601F5A0" data="FFD8FFE0
*/
if( StrOp.equals(m_DownloadUID, wDataReq.getid(node)) ) {
if(wDataReq.getrc(node) != 0) {
m_DownloadFilename[0] = '\0';
m_DownloadUID[0] = '\0';
m_DownloadPart = -1;
m_labDownloadState->SetLabel(wxT(""));
EnableDlg(true);
int action = wxMessageDialog( this, wxT("Error getting file data!"), _T("Rocrail"), wxOK | wxICON_EXCLAMATION ).ShowModal();
return;
}
wxString tempdir = wxFileName::GetTempDir();
char* filepath = StrOp.fmt("%s%c%s", (const char*)tempdir.mb_str(wxConvUTF8), SystemOp.getFileSeparator(), wDataReq.getfilename(node) );
iOFile f = FileOp.inst(filepath, OPEN_APPEND);
if( f != NULL ) {
const char* byteStr = wDataReq.getdata(node);
byte* filedata = StrOp.strToByte( byteStr );
int len = StrOp.len(byteStr)/2;
FileOp.write(f, (char*)filedata, len);
FileOp.base.del(f);
freeMem(filedata);
}
if( !wDataReq.isack(node) ) {
// get next part
iONode cmd = NodeOp.inst( wDataReq.name(), NULL, ELEMENT_NODE );
wDataReq.setcmd( cmd, wDataReq.abox_getdata );
wDataReq.setid( cmd, wDataReq.getid(node) );
wDataReq.setcategory( cmd, wDataReq.getcategory(node) );
wDataReq.setfilename(cmd, wDataReq.getfilename(node) );
m_DownloadPart++;
m_labDownloadState->SetLabel(wxString::Format(wxT("%d"), m_DownloadPart));
wDataReq.setdatapart(cmd, m_DownloadPart);
wxGetApp().sendToRocrail( cmd );
cmd->base.del(cmd);
}
else {
m_DownloadFilename[0] = '\0';
m_DownloadUID[0] = '\0';
m_DownloadPart = -1;
m_labDownloadState->SetLabel(wxT(""));
EnableDlg(true);
executeStub(filepath);
}
StrOp.free(filepath);
}
}
else if( wDataReq.getcmd(node) == wDataReq.abox_filedata ) {
TraceOp.trc( "aboxdlg", TRCLEVEL_INFO, __LINE__, 9999, "filedata..." );
// <datareq cmd="10" id="20150816081059871" category="Zomaar" datapart="0" dataparts="33" totalsize="794973" data=""
// controlcode="" server="infw2368A4D0" ack="true"/>
if( wDataReq.isack(node) ) {
if( StrOp.equals(m_AddedUID, wDataReq.getid(node) ) ) {
iONode cmd = NodeOp.inst( wDataReq.name(), NULL, ELEMENT_NODE );
wDataReq.setcmd( cmd, wDataReq.abox_filedata );
wDataReq.setid( cmd, wDataReq.getid(node) );
wDataReq.setcategory(cmd, wDataReq.getcategory(node));
wDataReq.setfilename(cmd, wDataReq.getfilename(node));
wDataReq.setdatapart(cmd, wDataReq.getdatapart(node)+1);
m_labUploadState->SetLabel(wxString::Format(wxT("%d"), wDataReq.getdatapart(cmd)));
if( readDataBlock(m_AddedFilename, cmd, wDataReq.getdatapart(cmd)) ) {
m_AddedFilename[0] = '\0';
m_AddedUID[0] = '\0';
m_labUploadState->SetLabel(wxT(""));
EnableDlg(true);
}
wxGetApp().sendToRocrail( cmd );
cmd->base.del(cmd);
}
}
}
else if( wDataReq.getcmd(node) == wDataReq.abox_getcategories ) {
m_ReadOnly = wDataReq.isreadonly(node)?true:false;
if( m_Enable )
m_Add->Enable(!m_ReadOnly);
m_Category->Clear();
wxString findtext = m_FindText->GetValue();
m_FindText->Clear();
m_FindText->SetValue(findtext);
iOStrTok tok = StrTokOp.inst( wDataReq.getcategory( node ), ',' );
while( StrTokOp.hasMoreTokens(tok) ) {
const char* category = StrTokOp.nextToken( tok );
m_Category->Append( wxString(category,wxConvUTF8) );
m_FindText->Append( wxString(category,wxConvUTF8) );
}
}
else if( wDataReq.getcmd(node) == wDataReq.abox_find ) {
m_SelectedStub = wxNOT_FOUND;
m_Open->Enable(false);
m_Modify->Enable(false);
m_Delete->Enable(false);
m_Stubs->DeleteAllItems();
clearStubList();
char* s = NodeOp.base.toString(node);
TraceOp.trc( "aboxdlg", TRCLEVEL_INFO, __LINE__, 9999, "found [%s]", s );
StrOp.free(s);
iONode stub = NodeOp.findNode( node, "stub");
while( stub != NULL ) {
iONode clone = (iONode)NodeOp.base.clone(stub);
ListOp.add(m_StubList, (obj)clone);
stub = NodeOp.findNextNode( node, stub);
}
initResult();
if(wDataReq.istoomanyhits(node)) {
// Show warning.
int action = wxMessageDialog( this, wxGetApp().getMsg("toomanyhits"), _T("Rocrail"), wxOK | wxICON_EXCLAMATION ).ShowModal();
}
}
}
// Read one table for one sweep value. Returns:
// 0 ... performed normally
// 1 ... error occurred
// Arguments:
// f ... pointer to file for reading
// debugMode ... debug messages flag
// fileName ... name of the file
// sweep ... sweep parameter name
// numOfVariables ... number of variables in table
// type ... type of variables with exeption of scale
// numOfVectors ... number of variables and probes in table
// faSweep ... pointer to fast access structure for sweep array
// tmpArray ... array of pointers to arrays
// faPtr ... array of fast access structures for vector arrays
// scale ... scale name
// name ... array of vector names
// dataList ... list of data dictionaries
int readTable(FILE *f, int debugMode, int postVersion,
const char *fileName, PyObject *sweep,
int numOfVariables, int type, int numOfVectors,
struct FastArray *faSweep, PyObject **tmpArray,
struct FastArray *faPtr, char *scale, char **name, PyObject *dataList)
{
int i, j, num, offset = 0, numOfColumns = numOfVectors;
npy_intp dims;
float *rawDataPos, *rawData = NULL;
PyObject *data = NULL;
// Read raw data blocks.
do num = readDataBlock(f, debugMode, postVersion, fileName, &rawData, &offset);
while(num == 0);
if(num > 0) goto readTableFailed;
data = PyDict_New(); // Create an empty dictionary.
if(data == NULL)
{
if(debugMode)
fprintf(debugFile, "HSpiceRead: failed to create data dictionary.\n");
goto readTableFailed;
}
// Increase number of columns if variables with exeption of scale are complex.
if(type == complex_var) numOfColumns = numOfColumns + numOfVariables - 1;
rawDataPos = rawData;
// TODO check sweeps with postVersion == 2001
if(postVersion == 2001)
{
if(sweep == NULL) num = (offset - 1) / numOfColumns
/ 2; // Number of rows.
else
{
num = (offset - 2) / numOfColumns / 2;
*((npy_double *)(faSweep->pos)) = *((double*)rawDataPos);
// Save sweep value.
rawDataPos = rawDataPos + 2;
faSweep->pos = faSweep->pos + faSweep->stride;
}
}
else
{
if(sweep == NULL) num = (offset - 1) / numOfColumns; // Number of rows.
else
{
num = (offset - 2) / numOfColumns;
*((npy_double *)(faSweep->pos)) = *rawDataPos; // Save sweep value.
rawDataPos = rawDataPos + 1;
faSweep->pos = faSweep->pos + faSweep->stride;
}
}
if(debugMode) fprintf(debugFile, "num=%d\n", num);
for(i = 0; i < numOfVectors; i++)
{
// Create array for i-th vector.
dims=num;
if(type == complex_var && i > 0 && i < numOfVariables)
tmpArray[i] = PyArray_SimpleNew(1, &dims, PyArray_CDOUBLE);
else
tmpArray[i] = PyArray_SimpleNew(1, &dims, PyArray_DOUBLE);
if(tmpArray[i] == NULL)
{
if(debugMode)
fprintf(debugFile, "HSpiceRead: failed to create array.\n");
for(j = 0; j < i + 1; j++) Py_XDECREF(tmpArray[j]);
goto readTableFailed;
}
}
for(i = 0; i < numOfVectors; i++) // Prepare fast access structures.
{
faPtr[i].data = ((PyArrayObject *)(tmpArray[i]))->data;
faPtr[i].pos = ((PyArrayObject *)(tmpArray[i]))->data;
faPtr[i].stride =
((PyArrayObject *)(tmpArray[i]))->strides[((PyArrayObject *)(tmpArray[i]))->nd -
1];
faPtr[i].length = PyArray_Size(tmpArray[i]);
}
if(postVersion == 2001)
{
for(i = 0; i < num; i++) // Save raw data.
{
struct FastArray *faPos = faPtr;
for(j = 0; j < numOfVectors; j++)
{
if(type == complex_var && j > 0 && j < numOfVariables)
{
((npy_cdouble *)(faPos->pos))->real = *((double *)rawDataPos);
rawDataPos = rawDataPos + 2;
((npy_cdouble *)(faPos->pos))->imag = *((double *)rawDataPos);
} else *((npy_double *)(faPos->pos)) = *((double *)rawDataPos);
rawDataPos = rawDataPos + 2;
faPos->pos = faPos->pos + faPos->stride;
faPos = faPos + 1;
}
}
}
else
{
for(i = 0; i < num; i++) // Save raw data.
{
struct FastArray *faPos = faPtr;
for(j = 0; j < numOfVectors; j++)
{
if(type == complex_var && j > 0 && j < numOfVariables)
{
((npy_cdouble *)(faPos->pos))->real = *rawDataPos;
rawDataPos = rawDataPos + 1;
((npy_cdouble *)(faPos->pos))->imag = *rawDataPos;
} else *((npy_double *)(faPos->pos)) = *rawDataPos;
rawDataPos = rawDataPos + 1;
faPos->pos = faPos->pos + faPos->stride;
faPos = faPos + 1;
}
}
}
PyMem_Free(rawData);
rawData = NULL;
// Insert vectors into dictionary.
num = PyDict_SetItemString(data, scale, tmpArray[0]);
i = -1;
if(num == 0) for(i = 0; i < numOfVectors - 1; i++)
{
num = PyDict_SetItemString(data, name[i], tmpArray[i + 1]);
if(num != 0) break;
}
for(j = 0; j < numOfVectors; j++) Py_XDECREF(tmpArray[j]);
if(num)
{
if(debugMode) {
if(i == -1) fprintf(debugFile,
"HSpiceRead: failed to insert vector %s into dictionary.\n",
scale);
else fprintf(debugFile,
"HSpiceRead: failed to insert vector %s into dictionary.\n",
name[i]);
}
goto readTableFailed;
}
// Insert table into the list of data dictionaries.
num = PyList_Append(dataList, data);
if(num)
{
if(debugMode) fprintf(debugFile,
"HSpiceRead: failed to append table to the list of data dictionaries.\n");
goto readTableFailed;
}
Py_XDECREF(data);
data = NULL;
return 0;
readTableFailed:
PyMem_Free(rawData);
Py_XDECREF(data);
return 1;
}
int main(void)
{
long j=0;
// long t=0, b=0;
byte i;
// byte tmp[60];
// byte rxPtr = 0;
// byte rxLen = 0;
TRIS_KS = TRIS_IN;
initBus();
for(i=0; i<NUM_SLAVES; i++)
setReq(i, 0);
ADPCFG = 0xFFFF;
LATB = 0;
TRISB = 0;
initMasterUart();
initInterruptUarts();
for(j=0; j<25000; j++);
unsigned char emptyLine[]=" ";
showString(emptyLine, 0);
showString(emptyLine, 1);
for(j=0; j<25000; j++);
showString("Diagnostic?", 0);
for(j=0; j<25000 && ((pollStatus() & 0x80) == 0); j++);
if(pollStatus() & 0x80)
diagBootMode();
showString("Starting up... ", 0);
showString(" ", 1);
while(1)
{
byte c = waitchar(0);
long t1, t2;
switch(c)
{
case HOST_CMD_SYNC:
{
sendByte(HOST_REPLY_SUCCESS);
break;
}
case HOST_CMD_PING:
{
t1 = waitchar(1);
if(t1 == HOST_CMD_PING)
sendByte(HOST_REPLY_SUCCESS);
else
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
case HOST_CMD_SYSCHECK:
{
byte err=0;
t1 = waitchar(1);
if(t1 != HOST_CMD_SYSCHECK)
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
for(i=0; i<NUM_SLAVES; i++)
{
switch(busWriteByte(BUS_CMD_PING, i))
{
case BUS_ERROR:
err++;
break;
case BUS_FAILURE:
err++;
break;
case 0:
{
byte len = readDataBlock(i);
switch(len)
{
case 0:
break;
case BUS_ERROR:
case BUS_FAILURE:
default:
err++;
}
}
break;
}
}
if(err == 0)
sendByte(HOST_REPLY_SUCCESS);
else
sendByte(HOST_REPLY_FAILURE);
break;
}
case HOST_CMD_DEPTH:
{
t1 = waitchar(1);
if(t1 != HOST_CMD_DEPTH)
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
if(busWriteByte(BUS_CMD_DEPTH, SLAVE_ID_DEPTH) != 0)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
int len = readDataBlock(SLAVE_ID_DEPTH);
if(len != 2)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
sendByte(HOST_REPLY_DEPTH);
sendByte(rxBuf[0]);
sendByte(rxBuf[1]);
byte cs = HOST_REPLY_DEPTH+rxBuf[0]+rxBuf[1];
sendByte(cs);
break;
}
case HOST_CMD_THRUSTERSTATE:
{
t1 = waitchar(1);
if(t1 != HOST_CMD_THRUSTERSTATE)
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
if(busWriteByte(BUS_CMD_THRUSTER_STATE, SLAVE_ID_THRUSTERS) != 0)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
int len = readDataBlock(SLAVE_ID_THRUSTERS);
if(len != 1)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
sendByte(HOST_REPLY_THRUSTERSTATE);
sendByte(rxBuf[0]);
byte cs = HOST_REPLY_THRUSTERSTATE+rxBuf[0];
sendByte(cs);
break;
}
case HOST_CMD_BOARDSTATUS:
{
t1 = waitchar(1);
if(t1 != HOST_CMD_BOARDSTATUS)
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
if(busWriteByte(BUS_CMD_BOARDSTATUS, SLAVE_ID_POWERBOARD) != 0)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
byte len = readDataBlock(SLAVE_ID_POWERBOARD);
if(len!=1)
{
sendByte(HOST_REPLY_FAILURE);
} else
{
rxBuf[0] &= 0xFD; // Clear kill switch bit
// Set kill switch bit based on the GPIO kill input
if(IN_KS == 1)
rxBuf[0] |= 0x02;
sendByte(HOST_REPLY_BOARDSTATUS);
sendByte(rxBuf[0]);
sendByte(HOST_REPLY_BOARDSTATUS+rxBuf[0]);
}
break;
}
case HOST_CMD_HARDKILL:
{
for(i=0; i<5; i++)
rxBuf[i] = waitchar(1);
byte cflag=0;
for(i=0; i<5; i++)
{
if(rxBuf[i] != hkSafety[i])
cflag=1;
}
if(cflag == 1)
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
} else
{
if(busWriteByte(BUS_CMD_HARDKILL, SLAVE_ID_HARDKILL) != 0)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
sendByte(HOST_REPLY_SUCCESS);
}
break;
}
case HOST_CMD_MARKER:
{
t1 = waitchar(1);
t2 = waitchar(1);
if((t1 != 0 && t1 != 1) || (t1+HOST_CMD_MARKER != t2))
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
if(busWriteByte(t1==0 ? BUS_CMD_MARKER1 : BUS_CMD_MARKER2, SLAVE_ID_MARKERS) != 0)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
sendByte(HOST_REPLY_SUCCESS);
break;
}
case HOST_CMD_BACKLIGHT:
{
t1 = waitchar(1);
t2 = waitchar(1);
const static unsigned char blCommands[]=
{BUS_CMD_LCD_LIGHT_OFF, BUS_CMD_LCD_LIGHT_ON, BUS_CMD_LCD_LIGHT_FLASH};
if((t1 != 0 && t1 != 1 && t1 != 2) || (t1+HOST_CMD_BACKLIGHT != t2))
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
if(busWriteByte(blCommands[t1], SLAVE_ID_LCD) != 0)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
sendByte(HOST_REPLY_SUCCESS);
break;
}
case HOST_CMD_THRUSTERS:
{
for(i=0; i<5; i++)
rxBuf[i] = waitchar(1);
t1 = waitchar(1);
t2 = waitchar(1);
byte cflag=0;
byte cs=0;
// Check the special sequence
for(i=0; i<5; i++)
{
cs += rxBuf[i];
if(rxBuf[i] != tkSafety[i])
cflag=1;
}
cs += t1 + HOST_CMD_THRUSTERS;
const static unsigned char tkCommands[]=
{
BUS_CMD_THRUSTER1_OFF, BUS_CMD_THRUSTER2_OFF,
BUS_CMD_THRUSTER3_OFF, BUS_CMD_THRUSTER4_OFF,
BUS_CMD_THRUSTER1_ON, BUS_CMD_THRUSTER2_ON,
BUS_CMD_THRUSTER3_ON, BUS_CMD_THRUSTER4_ON
};
if(cflag == 1 || t1 > 7 || (t2 != cs))
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
} else
{
if(busWriteByte(tkCommands[t1], SLAVE_ID_THRUSTERS) != 0)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
}
sendByte(HOST_REPLY_SUCCESS);
break;
}
case HOST_CMD_TEMPERATURE:
{
t1 = waitchar(1);
if(t1 != HOST_CMD_TEMPERATURE)
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
if(busWriteByte(BUS_CMD_TEMP, SLAVE_ID_TEMP) != 0)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
int len = readDataBlock(SLAVE_ID_TEMP);
if(len != 5)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
sendByte(HOST_REPLY_TEMPERATURE);
byte cs=0;
for(i=0; i<5; i++)
{
cs += rxBuf[i];
sendByte(rxBuf[i]);
}
sendByte(cs + HOST_REPLY_TEMPERATURE);
break;
}
case HOST_CMD_PRINTTEXT:
{
t1 = waitchar(1);
byte cs=HOST_CMD_PRINTTEXT+t1;
for(i=0; i<16; i++)
{
rxBuf[i] = waitchar(1);
cs += rxBuf[i];
}
t2 = waitchar(1);
if(t2 != cs || t1 > 1)
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
int err=0;
for(i=0; i<16 && err==0; i++)
{
err+=busWriteByte(BUS_CMD_LCD_WRITE, SLAVE_ID_LCD);
err+=busWriteByte(t1*16+i, SLAVE_ID_LCD);
err+=busWriteByte(rxBuf[i], SLAVE_ID_LCD);
}
err+=busWriteByte(BUS_CMD_LCD_REFRESH, SLAVE_ID_LCD);
if(err != 0)
sendByte(HOST_REPLY_FAILURE);
else
sendByte(HOST_REPLY_SUCCESS);
break;
}
case HOST_CMD_SONAR:
{
t1 = waitchar(1);
byte cs=HOST_CMD_SONAR+t1;
if(t1 != HOST_CMD_SONAR)
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
if(busWriteByte(BUS_CMD_SONAR, SLAVE_ID_SONAR) != 0)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
int len = readDataBlock(SLAVE_ID_SONAR);
if(len != 5)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
sendByte(HOST_REPLY_SONAR);
cs=0;
for(i=0; i<5; i++)
{
cs += rxBuf[i];
sendByte(rxBuf[i]);
}
sendByte(cs + HOST_REPLY_SONAR);
break;
}
case HOST_CMD_RUNTIMEDIAG:
{
t1 = waitchar(1);
t2 = waitchar(1);
if((t1 != 0 && t1 != 1) || (t1+HOST_CMD_RUNTIMEDIAG != t2))
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
diagMsg=t1;
if(t1==0)
showString("Runtime Diag Off", 1);
else
showString("Runtime Diag On ", 1);
sendByte(HOST_REPLY_SUCCESS);
break;
}
case HOST_CMD_SETSPEED:
{
t1 = 0; /* Error counter */
/* 12 bytes of speed, plus checksum */
for(i=0; i<9; i++)
rxBuf[i] = waitchar(1);
for(i=0; i<8; i++)
t1 += rxBuf[i];
t1 += HOST_CMD_SETSPEED;
if(rxBuf[8] != (t1 & 0xFF))
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
failsafeExpired = 0; /* Reset failsafe mechanism */
t1 = 0;
if(busWriteByte(SLAVE_MM1_WRITE_CMD, SLAVE_ID_MM1) != 0) t1++;
if(busWriteByte(rxBuf[0], SLAVE_ID_MM1) != 0) t1++;
if(busWriteByte(rxBuf[1], SLAVE_ID_MM1) != 0) t1++;
if(busWriteByte(SLAVE_MM2_WRITE_CMD, SLAVE_ID_MM2) != 0) t1++;
if(busWriteByte(rxBuf[2], SLAVE_ID_MM2) != 0) t1++;
if(busWriteByte(rxBuf[3], SLAVE_ID_MM2) != 0) t1++;
if(busWriteByte(SLAVE_MM3_WRITE_CMD, SLAVE_ID_MM3) != 0) t1++;
if(busWriteByte(rxBuf[4], SLAVE_ID_MM3) != 0) t1++;
if(busWriteByte(rxBuf[5], SLAVE_ID_MM3) != 0) t1++;
UARTSendSpeed(U2_MM_ADDR, rxBuf[6], rxBuf[7], 1);
if(t1 == 0)
sendByte(HOST_REPLY_SUCCESS);
else
sendByte(HOST_REPLY_FAILURE);
break;
}
case HOST_CMD_MOTOR_READ:
{
unsigned char resp[4];
t1 = waitchar(1);
if(t1 != HOST_CMD_MOTOR_READ)
{
sendByte(HOST_REPLY_BADCHKSUM);
break;
}
t1 = 0;
if(busWriteByte(SLAVE_MM1_READ_CMD, SLAVE_ID_MM1) != 0) t1++;
if(readDataBlock(SLAVE_ID_MM1) != 1) t1++;
resp[0] = rxBuf[0];
if(busWriteByte(SLAVE_MM2_READ_CMD, SLAVE_ID_MM2) != 0) t1++;
if(readDataBlock(SLAVE_ID_MM2) != 1) t1++;
resp[1] = rxBuf[0];
if(busWriteByte(SLAVE_MM3_READ_CMD, SLAVE_ID_MM3) != 0) t1++;
if(readDataBlock(SLAVE_ID_MM3) != 1) t1++;
resp[2] = rxBuf[0];
if(U2CanRead())
resp[3] = U2ReadByte();
else
resp[3] = 0xFF;
if(t1 != 0)
{
sendByte(HOST_REPLY_FAILURE);
break;
}
sendByte(HOST_CMD_MOTOR_REPLY);
sendByte(resp[0]);
sendByte(resp[1]);
sendByte(resp[2]);
sendByte(resp[3]);
sendByte(HOST_CMD_MOTOR_REPLY + resp[0] + resp[1] + resp[2] + resp[3]);
break;
}
}
}
}
void showBootDiag(int mode)
{
unsigned char tmp[16];
if(mode == 0)
{
//sprintf(tmp, "Status: %02X ", pollStatus());
byte sta = pollStatus();
sprintf(tmp, "Sta: %02X %c%c%c%c%c%c%c%c", sta,
(sta & 0x80) ? 'S' : '-',
(sta & 0x40) ? '?' : '-',
(sta & 0x20) ? '1' : '-',
(sta & 0x10) ? '2' : '-',
(sta & 0x08) ? '3' : '-',
(sta & 0x04) ? '4' : '-',
(sta & 0x02) ? 'K' : '-',
(sta & 0x01) ? 'W' : '-');
showString(tmp, 1);
}
if(mode == 1)
{
if(busWriteByte(BUS_CMD_DEPTH, SLAVE_ID_DEPTH) != 0)
{
showString("DEPTH FAIL ", 1);
return;
}
int len = readDataBlock(SLAVE_ID_DEPTH);
if(len != 2)
{
showString("DEPTH LEN FAIL", 1);
return;
}
sprintf(tmp, "Depth: %02X %02X ", rxBuf[0], rxBuf[1]);
showString(tmp, 1);
}
if(mode == 2)
{
if(busWriteByte(BUS_CMD_TEMP, SLAVE_ID_TEMP) != 0)
{
showString("TEMP FAIL ", 1);
return;
}
int len = readDataBlock(SLAVE_ID_TEMP);
if(len != 5)
{
showString("TEMP LEN FAIL ", 1);
return;
}
sprintf(tmp, "T:%02X %02X %02X %02X %02X", rxBuf[0], rxBuf[1], rxBuf[2], rxBuf[3], rxBuf[4]);
showString(tmp, 1);
}
}
