Atsumi Toda / Mbed 2 deprecated M-G354PDH0_serial_echo_1

2021 ver of EPSON IMU

Dependencies:   mbed QMC5883L SDFileSystem

main.cpp@2:a9f80f95aff9, 2021-04-26 (annotated)

Committer:
Joeatsumi
Date:
Mon Apr 26 04:46:14 2021 +0000
Revision:
2:a9f80f95aff9
Parent:
1:bf98282e69a4 
EPOSON IMU program in2021

Who changed what in which revision?

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