これは MMA7361 3軸アナログ加速度センサ の Nucleo F401RE 用のライブラリです。詳しい説明はLibrary中のReadMe.hに記載しています。文字化けや開けない場合はダウンロードしてみてください。Google Chrome では見れるはずです。
Dependents: Nucleo_L3GD20_MMA7361_Kalman
mma7361.cpp@5:83a3021f51f8, 2016-09-30 (annotated)
- Committer:
- hirokimineshita
- Date:
- Fri Sep 30 04:44:48 2016 +0000
- Revision:
- 5:83a3021f51f8
- Parent:
- 3:0359af763b6c
160930 add doxygen
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hirokimineshita | 0:ad6f3a862ed5 | 1 | #include "mbed.h" |
hirokimineshita | 0:ad6f3a862ed5 | 2 | #include "mma7361.h" |
hirokimineshita | 0:ad6f3a862ed5 | 3 | |
hirokimineshita | 0:ad6f3a862ed5 | 4 | /*---------------------------------description-----------------------------------------*/ |
hirokimineshita | 0:ad6f3a862ed5 | 5 | |
hirokimineshita | 0:ad6f3a862ed5 | 6 | // range=0 : range is 1.5G |
hirokimineshita | 0:ad6f3a862ed5 | 7 | // range=1 : range is 6.0G |
hirokimineshita | 0:ad6f3a862ed5 | 8 | |
hirokimineshita | 0:ad6f3a862ed5 | 9 | /*-------------------------------about mat[3][3]---------------------------------------*/ |
hirokimineshita | 0:ad6f3a862ed5 | 10 | |
hirokimineshita | 0:ad6f3a862ed5 | 11 | /* |
hirokimineshita | 0:ad6f3a862ed5 | 12 | | mat[0][0] mat[0][1] mat[0][2] | | A00 A10 A20 | : middle bits (=0G) of each axes when range is 1.5G |
hirokimineshita | 0:ad6f3a862ed5 | 13 | | mat[1][0] mat[1][1] mat[1][2] | = | A01 A11 A21 | : middle bits (=0G) of each axes when range is 6.0G |
hirokimineshita | 0:ad6f3a862ed5 | 14 | | mat[2][0] mat[2][1] mat[2][2] | | G00 G10 G20 | : bits/G of each axes when range is 1.5G |
hirokimineshita | 0:ad6f3a862ed5 | 15 | | mat[3][0] mat[3][1] mat[3][2] | | G01 G11 G21 | : bits/G of each axes when range is 6.0G |
hirokimineshita | 0:ad6f3a862ed5 | 16 | |
hirokimineshita | 0:ad6f3a862ed5 | 17 | You mast change this value in "mma7361.h". You can see this value with function : calibration() & show_mat() |
hirokimineshita | 0:ad6f3a862ed5 | 18 | */ |
hirokimineshita | 0:ad6f3a862ed5 | 19 | |
hirokimineshita | 0:ad6f3a862ed5 | 20 | /*---------------------------------description-----------------------------------------*/ |
hirokimineshita | 0:ad6f3a862ed5 | 21 | /*---------------------------------constracta------------------------------------------*/ |
hirokimineshita | 0:ad6f3a862ed5 | 22 | |
hirokimineshita | 0:ad6f3a862ed5 | 23 | mma7361::mma7361(PinName accelx,PinName accely,PinName accelz,PinName set,PinName set_range): dx(accelx),dy(accely),dz(accelz),ST(set),GS(set_range),pcdev(USBTX,USBRX) |
hirokimineshita | 0:ad6f3a862ed5 | 24 | { |
hirokimineshita | 0:ad6f3a862ed5 | 25 | mma7361::ST=0; |
hirokimineshita | 0:ad6f3a862ed5 | 26 | mma7361::GS=0; |
hirokimineshita | 0:ad6f3a862ed5 | 27 | mma7361::range=0; |
hirokimineshita | 0:ad6f3a862ed5 | 28 | mma7361::clear_mat(); |
hirokimineshita | 0:ad6f3a862ed5 | 29 | } |
hirokimineshita | 0:ad6f3a862ed5 | 30 | |
hirokimineshita | 0:ad6f3a862ed5 | 31 | /*---------------------------------constracta------------------------------------------*/ |
hirokimineshita | 0:ad6f3a862ed5 | 32 | /*-----------------------------------member--------------------------------------------*/ |
hirokimineshita | 0:ad6f3a862ed5 | 33 | |
hirokimineshita | 0:ad6f3a862ed5 | 34 | void mma7361::calibration(){ |
hirokimineshita | 0:ad6f3a862ed5 | 35 | mma7361::GS=0; |
hirokimineshita | 0:ad6f3a862ed5 | 36 | mma7361::range=0; |
hirokimineshita | 0:ad6f3a862ed5 | 37 | mma7361::ST=1; |
hirokimineshita | 0:ad6f3a862ed5 | 38 | wait_ms(250); |
hirokimineshita | 0:ad6f3a862ed5 | 39 | mma7361::ST=0; |
hirokimineshita | 0:ad6f3a862ed5 | 40 | |
hirokimineshita | 0:ad6f3a862ed5 | 41 | mma7361::pcdev.printf("Prease set devise with z-axis is going up\n\r"); |
hirokimineshita | 0:ad6f3a862ed5 | 42 | mma7361::pcdev.printf("Press Enter key\n\r"); |
hirokimineshita | 0:ad6f3a862ed5 | 43 | while(getchar()==-35); |
hirokimineshita | 0:ad6f3a862ed5 | 44 | int ax_0_0 = mma7361::read_bit(x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 45 | int ay_0_0 = mma7361::read_bit(y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 46 | int gz_0_0 = mma7361::read_bit(z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 47 | for(int j=0; j<100; j++){ |
hirokimineshita | 0:ad6f3a862ed5 | 48 | mma7361::low_pass_filter(&ax_0_0,x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 49 | mma7361::low_pass_filter(&ay_0_0,y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 50 | mma7361::low_pass_filter(&gz_0_0,z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 51 | } |
hirokimineshita | 0:ad6f3a862ed5 | 52 | mma7361::GS=1; |
hirokimineshita | 0:ad6f3a862ed5 | 53 | wait_ms(250); |
hirokimineshita | 0:ad6f3a862ed5 | 54 | int ax_1_0 = mma7361::read_bit(x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 55 | int ay_1_0 = mma7361::read_bit(y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 56 | int gz_1_0 = mma7361::read_bit(z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 57 | for(int j=0; j<100; j++){ |
hirokimineshita | 0:ad6f3a862ed5 | 58 | mma7361::low_pass_filter(&ax_1_0,x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 59 | mma7361::low_pass_filter(&ay_1_0,y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 60 | mma7361::low_pass_filter(&gz_1_0,z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 61 | } |
hirokimineshita | 0:ad6f3a862ed5 | 62 | mma7361::GS=0; |
hirokimineshita | 0:ad6f3a862ed5 | 63 | wait_ms(250); |
hirokimineshita | 0:ad6f3a862ed5 | 64 | |
hirokimineshita | 0:ad6f3a862ed5 | 65 | mma7361::pcdev.printf("Prease set devise with x-axis is going up\n\r"); |
hirokimineshita | 0:ad6f3a862ed5 | 66 | mma7361::pcdev.printf("Press Enter key\n\r"); |
hirokimineshita | 0:ad6f3a862ed5 | 67 | while(getchar()==-35); |
hirokimineshita | 0:ad6f3a862ed5 | 68 | int ay_0_1 = mma7361::read_bit(y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 69 | int az_0_0 = mma7361::read_bit(z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 70 | int gx_0_0 = mma7361::read_bit(x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 71 | for(int j=0; j<100; j++){ |
hirokimineshita | 0:ad6f3a862ed5 | 72 | mma7361::low_pass_filter(&ay_0_1,y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 73 | mma7361::low_pass_filter(&az_0_0,z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 74 | mma7361::low_pass_filter(&gx_0_0,x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 75 | } |
hirokimineshita | 0:ad6f3a862ed5 | 76 | mma7361::GS=1; |
hirokimineshita | 0:ad6f3a862ed5 | 77 | wait_ms(250); |
hirokimineshita | 0:ad6f3a862ed5 | 78 | int ay_1_1 = mma7361::read_bit(y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 79 | int az_1_0 = mma7361::read_bit(z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 80 | int gx_1_0 = mma7361::read_bit(x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 81 | for(int j=0; j<100; j++){ |
hirokimineshita | 0:ad6f3a862ed5 | 82 | mma7361::low_pass_filter(&ay_1_1,y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 83 | mma7361::low_pass_filter(&az_1_0,z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 84 | mma7361::low_pass_filter(&gx_1_0,x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 85 | } |
hirokimineshita | 0:ad6f3a862ed5 | 86 | mma7361::GS=0; |
hirokimineshita | 0:ad6f3a862ed5 | 87 | wait_ms(250); |
hirokimineshita | 0:ad6f3a862ed5 | 88 | |
hirokimineshita | 0:ad6f3a862ed5 | 89 | mma7361::pcdev.printf("Prease set devise with y-axis is going up\n\r"); |
hirokimineshita | 0:ad6f3a862ed5 | 90 | mma7361::pcdev.printf("Press Enter key\n\r"); |
hirokimineshita | 0:ad6f3a862ed5 | 91 | while(getchar()==-35); |
hirokimineshita | 0:ad6f3a862ed5 | 92 | int ax_0_1 = mma7361::read_bit(x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 93 | int az_0_1 = mma7361::read_bit(z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 94 | int gy_0_0 = mma7361::read_bit(y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 95 | for(int j=0; j<100; j++){ |
hirokimineshita | 0:ad6f3a862ed5 | 96 | mma7361::low_pass_filter(&ax_0_1,x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 97 | mma7361::low_pass_filter(&az_0_1,z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 98 | mma7361::low_pass_filter(&gy_0_0,y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 99 | } |
hirokimineshita | 0:ad6f3a862ed5 | 100 | mma7361::GS=1; |
hirokimineshita | 0:ad6f3a862ed5 | 101 | wait_ms(250); |
hirokimineshita | 0:ad6f3a862ed5 | 102 | int ax_1_1 = mma7361::read_bit(x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 103 | int az_1_1 = mma7361::read_bit(z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 104 | int gy_1_0 = mma7361::read_bit(y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 105 | for(int j=0; j<100; j++){ |
hirokimineshita | 0:ad6f3a862ed5 | 106 | mma7361::low_pass_filter(&ax_1_1,x_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 107 | mma7361::low_pass_filter(&az_1_1,z_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 108 | mma7361::low_pass_filter(&gy_1_0,y_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 109 | } |
hirokimineshita | 0:ad6f3a862ed5 | 110 | mma7361::GS=0; |
hirokimineshita | 0:ad6f3a862ed5 | 111 | wait_ms(250); |
hirokimineshita | 0:ad6f3a862ed5 | 112 | |
hirokimineshita | 0:ad6f3a862ed5 | 113 | ax_0_0=(ax_0_0+ax_0_1)/2; |
hirokimineshita | 0:ad6f3a862ed5 | 114 | ay_0_0=(ay_0_0+ay_0_1)/2; |
hirokimineshita | 0:ad6f3a862ed5 | 115 | az_0_0=(az_0_0+az_0_1)/2; |
hirokimineshita | 0:ad6f3a862ed5 | 116 | ax_1_0=(ax_1_0+ax_1_1)/2; |
hirokimineshita | 0:ad6f3a862ed5 | 117 | ay_1_0=(ay_1_0+ay_1_1)/2; |
hirokimineshita | 0:ad6f3a862ed5 | 118 | az_1_0=(az_1_0+az_1_1)/2; |
hirokimineshita | 0:ad6f3a862ed5 | 119 | gx_0_0-=ax_0_0; |
hirokimineshita | 0:ad6f3a862ed5 | 120 | gy_0_0-=ay_0_0; |
hirokimineshita | 0:ad6f3a862ed5 | 121 | gz_0_0-=az_0_0; |
hirokimineshita | 0:ad6f3a862ed5 | 122 | gx_1_0-=ax_0_1; |
hirokimineshita | 0:ad6f3a862ed5 | 123 | gy_1_0-=ay_0_1; |
hirokimineshita | 0:ad6f3a862ed5 | 124 | gz_1_0-=az_0_1; |
hirokimineshita | 0:ad6f3a862ed5 | 125 | |
hirokimineshita | 0:ad6f3a862ed5 | 126 | mma7361::set_each_num(ax_0_0,ay_0_0,az_0_0,ax_1_0,ay_1_0,az_1_0,gx_0_0,gy_0_0,gz_0_0,gx_1_0,gy_1_0,gz_1_0,0); |
hirokimineshita | 0:ad6f3a862ed5 | 127 | wait_ms(100); |
hirokimineshita | 0:ad6f3a862ed5 | 128 | } |
hirokimineshita | 0:ad6f3a862ed5 | 129 | |
hirokimineshita | 0:ad6f3a862ed5 | 130 | void mma7361::set_each_num(int ax_0_0,int ay_0_0,int az_0_0,int ax_1_0,int ay_1_0,int az_1_0,int gx_0_0,int gy_0_0,int gz_0_0,int gx_1_0,int gy_1_0,int gz_1_0,int new_range){ |
hirokimineshita | 0:ad6f3a862ed5 | 131 | //def or -1 -> don' change value |
hirokimineshita | 0:ad6f3a862ed5 | 132 | mma7361::GS=new_range; |
hirokimineshita | 0:ad6f3a862ed5 | 133 | mma7361::range=new_range; |
hirokimineshita | 0:ad6f3a862ed5 | 134 | int temp[12]; |
hirokimineshita | 0:ad6f3a862ed5 | 135 | temp[0]=ax_0_0; |
hirokimineshita | 0:ad6f3a862ed5 | 136 | temp[1]=ay_0_0; |
hirokimineshita | 0:ad6f3a862ed5 | 137 | temp[2]=az_0_0; |
hirokimineshita | 0:ad6f3a862ed5 | 138 | temp[3]=ax_1_0; |
hirokimineshita | 0:ad6f3a862ed5 | 139 | temp[4]=ay_1_0; |
hirokimineshita | 0:ad6f3a862ed5 | 140 | temp[5]=az_1_0; |
hirokimineshita | 0:ad6f3a862ed5 | 141 | temp[6]=gx_0_0; |
hirokimineshita | 0:ad6f3a862ed5 | 142 | temp[7]=gy_0_0; |
hirokimineshita | 0:ad6f3a862ed5 | 143 | temp[8]=gz_0_0; |
hirokimineshita | 0:ad6f3a862ed5 | 144 | temp[9]=gx_1_0; |
hirokimineshita | 0:ad6f3a862ed5 | 145 | temp[10]=gy_1_0; |
hirokimineshita | 0:ad6f3a862ed5 | 146 | temp[11]=gz_1_0; |
hirokimineshita | 0:ad6f3a862ed5 | 147 | for(int i=0; i<4; i++){ |
hirokimineshita | 0:ad6f3a862ed5 | 148 | for(int j=0; j<3; j++){ |
hirokimineshita | 0:ad6f3a862ed5 | 149 | if(temp[i*3+j]!=def){ |
hirokimineshita | 0:ad6f3a862ed5 | 150 | mma7361::mat[i][j]=temp[i*3+j]; |
hirokimineshita | 0:ad6f3a862ed5 | 151 | } |
hirokimineshita | 0:ad6f3a862ed5 | 152 | } |
hirokimineshita | 0:ad6f3a862ed5 | 153 | } |
hirokimineshita | 0:ad6f3a862ed5 | 154 | } |
hirokimineshita | 0:ad6f3a862ed5 | 155 | |
hirokimineshita | 3:0359af763b6c | 156 | void mma7361::set_num(int (*new_mat)[3],int new_range){ |
hirokimineshita | 0:ad6f3a862ed5 | 157 | mma7361::GS=new_range; |
hirokimineshita | 0:ad6f3a862ed5 | 158 | mma7361::range=new_range; |
hirokimineshita | 0:ad6f3a862ed5 | 159 | for(int i=0; i<4; i++){ |
hirokimineshita | 0:ad6f3a862ed5 | 160 | for(int j=0; j<3; j++){ |
hirokimineshita | 3:0359af763b6c | 161 | if(new_mat[i][j]!=def){ |
hirokimineshita | 3:0359af763b6c | 162 | mma7361::mat[i][j]=new_mat[i][j]; |
hirokimineshita | 0:ad6f3a862ed5 | 163 | } |
hirokimineshita | 0:ad6f3a862ed5 | 164 | } |
hirokimineshita | 0:ad6f3a862ed5 | 165 | } |
hirokimineshita | 0:ad6f3a862ed5 | 166 | } |
hirokimineshita | 0:ad6f3a862ed5 | 167 | |
hirokimineshita | 0:ad6f3a862ed5 | 168 | void mma7361::set_range(int new_range){ |
hirokimineshita | 0:ad6f3a862ed5 | 169 | mma7361::GS=new_range; |
hirokimineshita | 0:ad6f3a862ed5 | 170 | mma7361::range=new_range; |
hirokimineshita | 0:ad6f3a862ed5 | 171 | } |
hirokimineshita | 0:ad6f3a862ed5 | 172 | |
hirokimineshita | 0:ad6f3a862ed5 | 173 | void mma7361::clear_mat(){ |
hirokimineshita | 0:ad6f3a862ed5 | 174 | mma7361::set_each_num(A00,A10,A20,A01,A11,A21,G00,G10,G20,G01,G11,G21,mma7361::range); |
hirokimineshita | 0:ad6f3a862ed5 | 175 | } |
hirokimineshita | 0:ad6f3a862ed5 | 176 | |
hirokimineshita | 0:ad6f3a862ed5 | 177 | int mma7361::read_bit(int axis){ |
hirokimineshita | 0:ad6f3a862ed5 | 178 | int v; |
hirokimineshita | 0:ad6f3a862ed5 | 179 | switch(axis){ |
hirokimineshita | 0:ad6f3a862ed5 | 180 | case 0: |
hirokimineshita | 0:ad6f3a862ed5 | 181 | v=mma7361::dx.read_u16(); |
hirokimineshita | 0:ad6f3a862ed5 | 182 | break; |
hirokimineshita | 0:ad6f3a862ed5 | 183 | case 1: |
hirokimineshita | 0:ad6f3a862ed5 | 184 | v=mma7361::dy.read_u16(); |
hirokimineshita | 0:ad6f3a862ed5 | 185 | break; |
hirokimineshita | 0:ad6f3a862ed5 | 186 | case 2: |
hirokimineshita | 0:ad6f3a862ed5 | 187 | v=mma7361::dz.read_u16(); |
hirokimineshita | 0:ad6f3a862ed5 | 188 | break; |
hirokimineshita | 0:ad6f3a862ed5 | 189 | default: |
hirokimineshita | 0:ad6f3a862ed5 | 190 | v=def; |
hirokimineshita | 0:ad6f3a862ed5 | 191 | break; |
hirokimineshita | 0:ad6f3a862ed5 | 192 | } |
hirokimineshita | 0:ad6f3a862ed5 | 193 | return v; |
hirokimineshita | 0:ad6f3a862ed5 | 194 | } |
hirokimineshita | 0:ad6f3a862ed5 | 195 | |
hirokimineshita | 0:ad6f3a862ed5 | 196 | int mma7361::read_bit_0(int axis){ |
hirokimineshita | 0:ad6f3a862ed5 | 197 | int v; |
hirokimineshita | 0:ad6f3a862ed5 | 198 | v=mma7361::read_bit(axis); |
hirokimineshita | 0:ad6f3a862ed5 | 199 | v-=mat[mma7361::range][axis]; |
hirokimineshita | 0:ad6f3a862ed5 | 200 | return v; |
hirokimineshita | 0:ad6f3a862ed5 | 201 | } |
hirokimineshita | 0:ad6f3a862ed5 | 202 | |
hirokimineshita | 0:ad6f3a862ed5 | 203 | float mma7361::read_by_g(int axis){ |
hirokimineshita | 0:ad6f3a862ed5 | 204 | float v; |
hirokimineshita | 0:ad6f3a862ed5 | 205 | v=mma7361::read_bit_0(axis); |
hirokimineshita | 0:ad6f3a862ed5 | 206 | v/=mat[mma7361::range+2][axis]; |
hirokimineshita | 0:ad6f3a862ed5 | 207 | return v; |
hirokimineshita | 0:ad6f3a862ed5 | 208 | } |
hirokimineshita | 0:ad6f3a862ed5 | 209 | |
hirokimineshita | 0:ad6f3a862ed5 | 210 | void mma7361::low_pass_filter(int *int_bit,int axis){ |
hirokimineshita | 0:ad6f3a862ed5 | 211 | *int_bit=*int_bit*0.9+mma7361::read_bit(axis)*0.1; |
hirokimineshita | 0:ad6f3a862ed5 | 212 | } |
hirokimineshita | 0:ad6f3a862ed5 | 213 | |
hirokimineshita | 0:ad6f3a862ed5 | 214 | void mma7361::show_mat(){ |
hirokimineshita | 0:ad6f3a862ed5 | 215 | for(int i=0; i<4; i++){ |
hirokimineshita | 0:ad6f3a862ed5 | 216 | for(int j=0; j<3; j++){ |
hirokimineshita | 0:ad6f3a862ed5 | 217 | mma7361::pcdev.printf("%d ",mat[i][j]); |
hirokimineshita | 0:ad6f3a862ed5 | 218 | } |
hirokimineshita | 0:ad6f3a862ed5 | 219 | mma7361::pcdev.printf("\n\r"); |
hirokimineshita | 0:ad6f3a862ed5 | 220 | } |
hirokimineshita | 0:ad6f3a862ed5 | 221 | } |
hirokimineshita | 0:ad6f3a862ed5 | 222 | |
hirokimineshita | 0:ad6f3a862ed5 | 223 | float mma7361::bit_to_g(int bit,int axis){ |
hirokimineshita | 0:ad6f3a862ed5 | 224 | float v=0; |
hirokimineshita | 0:ad6f3a862ed5 | 225 | v=bit-mma7361::mat[mma7361::range][axis]; |
hirokimineshita | 0:ad6f3a862ed5 | 226 | v/=mma7361::mat[mma7361::range+2][axis]; |
hirokimineshita | 0:ad6f3a862ed5 | 227 | return v; |
hirokimineshita | 0:ad6f3a862ed5 | 228 | } |
hirokimineshita | 0:ad6f3a862ed5 | 229 | |
hirokimineshita | 0:ad6f3a862ed5 | 230 | void mma7361::set_1st_bit(int *variable,int axis,float direction_G){ |
hirokimineshita | 0:ad6f3a862ed5 | 231 | *variable=mma7361::mat[mma7361::range][axis]+mma7361::mat[mma7361::range+2][axis]*direction_G; |
hirokimineshita | 0:ad6f3a862ed5 | 232 | } |
hirokimineshita | 0:ad6f3a862ed5 | 233 | |
hirokimineshita | 0:ad6f3a862ed5 | 234 | |
hirokimineshita | 0:ad6f3a862ed5 | 235 | float mma7361::rotate(int *a_value,int a_axis,int *b_value,int b_axis){ |
hirokimineshita | 0:ad6f3a862ed5 | 236 | float ga,gb,g; |
hirokimineshita | 0:ad6f3a862ed5 | 237 | ga=mma7361::bit_to_g(*a_value,a_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 238 | gb=mma7361::bit_to_g(*b_value,b_axis); |
hirokimineshita | 0:ad6f3a862ed5 | 239 | if(abs(ga)<THHO){ |
hirokimineshita | 0:ad6f3a862ed5 | 240 | ga=0; |
hirokimineshita | 0:ad6f3a862ed5 | 241 | } |
hirokimineshita | 0:ad6f3a862ed5 | 242 | if(abs(gb)<THHO){ |
hirokimineshita | 0:ad6f3a862ed5 | 243 | return 0; |
hirokimineshita | 0:ad6f3a862ed5 | 244 | }else{ |
hirokimineshita | 0:ad6f3a862ed5 | 245 | g=ga/gb; |
hirokimineshita | 0:ad6f3a862ed5 | 246 | g=atan(g); |
hirokimineshita | 0:ad6f3a862ed5 | 247 | return g; |
hirokimineshita | 0:ad6f3a862ed5 | 248 | } |
hirokimineshita | 0:ad6f3a862ed5 | 249 | } |
hirokimineshita | 0:ad6f3a862ed5 | 250 | |
hirokimineshita | 0:ad6f3a862ed5 | 251 | float mma7361::rad_to_deg(float rad){ |
hirokimineshita | 0:ad6f3a862ed5 | 252 | float deg; |
hirokimineshita | 0:ad6f3a862ed5 | 253 | deg=rad*180/PI; |
hirokimineshita | 0:ad6f3a862ed5 | 254 | return deg; |
hirokimineshita | 0:ad6f3a862ed5 | 255 | } |