MG354PDH0.cpp

00001 #include "mbed.h"
00002 #include "MG354PDH0.h"
00003 
00004 MG354PDH0::MG354PDH0(Serial* ps): p_port(ps){
00005     p_port->baud(460800);
00006 }
00007 
00008 void MG354PDH0::call_window1(){
00009     p_port->putc(0xFE);
00010     p_port->putc(0x01);
00011     p_port->putc(0x0d);
00012 }
00013 
00014 void MG354PDH0::call_window0(){
00015     p_port->putc(0xFE);
00016     p_port->putc(0x00);
00017     p_port->putc(0x0d);
00018  }
00019 
00020 void MG354PDH0::GLOB_CMD_read(){//IMUが使えるかどうかの確認
00021     
00022     p_port->putc(0x0A);
00023     p_port->putc(0x00);
00024     p_port->putc(0x0d);
00025       
00026     while(p_port->getc()!=0x0A){
00027       }
00028     val_GLOB_CMD_read[0] = p_port->getc();
00029     val_GLOB_CMD_read[1] = p_port->getc();
00030     
00031     i_GLOB_CMD_read=(val_GLOB_CMD_read[0]<<8)+val_GLOB_CMD_read[1];
00032     if(i_GLOB_CMD_read==0){
00033         //pc.printf("IMU is ready.1\r\n");
00034         }else{
00035             //pc.printf("IMU is not ready.1\r\n");
00036             }
00037             
00038 }
00039 
00040 void MG354PDH0::DIAG_STAT_read(){
00041     
00042     p_port->putc(0x04);
00043     p_port->putc(0x00);
00044     p_port->putc(0x0d);    
00045     
00046     while(p_port->getc()!=0x04){
00047       }
00048     val_DIAG_STAT_read[0] = p_port->getc();
00049     val_DIAG_STAT_read[1] = p_port->getc();
00050     
00051     i_DIAG_STAT_read=(val_DIAG_STAT_read[0]<<8)+val_DIAG_STAT_read[1];
00052     if(i_DIAG_STAT_read==0){
00053         //pc.printf("IMU is ready.2\r\n");
00054         }else{
00055             //pc.printf("IMU is not ready.2\r\n");
00056             }
00057 }
00058 
00059 void MG354PDH0::UART_CTRL_write(){//IMUのボーレートを480600,手動モードへ移行
00060     
00061     call_window1();
00062     
00063     p_port->putc(0x09);
00064     p_port->putc(0x00);
00065     p_port->putc(0x0d);
00066     
00067     p_port->putc(0x08);
00068     p_port->putc(0x00);
00069     p_port->putc(0x0d);
00070         
00071 }
00072 
00073 /*IMUが動作可能かどうかの確認*/
00074 void MG354PDH0::power_on_sequence1(){
00075     //pc.printf("power on sequence 1\r\n");
00076     call_window1();
00077     GLOB_CMD_read();
00078 }
00079 
00080 /*IMUが動作可能かどうかの確認*/
00081 void MG354PDH0::power_on_sequence2(){
00082     //pc.printf("power on sequence 2\r\n");
00083     call_window0();
00084     DIAG_STAT_read();
00085     }
00086 
00087 void MG354PDH0::move_to_sampling_mode(){
00088     call_window0();
00089     
00090     p_port->putc(0x83);
00091     p_port->putc(0x01);
00092     p_port->putc(0x0d);    
00093 }
00094 
00095 float MG354PDH0::read_angular_rate_x(){
00096     
00097     call_window0();
00098     
00099     p_port->putc(0x12);//register of gyro x (High)
00100     p_port->putc(0x00);
00101     p_port->putc(0x0d);    
00102     
00103     while(p_port->getc()!=0x12){
00104       }
00105     val_rate_x[0] = p_port->getc();
00106     val_rate_x[1] = p_port->getc();
00107 
00108     p_port->putc(0x14);//register of gyro x (Low)
00109     p_port->putc(0x00);
00110     p_port->putc(0x0d);    
00111     
00112     while(p_port->getc()!=0x14){
00113       }
00114     val_rate_x[2] = p_port->getc();
00115     val_rate_x[3] = p_port->getc();
00116     
00117     gyro_x=(val_rate_x[0]<<24)+(val_rate_x[1]<<16)+(val_rate_x[2]<<8)+val_rate_x[3];
00118     //SCALE_FACTOR 0.016
00119     gyro_x*=(SCALE_FACTOR_GYRO/65536);
00120     
00121     return gyro_x;
00122     
00123 }
00124     
00125 float MG354PDH0::read_angular_rate_y(){
00126     
00127     call_window0();
00128     
00129     p_port->putc(0x16);//register of gyro y (High)
00130     p_port->putc(0x00);
00131     p_port->putc(0x0d);    
00132     
00133     while(p_port->getc()!=0x16){
00134       }
00135     val_rate_y[0] = p_port->getc();
00136     val_rate_y[1] = p_port->getc();
00137 
00138     p_port->putc(0x18);//register of gyro y (Low)
00139     p_port->putc(0x00);
00140     p_port->putc(0x0d);    
00141     
00142     while(p_port->getc()!=0x18){
00143       }
00144     val_rate_y[2] = p_port->getc();
00145     val_rate_y[3] = p_port->getc();
00146     
00147     gyro_y=(val_rate_y[0]<<24)+(val_rate_y[1]<<16)+(val_rate_y[2]<<8)+val_rate_y[3];
00148     //SCALE_FACTOR 0.016
00149     gyro_y*=(SCALE_FACTOR_GYRO/65536);
00150     
00151     return gyro_y;
00152     
00153 }
00154     
00155 float MG354PDH0::read_angular_rate_z(){
00156     
00157     call_window0();
00158     
00159     p_port->putc(0x1A);//register of gyro z (High)
00160     p_port->putc(0x00);
00161     p_port->putc(0x0d);    
00162     
00163     while(p_port->getc()!=0x1A){
00164       }
00165     val_rate_z[0] = p_port->getc();
00166     val_rate_z[1] = p_port->getc();
00167 
00168     p_port->putc(0x1C);//register of gyro z (Low)
00169     p_port->putc(0x00);
00170     p_port->putc(0x0d);    
00171     
00172     while(p_port->getc()!=0x1C){
00173       }
00174     val_rate_z[2] = p_port->getc();
00175     val_rate_z[3] = p_port->getc();
00176     
00177     gyro_z=(val_rate_z[0]<<24)+(val_rate_z[1]<<16)+(val_rate_z[2]<<8)+val_rate_z[3];
00178     //SCALE_FACTOR 0.016
00179     gyro_z*=(SCALE_FACTOR_GYRO/65536);
00180     
00181     return gyro_z;
00182     
00183     }
00184     
00185 
00186 float MG354PDH0::read_acceleration_x(){
00187     
00188     call_window0();
00189     
00190     p_port->putc(0x1E);//register of gyro z (High)
00191     p_port->putc(0x00);
00192     p_port->putc(0x0d);    
00193     
00194     while(p_port->getc()!=0x1E){
00195       }
00196     val_acc_x[0] = p_port->getc();
00197     val_acc_x[1] = p_port->getc();
00198 
00199     p_port->putc(0x20);//register of gyro z (Low)
00200     p_port->putc(0x00);
00201     p_port->putc(0x0d);    
00202     
00203     while(p_port->getc()!=0x20){
00204       }
00205     val_acc_x[2] = p_port->getc();
00206     val_acc_x[3] = p_port->getc();
00207     
00208     acceleration_x=(val_acc_x[0]<<24)+(val_acc_x[1]<<16)+(val_acc_x[2]<<8)+val_acc_x[3];
00209     //SCALE_FACTOR 0.016
00210     acceleration_x*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
00211     
00212     return acceleration_x;
00213     
00214 }
00215     
00216 float MG354PDH0::read_acceleration_y(){
00217     
00218     call_window0();
00219     
00220     p_port->putc(0x22);//register of gyro z (High)
00221     p_port->putc(0x00);
00222     p_port->putc(0x0d);    
00223     
00224     while(p_port->getc()!=0x22){
00225       }
00226     val_acc_y[0] = p_port->getc();
00227     val_acc_y[1] = p_port->getc();
00228 
00229     p_port->putc(0x24);//register of gyro z (Low)
00230     p_port->putc(0x00);
00231     p_port->putc(0x0d);    
00232     
00233     while(p_port->getc()!=0x24){
00234       }
00235     val_acc_y[2] = p_port->getc();
00236     val_acc_y[3] = p_port->getc();
00237     
00238     acceleration_y=(val_acc_y[0]<<24)+(val_acc_y[1]<<16)+(val_acc_y[2]<<8)+val_acc_y[3];
00239     acceleration_y*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
00240     
00241     return acceleration_y;
00242     
00243     }
00244 
00245 float MG354PDH0::read_acceleration_z(){
00246     
00247     call_window0();
00248     
00249     p_port->putc(0x26);//register of gyro z (High)
00250     p_port->putc(0x00);
00251     p_port->putc(0x0d);    
00252     
00253     while(p_port->getc()!=0x26){
00254       }
00255     val_acc_z[0] = p_port->getc();
00256     val_acc_z[1] = p_port->getc();
00257 
00258     p_port->putc(0x28);//register of gyro z (Low)
00259     p_port->putc(0x00);
00260     p_port->putc(0x0d);    
00261     
00262     while(p_port->getc()!=0x28){
00263       }
00264     val_acc_z[2] = p_port->getc();
00265     val_acc_z[3] = p_port->getc();
00266     
00267     acceleration_z=(val_acc_z[0]<<24)+(val_acc_z[1]<<16)+(val_acc_z[2]<<8)+val_acc_z[3];
00268     acceleration_z*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
00269     
00270     return acceleration_z;
00271     
00272 }
00273     
00274 
00275 
00276 
00277 
00278