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); } }