Revision 0:9e92f863fde5, committed 2021-04-28

Comitter:

Joeatsumi

Date:

Wed Apr 28 07:39:09 2021 +0000

Commit message:

M-G354PDH0 program that utilizes library.

Changed in this revision

diff -r 000000000000 -r 9e92f863fde5 MG354PDH0.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MG354PDH0.cpp	Wed Apr 28 07:39:09 2021 +0000
@@ -0,0 +1,278 @@
+#include "mbed.h"
+#include "MG354PDH0.h"
+
+MG354PDH0::MG354PDH0(Serial* ps): p_port(ps){
+    p_port->baud(460800);
+}
+
+void MG354PDH0::call_window1(){
+    p_port->putc(0xFE);
+    p_port->putc(0x01);
+    p_port->putc(0x0d);
+}
+
+void MG354PDH0::call_window0(){
+    p_port->putc(0xFE);
+    p_port->putc(0x00);
+    p_port->putc(0x0d);
+ }
+
+void MG354PDH0::GLOB_CMD_read(){//IMUが使えるかどうかの確認
+    
+    p_port->putc(0x0A);
+    p_port->putc(0x00);
+    p_port->putc(0x0d);
+      
+    while(p_port->getc()!=0x0A){
+      }
+    val_GLOB_CMD_read[0] = p_port->getc();
+    val_GLOB_CMD_read[1] = p_port->getc();
+    
+    i_GLOB_CMD_read=(val_GLOB_CMD_read[0]<<8)+val_GLOB_CMD_read[1];
+    if(i_GLOB_CMD_read==0){
+        //pc.printf("IMU is ready.1\r\n");
+        }else{
+            //pc.printf("IMU is not ready.1\r\n");
+            }
+            
+}
+
+void MG354PDH0::DIAG_STAT_read(){
+    
+    p_port->putc(0x04);
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x04){
+      }
+    val_DIAG_STAT_read[0] = p_port->getc();
+    val_DIAG_STAT_read[1] = p_port->getc();
+    
+    i_DIAG_STAT_read=(val_DIAG_STAT_read[0]<<8)+val_DIAG_STAT_read[1];
+    if(i_DIAG_STAT_read==0){
+        //pc.printf("IMU is ready.2\r\n");
+        }else{
+            //pc.printf("IMU is not ready.2\r\n");
+            }
+}
+
+void MG354PDH0::UART_CTRL_write(){//IMUのボーレートを480600,手動モードへ移行
+    
+    call_window1();
+    
+    p_port->putc(0x09);
+    p_port->putc(0x00);
+    p_port->putc(0x0d);
+    
+    p_port->putc(0x08);
+    p_port->putc(0x00);
+    p_port->putc(0x0d);
+        
+}
+
+/*IMUが動作可能かどうかの確認*/
+void MG354PDH0::power_on_sequence1(){
+    //pc.printf("power on sequence 1\r\n");
+    call_window1();
+    GLOB_CMD_read();
+}
+
+/*IMUが動作可能かどうかの確認*/
+void MG354PDH0::power_on_sequence2(){
+    //pc.printf("power on sequence 2\r\n");
+    call_window0();
+    DIAG_STAT_read();
+    }
+
+void MG354PDH0::move_to_sampling_mode(){
+    call_window0();
+    
+    p_port->putc(0x83);
+    p_port->putc(0x01);
+    p_port->putc(0x0d);    
+}
+
+float MG354PDH0::read_angular_rate_x(){
+    
+    call_window0();
+    
+    p_port->putc(0x12);//register of gyro x (High)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x12){
+      }
+    val_rate_x[0] = p_port->getc();
+    val_rate_x[1] = p_port->getc();
+
+    p_port->putc(0x14);//register of gyro x (Low)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x14){
+      }
+    val_rate_x[2] = p_port->getc();
+    val_rate_x[3] = p_port->getc();
+    
+    gyro_x=(val_rate_x[0]<<24)+(val_rate_x[1]<<16)+(val_rate_x[2]<<8)+val_rate_x[3];
+    //SCALE_FACTOR 0.016
+    gyro_x*=(SCALE_FACTOR_GYRO/65536);
+    
+    return gyro_x;
+    
+}
+    
+float MG354PDH0::read_angular_rate_y(){
+    
+    call_window0();
+    
+    p_port->putc(0x16);//register of gyro y (High)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x16){
+      }
+    val_rate_y[0] = p_port->getc();
+    val_rate_y[1] = p_port->getc();
+
+    p_port->putc(0x18);//register of gyro y (Low)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x18){
+      }
+    val_rate_y[2] = p_port->getc();
+    val_rate_y[3] = p_port->getc();
+    
+    gyro_y=(val_rate_y[0]<<24)+(val_rate_y[1]<<16)+(val_rate_y[2]<<8)+val_rate_y[3];
+    //SCALE_FACTOR 0.016
+    gyro_y*=(SCALE_FACTOR_GYRO/65536);
+    
+    return gyro_y;
+    
+}
+    
+float MG354PDH0::read_angular_rate_z(){
+    
+    call_window0();
+    
+    p_port->putc(0x1A);//register of gyro z (High)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x1A){
+      }
+    val_rate_z[0] = p_port->getc();
+    val_rate_z[1] = p_port->getc();
+
+    p_port->putc(0x1C);//register of gyro z (Low)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x1C){
+      }
+    val_rate_z[2] = p_port->getc();
+    val_rate_z[3] = p_port->getc();
+    
+    gyro_z=(val_rate_z[0]<<24)+(val_rate_z[1]<<16)+(val_rate_z[2]<<8)+val_rate_z[3];
+    //SCALE_FACTOR 0.016
+    gyro_z*=(SCALE_FACTOR_GYRO/65536);
+    
+    return gyro_z;
+    
+    }
+    
+
+float MG354PDH0::read_acceleration_x(){
+    
+    call_window0();
+    
+    p_port->putc(0x1E);//register of gyro z (High)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x1E){
+      }
+    val_acc_x[0] = p_port->getc();
+    val_acc_x[1] = p_port->getc();
+
+    p_port->putc(0x20);//register of gyro z (Low)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x20){
+      }
+    val_acc_x[2] = p_port->getc();
+    val_acc_x[3] = p_port->getc();
+    
+    acceleration_x=(val_acc_x[0]<<24)+(val_acc_x[1]<<16)+(val_acc_x[2]<<8)+val_acc_x[3];
+    //SCALE_FACTOR 0.016
+    acceleration_x*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
+    
+    return acceleration_x;
+    
+}
+    
+float MG354PDH0::read_acceleration_y(){
+    
+    call_window0();
+    
+    p_port->putc(0x22);//register of gyro z (High)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x22){
+      }
+    val_acc_y[0] = p_port->getc();
+    val_acc_y[1] = p_port->getc();
+
+    p_port->putc(0x24);//register of gyro z (Low)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x24){
+      }
+    val_acc_y[2] = p_port->getc();
+    val_acc_y[3] = p_port->getc();
+    
+    acceleration_y=(val_acc_y[0]<<24)+(val_acc_y[1]<<16)+(val_acc_y[2]<<8)+val_acc_y[3];
+    acceleration_y*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
+    
+    return acceleration_y;
+    
+    }
+
+float MG354PDH0::read_acceleration_z(){
+    
+    call_window0();
+    
+    p_port->putc(0x26);//register of gyro z (High)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x26){
+      }
+    val_acc_z[0] = p_port->getc();
+    val_acc_z[1] = p_port->getc();
+
+    p_port->putc(0x28);//register of gyro z (Low)
+    p_port->putc(0x00);
+    p_port->putc(0x0d);    
+    
+    while(p_port->getc()!=0x28){
+      }
+    val_acc_z[2] = p_port->getc();
+    val_acc_z[3] = p_port->getc();
+    
+    acceleration_z=(val_acc_z[0]<<24)+(val_acc_z[1]<<16)+(val_acc_z[2]<<8)+val_acc_z[3];
+    acceleration_z*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
+    
+    return acceleration_z;
+    
+}
+    
+
+
+
+
+

diff -r 000000000000 -r 9e92f863fde5 MG354PDH0.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MG354PDH0.h	Wed Apr 28 07:39:09 2021 +0000
@@ -0,0 +1,61 @@
+
+class MG354PDH0 {
+    
+public: 
+    MG354PDH0(Serial* ps);
+
+    void call_window1();
+    void call_window0();
+    void GLOB_CMD_read();
+    void DIAG_STAT_read();
+    void UART_CTRL_write();
+    /*サンプリングモードへの移行*/
+    void move_to_sampling_mode();
+    /*IMUが動作可能かどうかの確認*/
+    void power_on_sequence1();
+    /*IMUが動作可能かどうかの確認*/
+    void power_on_sequence2();
+    
+    /*X軸周りの角速度を算出*/
+    float read_angular_rate_x();
+    /*Y軸周りの角速度を算出*/
+    float read_angular_rate_y();
+    /*Z軸周りの角速度を算出*/
+    float read_angular_rate_z();
+    
+    /*X軸の加速度を算出*/
+    float read_acceleration_x();
+    /*Y軸の加速度を算出*/
+    float read_acceleration_y();
+    /*Z軸の加速度を算出*/
+    float read_acceleration_z();
+        
+    float gyro_val[3];//角速度の値
+    float acc_val[3];//加速度の値
+     
+    
+private: 
+    Serial* p_port;
+    
+    #define SCALE_FACTOR_GYRO 0.016
+    #define SCALE_FACTOR_ACC 0.20
+    #define GRAVITIONAL_ACCELERATION 9.80665
+
+    char val_GLOB_CMD_read[2];
+    short i_GLOB_CMD_read;
+    
+    char val_DIAG_STAT_read[2];
+    short i_DIAG_STAT_read;
+    
+    char val_rate_x[4];
+    char val_rate_y[4];
+    char val_rate_z[4];
+    
+    char val_acc_x[4];
+    char val_acc_y[4];
+    char val_acc_z[4];
+    
+    float gyro_x,gyro_y,gyro_z;
+    float acceleration_x,acceleration_y,acceleration_z;
+    
+};
\ No newline at end of file