File content as of revision 4:a377ecb9364f:

#include "compass.h"

Compass::Compass(MySerial _compassSerial)
{
     printf(" _compassSerial %p...\r\n",_compassSerial);
     this->_compassSerial = _compassSerial;
     temp=_compassSerial;
     printf(" this->_compassSerial %p...\r\n",this->_compassSerial);
    buffer = new uint8_t [DATA_BUFFER_SIZE];
    _degree = 0;
    _MSB = 0;
    _LSB = 0;
    _in = _out = 0;
    
         printf(" this->_compassSerial %p...\r\n",this->_compassSerial);

  
    if(_compassSerial->writeable())
    {
            printf("_writable()...\r\n");
        this->_compassSerial->putc('1');
    }
    printf("_MSB = 1 wrote...\r\n");
    if(_compassSerial->writeable())
    {
        printf("_writable()...\r\n");
        this->_compassSerial->putc('2');
    }
    printf("_LSB = 2 wrote...\r\n");
    wait(0.1);

}

Compass::~Compass()
{
    delete[] buffer;
}

void Compass::putToBuffer(uint8_t _x)
{
    buffer[_in++] = _x;
    if(_in == DATA_BUFFER_SIZE-1)
    {
        _in &= 0x00;
    }
}

void Compass::clearBuffer()
{
    _in = _out = 0;
}

int Compass::read()
{
    printf("Entering Compass::read()...\r\n");
    printf(" this->_compassSerial %p...\r\n",this->_compassSerial);
    printf(" temp %p...\r\n",temp);

    printf("_writable()?...\r\n");
    if(_compassSerial->writeable())
    {
       printf("can write..\r\n");
    }
    
    printf("readable()?...\r\n");
    if(_compassSerial->writeable())
    {
       printf("can read..\r\n");
    }
    
    stop();
    clearBuffer();
    
   
    printf("0\r\n");
    resume();
    if(buffer[_out++] == ACK_RESUME_MSB && buffer[_out++] == ACK_RESUME_LSB)
    {
    }
    else
    {
        clearBuffer();
        return -1;
    }
    
    printf("1\r\n");
    run();
    if(buffer[_out++] == ACK_RUN_MSB && buffer[_out++] == ACK_RUN_LSB)
    {
    }
    else
    {
        clearBuffer();
        return -1;
    }
    
    printf("2\r\n");
    _MSB = buffer[_out++];
    _LSB = buffer[_out++];
    printf("_MSB: %x, _LSB: %x\r\n", _MSB, _LSB);
                
    printf("3\r\n");
    stop();
    if(buffer[_in - 1] == ACK_STOP_MSB && buffer[_in] == ACK_STOP_LSB)
    {
    }
    else
    {
        return -1;
    }
    _out = _in;
    clearBuffer();
    _degree=0;
    printf("Exiting Compass::read()...\r\n");
    return _degree;
}

void Compass::run() 
{
    printf("Entering Compass::run()...\r\n");
    write2Bytes(RUN_MSB, RUN_LSB);
}

void Compass::stop() 
{
    printf("Entering Compass::stop()...\r\n");
    write2Bytes(STOP_MSB, STOP_LSB);
}

void Compass::resume() 
{
    printf("Entering Compass::resume()...\r\n");
    write2Bytes(RESUME_MSB, RESUME_LSB);
}

void Compass::reset() 
{
    printf("Entering Compass::reset()...\r\n");
    write2Bytes(RST_MSB, RST_LSB);
}

void Compass::write2Bytes(char msb, char lsb) 
{
    printf("before writable...\r\n");
    if(_compassSerial->writeable())
    {
        this->_compassSerial->putc(msb);
    }
    printf("_MSB = %x wrote...\r\n", msb);
    if(_compassSerial->writeable())
    {
        this->_compassSerial->putc(lsb);
    }
    printf("_LSB = %x wrote...\r\n", lsb);
    wait(0.1);
}