SerialRazor.cpp

00001 
00002 #include "mbed.h"
00003 #include "SerialRazor.h"
00004 
00005 SerialRazor::SerialRazor( size_t bufferSize, PinName tx, PinName rx ) : Serial(  tx,  rx )  {
00006 
00007     Serial::baud(57600);
00008     attach( this, &SerialRazor::handleInterrupt );
00009     m_buff = (char *) malloc( bufferSize );
00010     m_buffSize = bufferSize;
00011 }
00012 
00013 
00014 
00015 SerialRazor::~SerialRazor()
00016 {
00017     if( m_buff )
00018         free( m_buff );
00019 }
00020 
00021 void SerialRazor::handleInterrupt()
00022 {
00023     
00024     while( Serial::readable())
00025     {
00026         for(int i=0; i< m_buffSize; i++) {
00027         m_buff[i] = Serial::getc();
00028         if(m_buff[i] == '\n') {
00029             m_buff[i] = 0;
00030             sscanf(m_buff,"!ANG:%f,%f,%f", &roll, &pitch, &yaw); 
00031             return;
00032         } 
00033         }   
00034        
00035     }
00036 }
00037 
00038 
00039 
00040 float SerialRazor::getRoll(void){
00041 
00042     return roll;
00043 
00044 }
00045 
00046 float SerialRazor::getPitch(void){
00047 
00048     return pitch;
00049 
00050 }
00051 
00052 float SerialRazor::getYaw(void){
00053 
00054     return yaw;
00055 
00056 }
00057