/*
                                 HiTechnic Compass Driver
                                 HTCompassDrv.h
                                 Version  1.0.2.


This program reads the HiTechnic compass and displays the heading on the screen.
Two NXT buttons can also be used
      Left Arrow Button - send command to put the compass in Calibration Mode
      Right Arrow Button - send command to put the compass in Read Mode

The memory model for the HiTechnic compass is

Address       Type            Contents
- - - - - - - - - - - - - - - - - - - - - - - - -
00 – 07H    chars       Sensor version number
08 – 0FH    chars       Manufacturer
10 – 17H    chars       Sensor type
18 – 3DH    bytes       Not used
3E, 3FH    chars       Reserved
40H          byte          Not used
41H          byte          Mode control
42H          byte          Heading } two degree heading
43H          byte          Heading } one degree adder
44, 45H    word          Heading (low byte, high byte)

*/



tSensors CompassPort;

void HTCompassSensor(tSensors port)
{
   SensorType[port] = sensorI2CCustomStd; // this is line 35 !!!!!!!!!!!!!!
  wait10Msec(5);
  nI2CBytesReady[port]=0;
  while(nI2CStatus[port]== STAT_COMM_PENDING)
  wait1Msec(2);

  CompassPort=port;
}


int HTCompassValue(tSensors port)
{
  int hUpper, hLower, heading;
  TButtons nBtn;

   typedef struct{
    byte nMsgSize;
    byte nDeviceAddress;
    byte nLocationPtr;
    byte nCompassMode;
  } TI2C_Output;

  byte replyMessage[6];

  TI2C_Output sOutput;

     nI2CBytesReady[S3] = 0;
      sOutput.nMsgSize = 2;                            //read the value from the sensor
    sOutput.nDeviceAddress = 0x02;
    sOutput.nLocationPtr = 0x42;
    sendI2CMsg(S3, sOutput.nMsgSize, 6);

    while (nI2CStatus[S3] == STAT_COMM_PENDING)
      wait1Msec(2);

    if (nI2CStatus[S3] == NO_ERR)
    {
      readI2CReply(S3, replyMessage[0], 6);            //get the compass heading

      hUpper = (replyMessage[0] & 0x00FF);
      hLower = (replyMessage[1] & 0x00FF);
      heading = (hUpper << 1) | hLower ;                //convert contents of 0x44 0x45

    }

    return heading;
}