Atsumi Toda / Mbed 2 deprecated M-G354PDH0_serial_echo_1

2021 ver of EPSON IMU

Dependencies:   mbed QMC5883L SDFileSystem

main.cpp@0:3ba177faeee0, 2019-08-27 (annotated)

Committer:
Joeatsumi
Date:
Tue Aug 27 03:29:29 2019 +0000
Revision:
0:3ba177faeee0
Child:
1:bf98282e69a4
This program is for inertial measurement system and supposed to use EPSON G354 6axis sensor.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:3ba177faeee0 1 /*EPSONのセンサを試すために組んだ20190827。*/
Joeatsumi 0:3ba177faeee0 2
Joeatsumi 0:3ba177faeee0 3 #include "mbed.h"
Joeatsumi 0:3ba177faeee0 4 #include "string.h"
Joeatsumi 0:3ba177faeee0 5
Joeatsumi 0:3ba177faeee0 6 #include "QMC5883L.h"//地磁気センサのライブラリ
Joeatsumi 0:3ba177faeee0 7 #include "SDFileSystem.h"//sdカードを操作するためのライブラリ
Joeatsumi 0:3ba177faeee0 8
Joeatsumi 0:3ba177faeee0 9 #define SCALE_FACTOR_GYRO 0.016
Joeatsumi 0:3ba177faeee0 10 #define SCALE_FACTOR_ACC 0.20
Joeatsumi 0:3ba177faeee0 11 #define GRAVITIONAL_ACCELERATION 9.80665
Joeatsumi 0:3ba177faeee0 12
Joeatsumi 0:3ba177faeee0 13 Serial pc(USBTX, USBRX); // tx, rx
Joeatsumi 0:3ba177faeee0 14 QMC5883L compass(p9,p10);
Joeatsumi 0:3ba177faeee0 15 Serial device(p13, p14); // tx, rx
Joeatsumi 0:3ba177faeee0 16 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
Joeatsumi 0:3ba177faeee0 17 DigitalIn switch_blue(p21);//sdカードへの書き込みを終了させる
Joeatsumi 0:3ba177faeee0 18
Joeatsumi 0:3ba177faeee0 19 /*-----------------プロトタイプ宣言----------------*/
Joeatsumi 0:3ba177faeee0 20 void pc_rx ();
Joeatsumi 0:3ba177faeee0 21 void dev_rx ();
Joeatsumi 0:3ba177faeee0 22 void call_window1();
Joeatsumi 0:3ba177faeee0 23 void call_window0();
Joeatsumi 0:3ba177faeee0 24 void GLOB_CMD_read();
Joeatsumi 0:3ba177faeee0 25 void DIAG_STAT_read();
Joeatsumi 0:3ba177faeee0 26 void UART_CTRL_write();
Joeatsumi 0:3ba177faeee0 27 /*サンプリングモードへの移行*/
Joeatsumi 0:3ba177faeee0 28 void move_to_sampling_mode();
Joeatsumi 0:3ba177faeee0 29 /*IMUが動作可能かどうかの確認*/
Joeatsumi 0:3ba177faeee0 30 void power_on_sequence1();
Joeatsumi 0:3ba177faeee0 31 /*IMUが動作可能かどうかの確認*/
Joeatsumi 0:3ba177faeee0 32 void power_on_sequence2();
Joeatsumi 0:3ba177faeee0 33
Joeatsumi 0:3ba177faeee0 34 /*X軸周りの角速度を算出*/
Joeatsumi 0:3ba177faeee0 35 float read_angular_rate_x();
Joeatsumi 0:3ba177faeee0 36 /*Y軸周りの角速度を算出*/
Joeatsumi 0:3ba177faeee0 37 float read_angular_rate_y();
Joeatsumi 0:3ba177faeee0 38 /*Z軸周りの角速度を算出*/
Joeatsumi 0:3ba177faeee0 39 float read_angular_rate_z();
Joeatsumi 0:3ba177faeee0 40
Joeatsumi 0:3ba177faeee0 41 /*X軸の加速度を算出*/
Joeatsumi 0:3ba177faeee0 42 float read_acceleration_x();
Joeatsumi 0:3ba177faeee0 43 /*Y軸の加速度を算出*/
Joeatsumi 0:3ba177faeee0 44 float read_acceleration_y();
Joeatsumi 0:3ba177faeee0 45 /*Z軸の加速度を算出*/
Joeatsumi 0:3ba177faeee0 46 float read_acceleration_z();
Joeatsumi 0:3ba177faeee0 47 /*-----------------------------------------------*/
Joeatsumi 0:3ba177faeee0 48 /*-------------タイマー関数---------------*/
Joeatsumi 0:3ba177faeee0 49 Timer passed_time;
Joeatsumi 0:3ba177faeee0 50 /*---------------------------------------*/
Joeatsumi 0:3ba177faeee0 51 /*-------------------各種の変数-----------------*/
Joeatsumi 0:3ba177faeee0 52 float gyro_val[3];//角速度の値
Joeatsumi 0:3ba177faeee0 53 float angle_val[3];//角度の値
Joeatsumi 0:3ba177faeee0 54
Joeatsumi 0:3ba177faeee0 55 float acc_val[3];//加速度の値
Joeatsumi 0:3ba177faeee0 56 float acc_angle[2];//加速度から計算した角度の値
Joeatsumi 0:3ba177faeee0 57
Joeatsumi 0:3ba177faeee0 58 int16_t mag_val[3];//地磁気の値
Joeatsumi 0:3ba177faeee0 59
Joeatsumi 0:3ba177faeee0 60 float time_new,time_old;
Joeatsumi 0:3ba177faeee0 61 /*---------------------------------------------*/
Joeatsumi 0:3ba177faeee0 62
Joeatsumi 0:3ba177faeee0 63
Joeatsumi 0:3ba177faeee0 64 int main() {
Joeatsumi 0:3ba177faeee0 65
Joeatsumi 0:3ba177faeee0 66 pc.baud(460800);
Joeatsumi 0:3ba177faeee0 67 device.baud(460800);
Joeatsumi 0:3ba177faeee0 68
Joeatsumi 0:3ba177faeee0 69 // pc.attach(pc_rx, Serial::RxIrq);
Joeatsumi 0:3ba177faeee0 70 //device.attach(dev_rx, Serial::RxIrq);
Joeatsumi 0:3ba177faeee0 71
Joeatsumi 0:3ba177faeee0 72 /*
Joeatsumi 0:3ba177faeee0 73 power on
Joeatsumi 0:3ba177faeee0 74 wait 800ms
Joeatsumi 0:3ba177faeee0 75 */
Joeatsumi 0:3ba177faeee0 76 wait(1.0);
Joeatsumi 0:3ba177faeee0 77
Joeatsumi 0:3ba177faeee0 78 compass.init();//地磁気センサの起動
Joeatsumi 0:3ba177faeee0 79
Joeatsumi 0:3ba177faeee0 80 power_on_sequence1();//IMUが動作可能かどうかの確認
Joeatsumi 0:3ba177faeee0 81 power_on_sequence2();//IMUが動作可能かどうかの確認
Joeatsumi 0:3ba177faeee0 82 UART_CTRL_write();//IMUのボーレートを480600,手動モードへ移行
Joeatsumi 0:3ba177faeee0 83 move_to_sampling_mode();//サンプリングモードへの移行
Joeatsumi 0:3ba177faeee0 84
Joeatsumi 0:3ba177faeee0 85 passed_time.start();
Joeatsumi 0:3ba177faeee0 86 time_old=passed_time.read();
Joeatsumi 0:3ba177faeee0 87
Joeatsumi 0:3ba177faeee0 88 /*ジャイロで計算する初期角度の設定*/
Joeatsumi 0:3ba177faeee0 89 angle_val[0]=0.0;
Joeatsumi 0:3ba177faeee0 90 angle_val[1]=0.0;
Joeatsumi 0:3ba177faeee0 91 angle_val[2]=0.0;
Joeatsumi 0:3ba177faeee0 92 /*----------------------------*/
Joeatsumi 0:3ba177faeee0 93
Joeatsumi 0:3ba177faeee0 94 /*sdカードにファイルを構成する そのファイルを書き込み状態にする*/
Joeatsumi 0:3ba177faeee0 95 mkdir("/sd/IMU_logger", 0777);
Joeatsumi 0:3ba177faeee0 96
Joeatsumi 0:3ba177faeee0 97 FILE *fp = fopen("/sd/IMU_logger/log.csv", "w");
Joeatsumi 0:3ba177faeee0 98
Joeatsumi 0:3ba177faeee0 99 if(fp == NULL) {
Joeatsumi 0:3ba177faeee0 100 error("Could not open file for write\n");
Joeatsumi 0:3ba177faeee0 101 }
Joeatsumi 0:3ba177faeee0 102 /*-------------------------------------------------------*/
Joeatsumi 0:3ba177faeee0 103
Joeatsumi 0:3ba177faeee0 104 pc.printf("Writing start!\r\n");
Joeatsumi 0:3ba177faeee0 105
Joeatsumi 0:3ba177faeee0 106 while(1){
Joeatsumi 0:3ba177faeee0 107
Joeatsumi 0:3ba177faeee0 108 gyro_val[0]=read_angular_rate_x();//X軸周りの角速度の算出
Joeatsumi 0:3ba177faeee0 109 gyro_val[1]=read_angular_rate_y();//Y軸周りの角速度の算出
Joeatsumi 0:3ba177faeee0 110 gyro_val[2]=read_angular_rate_z();//Z軸周りの角速度の算出
Joeatsumi 0:3ba177faeee0 111
Joeatsumi 0:3ba177faeee0 112 acc_val[0]=read_acceleration_x();//X軸の加速度の算出
Joeatsumi 0:3ba177faeee0 113 acc_val[1]=read_acceleration_y();//Y軸の加速度の算出
Joeatsumi 0:3ba177faeee0 114 acc_val[2]=read_acceleration_z();//Z軸の加速度の算出
Joeatsumi 0:3ba177faeee0 115
Joeatsumi 0:3ba177faeee0 116 mag_val[0]=compass.getMagXvalue();//X軸の地磁気の算出
Joeatsumi 0:3ba177faeee0 117 mag_val[1]=compass.getMagYvalue();//Y軸の地磁気の算出
Joeatsumi 0:3ba177faeee0 118 mag_val[2]=compass.getMagZvalue();//Z軸の地磁気の算出
Joeatsumi 0:3ba177faeee0 119
Joeatsumi 0:3ba177faeee0 120 time_new=passed_time.read();//時間更新
Joeatsumi 0:3ba177faeee0 121
Joeatsumi 0:3ba177faeee0 122 angle_val[0]+=(time_new-time_old)*gyro_val[0];//X軸周りの角度の算出
Joeatsumi 0:3ba177faeee0 123 angle_val[1]+=(time_new-time_old)*gyro_val[1];//Y軸周りの角度の算出
Joeatsumi 0:3ba177faeee0 124 angle_val[2]+=(time_new-time_old)*gyro_val[2];//Z軸周りの角度の算出
Joeatsumi 0:3ba177faeee0 125
Joeatsumi 0:3ba177faeee0 126 //pc.printf("%f:%f:%f\r\n",gyro_val[0],gyro_val[1],gyro_val[2]);
Joeatsumi 0:3ba177faeee0 127 //pc.printf("%f:%f:%f\r\n",angle_val[0],angle_val[1],angle_val[2]);
Joeatsumi 0:3ba177faeee0 128 //pc.printf("%f:%f:%f\r\n",acc_val[0],acc_val[1],acc_val[2]);
Joeatsumi 0:3ba177faeee0 129
Joeatsumi 0:3ba177faeee0 130 fprintf(fp,"%f,%f,%f,%f\r\n",time_new,angle_val[0],angle_val[1],angle_val[2]);//sdファイルに書き込み
Joeatsumi 0:3ba177faeee0 131
Joeatsumi 0:3ba177faeee0 132 time_old=time_new;
Joeatsumi 0:3ba177faeee0 133
Joeatsumi 0:3ba177faeee0 134 if(switch_blue==1){//青いタクトスイッチが押されている事が確認されたら
Joeatsumi 0:3ba177faeee0 135
Joeatsumi 0:3ba177faeee0 136 fclose(fp);//sdカードに作ったファイルを閉じる(書き込みをやめる)
Joeatsumi 0:3ba177faeee0 137 pc.printf("Writing finish!");
Joeatsumi 0:3ba177faeee0 138 break;//while文から離脱
Joeatsumi 0:3ba177faeee0 139
Joeatsumi 0:3ba177faeee0 140 }else{}
Joeatsumi 0:3ba177faeee0 141
Joeatsumi 0:3ba177faeee0 142 wait(0.01);
Joeatsumi 0:3ba177faeee0 143
Joeatsumi 0:3ba177faeee0 144 }//while 終わり
Joeatsumi 0:3ba177faeee0 145 }//main終わり
Joeatsumi 0:3ba177faeee0 146
Joeatsumi 0:3ba177faeee0 147 /*------------------関数の宣言------------------*/
Joeatsumi 0:3ba177faeee0 148 void pc_rx () {
Joeatsumi 0:3ba177faeee0 149 device.putc(pc.getc());
Joeatsumi 0:3ba177faeee0 150 }
Joeatsumi 0:3ba177faeee0 151
Joeatsumi 0:3ba177faeee0 152 void dev_rx () {
Joeatsumi 0:3ba177faeee0 153 pc.putc(device.getc());
Joeatsumi 0:3ba177faeee0 154 }
Joeatsumi 0:3ba177faeee0 155
Joeatsumi 0:3ba177faeee0 156 void call_window1(){
Joeatsumi 0:3ba177faeee0 157 device.putc(0xFE);
Joeatsumi 0:3ba177faeee0 158 device.putc(0x01);
Joeatsumi 0:3ba177faeee0 159 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 160 }
Joeatsumi 0:3ba177faeee0 161
Joeatsumi 0:3ba177faeee0 162 void call_window0(){
Joeatsumi 0:3ba177faeee0 163 device.putc(0xFE);
Joeatsumi 0:3ba177faeee0 164 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 165 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 166 }
Joeatsumi 0:3ba177faeee0 167
Joeatsumi 0:3ba177faeee0 168 void GLOB_CMD_read(){//IMUが使えるかどうかの確認
Joeatsumi 0:3ba177faeee0 169
Joeatsumi 0:3ba177faeee0 170 char val[2];
Joeatsumi 0:3ba177faeee0 171 short i;
Joeatsumi 0:3ba177faeee0 172
Joeatsumi 0:3ba177faeee0 173 device.putc(0x0A);
Joeatsumi 0:3ba177faeee0 174 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 175 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 176
Joeatsumi 0:3ba177faeee0 177 while(device.getc()!=0x0A){
Joeatsumi 0:3ba177faeee0 178 }
Joeatsumi 0:3ba177faeee0 179 val[0] = device.getc();
Joeatsumi 0:3ba177faeee0 180 val[1] = device.getc();
Joeatsumi 0:3ba177faeee0 181
Joeatsumi 0:3ba177faeee0 182 i=(val[0]<<8)+val[1];
Joeatsumi 0:3ba177faeee0 183 if(i==0){
Joeatsumi 0:3ba177faeee0 184 pc.printf("IMU is ready.1\r\n");
Joeatsumi 0:3ba177faeee0 185 }else{
Joeatsumi 0:3ba177faeee0 186 pc.printf("IMU is not ready.1\r\n");
Joeatsumi 0:3ba177faeee0 187 }
Joeatsumi 0:3ba177faeee0 188
Joeatsumi 0:3ba177faeee0 189 }
Joeatsumi 0:3ba177faeee0 190
Joeatsumi 0:3ba177faeee0 191 void DIAG_STAT_read(){
Joeatsumi 0:3ba177faeee0 192
Joeatsumi 0:3ba177faeee0 193 char val[2];
Joeatsumi 0:3ba177faeee0 194 short i;
Joeatsumi 0:3ba177faeee0 195
Joeatsumi 0:3ba177faeee0 196 device.putc(0x04);
Joeatsumi 0:3ba177faeee0 197 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 198 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 199
Joeatsumi 0:3ba177faeee0 200
Joeatsumi 0:3ba177faeee0 201 while(device.getc()!=0x04){
Joeatsumi 0:3ba177faeee0 202 }
Joeatsumi 0:3ba177faeee0 203 val[0] = device.getc();
Joeatsumi 0:3ba177faeee0 204 val[1] = device.getc();
Joeatsumi 0:3ba177faeee0 205
Joeatsumi 0:3ba177faeee0 206 i=(val[0]<<8)+val[1];
Joeatsumi 0:3ba177faeee0 207 if(i==0){
Joeatsumi 0:3ba177faeee0 208 pc.printf("IMU is ready.2\r\n");
Joeatsumi 0:3ba177faeee0 209 }else{
Joeatsumi 0:3ba177faeee0 210 pc.printf("IMU is not ready.2\r\n");
Joeatsumi 0:3ba177faeee0 211 }
Joeatsumi 0:3ba177faeee0 212 }
Joeatsumi 0:3ba177faeee0 213
Joeatsumi 0:3ba177faeee0 214 void UART_CTRL_write(){//IMUのボーレートを480600,手動モードへ移行
Joeatsumi 0:3ba177faeee0 215
Joeatsumi 0:3ba177faeee0 216 call_window1();
Joeatsumi 0:3ba177faeee0 217
Joeatsumi 0:3ba177faeee0 218 device.putc(0x09);
Joeatsumi 0:3ba177faeee0 219 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 220 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 221
Joeatsumi 0:3ba177faeee0 222 device.putc(0x08);
Joeatsumi 0:3ba177faeee0 223 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 224 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 225
Joeatsumi 0:3ba177faeee0 226 }
Joeatsumi 0:3ba177faeee0 227
Joeatsumi 0:3ba177faeee0 228 /*IMUが動作可能かどうかの確認*/
Joeatsumi 0:3ba177faeee0 229 void power_on_sequence1(){
Joeatsumi 0:3ba177faeee0 230 call_window1();
Joeatsumi 0:3ba177faeee0 231 GLOB_CMD_read();
Joeatsumi 0:3ba177faeee0 232 }
Joeatsumi 0:3ba177faeee0 233
Joeatsumi 0:3ba177faeee0 234 /*IMUが動作可能かどうかの確認*/
Joeatsumi 0:3ba177faeee0 235 void power_on_sequence2(){
Joeatsumi 0:3ba177faeee0 236 call_window0();
Joeatsumi 0:3ba177faeee0 237 DIAG_STAT_read();
Joeatsumi 0:3ba177faeee0 238 }
Joeatsumi 0:3ba177faeee0 239
Joeatsumi 0:3ba177faeee0 240 void move_to_sampling_mode(){
Joeatsumi 0:3ba177faeee0 241 call_window0();
Joeatsumi 0:3ba177faeee0 242
Joeatsumi 0:3ba177faeee0 243 device.putc(0x83);
Joeatsumi 0:3ba177faeee0 244 device.putc(0x01);
Joeatsumi 0:3ba177faeee0 245 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 246 }
Joeatsumi 0:3ba177faeee0 247
Joeatsumi 0:3ba177faeee0 248 float read_angular_rate_x(){
Joeatsumi 0:3ba177faeee0 249
Joeatsumi 0:3ba177faeee0 250 char val[4];
Joeatsumi 0:3ba177faeee0 251 float gyro_x;
Joeatsumi 0:3ba177faeee0 252
Joeatsumi 0:3ba177faeee0 253 call_window0();
Joeatsumi 0:3ba177faeee0 254
Joeatsumi 0:3ba177faeee0 255 device.putc(0x12);//register of gyro x (High)
Joeatsumi 0:3ba177faeee0 256 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 257 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 258
Joeatsumi 0:3ba177faeee0 259 while(device.getc()!=0x12){
Joeatsumi 0:3ba177faeee0 260 }
Joeatsumi 0:3ba177faeee0 261 val[0] = device.getc();
Joeatsumi 0:3ba177faeee0 262 val[1] = device.getc();
Joeatsumi 0:3ba177faeee0 263
Joeatsumi 0:3ba177faeee0 264 device.putc(0x14);//register of gyro x (Low)
Joeatsumi 0:3ba177faeee0 265 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 266 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 267
Joeatsumi 0:3ba177faeee0 268 while(device.getc()!=0x14){
Joeatsumi 0:3ba177faeee0 269 }
Joeatsumi 0:3ba177faeee0 270 val[2] = device.getc();
Joeatsumi 0:3ba177faeee0 271 val[3] = device.getc();
Joeatsumi 0:3ba177faeee0 272
Joeatsumi 0:3ba177faeee0 273 gyro_x=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
Joeatsumi 0:3ba177faeee0 274 //SCALE_FACTOR 0.016
Joeatsumi 0:3ba177faeee0 275 gyro_x*=(SCALE_FACTOR_GYRO/65536);
Joeatsumi 0:3ba177faeee0 276
Joeatsumi 0:3ba177faeee0 277 return gyro_x;
Joeatsumi 0:3ba177faeee0 278
Joeatsumi 0:3ba177faeee0 279 }
Joeatsumi 0:3ba177faeee0 280
Joeatsumi 0:3ba177faeee0 281 float read_angular_rate_y(){
Joeatsumi 0:3ba177faeee0 282
Joeatsumi 0:3ba177faeee0 283 char val[4];
Joeatsumi 0:3ba177faeee0 284 float gyro_y;
Joeatsumi 0:3ba177faeee0 285
Joeatsumi 0:3ba177faeee0 286 call_window0();
Joeatsumi 0:3ba177faeee0 287
Joeatsumi 0:3ba177faeee0 288 device.putc(0x16);//register of gyro y (High)
Joeatsumi 0:3ba177faeee0 289 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 290 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 291
Joeatsumi 0:3ba177faeee0 292 while(device.getc()!=0x16){
Joeatsumi 0:3ba177faeee0 293 }
Joeatsumi 0:3ba177faeee0 294 val[0] = device.getc();
Joeatsumi 0:3ba177faeee0 295 val[1] = device.getc();
Joeatsumi 0:3ba177faeee0 296
Joeatsumi 0:3ba177faeee0 297 device.putc(0x18);//register of gyro y (Low)
Joeatsumi 0:3ba177faeee0 298 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 299 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 300
Joeatsumi 0:3ba177faeee0 301 while(device.getc()!=0x18){
Joeatsumi 0:3ba177faeee0 302 }
Joeatsumi 0:3ba177faeee0 303 val[2] = device.getc();
Joeatsumi 0:3ba177faeee0 304 val[3] = device.getc();
Joeatsumi 0:3ba177faeee0 305
Joeatsumi 0:3ba177faeee0 306 gyro_y=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
Joeatsumi 0:3ba177faeee0 307 //SCALE_FACTOR 0.016
Joeatsumi 0:3ba177faeee0 308 gyro_y*=(SCALE_FACTOR_GYRO/65536);
Joeatsumi 0:3ba177faeee0 309
Joeatsumi 0:3ba177faeee0 310 return gyro_y;
Joeatsumi 0:3ba177faeee0 311
Joeatsumi 0:3ba177faeee0 312 }
Joeatsumi 0:3ba177faeee0 313
Joeatsumi 0:3ba177faeee0 314 float read_angular_rate_z(){
Joeatsumi 0:3ba177faeee0 315
Joeatsumi 0:3ba177faeee0 316 char val[4];
Joeatsumi 0:3ba177faeee0 317 float gyro_z;
Joeatsumi 0:3ba177faeee0 318
Joeatsumi 0:3ba177faeee0 319 call_window0();
Joeatsumi 0:3ba177faeee0 320
Joeatsumi 0:3ba177faeee0 321 device.putc(0x1A);//register of gyro z (High)
Joeatsumi 0:3ba177faeee0 322 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 323 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 324
Joeatsumi 0:3ba177faeee0 325 while(device.getc()!=0x1A){
Joeatsumi 0:3ba177faeee0 326 }
Joeatsumi 0:3ba177faeee0 327 val[0] = device.getc();
Joeatsumi 0:3ba177faeee0 328 val[1] = device.getc();
Joeatsumi 0:3ba177faeee0 329
Joeatsumi 0:3ba177faeee0 330 device.putc(0x1C);//register of gyro z (Low)
Joeatsumi 0:3ba177faeee0 331 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 332 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 333
Joeatsumi 0:3ba177faeee0 334 while(device.getc()!=0x1C){
Joeatsumi 0:3ba177faeee0 335 }
Joeatsumi 0:3ba177faeee0 336 val[2] = device.getc();
Joeatsumi 0:3ba177faeee0 337 val[3] = device.getc();
Joeatsumi 0:3ba177faeee0 338
Joeatsumi 0:3ba177faeee0 339 gyro_z=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
Joeatsumi 0:3ba177faeee0 340 //SCALE_FACTOR 0.016
Joeatsumi 0:3ba177faeee0 341 gyro_z*=(SCALE_FACTOR_GYRO/65536);
Joeatsumi 0:3ba177faeee0 342
Joeatsumi 0:3ba177faeee0 343 return gyro_z;
Joeatsumi 0:3ba177faeee0 344
Joeatsumi 0:3ba177faeee0 345 }
Joeatsumi 0:3ba177faeee0 346
Joeatsumi 0:3ba177faeee0 347 float read_acceleration_x(){
Joeatsumi 0:3ba177faeee0 348
Joeatsumi 0:3ba177faeee0 349 char val[4];
Joeatsumi 0:3ba177faeee0 350 float acceleration_x;
Joeatsumi 0:3ba177faeee0 351
Joeatsumi 0:3ba177faeee0 352 call_window0();
Joeatsumi 0:3ba177faeee0 353
Joeatsumi 0:3ba177faeee0 354 device.putc(0x1E);//register of gyro z (High)
Joeatsumi 0:3ba177faeee0 355 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 356 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 357
Joeatsumi 0:3ba177faeee0 358 while(device.getc()!=0x1E){
Joeatsumi 0:3ba177faeee0 359 }
Joeatsumi 0:3ba177faeee0 360 val[0] = device.getc();
Joeatsumi 0:3ba177faeee0 361 val[1] = device.getc();
Joeatsumi 0:3ba177faeee0 362
Joeatsumi 0:3ba177faeee0 363 device.putc(0x20);//register of gyro z (Low)
Joeatsumi 0:3ba177faeee0 364 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 365 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 366
Joeatsumi 0:3ba177faeee0 367 while(device.getc()!=0x20){
Joeatsumi 0:3ba177faeee0 368 }
Joeatsumi 0:3ba177faeee0 369 val[2] = device.getc();
Joeatsumi 0:3ba177faeee0 370 val[3] = device.getc();
Joeatsumi 0:3ba177faeee0 371
Joeatsumi 0:3ba177faeee0 372 acceleration_x=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
Joeatsumi 0:3ba177faeee0 373 //SCALE_FACTOR 0.016
Joeatsumi 0:3ba177faeee0 374 acceleration_x*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
Joeatsumi 0:3ba177faeee0 375
Joeatsumi 0:3ba177faeee0 376 return acceleration_x;
Joeatsumi 0:3ba177faeee0 377
Joeatsumi 0:3ba177faeee0 378 }
Joeatsumi 0:3ba177faeee0 379
Joeatsumi 0:3ba177faeee0 380 float read_acceleration_y(){
Joeatsumi 0:3ba177faeee0 381
Joeatsumi 0:3ba177faeee0 382 char val[4];
Joeatsumi 0:3ba177faeee0 383 float acceleration_y;
Joeatsumi 0:3ba177faeee0 384
Joeatsumi 0:3ba177faeee0 385 call_window0();
Joeatsumi 0:3ba177faeee0 386
Joeatsumi 0:3ba177faeee0 387 device.putc(0x22);//register of gyro z (High)
Joeatsumi 0:3ba177faeee0 388 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 389 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 390
Joeatsumi 0:3ba177faeee0 391 while(device.getc()!=0x22){
Joeatsumi 0:3ba177faeee0 392 }
Joeatsumi 0:3ba177faeee0 393 val[0] = device.getc();
Joeatsumi 0:3ba177faeee0 394 val[1] = device.getc();
Joeatsumi 0:3ba177faeee0 395
Joeatsumi 0:3ba177faeee0 396 device.putc(0x24);//register of gyro z (Low)
Joeatsumi 0:3ba177faeee0 397 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 398 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 399
Joeatsumi 0:3ba177faeee0 400 while(device.getc()!=0x24){
Joeatsumi 0:3ba177faeee0 401 }
Joeatsumi 0:3ba177faeee0 402 val[2] = device.getc();
Joeatsumi 0:3ba177faeee0 403 val[3] = device.getc();
Joeatsumi 0:3ba177faeee0 404
Joeatsumi 0:3ba177faeee0 405 acceleration_y=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
Joeatsumi 0:3ba177faeee0 406 acceleration_y*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
Joeatsumi 0:3ba177faeee0 407
Joeatsumi 0:3ba177faeee0 408 return acceleration_y;
Joeatsumi 0:3ba177faeee0 409
Joeatsumi 0:3ba177faeee0 410 }
Joeatsumi 0:3ba177faeee0 411
Joeatsumi 0:3ba177faeee0 412 float read_acceleration_z(){
Joeatsumi 0:3ba177faeee0 413
Joeatsumi 0:3ba177faeee0 414 char val[4];
Joeatsumi 0:3ba177faeee0 415 float acceleration_z;
Joeatsumi 0:3ba177faeee0 416
Joeatsumi 0:3ba177faeee0 417 call_window0();
Joeatsumi 0:3ba177faeee0 418
Joeatsumi 0:3ba177faeee0 419 device.putc(0x26);//register of gyro z (High)
Joeatsumi 0:3ba177faeee0 420 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 421 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 422
Joeatsumi 0:3ba177faeee0 423 while(device.getc()!=0x26){
Joeatsumi 0:3ba177faeee0 424 }
Joeatsumi 0:3ba177faeee0 425 val[0] = device.getc();
Joeatsumi 0:3ba177faeee0 426 val[1] = device.getc();
Joeatsumi 0:3ba177faeee0 427
Joeatsumi 0:3ba177faeee0 428 device.putc(0x28);//register of gyro z (Low)
Joeatsumi 0:3ba177faeee0 429 device.putc(0x00);
Joeatsumi 0:3ba177faeee0 430 device.putc(0x0d);
Joeatsumi 0:3ba177faeee0 431
Joeatsumi 0:3ba177faeee0 432 while(device.getc()!=0x28){
Joeatsumi 0:3ba177faeee0 433 }
Joeatsumi 0:3ba177faeee0 434 val[2] = device.getc();
Joeatsumi 0:3ba177faeee0 435 val[3] = device.getc();
Joeatsumi 0:3ba177faeee0 436
Joeatsumi 0:3ba177faeee0 437 acceleration_z=(val[0]<<24)+(val[1]<<16)+(val[2]<<8)+val[3];
Joeatsumi 0:3ba177faeee0 438 acceleration_z*=((SCALE_FACTOR_ACC/65536)* (GRAVITIONAL_ACCELERATION/1000.0));
Joeatsumi 0:3ba177faeee0 439
Joeatsumi 0:3ba177faeee0 440 return acceleration_z;
Joeatsumi 0:3ba177faeee0 441
Joeatsumi 0:3ba177faeee0 442 }
Joeatsumi 0:3ba177faeee0 443
Joeatsumi 0:3ba177faeee0 444 /*-------------------------------------------*/