Atsumi Toda / Mbed 2 deprecated M-G354PDH0_serial_echo_1

2021 ver of EPSON IMU

Dependencies:   mbed QMC5883L SDFileSystem

main.cpp@1:bf98282e69a4, 2019-08-31 (annotated)

Committer:
Joeatsumi
Date:
Sat Aug 31 05:26:12 2019 +0000
Revision:
1:bf98282e69a4
Parent:
0:3ba177faeee0
Child:
2:a9f80f95aff9
Tilt compensated geo magnetorometer.

Who changed what in which revision?

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