/*----------------------------------------------------------------------
 *
 * I3DM-GX2 Interface Software
 *
 *----------------------------------------------------------------------
 * (c) 2008 Microstrain, Inc.
 *----------------------------------------------------------------------
 * THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING
 * CUSTOMERS WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER
 * FOR THEM TO SAVE TIME. AS A RESULT, MICROSTRAIN SHALL NOT BE HELD LIABLE
 * FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY
 * CLAIMS ARISING FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY
 * CUSTOMERS OF THE CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH
 * THEIR PRODUCTS.
 *---------------------------------------------------------------------*/

/*----------------------------------------------------------------------
 * i3dmgx2_Cmd.c
 *
 * 3DM-GX2 Sensor device functions.
 *
 * This platform independent module relies on the communication functions 
 * found in either i3dmgx2SerialWin.c or i3dmgx2SerialLinux.c 
 * (your choice, depending on platform).
 *--------------------------------------------------------------------*/
 
#ifndef DEBUG
#define DEBUG 0
#endif

#include <stdio.h>
#include "i3dmgx2_Cmd.h"
#include "i3dmgx2_Utils.h"
#include "i3dmgx2_Errors.h"
#include "i3dmgx2_Serial.h"


/*----------------------------------------------------------------------
 * i3dmgx2_openPort
 *
 * Open a serial communications port. (platform independent).
 *
 * parameters   portNum  : a port number (1..n).
 *--------------------------------------------------------------------*/

int i3dmgx2_openPort(int portNum, int baudrate, int size, int parity, 
					 int stopbits, int inputBuff, int outputBuff) {
    int errcheck;
    int porth;

    errcheck = openPort(portNum, inputBuff, outputBuff);
    if (errcheck<0) {
        return errcheck;
    }
    porth = errcheck;  /* no error, so this is the port number. */

    /* set communications parameters */
    errcheck = setCommParameters(porth, baudrate, size, parity, stopbits);
    if (errcheck!=I3DMGX2_COMM_OK) {
        return errcheck;
    }

    /* set timeouts */
    errcheck = setCommTimeouts(porth, 50, 50); /* Read Write */
    if (errcheck!=I3DMGX2_COMM_OK) {
        return errcheck;
    }
    return porth;
}
/*----------------------------------------------------------------------
 * i3dmgx2_closeDevice
 *
 * parameters   portNum      : the number of the sensor device 
 *                            
 * Close a device, and also any underlying port.
 *--------------------------------------------------------------------*/
void i3dmgx2_closeDevice(int portNum) {
    
        closePort(portNum);
       portNum = 0;
 }
/*----------------------------------------------------------------------
 * i3dmgx2_RawSensor  0xC1
 *
 * parameters   portNum : the number of the sensor device (1..n)
 *              pI3Record : struct to receive floating point values for
 *                          raw Accel (x,y,z) and raw Ang (x,y,z)
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *---------------------------------------------------------------------*/
int i3dmgx2_RawSensor(int portNum, unsigned char *pRecord) {
    int responseLength = 31;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_RAW_ACCELEROMETER; /* value is 0xC1 */
	int i = 0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
   		status = receiveData(portNum, &pRecord[0], responseLength);
        if (DEBUG) printf("Raw Sensor i3dmgx2_send: rx status : %d and responseLength %d\n", status, responseLength);
			
    	if (status==I3DMGX2_COMM_OK) {
            wChecksum = convert2ushort(&pRecord[responseLength-2]);
	        wCalculatedCheckSum = i3dmgx2_Checksum(&pRecord[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
			if(wChecksum != wCalculatedCheckSum)		
				return	status = I3DMGX2_CHECKSUM_ERROR; 
		}else
			return status = I3DMGX2_COMM_READ_ERROR;	
	}else
		status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_AccelandAngRate 0xC2
 *
 * parameters   portNum : the number of the sensor device (1..n)
 *              pI3Record : struct to receive floating point values for
 *							Accel (x,y,z) and Ang (x,y,z)
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_AccelAndAngRate(int portNum, unsigned char* pRecord) {
    int responseLength = 31;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_ACCELERATION_ANGU; /* value is 0xC2 */
	int i = 0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
    	status = receiveData(portNum, &pRecord[0], responseLength);
        if (DEBUG) printf("Accel Ang i3dmgx2_send: rx status : %d  and responseLength %d\n", status, responseLength);  
    	if (status==I3DMGX2_COMM_OK) {
            wChecksum = convert2ushort(&pRecord[responseLength-2]);
	        wCalculatedCheckSum = i3dmgx2_Checksum(&pRecord[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
			if(wChecksum != wCalculatedCheckSum)
				return	status = I3DMGX2_CHECKSUM_ERROR;
		}else
		    return status = I3DMGX2_COMM_READ_ERROR;			
	}else
		status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_DeltaAngAndVelocity 0xC3
 *
 * parameters   portNum : the number of the sensor device (1..n)
 *              pI3Record : struct to receive floating point values for
 *							Accel (x,y,z) and Ang (x,y,z)
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_DeltaAngAndVelocity(int portNum, unsigned char* Bresponse) {
    int responseLength = 31;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_DELTA_ANGLE_VELOC; /* value is 0xC3 */
	int i = 0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("DeltaAngAndVelocity i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
     	status = receiveData(portNum, &Bresponse[0], responseLength);
        if (DEBUG) printf("DeltaAngAndVelocity i3dmgx2_send: rx status : %d  and responseLength %d\n", status, responseLength);		   
    	if (status==I3DMGX2_COMM_OK) {
            wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	        wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
			if(wChecksum != wCalculatedCheckSum)
			   return	status = I3DMGX2_CHECKSUM_ERROR;
		}else
		   return status = I3DMGX2_COMM_READ_ERROR;
	}else
		status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_OrientMatrix	0xC5
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *              pI3Record    : pointer to a  float matrix
 *                             which will contain the Orientation data
 *                             upon return.
 *              
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_OrientMatrix(int portNum, unsigned char* Bresponse) {
    int responseLength = 43;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_ORRIENTATION_MAT; /* value is 0xC5 */
	int i = 0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("Orient Matrix i3dmgx2_send: tx status : %d\n", status);
	if (status == I3DMGX2_COMM_OK) {
    		status = receiveData(portNum, &Bresponse[0], responseLength);
            if (DEBUG) printf("Orient Matrix i3dmgx2_send: rx status : %d  and responseLength %d\n", status, responseLength);
					   
    		if (status==I3DMGX2_COMM_OK) {
                wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	            wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
					if(wChecksum != wCalculatedCheckSum)
						return	status = I3DMGX2_CHECKSUM_ERROR;
			}else
			    return status = I3DMGX2_COMM_READ_ERROR;  
	}else
		status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_OrientUpMatrix 0xC6
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *              pI3Record    : pointer to a  float matrix
 *                             which will contain the Orientation Update
 *                             data upon return.
 *              
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns 
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_OrientUpMatrix(int portNum, unsigned char* Bresponse) {
    int responseLength = 43;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_ATTITUDE_UP_MATRIX; /* value is 0xC6 */
	int i = 0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("Orient Update i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
      		status = receiveData(portNum, &Bresponse[0], responseLength);
            if (DEBUG) printf("Orient Update i3dmgx2_send: rx status : %d  and responseLength %d\n", status, responseLength);
					   
    		if (status==I3DMGX2_COMM_OK) {
                wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	            wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
					if(wChecksum != wCalculatedCheckSum)
						return	status = I3DMGX2_CHECKSUM_ERROR;
			}else
			    return status = I3DMGX2_COMM_READ_ERROR;
	}else
		status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_ScaledMagVec	0xC7
 *
 * parameters   portNum : the number of the sensor device (1..n)
 *              pI3Record : struct to receive floating point values for
 *                          scaled Magnetometer Vectors (x,y,z)
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns 
 *                          an error code.
 *---------------------------------------------------------------------*/
int i3dmgx2_ScaledMagVec(int portNum, unsigned char* Bresponse) {
    int responseLength = 19;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_MAGNETROMETER_VECT; /* value is 0xC7 */
	int i = 0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("Scaled Mag Vector i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
      		status = receiveData(portNum, &Bresponse[0], responseLength);
            if (DEBUG) printf("Scaled Mag Vectorr i3dmgx2_send: rx status : %d and responseLength %d\n", status, responseLength);
    		if (status==I3DMGX2_COMM_OK) {
                wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	            wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
					if(wChecksum != wCalculatedCheckSum)
						return	status = I3DMGX2_CHECKSUM_ERROR;
			}else
					return status = I3DMGX2_COMM_READ_ERROR;			
		}else
			status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_AccelAngOreint	0xC8
 *
 * parameters   portNum : the number of the sensor device (1..n)
 *              pI3Record : struct to receive floating point values for
 *                          Acceleration and Angular Rates (x,y,z)
 *                          and Orientation Matrix M1.1 - M1.2 ... M3.3
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns 
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_AccelAngOreint(int portNum, unsigned char* Bresponse) {
    int responseLength = 67;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_ACCEL_ANG_ORIENT; /* value is 0xC8 */
	int i = 0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("Accel AngRate OrientMatx i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
       		status = receiveData(portNum, &Bresponse[0], responseLength);
            if (DEBUG) printf("Accel Ang Orient i3dmgx2_send: rx status : %d and responseLength %d\n", status, responseLength);
    		if (status==I3DMGX2_COMM_OK) {
                wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	            wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
					if(wChecksum != wCalculatedCheckSum)
						return	status = I3DMGX2_CHECKSUM_ERROR;
			}else
					return status = I3DMGX2_COMM_READ_ERROR;  	
	}else
			status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_AccAngMagRate	0xCB
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *              mag       : array which will contain mag data 
 *                         (3 elements X Y and Z)
 *              accel     : array which will contain accleration data 
 *                         (3 elements X Y and Z)
 *              angRate   : array which will contain angular rate data 
 *                         (3 elements X Y and Z)
 *              
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_AccAngMagRate(int portNum, unsigned char* Bresponse) {
	int responseLength = 43;
	int status = 0, i =0;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_ACCEL_ANG_MAG_VECTO;  //0xCB
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("Get Acc ang mag i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
    		status = receiveData(portNum, &Bresponse[0], responseLength);
			if (DEBUG) printf("Accel ang mag rate i3dmgx2_send: rx status : %d and responseLength %d\n", status, responseLength); /* temp adj 8-1-08 */
    		if (status==I3DMGX2_COMM_OK) {
                wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	            wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
	            if(wChecksum != wCalculatedCheckSum){
                    status = I3DMGX2_CHECKSUM_ERROR;
                }
            }else
                status = I3DMGX2_COMM_READ_ERROR;
    }else
		status = I3DMGX2_COMM_WRITE_ERROR;       
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_GetFullMatrix 0xCC
 *  	
 * parameters   portNum    : the number of the sensor device (1..n)
 *              I3Record   : 18 positional stucture of defined type 
 *                           I3Record to capture data in floating point
 *							   
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_GetFullMatrix(int portNum, unsigned char* Bresponse) {
	int responseLength = 79;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_ACEL_ANG_MAG_VEC_OR; /* value is 0xCC */
	int i=0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
    		status = receiveData(portNum, &Bresponse[0], responseLength);
            if (DEBUG) printf("i3dmgx2_send: rx status : %d and responseLength %d\n", status, responseLength); /* temp adj 8-1-08 */
    		if (status==I3DMGX2_COMM_OK) {
                wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	            wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
				if(wChecksum != wCalculatedCheckSum)
                   return status = I3DMGX2_CHECKSUM_ERROR;
				}else
				return status = I3DMGX2_COMM_READ_ERROR;
    }else
		status = I3DMGX2_COMM_WRITE_ERROR;  
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_captureGyroBias	0xCD
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *              Bresponse    : pointer to a buffer containing returned values
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns 
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_captureGyroBias(int portNum, short sampt, BYTE *BiasBuff, unsigned char *Bresponse) {
    char cmd = CMD_CAPTURE_GYRO_BIAS; //0xCD
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	int responseLength = 19;
    int status;
	int i = 0;
	unsigned char Coutbuff[5];
	
	/* initialize send buffer to obtain gyro bias */
	Coutbuff[0] = 0xCD;   //Command code for Gyro Bias
	Coutbuff[1] = 0xC1;   // Required identifier
    Coutbuff[2] = 0x29;   // Required identifier
    Coutbuff[3] = (sampt & MSB_MASK) >> 8;  //sample time with mask
	Coutbuff[4] =  sampt & LSB_MASK; 
	
	printf("...one %d/ms moment while gyro bias values are retrieved\n", sampt);

	status = sendBuffData(portNum, &Coutbuff[0], 5);
    if (DEBUG) printf(" Capture Gyro Bias  Send struct status is %d\n", status);
	if (status == I3DMGX2_COMM_OK) {
		    status = receiveData(portNum, &Bresponse[0], responseLength);
			if (DEBUG) printf(" Capture Gyro Bias Receive  status is %d\n", status);
			if (status == I3DMGX2_COMM_OK) {
			    wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	            wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
				if(wChecksum != wCalculatedCheckSum)
					return status = I3DMGX2_CHECKSUM_ERROR;
	        }else
			    return status = I3DMGX2_COMM_READ_ERROR;        			
			
			for (i=0; i<12; i++)
				BiasBuff[i] = Bresponse[1 +i];
	}else
		return status = I3DMGX2_COMM_WRITE_ERROR;
	return status;

}
/*----------------------------------------------------------------------
 * i3dmgx2_WriteGyroBias	0xCA
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *              BiasBuff     : a buffer containing 3 floating point bias(es)
 *              PI3Record    : pointer to a buffer containing returned values
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns 
 *                          an error code.
 *--------------------------------------------------------------------*/

int i3dmgx2_WriteGyroBias(int portNum, BYTE *BiasBuff, unsigned char* Bresponse) {
    char cmd = CMD_WRITE_GYRO_BIAS; //0xCD
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	int responseLength = 19;
    int status;
	int i = 0;
    unsigned char BiasoutBuff[15];

    BiasoutBuff[0] = 0xCA; //CMD_WRITE_GYRO_BIAS;
	BiasoutBuff[1] = 0x12; // Required identifier
	BiasoutBuff[2] = 0xA5; // Required identifier
	for (i=0; i<12; i++)   //repacks the float values in big endian format
		BiasoutBuff[3 + i] = BiasBuff[i];
		
	status = sendBuffData(portNum, &BiasoutBuff[0], 15); // 15 equals command byte + 14 bytes of data
     
	if (DEBUG) printf(" Write Gyro Bias Send struct status is %d\n", status);
        if (status == I3DMGX2_COMM_OK) {
	    status = receiveData(portNum, &Bresponse[0], responseLength);
	    if (DEBUG) printf(" Write  Gyro Bias Receive  status is %d\n", status);
        if (status == I3DMGX2_COMM_OK) {
			wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	        wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); 
		    if(wChecksum != wCalculatedCheckSum)
			   return status = I3DMGX2_CHECKSUM_ERROR;
	    }else			   
 		   return status = I3DMGX2_COMM_READ_ERROR;
	}else
	    return status = I3DMGX2_COMM_WRITE_ERROR;

	return status;
 }
/*----------------------------------------------------------------------
 * i3dmgx2_EulerAngles	0xCE
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *             I3dmgx2Set    : pointer to a struct containing the values 
 *                             pitch angle in degrees
 *              
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns 
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_EulerAngles(int portNum, unsigned char* Bresponse) {
    int responseLength = 19;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_EULER_ANGLES; /* value is 0xCE */
	int i = 0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("Euler Ang i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
    	status = receiveData(portNum, &Bresponse[0], responseLength);
        if (DEBUG) printf("Euler Ang i3dmgx2_send: rx status : %d and responseLength %d\n", status);
    	if (status==I3DMGX2_COMM_OK) {
            wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	        wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
			if(wChecksum != wCalculatedCheckSum)
				return status = I3DMGX2_CHECKSUM_ERROR;
		}else
			return status = I3DMGX2_COMM_READ_ERROR;
    	
	}else
		return status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
 }
/*----------------------------------------------------------------------
 * i3dmgx2_EulerAngRate	0xCF
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *             I3dmgx2Set    : pointer to a struct containing the values 
 *                             for pitch angles in degrees and
 *							   Angle rates x y and z
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_EulerAngRate(int portNum, unsigned char* Bresponse) {
    int responseLength = 31;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_EULER_ANGLES_ANG_RT; /* value is 0xCF */
	int i = 0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("i3dmgx2_send Euler and Ang Rate: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) { 
	   	status = receiveData(portNum, &Bresponse[0], responseLength);
        if (DEBUG) printf("Euler Ang Rate i3dmgx2_send: rx status : %d and responseLength %d\n", status, responseLength);
        if (status==I3DMGX2_COMM_OK) {
            wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	        wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
			if(wChecksum != wCalculatedCheckSum)
				return status = I3DMGX2_CHECKSUM_ERROR;
		}else
			return status = I3DMGX2_COMM_READ_ERROR;
	} else
		return status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_TransferNonVolatile	0xD0
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *              transfer     : numeric value identifying which quantity 
 *                             to Transfer: 1 == Accel Bias						   
 *                                          2 == Gyro Bias
 *                            Any other value results in no action taken
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_TransferNonVolatile(int portNum, int transfer, unsigned char *Bresponse) {
    int responseLength = 9;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	char cmd = CMD_TRANSFER_NONV_MEM; /* value is 0xD0 */
	int i = 0;
	unsigned char outbuff[5];
			 
    outbuff[0] = 0xD0; // Command Code Identifier
	outbuff[1] = 0xC1; // Required identifier
	outbuff[2] = 0x29; // Required identifier
	outbuff[3] =  (transfer & MSB_MASK) >> 8; //1st byte of quantity to transfer
	outbuff[4] =  transfer & LSB_MASK;       //2ond byte of quantity to transfer
	                  // 1==Accel Bias 2==Gyro Bias, any other value results in no action 

	status = sendBuffData(portNum, &outbuff[0], 5);
   	if (DEBUG) printf(" Transfer Non-V MEM Send struct status is %d\n", status);
        if (status == I3DMGX2_COMM_OK) {
	    status = receiveData(portNum, &Bresponse[0], responseLength);
	     if (DEBUG) printf(" Transfer Non-V MEM Receive  status is %d\n", status);
		     if (status == I3DMGX2_COMM_OK) {
			     wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	             wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2);
                  
			    if(wChecksum != wCalculatedCheckSum)
				   return status = I3DMGX2_CHECKSUM_ERROR;
	        }else			   
 			    return status = I3DMGX2_COMM_READ_ERROR;   
		}else
	       return status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
}
/*----------------------------------------------------------------------
 * i3dmgx2_Tempatures	0xD1
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *              pI3Temps     : pointer to a struct containing Tempature
 *                             Accel and Temp gyro x y and z values.
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_Tempatures(int portNum, unsigned char* Bresponse) {
    int responseLength = 15;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_TEMPERATURES; /* value is 0xD1 */
	int i = 0;
	WORD zetemp = 0;
	
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("Temperatures i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
    	status = receiveData(portNum, &Bresponse[0], responseLength);
        if (DEBUG) printf("Temperatures i3dmgx2_send: rx status : %d and responseLength %d\n", status);
    	if (status==I3DMGX2_COMM_OK) {
            wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	        wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
			if(wChecksum != wCalculatedCheckSum)
				return status = I3DMGX2_CHECKSUM_ERROR;
		}else
			return status = I3DMGX2_COMM_READ_ERROR;		
	}else
		return status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
 }
/*----------------------------------------------------------------------
 * i3dmgx2_GyroStab	0xD2
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *             I3dmgx2Set    : pointer to a struct containing the values 
 *                             gyro stab accel angR and Mag             
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_GyroStab(int portNum, unsigned char* Bresponse) {
    int responseLength = 43;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_GYRO_STAB_A_AR_MG; /* value is 0xD2 */
	int i = 0;
		
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("GyroStab i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
    	status = receiveData(portNum, &Bresponse[0], responseLength);
        if (DEBUG) printf("GyroStab i3dmgx2_send: rx status : %d and responseLength %d\n", status);
        if (status==I3DMGX2_COMM_OK) {
            wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	        wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
			if(wChecksum != wCalculatedCheckSum)
			return status = I3DMGX2_CHECKSUM_ERROR;
		}else
			return status = I3DMGX2_COMM_READ_ERROR;
   	}else
		return status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
 }
/*----------------------------------------------------------------------
 * i3dmgx2_DeltaAngVel	0xD3
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *             I3dmgx2Set    : pointer to a struct containing the values 
 *                             gyro stab accel angR and Mag
 *              
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_DeltaAngVel(int portNum, unsigned char* Bresponse) {
    int responseLength = 43;
	int status;
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	unsigned char cmd = CMD_DELTA_ANGVEL_MAGV; /* value is 0xD3 */
	int i = 0;
		
	status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("DeltaAngVel i3dmgx2_send: tx status : %d\n", status);
    if (status == I3DMGX2_COMM_OK) {
    	status = receiveData(portNum, &Bresponse[0], responseLength);
        if (DEBUG) printf("DeltaAngVel i3dmgx2_send: rx status : %d and responseLength %d\n", status);
        if (status==I3DMGX2_COMM_OK) {
            wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	        wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
			if(wChecksum != wCalculatedCheckSum)
			  return status = I3DMGX2_CHECKSUM_ERROR;
		}else
			return status = I3DMGX2_COMM_READ_ERROR;
    }else
		return status = I3DMGX2_COMM_WRITE_ERROR;
	return status;
 }
/*----------------------------------------------------------------------
 * i3dmgx2_getEEPROMValue	0xE5
 *
 * parameters   portNum    : the number of the sensor device (1..n)
 *              address      : the EEPROM address location
 *              readFlag     : specifies the number of reads required.
 *                             and identifies data type long or float.
 *              value        : the value to get at the address specified
 *
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *--------------------------------------------------------------------*/
int i3dmgx2_getEEPROMValue(int portNum, char address, int readFlag, int *readval) {
	int status = 0;
	long bytesRead = 0;
	unsigned char Bresponse[5]  = {0};
	unsigned char BFresponse[5]  = {0};
	unsigned char ConvertBuff[4] = {0};
	int responseLength = 5;
	unsigned short nvalL=0, nvalA=0, nvalB=0;
	float nvalF=0.0;
	unsigned char zoutbuff[4];
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;

	zoutbuff[0] = 0xE5;     // EEPROM command identifier
	zoutbuff[1] = 0x00;     // Required identifier
	zoutbuff[2] = 0xFC;     // Required identifier
	zoutbuff[3] = address;  // EEPROM address location

	status = sendBuffData(portNum, &zoutbuff[0], 4);
	if (DEBUG) printf(" eeprom read status is %d\n", status);
    if (status == I3DMGX2_COMM_OK) { 
		status = receiveData(portNum, &Bresponse[0], 5);
        if (status == I3DMGX2_COMM_OK) {
            status = I3DMGX2_OK;
			  wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	          wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
			if(wChecksum != wCalculatedCheckSum)
				return status = I3DMGX2_CHECKSUM_ERROR;
			
            nvalA = convert2ushort(&Bresponse[1]);
			
			if (readFlag > 0) {
				ConvertBuff[3] = Bresponse[2];
				ConvertBuff[2] = Bresponse[1];
				zoutbuff[3] = address + 2;

				status = sendBuffData(portNum, &zoutbuff[0], 4);
				if (status != I3DMGX2_COMM_OK)
				   return status;
		
				status = receiveData(portNum, &BFresponse[0], 5);

				if (status == I3DMGX2_COMM_OK) {
					status = I3DMGX2_OK;
					wChecksum = convert2ushort(&Bresponse[responseLength-2]);
					wCalculatedCheckSum = i3dmgx2_Checksum(&BFresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
					if(wChecksum != wCalculatedCheckSum)
						return status = I3DMGX2_CHECKSUM_ERROR;
					nvalB = convert2ushort(&BFresponse[1]);
					
					ConvertBuff[1] = BFresponse[2];
				    ConvertBuff[0] = BFresponse[1];
					nvalF = FloatFromBytes(&ConvertBuff[0]);
					nvalL = convert2ulong(&ConvertBuff[0]);
					if (readFlag == 1){
						printf("  At addr.:0x%X where value is:   %ld \n", zoutbuff[3], nvalL);
						*readval = nvalL;
					}
					else{
						printf("  At addr.:0x%X where value is: %f \n", zoutbuff[3], nvalF);
						*readval = nvalF;
					}
				}else
					return status = I3DMGX2_COMM_READ_ERROR;
			}else{
					printf("  At addr.:0x%X where value is: %d \t0x%X\n", address, nvalA, nvalA);
					*readval = nvalA;
			}
        }else
            return status = I3DMGX2_COMM_READ_ERROR;
    }
    return status;
}

/*----------------------------------------------------------------------
 * i3dmgx2_getDeviceIdentity  0xEA
 *
 * parameters   portNum : the number of the sensor device (1..n)
 *              flag      : identifier for which Identity to obtain.
 *              id        : a pointer to char string, already allocated.
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *
 * WARNING - does not check to see if you have allocated enough space
 *           12 bytes for the string to contain the firmware version.
 *--------------------------------------------------------------------*/
int i3dmgx2_getDeviceIdentiy(int portNum, char flag, unsigned char* Bresponse) {
    char cmd = (char) CMD_GET_DEVICE_ID;		//0xEA
    unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
    int status; 
	int responseLength = 20; 
	unsigned char iden_buff[2];
	
	iden_buff[0] = 0xEA; // Device Identity Command Code
	iden_buff[1] = flag; // Identifier of specific device identity component to obtain

	status = sendBuffData(portNum, &iden_buff[0], 2);
	if (DEBUG) printf("Get Identity_send: tx status : %d\n", status);
	if (status == I3DMGX2_COMM_OK) {
    	status = receiveData(portNum, &Bresponse[0], responseLength);
		if (DEBUG) printf("Get Identity i3dmgx2_send: rx status : %d and responseLength %d\n", status, responseLength);
    	if (status==I3DMGX2_COMM_OK) {
			wChecksum = convert2ushort(&Bresponse[responseLength-2]);
			wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
			if(wChecksum != wCalculatedCheckSum)
				return	status = I3DMGX2_CHECKSUM_ERROR;
		}else
			return status = I3DMGX2_COMM_READ_ERROR;
	}else return status;
   	
return I3DMGX2_OK;
}
/*----------------------------------------------------------------------
 * i3dmgx2_getFirmwareVersion  0xE9
 *
 * parameters   portNum : the number of the sensor device (1..n)
 *              firmware  : a pointer to char string, already allocated.
 *
 * returns:     errorCode : I3DMGX2_OK if succeeded, otherwise returns
 *                          an error code.
 *
 * WARNING - does not check to see if you have allocated enough space
 *           12 bytes for the string to contain the firmware version.
 *---------------------------------------------------------------------*/

int i3dmgx2_getFirmwareVersion(int portNum, char *firmware) {
    unsigned char cmd = (char) CMD_FIRWARE_VERSION;
    unsigned char Bresponse[7] = {0}; 
	unsigned short wChecksum = 0;
    unsigned short wCalculatedCheckSum = 0;
	
	short firmwareNum=0;
    short majorNum, minorNum, buildNum;
    int status;
	int responseLength = 7; 
	  	
    status = sendBuffData(portNum, &cmd, 1);
    if (DEBUG) printf("FirmWare_send: tx status : %d\n", status);
	if (status == I3DMGX2_COMM_OK) {
		if (responseLength>0) {
    		status = receiveData(portNum, &Bresponse[0], responseLength);
            if (DEBUG) printf("FirmWare i3dmgx2_send: rx status : %d and responseLength %d\n", status, responseLength);
    		if (status==I3DMGX2_COMM_OK) {
                wChecksum = convert2ushort(&Bresponse[responseLength-2]);
	            wCalculatedCheckSum = i3dmgx2_Checksum(&Bresponse[0], responseLength-2); //calculate the checkusm, 29 = 31-2 don't include the checksum bytes
					if(wChecksum != wCalculatedCheckSum)
						return	status = I3DMGX2_CHECKSUM_ERROR;
			}else
					return status = I3DMGX2_COMM_READ_ERROR;
		}

		firmwareNum = convert2short(&Bresponse[3]);
		if (firmwareNum > 0) {
			/* format for firmware number is #.#.## */
	        majorNum = firmwareNum / 1000;
		    minorNum = (firmwareNum % 1000) / 100;
			buildNum = firmwareNum % 100;
		    sprintf(firmware, "%d.%d.%d", majorNum, minorNum, buildNum);
		} 
		return I3DMGX2_OK;
	}else return status;
}
/*-------------- end of i3dmgx2Adapter.c ----------------------*/
