Exemple #1
0
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;
	}
}
Exemple #2
0
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;
		}
	}

}