void send2DebugPort(unsigned char* protData, unsigned char hilOn){ unsigned int bufLen,i; // add the data to the circular buffer for(i = 1; i <= protData[0]; i += 1 ) { writeBack(logBuffer, protData[i] ); } // get the Length of the logBuffer bufLen = getLength(logBuffer); // if the interrupt catched up with the circularBuffer // then turn on the DMA if(!(DMA0CONbits.CHEN) && bufLen> 0){ // Configure the bytes to send DMA0CNT = bufLen<= (MAXSEND-1)? bufLen-1: MAXSEND-1; // copy the buffer to the DMA channel outgoing buffer copyBufferToDMA((unsigned char) DMA0CNT+1); // Enable the DMA DMA0CONbits.CHEN = 1; // Init the transmission DMA0REQbits.FORCE = 1; } }
void UART_putChar(uint8_t id, char ch) { if(id == UART1_ID){ if (getLength(transmitBufferUart1) != QUEUESIZE) { writeBack(transmitBufferUart1, ch); if (U1STAbits.TRMT) { IFS0bits.U1TXIF = 1; } } }else if(id == UART2_ID){ if (getLength(transmitBufferUart2) != QUEUESIZE) { writeBack(transmitBufferUart2, ch); if (U2STAbits.TRMT) { IFS1bits.U2TXIF = 1; } } } }
/** * Write data to the file. * * Write from buffer, len bytes to the current seek position. * On each invocation to write, the seek position of the file handle * is incremented atomically, by the number of bytes returned. * * The cached filesize in the FD is updated on this call. Also, the * FT file size is updated only if a new page(s) has been written too, * to reduce the number of FT writes. * * @param fd File handle * @param buffer the buffer from which to write data * @param len number of bytes to write * @return number of bytes written on success, MICROBIT_NO_RESOURCES if data did * not get written to flash or the file system has not been initialised, * or this file was not opened with the MB_WRITE flag set, MICROBIT_INVALID_PARAMETER * if the given file handle is invalid. * * @code * MicroBitFileSystem f(); * int fd = f.open("test.txt", MB_WRITE); * if(f.write(fd, "hello!", 7) != 7) * print("error writing"); * @endcode */ int MicroBitFileSystem::write(int fd, uint8_t* buffer, int size) { FileDescriptor *file; int bytesCopied = 0; int segmentSize; // Protect against accidental re-initialisation if ((status & MBFS_STATUS_INITIALISED) == 0) return MICROBIT_NOT_SUPPORTED; // Ensure the file is open. file = getFileDescriptor(fd); if (file == NULL || buffer == NULL || size == 0) return MICROBIT_INVALID_PARAMETER; // Determine how to handle the write. If the buffer size is less than our cache size, // write the data via the cache. Otherwise, a direct write through is likely more efficient. // This may take a few iterations if the cache is already quite full. if (size < MBFS_CACHE_SIZE) { while (bytesCopied < size) { segmentSize = min(size, MBFS_CACHE_SIZE - file->cacheLength); memcpy(&file->cache[file->cacheLength], buffer, segmentSize); file->cacheLength += segmentSize; bytesCopied += segmentSize; if (file->cacheLength == MBFS_CACHE_SIZE) writeBack(file); } return bytesCopied; } // If we have a relatively large block, then write it directly ( writeBack(file); return writeBuffer(file, buffer, size); }
// Interrupt service routine for SPI1 Slave void __attribute__((__interrupt__, no_auto_psv)) _SPI1Interrupt(void) { // if we received a byte if (SPI1STATbits.SPIRBF == 1) { // put the received data in the circular buffer writeBack(protBuffer, (unsigned char)SPI1BUF); } // clear the interrupt IFS0bits.SPI1IF = 0; }
void UART2PutChar( char ch ) { if(getLength(UART_transmitBuffer)!=BSIZE) { writeBack(UART_transmitBuffer,ch); IEC1bits.U2TXIE=1; } if(U2STAbits.TRMT) { IFS1bits.U2TXIF=1; } }
/**************************************************************************** Function IntUart1Handler Parameters None. Returns None. Description Interrupt Handle for the uart. with the PIC32 architecture both send and receive are handled within the same interrupt Notes Author Max Dunne, 2011.11.10 ****************************************************************************/ void __ISR(_UART2_VECTOR, ipl4) IntUart2Handler(void) { if (mU2RXGetIntFlag()) { mU2RXClearIntFlag(); writeBack(receiveBufferUart2, (unsigned char) U2RXREG); } if (mU2TXGetIntFlag()) { mU2TXClearIntFlag(); if (!(getLength(transmitBufferUart2) == 0)) { U2TXREG = readFront(transmitBufferUart2); } } }
void first4thread::performAuthAction(QString action, QDomElement e) { Auth auth; QDomDocument doc("first4"); if(action.section(".", 1, 1) == "Login") { int answ = auth.loginUser(e.attribute("Username"), e.attribute("Password")); QDomElement root = doc.createElement("first4server"); doc.appendChild(root); QDomElement request = doc.createElement("Request"); request.setAttribute("Action", action); request.setAttribute("Answer", answ); if(answ == 0) { request.setAttribute("Token", token); request.setAttribute("Fullname", fullname); root.appendChild(request); writeBack(doc); } else { root.appendChild(request); writeBack(doc); closeConnection(); } } else if(action.section(".", 1, 1) == "Logout") { if(auth.logoutUser(e.attribute("Username"))) { tcpSocket->flush(); tcpSocket->disconnectFromHost(); this->terminate(); } } else qDebug() << "Processing Auth action: " << action; }
void __attribute__((__interrupt__, no_auto_psv)) _U2RXInterrupt(void) { while(U2STAbits.URXDA == 1){ writeBack(UART_receiveBuffer, (unsigned char)U2RXREG); } // If there was an overun error clear it and continue if (U2STAbits.OERR == 1){ U2STAbits.OERR = 0; } // clear the interrupt IFS1bits.U2RXIF = 0; }
// Interrupt service routine for U2 HIL protocol port void __attribute__((__interrupt__, no_auto_psv)) _U2RXInterrupt(void){ // Read the buffer while it has data // and add it to the circular buffer while(U2STAbits.URXDA == 1){ writeBack(uartBufferIn, (unsigned char)U2RXREG); } // If there was an overun error clear it and continue if (U2STAbits.OERR == 1){ U2STAbits.OERR = 0; } // clear the interrupt IFS1bits.U2RXIF = 0; }
/** * @Function PutChar(char ch) * @param ch - the char to be sent out the serial port * @return None. * @brief adds char to the end of the circular buffer and forces the interrupt flag * high if nothing is currently transmitting * @author Max Dunne, 2011.11.10 */ void PutChar(char ch) { if (getLength(transmitBuffer) != QUEUESIZE) { AddingToTransmit = TRUE; writeBack(transmitBuffer, ch); AddingToTransmit = FALSE; if (U1STAbits.TRMT) { INTSetFlag(INT_U1TX); } //re-enter the interrupt if we removed a character while getting another one if (TransmitCollisionOccured) { INTSetFlag(INT_U1TX); TransmitCollisionOccured = FALSE; } } }
void SequentialFile::commit() { record_frame_t* next_to_current_record = (record_frame_t*)offset2record(next_write_offset); next_to_current_record--; SequentialFile::Record* current_record = ((Record*)next_to_current_record)->getStartHeader(); if(current_record->isEndOfTransaction()) // no operations to commit return; if(current_record->isValid()) { current_record->setEndOfTransaction( true ); writeBack(); } // else: no operations in database }
void PutChar(char ch) { if (getLength(transmitBuffer) != QUEUESIZE) { writeBack(transmitBuffer, ch); if (!INTGetEnable(INT_U1TX)) { INTEnable(INT_U1TX, INT_ENABLED); //INTSetFlag(INT_U1TX); } // if (U1STAbits.TRMT) { // INTEnable(INT_U1TX, INT_ENABLED); // INTSetFlag(INT_U1TX); // } else if (!INTGetEnable(INT_U1TX)) { // INTEnable(INT_U1TX, INT_ENABLED); // //INTSetFlag(INT_U1TX); // } } }
/** * Writes back all state associated with the given file to FLASH memory, * leaving the file open. * * @param fd file descriptor - obtained with open(). * @return MICROBIT_OK on success, MICROBIT_NOT_SUPPORTED if the file system has not * been initialised, MICROBIT_INVALID_PARAMETER if the given file handle * is invalid. * * @code * MicroBitFileSystem f(); * int fd = f.open("test.txt", MB_READ); * * ... * * f.flush(fd); * @endcode */ int MicroBitFileSystem::flush(int fd) { // Protect against accidental re-initialisation if ((status & MBFS_STATUS_INITIALISED) == 0) return MICROBIT_NOT_SUPPORTED; FileDescriptor *file = getFileDescriptor(fd); // Ensure the file is open. if(file == NULL) return MICROBIT_INVALID_PARAMETER; // Flush any data in the writeback cache. writeBack(file); // If the file has changed size, create an updated directory entry for the file, reflecting it's new length. if (file->dirent->length != file->length) { DirectoryEntry d = *file->dirent; d.length = file->length; // Do some optimising to reduce FLASH churn if this is the first write to a file. No need then to create a new dirent... if (file->dirent->flags == MBFS_DIRECTORY_ENTRY_NEW) { d.flags = MBFS_DIRECTORY_ENTRY_VALID; flash.flash_write(file->dirent, &d, sizeof(DirectoryEntry)); } // Otherwise, replace the dirent with a freshly allocated one, and mark the other as INVALID. else { DirectoryEntry *newDirent; uint16_t value = MBFS_DELETED; // invalidate the old directory entry and create a new one with the updated data. flash.flash_write(&file->dirent->flags, &value, 2); newDirent = createDirectoryEntry(file->directory); flash.flash_write(newDirent, &d, sizeof(DirectoryEntry)); } } return MICROBIT_OK; }
bool Log::BlockBuffer::prepareBlock(uint32_t blockId) { // Check valid id. if (!isValidBlockId(blockId)) return false; // If the block is dirty, write it back. if (!writeBack()) return false; // If the block is already loaded, just return. if (id == blockId && ready) return true; // Read in block. if (!Disk::diskRead(getBlockOffset(blockId), block, BLOCK_SIZE)) return false; id = blockId; ready = true; return true; }
void __attribute__((__interrupt__,__auto_psv__)) _U1RXInterrupt(void) { // _U1RXIF = 0; // Read the buffer while it has data // and add it to the circular buffer while (U1STAbits.URXDA == 1) { writeBack(uartMavlinkInBuffer, (unsigned char) U1RXREG); } // If there was an overun error clear it and continue if (U1STAbits.OERR == 1) { _U1RXIF = 0; U1STAbits.OERR = 0; } // clear the interrupt IFS0bits.U1RXIF = 0; }
/** * Move the current position of a file handle, to be used for * subsequent read/write calls. * * The offset modifier can be: * - MB_SEEK_SET set the absolute seek position. * - MB_SEEK_CUR set the seek position based on the current offset. * - MB_SEEK_END set the seek position from the end of the file. * E.g. to seek to 2nd-to-last byte, use offset=-1. * * @param fd file handle, obtained with open() * @param offset new offset, can be positive/negative. * @param flags * @return new offset position on success, MICROBIT_NOT_SUPPORTED if the file system * is not intiialised, MICROBIT_INVALID_PARAMETER if the flag given is invalid * or the file handle is invalid. * * @code * MicroBitFileSystem f; * int fd = f.open("test.txt", MB_READ); * f.seek(fd, -100, MB_SEEK_END); //100 bytes before end of file. * @endcode */ int MicroBitFileSystem::seek(int fd, int offset, uint8_t flags) { FileDescriptor *file; int position; // Protect against accidental re-initialisation if ((status & MBFS_STATUS_INITIALISED) == 0) return MICROBIT_NOT_SUPPORTED; // Ensure the file is open. file = getFileDescriptor(fd); if (file == NULL) return MICROBIT_INVALID_PARAMETER; // Flush any data in the writeback cache. writeBack(file); position = file->seek; if(flags == MB_SEEK_SET) position = offset; if(flags == MB_SEEK_END) position = file->length + offset; if (flags == MB_SEEK_CUR) position = file->seek + offset; if (position < 0 || (uint32_t)position > file->length) return MICROBIT_INVALID_PARAMETER; file->seek = position; return position; }
/**************************************************************************** Function IntUart1Handler Parameters None. Returns None. Description Interrupt Handle for the uart. with the PIC32 architecture both send and receive are handled within the same interrupt Notes Author Max Dunne, 2011.11.10 ****************************************************************************/ void __ISR(_UART1_VECTOR, ipl4) IntUart1Handler(void) { if (INTGetFlag(INT_U1RX)) { INTClearFlag(INT_U1RX); if (!GettingFromReceive) { writeBack(receiveBuffer, (unsigned char) U1RXREG); } else { //acknowledge we have a collision and return ReceiveCollisionOccured = TRUE; } } if (INTGetFlag(INT_U1TX)) { INTClearFlag(INT_U1TX); if (!(getLength(transmitBuffer) == 0)) { if (!AddingToTransmit) { U1TXREG = readFront(transmitBuffer); } else { //acknowledge we have a collision and return TransmitCollisionOccured = TRUE; } } } }
/**************************************************************************** Function IntUart1Handler Parameters None. Returns None. Description Interrupt Handle for the uart. with the PIC32 architecture both send and receive are handled within the same interrupt Notes Author Max Dunne, 2011.11.10 ****************************************************************************/ void __ISR(_UART1_VECTOR, IPL4AUTO) IntUart1Handler(void) { static int transmitcount = 0; static int clearcount = 0; static int stallcount = 0; if (INTGetFlag(INT_U1RX)) { LATBbits.LATB8 ^= 1; writeBack(receiveBuffer, (unsigned char) U1RXREG); INTClearFlag(INT_U1RX); } if (INTGetFlag(INT_U1TX)) { //INTClearFlag(INT_U1TX); //IFS1bits.U1TXIF=0; //INTSTATbits. //Nop(); //LATBbits.LATB7=0; //LATBbits.LATB8=PORTCbits.RC3; //LATCbits.LATC3^=1; if (!(getLength(transmitBuffer) == 0)) { transmitcount++; U1TXREG = readFront(transmitBuffer); INTClearFlag(INT_U1TX); stallcount = 0; } else { clearcount++; //INTEnable(INT_U1TX, 0); stallcount++; Nop(); } if (stallcount > 1) { INTEnable(INT_U1TX, INT_DISABLED); INTClearFlag(INT_U1TX); } } }
/** * Read data from the file. * * Read len bytes from the current seek position in the file, into the * buffer. On each invocation to read, the seek position of the file * handle is incremented atomically, by the number of bytes returned. * * @param fd File handle, obtained with open() * @param buffer to store data * @param size number of bytes to read * @return number of bytes read on success, MICROBIT_NOT_SUPPORTED if the file * system is not initialised, or this file was not opened with the * MB_READ flag set, MICROBIT_INVALID_PARAMETER if the given file handle * is invalid. * * @code * MicroBitFileSystem f; * int fd = f.open("read.txt", MB_READ); * if(f.read(fd, buffer, 100) != 100) * print("read error"); * @endcode */ int MicroBitFileSystem::read(int fd, uint8_t* buffer, int size) { FileDescriptor *file; uint16_t block; uint8_t *readPointer; uint8_t *writePointer; uint32_t offset; uint32_t position = 0; int bytesCopied = 0; int segmentLength; // Protect against accidental re-initialisation if ((status & MBFS_STATUS_INITIALISED) == 0) return MICROBIT_NOT_SUPPORTED; // Ensure the file is open. file = getFileDescriptor(fd); if (file == NULL || buffer == NULL || size == 0) return MICROBIT_INVALID_PARAMETER; // Flush any data in the writeback cache before we change the seek pointer. writeBack(file); // Validate the read length. size = min(size, file->length - file->seek); // Find the read position. block = file->dirent->first_block; // Walk the file table until we reach the start block while (file->seek - position > MBFS_BLOCK_SIZE) { block = getNextFileBlock(block); position += MBFS_BLOCK_SIZE; } // Once we have the correct start block, handle the byte offset. offset = file->seek - position; // Now, start copying bytes into the requested buffer. writePointer = buffer; while (bytesCopied < size) { // First, determine if we need to write a partial block. readPointer = (uint8_t *)getBlock(block) + offset; segmentLength = min(size - bytesCopied, MBFS_BLOCK_SIZE - offset); if(segmentLength > 0) memcpy(writePointer, readPointer, segmentLength); bytesCopied += segmentLength; writePointer += segmentLength; offset += segmentLength; if (offset == MBFS_BLOCK_SIZE) { block = getNextFileBlock(block); offset = 0; } } file->seek += bytesCopied; return bytesCopied; }
void midgSplit (unsigned char * data) { // Static Variables: CAREFUL static unsigned char prevBuffer [2*MAXLOGLEN]; static unsigned char previousComplete =1; static unsigned char indexLast = 0; static unsigned char incompleteLen = 0; // local variables unsigned char i; unsigned char headerFound=0, noMoreBytes = 1; // FIXME: this requires MIDG_CHUNKSIZE < 257. // if data[0] is 255, then i overflows from 255 to 0 and this is infinite // Add the received bytes to the protocol parsing circular buffer for(i = 1; i <= data[0]; i += 1 ) { writeBack(midgBuffer, data[i]); } // update the noMoreBytes flag accordingly noMoreBytes = (data[0]>0)?0:1; while (!noMoreBytes){ // if the previous message was complete then read from the circular buffer // and make sure that you are into the begining of a message if(previousComplete){ while (!headerFound && !noMoreBytes) { // move along until you find a 0x81 or run out of bytes while (getLength(midgBuffer)>3 && peak(midgBuffer)!=0x81){ readFront(midgBuffer); } // if you found a dollar then make sure the next one is an A1 // You always make sure to have 4 bytes remaining (including // BOF so you can read the ID and the length) if(getLength(midgBuffer)>3 && peak(midgBuffer) == 0x81){ // read it prevBuffer[indexLast++] = readFront(midgBuffer); // if next is a 0xA1 if (peak(midgBuffer) == 0xA1){ // read the sync prevBuffer[indexLast++] = readFront(midgBuffer); // read the ID prevBuffer[indexLast++] = readFront(midgBuffer); // read the count prevBuffer[indexLast++] = readFront(midgBuffer); // start monitoring the pending count incompleteLen = prevBuffer[indexLast -1]; // and signal you found a header headerFound = 1; // and set as incomplete the sentece previousComplete = 0; } } else { noMoreBytes = 1; } // else no dollar } // while we found header && no more bytes } // if previous complete // At this point either you found a header from a previous complete // or you are reading from a message that was incomplete the last time // in any of those two cases, keep reading until you run out of bytes // or incompleteLen == 0 while ((incompleteLen > 0) && !noMoreBytes){ while (incompleteLen >0 && getLength(midgBuffer)>2){ prevBuffer[indexLast++] = readFront(midgBuffer); incompleteLen --; } // if you completed the Length of the payload and you have // at least two bytes to read to get the checksum if (incompleteLen == 0 && getLength(midgBuffer)>=2){ // read the checksum 0 prevBuffer[indexLast++] = readFront(midgBuffer); // read the checksum 1 prevBuffer[indexLast++] = readFront(midgBuffer); // Parse the message. // Note that parse does the checksumming midgParse(prevBuffer); // Reset the variables previousComplete =1; indexLast = 0; headerFound = 0; memset(prevBuffer, 0, sizeof(prevBuffer)); } else { noMoreBytes =1; } } } // big outer loop }
void logData (unsigned char hilOn, unsigned char* data4SPI){ unsigned char rawSentence[35]; // sample period variable static unsigned char samplePeriod = 1; static unsigned char tmpBuf [37]; // temp var to store the assembled message unsigned char i; unsigned char len2SPI=0; unsigned char bufLen = 0; unsigned char aknSentence[6]; memset(tmpBuf, 0, sizeof(tmpBuf)); switch (samplePeriod){ case 1: //GPS rawSentence[0] = gpsControlData.year ; rawSentence[1] = gpsControlData.month ; rawSentence[2] = gpsControlData.day ; rawSentence[3] = gpsControlData.hour ; rawSentence[4] = gpsControlData.min ; rawSentence[5] = gpsControlData.sec ; rawSentence[6] = gpsControlData.lat.chData[0]; rawSentence[7] = gpsControlData.lat.chData[1]; rawSentence[8] = gpsControlData.lat.chData[2]; rawSentence[9] = gpsControlData.lat.chData[3]; rawSentence[10] = gpsControlData.lon.chData[0]; rawSentence[11] = gpsControlData.lon.chData[1]; rawSentence[12] = gpsControlData.lon.chData[2]; rawSentence[13] = gpsControlData.lon.chData[3]; rawSentence[14] = gpsControlData.height.chData[0]; rawSentence[15] = gpsControlData.height.chData[1]; rawSentence[16] = gpsControlData.height.chData[2]; rawSentence[17] = gpsControlData.height.chData[3]; rawSentence[18] = gpsControlData.cog.chData[0]; rawSentence[19] = gpsControlData.cog.chData[1]; rawSentence[20] = gpsControlData.sog.chData[0]; rawSentence[21] = gpsControlData.sog.chData[1]; rawSentence[22] = gpsControlData.hdop.chData[0]; rawSentence[23] = gpsControlData.hdop.chData[1]; rawSentence[24] = gpsControlData.fix ; rawSentence[25] = gpsControlData.sats ; rawSentence[26] = gpsControlData.newValue ; // assemble the GPS data for protocol sending assembleMsg(&rawSentence[0], GPSMSG_LEN, GPSMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < GPSMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1] = tmpBuf[i]; } // set the total data out for SPI len2SPI = GPSMSG_LEN+7; break; case 2: // LOAD rawSentence[0] = statusControlData.load ; rawSentence[1] = statusControlData.vdetect ; rawSentence[2] = statusControlData.bVolt.chData[0] ; rawSentence[3] = statusControlData.bVolt.chData[1] ; // assemble the CPU load data for protocol sending assembleMsg(&rawSentence[0], LOADMSG_LEN, LOADMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < LOADMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1] = tmpBuf[i]; } // set the total data out for SPI len2SPI = LOADMSG_LEN+7; break; case 3: // RAW or XYZ depending if we are logging raw @ 100 #ifdef LOGRAW100 // If we need to log raw at 100 Hz rawSentence[0] = xyzControlData.Xcoord.chData[0]; rawSentence[1] = xyzControlData.Xcoord.chData[1]; rawSentence[2] = xyzControlData.Xcoord.chData[2]; rawSentence[3] = xyzControlData.Xcoord.chData[3]; rawSentence[4] = xyzControlData.Ycoord.chData[0]; rawSentence[5] = xyzControlData.Ycoord.chData[1]; rawSentence[6] = xyzControlData.Ycoord.chData[2]; rawSentence[7] = xyzControlData.Ycoord.chData[3]; rawSentence[8] = xyzControlData.Zcoord.chData[0]; rawSentence[9] = xyzControlData.Zcoord.chData[1]; rawSentence[10]= xyzControlData.Zcoord.chData[2]; rawSentence[11]= xyzControlData.Zcoord.chData[3]; rawSentence[12]= xyzControlData.VX.chData[0] ; rawSentence[13]= xyzControlData.VX.chData[1] ; rawSentence[14]= xyzControlData.VX.chData[2] ; rawSentence[15]= xyzControlData.VX.chData[3] ; rawSentence[16]= xyzControlData.VY.chData[0] ; rawSentence[17]= xyzControlData.VY.chData[1] ; rawSentence[18]= xyzControlData.VY.chData[2] ; rawSentence[19]= xyzControlData.VY.chData[3] ; rawSentence[20]= xyzControlData.VZ.chData[0] ; rawSentence[21]= xyzControlData.VZ.chData[1] ; rawSentence[22]= xyzControlData.VZ.chData[2] ; rawSentence[23]= xyzControlData.VZ.chData[3] ; // assemble the XYZ data for protocol sending assembleMsg(&rawSentence[0], XYZMSG_LEN, XYZMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < XYZMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1+len2SPI] = tmpBuf[i]; } // set the total data out for SPI len2SPI = XYZMSG_LEN+7; #else rawSentence[0] = rawControlData.gyroX.chData[0]; rawSentence[1] = rawControlData.gyroX.chData[1]; rawSentence[2] = rawControlData.gyroY.chData[0]; rawSentence[3] = rawControlData.gyroY.chData[1]; rawSentence[4] = rawControlData.gyroZ.chData[0]; rawSentence[5] = rawControlData.gyroZ.chData[1]; rawSentence[6] = rawControlData.accelX.chData[0]; rawSentence[7] = rawControlData.accelX.chData[1]; rawSentence[8] = rawControlData.accelY.chData[0]; rawSentence[9] = rawControlData.accelY.chData[1]; rawSentence[10] = rawControlData.accelZ.chData[0]; rawSentence[11] = rawControlData.accelZ.chData[1]; rawSentence[12] = rawControlData.magX.chData[0]; rawSentence[13] = rawControlData.magX.chData[1]; rawSentence[14] = rawControlData.magY.chData[0]; rawSentence[15] = rawControlData.magY.chData[1]; rawSentence[16] = rawControlData.magZ.chData[0]; rawSentence[17] = rawControlData.magZ.chData[1]; // included in SLUGS MKII rawSentence[18] = rawControlData.baro.chData[0]; rawSentence[19] = rawControlData.baro.chData[1]; rawSentence[20] = rawControlData.pito.chData[0]; rawSentence[21] = rawControlData.pito.chData[1]; rawSentence[22] = rawControlData.powr.chData[0]; rawSentence[23] = rawControlData.powr.chData[1]; rawSentence[24] = rawControlData.ther.chData[0]; rawSentence[25] = rawControlData.ther.chData[1]; // assemble the Raw Sensor data for protocol sending assembleMsg(&rawSentence[0], RAWMSG_LEN, RAWMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < RAWMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1] = tmpBuf[i]; } // set the total data out for SPI len2SPI = RAWMSG_LEN+7; #endif break; case 4: // Dynamic and Reboot (if required) rawSentence[0] = dynTempControlData.dynamic.chData[0]; rawSentence[1] = dynTempControlData.dynamic.chData[1]; rawSentence[2] = dynTempControlData.dynamic.chData[2]; rawSentence[3] = dynTempControlData.dynamic.chData[3]; rawSentence[4] = dynTempControlData.stat.chData[0] ; rawSentence[5] = dynTempControlData.stat.chData[1] ; rawSentence[6] = dynTempControlData.stat.chData[2] ; rawSentence[7] = dynTempControlData.stat.chData[3] ; rawSentence[8] = dynTempControlData.temp.chData[0] ; rawSentence[9] = dynTempControlData.temp.chData[1] ; // assemble the Raw Sensor data for protocol sending assembleMsg(&rawSentence[0], DYNMSG_LEN, DYNMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < DYNMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1] = tmpBuf[i]; } // set the total data out for SPI len2SPI = DYNMSG_LEN+7; // it there has been a reboot if(aknControlData.sensorReboot >0){ // clear tmpBuf memset(tmpBuf, 0, sizeof(tmpBuf)); // configure the akn sentence memset(aknSentence,0, 6); aknSentence[3] = aknControlData.sensorReboot; // assemble the message assembleMsg(&aknSentence[0], AKNMSG_LEN, AKNMSG_ID, tmpBuf); // add it to the SPI QUEUE for( i = 0; i < AKNMSG_LEN+7; i += 1 ){ data4SPI[i+1+len2SPI] = tmpBuf[i]; } // clear the flag aknControlData.sensorReboot = 0; // set the total data out for SPI len2SPI += AKNMSG_LEN+7; } break; case 5: // Bias rawSentence[0] = biasControlData.axb.chData[0] ; rawSentence[1] = biasControlData.axb.chData[1] ; rawSentence[2] = biasControlData.axb.chData[2] ; rawSentence[3] = biasControlData.axb.chData[3] ; rawSentence[4] = biasControlData.ayb.chData[0] ; rawSentence[5] = biasControlData.ayb.chData[1] ; rawSentence[6] = biasControlData.ayb.chData[2] ; rawSentence[7] = biasControlData.ayb.chData[3] ; rawSentence[8] = biasControlData.azb.chData[0] ; rawSentence[9] = biasControlData.azb.chData[1] ; rawSentence[10]= biasControlData.azb.chData[2]; rawSentence[11]= biasControlData.azb.chData[3]; rawSentence[12]= biasControlData.gxb.chData[0]; rawSentence[13]= biasControlData.gxb.chData[1]; rawSentence[14]= biasControlData.gxb.chData[2]; rawSentence[15]= biasControlData.gxb.chData[3]; rawSentence[16]= biasControlData.gyb.chData[0]; rawSentence[17]= biasControlData.gyb.chData[1]; rawSentence[18]= biasControlData.gyb.chData[2]; rawSentence[19]= biasControlData.gyb.chData[3]; rawSentence[20]= biasControlData.gzb.chData[0]; rawSentence[21]= biasControlData.gzb.chData[1]; rawSentence[22]= biasControlData.gzb.chData[2]; rawSentence[23]= biasControlData.gzb.chData[3]; // assemble the Raw Sensor data for protocol sending assembleMsg(&rawSentence[0], BIAMSG_LEN, BIAMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < BIAMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1] = tmpBuf[i]; } // set the total data out for SPI len2SPI = BIAMSG_LEN+7; break; case 6: // Diagnostic rawSentence[0] = diagControlData.fl1.chData[0]; rawSentence[1] = diagControlData.fl1.chData[1]; rawSentence[2] = diagControlData.fl1.chData[2]; rawSentence[3] = diagControlData.fl1.chData[3]; rawSentence[4] = diagControlData.fl2.chData[0]; rawSentence[5] = diagControlData.fl2.chData[1]; rawSentence[6] = diagControlData.fl2.chData[2]; rawSentence[7] = diagControlData.fl2.chData[3]; rawSentence[8] = diagControlData.fl3.chData[0]; rawSentence[9] = diagControlData.fl3.chData[1]; rawSentence[10]= diagControlData.fl3.chData[2]; rawSentence[11]= diagControlData.fl3.chData[3]; rawSentence[12]= diagControlData.sh1.chData[0]; rawSentence[13]= diagControlData.sh1.chData[1]; rawSentence[14]= diagControlData.sh2.chData[0]; rawSentence[15]= diagControlData.sh2.chData[1]; rawSentence[16]= diagControlData.sh3.chData[0]; rawSentence[17]= diagControlData.sh3.chData[1]; // assemble the Diagnostic data for protocol sending assembleMsg(&rawSentence[0], DIAMSG_LEN, DIAMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < DIAMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1] = tmpBuf[i]; } // set the total data out for SPI len2SPI = (DIAMSG_LEN+7); break; case 7: // Pilot Console Data rawSentence[0] = pilControlData.dt.chData[0] ; rawSentence[1] = pilControlData.dt.chData[1] ; rawSentence[2] = pilControlData.dla.chData[0]; rawSentence[3] = pilControlData.dla.chData[1]; rawSentence[4] = pilControlData.dra.chData[0]; rawSentence[5] = pilControlData.dra.chData[1]; rawSentence[6] = pilControlData.dr.chData[0] ; rawSentence[7] = pilControlData.dr.chData[1] ; rawSentence[8] = pilControlData.de.chData[0] ; rawSentence[9] = pilControlData.de.chData[1] ; // assemble the Pilot Console data for protocol sending assembleMsg(&rawSentence[0], PILMSG_LEN, PILMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < PILMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1] = tmpBuf[i]; } // set the total data out for SPI len2SPI = (PILMSG_LEN+7); break; case 8: // Sensor Data in meaningful units rawSentence[0] = senControlData.Ax.chData[0]; rawSentence[1] = senControlData.Ax.chData[1]; rawSentence[2] = senControlData.Ax.chData[2]; rawSentence[3] = senControlData.Ax.chData[3]; rawSentence[4] = senControlData.Ay.chData[0]; rawSentence[5] = senControlData.Ay.chData[1]; rawSentence[6] = senControlData.Ay.chData[2]; rawSentence[7] = senControlData.Ay.chData[3]; rawSentence[8] = senControlData.Az.chData[0]; rawSentence[9] = senControlData.Az.chData[1]; rawSentence[10]= senControlData.Az.chData[2]; rawSentence[11]= senControlData.Az.chData[3]; rawSentence[12]= senControlData.Mx.chData[0]; rawSentence[13]= senControlData.Mx.chData[1]; rawSentence[14]= senControlData.Mx.chData[2]; rawSentence[15]= senControlData.Mx.chData[3]; rawSentence[16]= senControlData.My.chData[0]; rawSentence[17]= senControlData.My.chData[1]; rawSentence[18]= senControlData.My.chData[2]; rawSentence[19]= senControlData.My.chData[3]; rawSentence[20]= senControlData.Mz.chData[0]; rawSentence[21]= senControlData.Mz.chData[1]; rawSentence[22]= senControlData.Mz.chData[2]; rawSentence[23]= senControlData.Mz.chData[3]; // assemble the Pilot Console data for protocol sending assembleMsg(&rawSentence[0], SENMSG_LEN, SENMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < SENMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1] = tmpBuf[i]; } // set the total data out for SPI len2SPI = (SENMSG_LEN+7); break; default: data4SPI[0] = 0; break; } memset(tmpBuf, 0, sizeof(tmpBuf)); // Attitude data. Gets included every sample time // ============================================== rawSentence[0] = attitudeControlData.roll.chData[0] ; rawSentence[1] = attitudeControlData.roll.chData[1] ; rawSentence[2] = attitudeControlData.roll.chData[2] ; rawSentence[3] = attitudeControlData.roll.chData[3] ; rawSentence[4] = attitudeControlData.pitch.chData[0] ; rawSentence[5] = attitudeControlData.pitch.chData[1] ; rawSentence[6] = attitudeControlData.pitch.chData[2] ; rawSentence[7] = attitudeControlData.pitch.chData[3] ; rawSentence[8] = attitudeControlData.yaw.chData[0] ; rawSentence[9] = attitudeControlData.yaw.chData[1] ; rawSentence[10] =attitudeControlData.yaw.chData[2] ; rawSentence[11] =attitudeControlData.yaw.chData[3] ; rawSentence[12] =attitudeControlData.p.chData[0] ; rawSentence[13] =attitudeControlData.p.chData[1] ; rawSentence[14] =attitudeControlData.p.chData[2] ; rawSentence[15] =attitudeControlData.p.chData[3] ; rawSentence[16] =attitudeControlData.q.chData[0] ; rawSentence[17] =attitudeControlData.q.chData[1] ; rawSentence[18] =attitudeControlData.q.chData[2] ; rawSentence[19] =attitudeControlData.q.chData[3] ; rawSentence[20] =attitudeControlData.r.chData[0] ; rawSentence[21] =attitudeControlData.r.chData[1] ; rawSentence[22] =attitudeControlData.r.chData[2] ; rawSentence[23] =attitudeControlData.r.chData[3] ; rawSentence[24] =attitudeControlData.timeStamp.chData[0]; rawSentence[25] =attitudeControlData.timeStamp.chData[1]; // assemble the Attitude data for protocol sending assembleMsg(&rawSentence[0], ATTMSG_LEN, ATTMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < ATTMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1+len2SPI] = tmpBuf[i]; } // increment the data counter for SPI len2SPI += ATTMSG_LEN+7; memset(tmpBuf, 0, sizeof(tmpBuf)); // XYZ data. Gets included every sample time // ============================================== #ifdef LOGRAW100 rawSentence[0] = rawControlData.gyroX.chData[0]; rawSentence[1] = rawControlData.gyroX.chData[1]; rawSentence[2] = rawControlData.gyroY.chData[0]; rawSentence[3] = rawControlData.gyroY.chData[1]; rawSentence[4] = rawControlData.gyroZ.chData[0]; rawSentence[5] = rawControlData.gyroZ.chData[1]; rawSentence[6] = rawControlData.accelX.chData[0]; rawSentence[7] = rawControlData.accelX.chData[1]; rawSentence[8] = rawControlData.accelY.chData[0]; rawSentence[9] = rawControlData.accelY.chData[1]; rawSentence[10] = rawControlData.accelZ.chData[0]; rawSentence[11] = rawControlData.accelZ.chData[1]; rawSentence[12] = rawControlData.magX.chData[0]; rawSentence[13] = rawControlData.magX.chData[1]; rawSentence[14] = rawControlData.magY.chData[0]; rawSentence[15] = rawControlData.magY.chData[1]; rawSentence[16] = rawControlData.magZ.chData[0]; rawSentence[17] = rawControlData.magZ.chData[1]; // included in SLUGS MKII rawSentence[18] = rawControlData.baro.chData[0]; rawSentence[19] = rawControlData.baro.chData[1]; rawSentence[20] = rawControlData.pito.chData[0]; rawSentence[21] = rawControlData.pito.chData[1]; rawSentence[22] = rawControlData.powr.chData[0]; rawSentence[23] = rawControlData.powr.chData[1]; rawSentence[24] = rawControlData.ther.chData[0]; rawSentence[25] = rawControlData.ther.chData[1]; // assemble the Raw Sensor data for protocol sending assembleMsg(&rawSentence[0], RAWMSG_LEN, RAWMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < RAWMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1+len2SPI] = tmpBuf[i]; } // set the total data out for SPI data4SPI[0] = len2SPI + RAWMSG_LEN+7; #else rawSentence[0] = xyzControlData.Xcoord.chData[0]; rawSentence[1] = xyzControlData.Xcoord.chData[1]; rawSentence[2] = xyzControlData.Xcoord.chData[2]; rawSentence[3] = xyzControlData.Xcoord.chData[3]; rawSentence[4] = xyzControlData.Ycoord.chData[0]; rawSentence[5] = xyzControlData.Ycoord.chData[1]; rawSentence[6] = xyzControlData.Ycoord.chData[2]; rawSentence[7] = xyzControlData.Ycoord.chData[3]; rawSentence[8] = xyzControlData.Zcoord.chData[0]; rawSentence[9] = xyzControlData.Zcoord.chData[1]; rawSentence[10]= xyzControlData.Zcoord.chData[2]; rawSentence[11]= xyzControlData.Zcoord.chData[3]; rawSentence[12]= xyzControlData.VX.chData[0] ; rawSentence[13]= xyzControlData.VX.chData[1] ; rawSentence[14]= xyzControlData.VX.chData[2] ; rawSentence[15]= xyzControlData.VX.chData[3] ; rawSentence[16]= xyzControlData.VY.chData[0] ; rawSentence[17]= xyzControlData.VY.chData[1] ; rawSentence[18]= xyzControlData.VY.chData[2] ; rawSentence[19]= xyzControlData.VY.chData[3] ; rawSentence[20]= xyzControlData.VZ.chData[0] ; rawSentence[21]= xyzControlData.VZ.chData[1] ; rawSentence[22]= xyzControlData.VZ.chData[2] ; rawSentence[23]= xyzControlData.VZ.chData[3] ; // assemble the XYZ data for protocol sending assembleMsg(&rawSentence[0], XYZMSG_LEN, XYZMSG_ID, tmpBuf); // add it to the circular buffer and SPI queue for( i = 0; i < XYZMSG_LEN+7; i += 1 ){ writeBack(logBuffer,tmpBuf[i]); data4SPI[i+1+len2SPI] = tmpBuf[i]; } // set the total data out for SPI data4SPI[0] = len2SPI + XYZMSG_LEN+7; #endif // increment/overflow the samplePeriod counter // configured for 10 Hz in non vital messages samplePeriod = (samplePeriod >= 8)? 1: samplePeriod + 1; // get the Length of the logBuffer bufLen = getLength(logBuffer); // if HIL is ON do not transmit diagnostic data in the diagnostic port // let the circular buffer overwrite itself hilOn =1; if (hilOn!= 1){ // if the interrupt catched up with the circularBuffer // then turn on the DMA if(!(DMA0CONbits.CHEN) && bufLen> 0){ // Configure the bytes to send DMA0CNT = bufLen<= (MAXSEND-1)? bufLen-1: MAXSEND-1; // copy the buffer to the DMA channel outgoing buffer copyBufferToDMA((unsigned char) DMA0CNT+1); // Enable the DMA DMA0CONbits.CHEN = 1; // Init the transmission DMA0REQbits.FORCE = 1; } } }
// TODO: Include a messaging Mechanism for immediate or once in time messages // TODO: Include a File option for Archiving checksum fails for debuging float protParseDecode (unsigned char* fromSPI, FILE* outFile, unsigned char prevException){ #else void protParseDecode (unsigned char* fromSPI){ #endif // Static variables CAREFUL static unsigned char prevBuffer[2*MAXLOGLEN]; static unsigned char previousComplete =1; static unsigned char indexLast = 0; #ifdef _IN_PC_ static long long checkSumFail = 0; static long long totalPackets = 0; static float test = 0.0; float alpha = 0.3; #endif // local variables unsigned char i; unsigned char tmpChksum = 0, headerFound=0, noMoreBytes = 1; unsigned char trailerFound = 0; //unsigned char logSize = 0; // Add the received bytes to the protocol parsing circular buffer for(i = 1; i <= fromSPI[0]; i += 1 ) //for(i = 0; i <= 95; i += 1 ) { writeBack(ppBuffer, fromSPI[i]); } // update the noMoreBytes flag accordingly noMoreBytes = (fromSPI[0]>0)?0:1; // noMoreBytes = 0; while (!noMoreBytes){ // if the previous message was complete then read from the circular buffer // and make sure that you are into the begining of a message if(previousComplete){ while (!headerFound && !noMoreBytes) { // move along until you find a dollar sign or run out of bytes while (getLength(ppBuffer)>1 && peak(ppBuffer)!=DOLLAR){ readFront(ppBuffer); } // if you found a dollar then make sure the next one is an AT if(getLength(ppBuffer)>1 && peak(ppBuffer) == DOLLAR){ // read it prevBuffer[indexLast++] = readFront(ppBuffer); // if next is a at sign if (peak(ppBuffer) == AT){ // read it prevBuffer[indexLast++] = readFront(ppBuffer); // and signal you found a header headerFound = 1; // and set as incomplete the sentece previousComplete = 0; } } else { noMoreBytes = 1; } // else no dollar } // while we found header && no more bytes }// if previous complete // At this point either you found a header from a previous complete // or you are reading from a message that was incomplete the last time // in any of those two cases, keep reading until you run out of bytes // or find a STAR and an AT while (!trailerFound && !noMoreBytes){ while (getLength(ppBuffer)>2 && peak(ppBuffer)!=STAR){ prevBuffer[indexLast++] = readFront(ppBuffer); } // if you found a STAR (42) and stil have bytes if (getLength(ppBuffer)>2 && peak(ppBuffer)==STAR){ // read it prevBuffer[indexLast++] = readFront(ppBuffer); // if you still have 2 bytes if (getLength(ppBuffer)>1){ // and the next byte is an AT sign if (peak(ppBuffer)==AT){ // then you found a trailer trailerFound =1; } } else { noMoreBytes =1; } } else { // no more bytes noMoreBytes =1; } } // if you found a trailer, then the message is done if(trailerFound){ // read the AT and the checksum prevBuffer[indexLast++] = readFront(ppBuffer); prevBuffer[indexLast] = readFront(ppBuffer); // Compute the checksum tmpChksum= getChecksum(prevBuffer, indexLast-1); #ifdef _IN_PC_ totalPackets++; #endif // if the checksum is valid if (tmpChksum ==prevBuffer[indexLast]){ // update the states depending on the message updateStates(&prevBuffer[0]); // increment the log size //logSize += (indexLast+1); #ifdef _IN_PC_ // if in PC and loggin is enabled if ((outFile != NULL)){ printState(outFile, prevException); } //test = alpha*test; #endif } else{ #ifdef _IN_PC_ checkSumFail++; //test = (1.0-alpha) + alpha*test; #endif } // get everything ready to start all-over previousComplete =1; indexLast = 0; headerFound = 0; trailerFound = 0; memset(prevBuffer, 0, sizeof(prevBuffer)); }else { // you ran out of bytes // No More Bytes noMoreBytes = 1; }// else no star } // big outer while (no more bytes) #ifdef _IN_PC_ if (totalPackets>0){ //test = ((float)checkSumFail/(float)totalPackets); test = (float)checkSumFail; } else { test = 0.0; } return test; #endif } unsigned char getFilterOnOff (void){ return filterControlData; } // ================================ // hardware in the loop methods // ================================ void hil_getRawRead(short * rawData){ rawData[0] = rawControlData.gyroX.shData; rawData[1] = rawControlData.gyroY.shData; rawData[2] = rawControlData.gyroZ.shData; rawData[3] = rawControlData.accelX.shData; rawData[4] = rawControlData.accelY.shData; rawData[5] = rawControlData.accelZ.shData; rawData[6] = rawControlData.magX.shData; rawData[7] = rawControlData.magY.shData; rawData[8] = rawControlData.magZ.shData; rawData[9] = rawControlData.baro.shData; rawData[10] = rawControlData.pito.shData; rawData[11] = rawControlData.powr.shData; rawData[12] = rawControlData.ther.shData; }
int DatagramSocket::writeBack( const char * message) { size_t length=strlen(message)+1; return writeBack(message,length); }