compass.cpp

00001 #include "compass.h"
00002 /*
00003 Serial pc(USBTX, USBRX);
00004 
00005 
00006 */
00007 
00008 COMPASS::COMPASS(MySerial* serial)
00009 {
00010    this->_serial = serial;
00011    
00012    serial->baud(56000);
00013    serial->format(8,SerialBase::None, 1);
00014    
00015    init();
00016 }
00017 
00018 uint16_t COMPASS::read()
00019 {
00020     printf("before resume  \r\n");
00021     resume();
00022     printf("after resume \r\n");
00023 
00024     wait(0.5);
00025          printf("before run \r\n");
00026 
00027     run();
00028              printf("after run \r\n");
00029 
00030     printf("enter while buffer_count: %d \r\n",buffer_count);
00031 
00032     
00033     
00034     printf("enter while 1 \r\n");
00035     while(1)
00036     {
00037           if(buffer_count<=2)
00038          {
00039              buffer_count=0;
00040              break;
00041          }
00042          
00043          buffer_count--;
00044          uint8_t tempc1=buffer[buffer_count];
00045          buffer_count--;
00046          uint8_t tempc2=buffer[buffer_count];
00047          
00048                  temp[0]= tempc1;
00049                  temp[1]= tempc2;
00050                  
00051                  if( 0xa0==( (temp[0]) & 0xe0))
00052                  { 
00053                     flag=1;
00054                     printf("match !!!!!\r\n");
00055                  }
00056              
00057             
00058             
00059         
00060         
00061         if(flag==1)
00062         {
00063                       twobytes=(temp[0])*256+(uint8_t)(temp[1]);
00064             
00065                      digits= temp[1] & 0x0f;
00066             
00067                       tens= (temp[1]>>4) & 0x0f;
00068             
00069                       hundreds= temp[0] & 0x07;
00070         
00071                      _degree=100*hundreds+10*tens+digits;
00072                       printf("buffer[0]: %x\r\n",temp[0]);
00073                       printf("buffer[1]: %x\r\n",temp[1]);
00074                      temp[0]=0;
00075                      temp[1]=0;
00076                      flag=0;
00077            
00078             buffer_count=0;
00079             
00080              
00081              // printf("hundreds: %d\r\n",hundreds);
00082 //              printf("tens: %d\r\n",tens);
00083 //              printf("digits: %d\r\n",digits);
00084           
00085                resume();
00086                
00087                
00088                _degree+=90;
00089                
00090                if(_degree>=360)
00091                _degree-=360;
00092               
00093                    printf("degree: %d\r\n",_degree);
00094               printf("----------------------\r\n");
00095                return _degree;
00096 //            break;
00097         }
00098    
00099     }
00100      // printf("leave while 1 \r\n");
00101 //       wait(0.5);
00102 //       
00103 //     printf("before resume2 \r\n");
00104 //
00105 //     
00106 //           printf("after resume2 \r\n");
00107 
00108            
00109          
00110     return 0;
00111 }
00112 
00113 void COMPASS::init() 
00114 {
00115     buffer[0]=0;
00116     buffer[1]=0;
00117     count=0;
00118     flag=0;
00119     buffer_count=0;
00120     for(int i=0;i<_BUFFER_SIZE;i++)
00121     {
00122         buffer[i]=0;
00123     }
00124 }
00125 
00126 void COMPASS::run() 
00127 {
00128     write2Bytes(RUN_MSB,RUN_LSB);
00129 }
00130 
00131 void COMPASS::stop() 
00132 {
00133     write2Bytes(STOP_MSB,STOP_LSB);
00134 }
00135 
00136 void COMPASS::resume() 
00137 {
00138     write2Bytes(RESUME_MSB,RESUME_LSB);
00139 }
00140 
00141 void COMPASS::reset() 
00142 {
00143     write2Bytes(RST_MSB,RST_LSB);
00144 }
00145 
00146 void COMPASS::write2Bytes(char msb, char lsb) 
00147 {
00148     _serial->putc(lsb);
00149     _serial->putc(msb);
00150     
00151 }
00152 
00153 void COMPASS::putToBuffer(uint8_t data) 
00154 {
00155     if(buffer_count<_BUFFER_SIZE-10)
00156     buffer_count++;
00157     else
00158      {
00159      printf("Error full buffer \r\n");
00160      buffer_count=0;
00161      }
00162      
00163     buffer[buffer_count]=data;
00164     
00165 }
00166 //
00167 //void COMPASS::check_time_out()
00168 //{
00169 //    if(flag == 1)
00170 //    {
00171 //        ON();
00172 //        wait(5);
00173 //        mbed_reset();
00174 //    }
00175 //    else
00176 //    {
00177 //        OFF();
00178 //    }
00179 //}
00180 //
00181 //void COMPASS::time_out_init()
00182 //{
00183 //    setFlag();
00184 //    time_out.detach();
00185 //    time_out.attach(this, &Buzzer::check_time_out, TIME_OUT);
00186 //}