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