9dof.cpp

00001 #include "9dof.h"
00002 
00003 dof9RazorImu::dof9RazorImu(PinName tx, PinName rx) 
00004 {
00005     razor = new Serial(tx, rx);
00006     razor->baud(BAUD_RATE);
00007 }
00008 
00009 void dof9RazorImu::update(void) 
00010 {
00011   
00012    if(DOFFW == 0){
00013      razor->printf("4");
00014         while (razor->getc() != '$');
00015         razor->scanf(",%d,%d,%d", &acc_x, &acc_y, &acc_z);
00016         razor->scanf(",%d,%d,%d", &gyro_x, &gyro_y, &gyro_z);
00017         razor->scanf(",%d,%d,%d,#", &mag_x, &mag_y, &mag_z);
00018     }else{
00019         while (razor->getc() != '$');
00020         razor->scanf(",%d,%d,%d", &gyro_x, &gyro_y, &gyro_z);
00021         razor->scanf(",%d,%d,%d", &acc_x, &acc_y, &acc_z);    
00022         razor->scanf(",%d,%d,%d,#", &mag_x, &mag_y, &mag_z);
00023     }
00024 }
00025 
00026 
00027 int dof9RazorImu::getGyroX(void)
00028 { return gyro_x; }
00029 
00030 int dof9RazorImu::getGyroY(void)
00031 { return gyro_y; }
00032 
00033 int dof9RazorImu::getGyroZ(void)
00034 { return gyro_z; }
00035 
00036 int dof9RazorImu::getAccX(void)
00037 { return acc_x; }
00038 
00039 int dof9RazorImu::getAccY(void)
00040 { return acc_x; }
00041 
00042 int dof9RazorImu::getAccZ(void)
00043 { return acc_z; }
00044 
00045 int dof9RazorImu::getMagX(void)
00046 { return mag_x; }
00047 
00048 int dof9RazorImu::getMagY(void)
00049 { return mag_y; }
00050 
00051 int dof9RazorImu::getMagZ(void)
00052 { return mag_z; }