Atsumi Toda / MG354PDH0

M-G354PDH0 library.

Dependents:   M-G354PDH0_serial_Lib INS_GNSS_logger_1 UBX_GPS_muliti_SPI_4_logger_ver

MG354PDH0.cpp@0:9e92f863fde5, 2021-04-28 (annotated)

Committer:
Joeatsumi
Date:
Wed Apr 28 07:39:09 2021 +0000
Revision:
0:9e92f863fde5
M-G354PDH0 program that utilizes library.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:9e92f863fde5 1 #include "mbed.h"
Joeatsumi 0:9e92f863fde5 2 #include "MG354PDH0.h"
Joeatsumi 0:9e92f863fde5 3
Joeatsumi 0:9e92f863fde5 4 MG354PDH0::MG354PDH0(Serial* ps): p_port(ps){
Joeatsumi 0:9e92f863fde5 5 p_port->baud(460800);
Joeatsumi 0:9e92f863fde5 6 }
Joeatsumi 0:9e92f863fde5 7
Joeatsumi 0:9e92f863fde5 8 void MG354PDH0::call_window1(){
Joeatsumi 0:9e92f863fde5 9 p_port->putc(0xFE);
Joeatsumi 0:9e92f863fde5 10 p_port->putc(0x01);
Joeatsumi 0:9e92f863fde5 11 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 12 }
Joeatsumi 0:9e92f863fde5 13
Joeatsumi 0:9e92f863fde5 14 void MG354PDH0::call_window0(){
Joeatsumi 0:9e92f863fde5 15 p_port->putc(0xFE);
Joeatsumi 0:9e92f863fde5 16 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 17 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 18 }
Joeatsumi 0:9e92f863fde5 19
Joeatsumi 0:9e92f863fde5 20 void MG354PDH0::GLOB_CMD_read(){//IMUが使えるかどうかの確認
Joeatsumi 0:9e92f863fde5 21
Joeatsumi 0:9e92f863fde5 22 p_port->putc(0x0A);
Joeatsumi 0:9e92f863fde5 23 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 24 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 25
Joeatsumi 0:9e92f863fde5 26 while(p_port->getc()!=0x0A){
Joeatsumi 0:9e92f863fde5 27 }
Joeatsumi 0:9e92f863fde5 28 val_GLOB_CMD_read[0] = p_port->getc();
Joeatsumi 0:9e92f863fde5 29 val_GLOB_CMD_read[1] = p_port->getc();
Joeatsumi 0:9e92f863fde5 30
Joeatsumi 0:9e92f863fde5 31 i_GLOB_CMD_read=(val_GLOB_CMD_read[0]<<8)+val_GLOB_CMD_read[1];
Joeatsumi 0:9e92f863fde5 32 if(i_GLOB_CMD_read==0){
Joeatsumi 0:9e92f863fde5 33 //pc.printf("IMU is ready.1\r\n");
Joeatsumi 0:9e92f863fde5 34 }else{
Joeatsumi 0:9e92f863fde5 35 //pc.printf("IMU is not ready.1\r\n");
Joeatsumi 0:9e92f863fde5 36 }
Joeatsumi 0:9e92f863fde5 37
Joeatsumi 0:9e92f863fde5 38 }
Joeatsumi 0:9e92f863fde5 39
Joeatsumi 0:9e92f863fde5 40 void MG354PDH0::DIAG_STAT_read(){
Joeatsumi 0:9e92f863fde5 41
Joeatsumi 0:9e92f863fde5 42 p_port->putc(0x04);
Joeatsumi 0:9e92f863fde5 43 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 44 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 45
Joeatsumi 0:9e92f863fde5 46 while(p_port->getc()!=0x04){
Joeatsumi 0:9e92f863fde5 47 }
Joeatsumi 0:9e92f863fde5 48 val_DIAG_STAT_read[0] = p_port->getc();
Joeatsumi 0:9e92f863fde5 49 val_DIAG_STAT_read[1] = p_port->getc();
Joeatsumi 0:9e92f863fde5 50
Joeatsumi 0:9e92f863fde5 51 i_DIAG_STAT_read=(val_DIAG_STAT_read[0]<<8)+val_DIAG_STAT_read[1];
Joeatsumi 0:9e92f863fde5 52 if(i_DIAG_STAT_read==0){
Joeatsumi 0:9e92f863fde5 53 //pc.printf("IMU is ready.2\r\n");
Joeatsumi 0:9e92f863fde5 54 }else{
Joeatsumi 0:9e92f863fde5 55 //pc.printf("IMU is not ready.2\r\n");
Joeatsumi 0:9e92f863fde5 56 }
Joeatsumi 0:9e92f863fde5 57 }
Joeatsumi 0:9e92f863fde5 58
Joeatsumi 0:9e92f863fde5 59 void MG354PDH0::UART_CTRL_write(){//IMUのボーレートを480600,手動モードへ移行
Joeatsumi 0:9e92f863fde5 60
Joeatsumi 0:9e92f863fde5 61 call_window1();
Joeatsumi 0:9e92f863fde5 62
Joeatsumi 0:9e92f863fde5 63 p_port->putc(0x09);
Joeatsumi 0:9e92f863fde5 64 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 65 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 66
Joeatsumi 0:9e92f863fde5 67 p_port->putc(0x08);
Joeatsumi 0:9e92f863fde5 68 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 69 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 70
Joeatsumi 0:9e92f863fde5 71 }
Joeatsumi 0:9e92f863fde5 72
Joeatsumi 0:9e92f863fde5 73 /*IMUが動作可能かどうかの確認*/
Joeatsumi 0:9e92f863fde5 74 void MG354PDH0::power_on_sequence1(){
Joeatsumi 0:9e92f863fde5 75 //pc.printf("power on sequence 1\r\n");
Joeatsumi 0:9e92f863fde5 76 call_window1();
Joeatsumi 0:9e92f863fde5 77 GLOB_CMD_read();
Joeatsumi 0:9e92f863fde5 78 }
Joeatsumi 0:9e92f863fde5 79
Joeatsumi 0:9e92f863fde5 80 /*IMUが動作可能かどうかの確認*/
Joeatsumi 0:9e92f863fde5 81 void MG354PDH0::power_on_sequence2(){
Joeatsumi 0:9e92f863fde5 82 //pc.printf("power on sequence 2\r\n");
Joeatsumi 0:9e92f863fde5 83 call_window0();
Joeatsumi 0:9e92f863fde5 84 DIAG_STAT_read();
Joeatsumi 0:9e92f863fde5 85 }
Joeatsumi 0:9e92f863fde5 86
Joeatsumi 0:9e92f863fde5 87 void MG354PDH0::move_to_sampling_mode(){
Joeatsumi 0:9e92f863fde5 88 call_window0();
Joeatsumi 0:9e92f863fde5 89
Joeatsumi 0:9e92f863fde5 90 p_port->putc(0x83);
Joeatsumi 0:9e92f863fde5 91 p_port->putc(0x01);
Joeatsumi 0:9e92f863fde5 92 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 93 }
Joeatsumi 0:9e92f863fde5 94
Joeatsumi 0:9e92f863fde5 95 float MG354PDH0::read_angular_rate_x(){
Joeatsumi 0:9e92f863fde5 96
Joeatsumi 0:9e92f863fde5 97 call_window0();
Joeatsumi 0:9e92f863fde5 98
Joeatsumi 0:9e92f863fde5 99 p_port->putc(0x12);//register of gyro x (High)
Joeatsumi 0:9e92f863fde5 100 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 101 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 102
Joeatsumi 0:9e92f863fde5 103 while(p_port->getc()!=0x12){
Joeatsumi 0:9e92f863fde5 104 }
Joeatsumi 0:9e92f863fde5 105 val_rate_x[0] = p_port->getc();
Joeatsumi 0:9e92f863fde5 106 val_rate_x[1] = p_port->getc();
Joeatsumi 0:9e92f863fde5 107
Joeatsumi 0:9e92f863fde5 108 p_port->putc(0x14);//register of gyro x (Low)
Joeatsumi 0:9e92f863fde5 109 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 110 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 111
Joeatsumi 0:9e92f863fde5 112 while(p_port->getc()!=0x14){
Joeatsumi 0:9e92f863fde5 113 }
Joeatsumi 0:9e92f863fde5 114 val_rate_x[2] = p_port->getc();
Joeatsumi 0:9e92f863fde5 115 val_rate_x[3] = p_port->getc();
Joeatsumi 0:9e92f863fde5 116
Joeatsumi 0:9e92f863fde5 117 gyro_x=(val_rate_x[0]<<24)+(val_rate_x[1]<<16)+(val_rate_x[2]<<8)+val_rate_x[3];
Joeatsumi 0:9e92f863fde5 118 //SCALE_FACTOR 0.016
Joeatsumi 0:9e92f863fde5 119 gyro_x*=(SCALE_FACTOR_GYRO/65536);
Joeatsumi 0:9e92f863fde5 120
Joeatsumi 0:9e92f863fde5 121 return gyro_x;
Joeatsumi 0:9e92f863fde5 122
Joeatsumi 0:9e92f863fde5 123 }
Joeatsumi 0:9e92f863fde5 124
Joeatsumi 0:9e92f863fde5 125 float MG354PDH0::read_angular_rate_y(){
Joeatsumi 0:9e92f863fde5 126
Joeatsumi 0:9e92f863fde5 127 call_window0();
Joeatsumi 0:9e92f863fde5 128
Joeatsumi 0:9e92f863fde5 129 p_port->putc(0x16);//register of gyro y (High)
Joeatsumi 0:9e92f863fde5 130 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 131 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 132
Joeatsumi 0:9e92f863fde5 133 while(p_port->getc()!=0x16){
Joeatsumi 0:9e92f863fde5 134 }
Joeatsumi 0:9e92f863fde5 135 val_rate_y[0] = p_port->getc();
Joeatsumi 0:9e92f863fde5 136 val_rate_y[1] = p_port->getc();
Joeatsumi 0:9e92f863fde5 137
Joeatsumi 0:9e92f863fde5 138 p_port->putc(0x18);//register of gyro y (Low)
Joeatsumi 0:9e92f863fde5 139 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 140 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 141
Joeatsumi 0:9e92f863fde5 142 while(p_port->getc()!=0x18){
Joeatsumi 0:9e92f863fde5 143 }
Joeatsumi 0:9e92f863fde5 144 val_rate_y[2] = p_port->getc();
Joeatsumi 0:9e92f863fde5 145 val_rate_y[3] = p_port->getc();
Joeatsumi 0:9e92f863fde5 146
Joeatsumi 0:9e92f863fde5 147 gyro_y=(val_rate_y[0]<<24)+(val_rate_y[1]<<16)+(val_rate_y[2]<<8)+val_rate_y[3];
Joeatsumi 0:9e92f863fde5 148 //SCALE_FACTOR 0.016
Joeatsumi 0:9e92f863fde5 149 gyro_y*=(SCALE_FACTOR_GYRO/65536);
Joeatsumi 0:9e92f863fde5 150
Joeatsumi 0:9e92f863fde5 151 return gyro_y;
Joeatsumi 0:9e92f863fde5 152
Joeatsumi 0:9e92f863fde5 153 }
Joeatsumi 0:9e92f863fde5 154
Joeatsumi 0:9e92f863fde5 155 float MG354PDH0::read_angular_rate_z(){
Joeatsumi 0:9e92f863fde5 156
Joeatsumi 0:9e92f863fde5 157 call_window0();
Joeatsumi 0:9e92f863fde5 158
Joeatsumi 0:9e92f863fde5 159 p_port->putc(0x1A);//register of gyro z (High)
Joeatsumi 0:9e92f863fde5 160 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 161 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 162
Joeatsumi 0:9e92f863fde5 163 while(p_port->getc()!=0x1A){
Joeatsumi 0:9e92f863fde5 164 }
Joeatsumi 0:9e92f863fde5 165 val_rate_z[0] = p_port->getc();
Joeatsumi 0:9e92f863fde5 166 val_rate_z[1] = p_port->getc();
Joeatsumi 0:9e92f863fde5 167
Joeatsumi 0:9e92f863fde5 168 p_port->putc(0x1C);//register of gyro z (Low)
Joeatsumi 0:9e92f863fde5 169 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 170 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 171
Joeatsumi 0:9e92f863fde5 172 while(p_port->getc()!=0x1C){
Joeatsumi 0:9e92f863fde5 173 }
Joeatsumi 0:9e92f863fde5 174 val_rate_z[2] = p_port->getc();
Joeatsumi 0:9e92f863fde5 175 val_rate_z[3] = p_port->getc();
Joeatsumi 0:9e92f863fde5 176
Joeatsumi 0:9e92f863fde5 177 gyro_z=(val_rate_z[0]<<24)+(val_rate_z[1]<<16)+(val_rate_z[2]<<8)+val_rate_z[3];
Joeatsumi 0:9e92f863fde5 178 //SCALE_FACTOR 0.016
Joeatsumi 0:9e92f863fde5 179 gyro_z*=(SCALE_FACTOR_GYRO/65536);
Joeatsumi 0:9e92f863fde5 180
Joeatsumi 0:9e92f863fde5 181 return gyro_z;
Joeatsumi 0:9e92f863fde5 182
Joeatsumi 0:9e92f863fde5 183 }
Joeatsumi 0:9e92f863fde5 184
Joeatsumi 0:9e92f863fde5 185
Joeatsumi 0:9e92f863fde5 186 float MG354PDH0::read_acceleration_x(){
Joeatsumi 0:9e92f863fde5 187
Joeatsumi 0:9e92f863fde5 188 call_window0();
Joeatsumi 0:9e92f863fde5 189
Joeatsumi 0:9e92f863fde5 190 p_port->putc(0x1E);//register of gyro z (High)
Joeatsumi 0:9e92f863fde5 191 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 192 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 193
Joeatsumi 0:9e92f863fde5 194 while(p_port->getc()!=0x1E){
Joeatsumi 0:9e92f863fde5 195 }
Joeatsumi 0:9e92f863fde5 196 val_acc_x[0] = p_port->getc();
Joeatsumi 0:9e92f863fde5 197 val_acc_x[1] = p_port->getc();
Joeatsumi 0:9e92f863fde5 198
Joeatsumi 0:9e92f863fde5 199 p_port->putc(0x20);//register of gyro z (Low)
Joeatsumi 0:9e92f863fde5 200 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 201 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 202
Joeatsumi 0:9e92f863fde5 203 while(p_port->getc()!=0x20){
Joeatsumi 0:9e92f863fde5 204 }
Joeatsumi 0:9e92f863fde5 205 val_acc_x[2] = p_port->getc();
Joeatsumi 0:9e92f863fde5 206 val_acc_x[3] = p_port->getc();
Joeatsumi 0:9e92f863fde5 207
Joeatsumi 0:9e92f863fde5 208 acceleration_x=(val_acc_x[0]<<24)+(val_acc_x[1]<<16)+(val_acc_x[2]<<8)+val_acc_x[3];
Joeatsumi 0:9e92f863fde5 209 //SCALE_FACTOR 0.016
Joeatsumi 0:9e92f863fde5 210 acceleration_x*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
Joeatsumi 0:9e92f863fde5 211
Joeatsumi 0:9e92f863fde5 212 return acceleration_x;
Joeatsumi 0:9e92f863fde5 213
Joeatsumi 0:9e92f863fde5 214 }
Joeatsumi 0:9e92f863fde5 215
Joeatsumi 0:9e92f863fde5 216 float MG354PDH0::read_acceleration_y(){
Joeatsumi 0:9e92f863fde5 217
Joeatsumi 0:9e92f863fde5 218 call_window0();
Joeatsumi 0:9e92f863fde5 219
Joeatsumi 0:9e92f863fde5 220 p_port->putc(0x22);//register of gyro z (High)
Joeatsumi 0:9e92f863fde5 221 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 222 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 223
Joeatsumi 0:9e92f863fde5 224 while(p_port->getc()!=0x22){
Joeatsumi 0:9e92f863fde5 225 }
Joeatsumi 0:9e92f863fde5 226 val_acc_y[0] = p_port->getc();
Joeatsumi 0:9e92f863fde5 227 val_acc_y[1] = p_port->getc();
Joeatsumi 0:9e92f863fde5 228
Joeatsumi 0:9e92f863fde5 229 p_port->putc(0x24);//register of gyro z (Low)
Joeatsumi 0:9e92f863fde5 230 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 231 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 232
Joeatsumi 0:9e92f863fde5 233 while(p_port->getc()!=0x24){
Joeatsumi 0:9e92f863fde5 234 }
Joeatsumi 0:9e92f863fde5 235 val_acc_y[2] = p_port->getc();
Joeatsumi 0:9e92f863fde5 236 val_acc_y[3] = p_port->getc();
Joeatsumi 0:9e92f863fde5 237
Joeatsumi 0:9e92f863fde5 238 acceleration_y=(val_acc_y[0]<<24)+(val_acc_y[1]<<16)+(val_acc_y[2]<<8)+val_acc_y[3];
Joeatsumi 0:9e92f863fde5 239 acceleration_y*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
Joeatsumi 0:9e92f863fde5 240
Joeatsumi 0:9e92f863fde5 241 return acceleration_y;
Joeatsumi 0:9e92f863fde5 242
Joeatsumi 0:9e92f863fde5 243 }
Joeatsumi 0:9e92f863fde5 244
Joeatsumi 0:9e92f863fde5 245 float MG354PDH0::read_acceleration_z(){
Joeatsumi 0:9e92f863fde5 246
Joeatsumi 0:9e92f863fde5 247 call_window0();
Joeatsumi 0:9e92f863fde5 248
Joeatsumi 0:9e92f863fde5 249 p_port->putc(0x26);//register of gyro z (High)
Joeatsumi 0:9e92f863fde5 250 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 251 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 252
Joeatsumi 0:9e92f863fde5 253 while(p_port->getc()!=0x26){
Joeatsumi 0:9e92f863fde5 254 }
Joeatsumi 0:9e92f863fde5 255 val_acc_z[0] = p_port->getc();
Joeatsumi 0:9e92f863fde5 256 val_acc_z[1] = p_port->getc();
Joeatsumi 0:9e92f863fde5 257
Joeatsumi 0:9e92f863fde5 258 p_port->putc(0x28);//register of gyro z (Low)
Joeatsumi 0:9e92f863fde5 259 p_port->putc(0x00);
Joeatsumi 0:9e92f863fde5 260 p_port->putc(0x0d);
Joeatsumi 0:9e92f863fde5 261
Joeatsumi 0:9e92f863fde5 262 while(p_port->getc()!=0x28){
Joeatsumi 0:9e92f863fde5 263 }
Joeatsumi 0:9e92f863fde5 264 val_acc_z[2] = p_port->getc();
Joeatsumi 0:9e92f863fde5 265 val_acc_z[3] = p_port->getc();
Joeatsumi 0:9e92f863fde5 266
Joeatsumi 0:9e92f863fde5 267 acceleration_z=(val_acc_z[0]<<24)+(val_acc_z[1]<<16)+(val_acc_z[2]<<8)+val_acc_z[3];
Joeatsumi 0:9e92f863fde5 268 acceleration_z*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
Joeatsumi 0:9e92f863fde5 269
Joeatsumi 0:9e92f863fde5 270 return acceleration_z;
Joeatsumi 0:9e92f863fde5 271
Joeatsumi 0:9e92f863fde5 272 }
Joeatsumi 0:9e92f863fde5 273
Joeatsumi 0:9e92f863fde5 274
Joeatsumi 0:9e92f863fde5 275
Joeatsumi 0:9e92f863fde5 276
Joeatsumi 0:9e92f863fde5 277
Joeatsumi 0:9e92f863fde5 278