/*
                                 Test HiTechnic Compass Sensor

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)

*/

task main()
{
  int hUpper, hLower, heading;

  TButtons nBtn;

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

  SensorType[S3] = sensorI2CCustomStd;
  wait10Msec(5);
  nI2CBytesReady[S3]=0;
  while(nI2CStatus[S3]== STAT_COMM_PENDING)
  wait1Msec(2);

  byte replyMessage[6];

  TI2C_Output sOutput;

  while(true)
  {
    nxtDisplayTextLine(1, "Mode = Read");
     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

         nxtDisplayTextLine(4, "U-val %d", hUpper);
      nxtDisplayTextLine(5, "L-val %d", hLower);
      nxtDisplayTextLine(6, "Heading = %d", heading);

    }
    else
    {
      nxtDisplayTextLine(1, "i2c err %d", nI2CStatus[S3]);
    }

  //check if a button has been pressed.

  nBtn = nNxtButtonPressed; // check for button press

     switch (nBtn)
     {
        case kLeftButton:
           {
              PlayTone(440,1000);
              sOutput.nMsgSize = 3;
              sOutput.nDeviceAddress = 0x02;
              sOutput.nLocationPtr = 0x41;               //Calibration mode command
              sOutput.nCompassMode = 0x43;

              nI2CBytesReady[S3] = 0;
               sendI2CMsg(S3, sOutput.nMsgSize, 0);
               wait1Msec(40);
               while ((nBtn = nNxtButtonPressed) != kRightButton)
                 {                        //wait for the right button to be pressed before leaving cal mode;
                    nxtDisplayTextLine(1, "Mode = Cal ");
                    wait1Msec(1000);
                    PlaySound(soundBlip);
                 }
              break;
           }
         case kRightButton:
            {
               PlaySound(soundBeepBeep);

               sOutput.nMsgSize = 3;
              sOutput.nDeviceAddress = 0x02;
              sOutput.nLocationPtr = 0x41;
               sOutput.nCompassMode   = 0x00;                  //Read mode command
              nI2CBytesReady[S3] = 0;
               sendI2CMsg(S3, sOutput.nMsgSize, 0);
               wait1Msec(1250);                                    //wait for the cal info to be saved
               break;
           }
      }

  }
}

/*
                                 HiTechnic Compass Driver
                                 HTCompassDrv.h
                                 Version  1.0.2.


*/



tSensors CompassPort;

void HTCompassSensor(tSensors port)
{
   SensorType[port] = sensorI2CCustomStd;
  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;
    //  hier kann spaeter noch fuer die Kalibrierung erweitert werden
     //  nBtn = nNxtButtonPressed; // check for button press
}

#include "HTCompassDrv.h"

task main()
{
   int heading;

   while(true)
   {
     HTCompassSensor(S3);  // setzt HTCompass an CompassPort=S3
     heading=HTCompassValue(CompassPort);
     nxtDisplayTextLine(6, "Heading = %5d", heading);
   }
}