HAVRM mine / MMA7361

これは MMA7361 3軸アナログ加速度センサ の Nucleo F401RE 用のライブラリです。詳しい説明はLibrary中のReadMe.hに記載しています。文字化けや開けない場合はダウンロードしてみてください。Google Chrome では見れるはずです。

Dependents:   Nucleo_L3GD20_MMA7361_Kalman

mma7361.cpp@0:ad6f3a862ed5, 2015-02-01 (annotated)

Committer:
hirokimineshita
Date:
Sun Feb 01 03:56:22 2015 +0000
Revision:
0:ad6f3a862ed5
Child:
3:0359af763b6c
this is 1st up of mma7361

Who changed what in which revision?

UserRevisionLine numberNew 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(): dx(PA_0),dy(PA_1),dz(PA_4),ST(PH_0),GS(PH_1),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 mma7361::mma7361(PinName accelx,PinName accely,PinName accelz): dx(accelx),dy(accely),dz(accelz),ST(PH_0),GS(PH_1),pcdev(USBTX,USBRX)
hirokimineshita 0:ad6f3a862ed5 32 {
hirokimineshita 0:ad6f3a862ed5 33 mma7361::ST=0;
hirokimineshita 0:ad6f3a862ed5 34 mma7361::GS=0;
hirokimineshita 0:ad6f3a862ed5 35 mma7361::range=0;
hirokimineshita 0:ad6f3a862ed5 36 mma7361::clear_mat();
hirokimineshita 0:ad6f3a862ed5 37 }
hirokimineshita 0:ad6f3a862ed5 38
hirokimineshita 0:ad6f3a862ed5 39 mma7361::mma7361(PinName accelx,PinName accely,PinName accelz,PinName set): dx(accelx),dy(accely),dz(accelz),ST(set),GS(PH_1),pcdev(USBTX,USBRX)
hirokimineshita 0:ad6f3a862ed5 40 {
hirokimineshita 0:ad6f3a862ed5 41 mma7361::ST=0;
hirokimineshita 0:ad6f3a862ed5 42 mma7361::GS=0;
hirokimineshita 0:ad6f3a862ed5 43 mma7361::range=0;
hirokimineshita 0:ad6f3a862ed5 44 mma7361::clear_mat();
hirokimineshita 0:ad6f3a862ed5 45 }
hirokimineshita 0:ad6f3a862ed5 46
hirokimineshita 0:ad6f3a862ed5 47 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 48 {
hirokimineshita 0:ad6f3a862ed5 49 mma7361::ST=0;
hirokimineshita 0:ad6f3a862ed5 50 mma7361::GS=0;
hirokimineshita 0:ad6f3a862ed5 51 mma7361::range=0;
hirokimineshita 0:ad6f3a862ed5 52 mma7361::clear_mat();
hirokimineshita 0:ad6f3a862ed5 53 }
hirokimineshita 0:ad6f3a862ed5 54
hirokimineshita 0:ad6f3a862ed5 55 /*---------------------------------constracta------------------------------------------*/
hirokimineshita 0:ad6f3a862ed5 56 /*-----------------------------------member--------------------------------------------*/
hirokimineshita 0:ad6f3a862ed5 57
hirokimineshita 0:ad6f3a862ed5 58 void mma7361::calibration(){
hirokimineshita 0:ad6f3a862ed5 59 mma7361::GS=0;
hirokimineshita 0:ad6f3a862ed5 60 mma7361::range=0;
hirokimineshita 0:ad6f3a862ed5 61 mma7361::ST=1;
hirokimineshita 0:ad6f3a862ed5 62 wait_ms(250);
hirokimineshita 0:ad6f3a862ed5 63 mma7361::ST=0;
hirokimineshita 0:ad6f3a862ed5 64
hirokimineshita 0:ad6f3a862ed5 65 mma7361::pcdev.printf("Prease set devise with z-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 ax_0_0 = mma7361::read_bit(x_axis);
hirokimineshita 0:ad6f3a862ed5 69 int ay_0_0 = mma7361::read_bit(y_axis);
hirokimineshita 0:ad6f3a862ed5 70 int gz_0_0 = mma7361::read_bit(z_axis);
hirokimineshita 0:ad6f3a862ed5 71 for(int j=0; j<100; j++){
hirokimineshita 0:ad6f3a862ed5 72 mma7361::low_pass_filter(&ax_0_0,x_axis);
hirokimineshita 0:ad6f3a862ed5 73 mma7361::low_pass_filter(&ay_0_0,y_axis);
hirokimineshita 0:ad6f3a862ed5 74 mma7361::low_pass_filter(&gz_0_0,z_axis);
hirokimineshita 0:ad6f3a862ed5 75 }
hirokimineshita 0:ad6f3a862ed5 76 mma7361::GS=1;
hirokimineshita 0:ad6f3a862ed5 77 wait_ms(250);
hirokimineshita 0:ad6f3a862ed5 78 int ax_1_0 = mma7361::read_bit(x_axis);
hirokimineshita 0:ad6f3a862ed5 79 int ay_1_0 = mma7361::read_bit(y_axis);
hirokimineshita 0:ad6f3a862ed5 80 int gz_1_0 = mma7361::read_bit(z_axis);
hirokimineshita 0:ad6f3a862ed5 81 for(int j=0; j<100; j++){
hirokimineshita 0:ad6f3a862ed5 82 mma7361::low_pass_filter(&ax_1_0,x_axis);
hirokimineshita 0:ad6f3a862ed5 83 mma7361::low_pass_filter(&ay_1_0,y_axis);
hirokimineshita 0:ad6f3a862ed5 84 mma7361::low_pass_filter(&gz_1_0,z_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 x-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 ay_0_1 = mma7361::read_bit(y_axis);
hirokimineshita 0:ad6f3a862ed5 93 int az_0_0 = mma7361::read_bit(z_axis);
hirokimineshita 0:ad6f3a862ed5 94 int gx_0_0 = mma7361::read_bit(x_axis);
hirokimineshita 0:ad6f3a862ed5 95 for(int j=0; j<100; j++){
hirokimineshita 0:ad6f3a862ed5 96 mma7361::low_pass_filter(&ay_0_1,y_axis);
hirokimineshita 0:ad6f3a862ed5 97 mma7361::low_pass_filter(&az_0_0,z_axis);
hirokimineshita 0:ad6f3a862ed5 98 mma7361::low_pass_filter(&gx_0_0,x_axis);
hirokimineshita 0:ad6f3a862ed5 99 }
hirokimineshita 0:ad6f3a862ed5 100 mma7361::GS=1;
hirokimineshita 0:ad6f3a862ed5 101 wait_ms(250);
hirokimineshita 0:ad6f3a862ed5 102 int ay_1_1 = mma7361::read_bit(y_axis);
hirokimineshita 0:ad6f3a862ed5 103 int az_1_0 = mma7361::read_bit(z_axis);
hirokimineshita 0:ad6f3a862ed5 104 int gx_1_0 = mma7361::read_bit(x_axis);
hirokimineshita 0:ad6f3a862ed5 105 for(int j=0; j<100; j++){
hirokimineshita 0:ad6f3a862ed5 106 mma7361::low_pass_filter(&ay_1_1,y_axis);
hirokimineshita 0:ad6f3a862ed5 107 mma7361::low_pass_filter(&az_1_0,z_axis);
hirokimineshita 0:ad6f3a862ed5 108 mma7361::low_pass_filter(&gx_1_0,x_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 mma7361::pcdev.printf("Prease set devise with y-axis is going up\n\r");
hirokimineshita 0:ad6f3a862ed5 114 mma7361::pcdev.printf("Press Enter key\n\r");
hirokimineshita 0:ad6f3a862ed5 115 while(getchar()==-35);
hirokimineshita 0:ad6f3a862ed5 116 int ax_0_1 = mma7361::read_bit(x_axis);
hirokimineshita 0:ad6f3a862ed5 117 int az_0_1 = mma7361::read_bit(z_axis);
hirokimineshita 0:ad6f3a862ed5 118 int gy_0_0 = mma7361::read_bit(y_axis);
hirokimineshita 0:ad6f3a862ed5 119 for(int j=0; j<100; j++){
hirokimineshita 0:ad6f3a862ed5 120 mma7361::low_pass_filter(&ax_0_1,x_axis);
hirokimineshita 0:ad6f3a862ed5 121 mma7361::low_pass_filter(&az_0_1,z_axis);
hirokimineshita 0:ad6f3a862ed5 122 mma7361::low_pass_filter(&gy_0_0,y_axis);
hirokimineshita 0:ad6f3a862ed5 123 }
hirokimineshita 0:ad6f3a862ed5 124 mma7361::GS=1;
hirokimineshita 0:ad6f3a862ed5 125 wait_ms(250);
hirokimineshita 0:ad6f3a862ed5 126 int ax_1_1 = mma7361::read_bit(x_axis);
hirokimineshita 0:ad6f3a862ed5 127 int az_1_1 = mma7361::read_bit(z_axis);
hirokimineshita 0:ad6f3a862ed5 128 int gy_1_0 = mma7361::read_bit(y_axis);
hirokimineshita 0:ad6f3a862ed5 129 for(int j=0; j<100; j++){
hirokimineshita 0:ad6f3a862ed5 130 mma7361::low_pass_filter(&ax_1_1,x_axis);
hirokimineshita 0:ad6f3a862ed5 131 mma7361::low_pass_filter(&az_1_1,z_axis);
hirokimineshita 0:ad6f3a862ed5 132 mma7361::low_pass_filter(&gy_1_0,y_axis);
hirokimineshita 0:ad6f3a862ed5 133 }
hirokimineshita 0:ad6f3a862ed5 134 mma7361::GS=0;
hirokimineshita 0:ad6f3a862ed5 135 wait_ms(250);
hirokimineshita 0:ad6f3a862ed5 136
hirokimineshita 0:ad6f3a862ed5 137 ax_0_0=(ax_0_0+ax_0_1)/2;
hirokimineshita 0:ad6f3a862ed5 138 ay_0_0=(ay_0_0+ay_0_1)/2;
hirokimineshita 0:ad6f3a862ed5 139 az_0_0=(az_0_0+az_0_1)/2;
hirokimineshita 0:ad6f3a862ed5 140 ax_1_0=(ax_1_0+ax_1_1)/2;
hirokimineshita 0:ad6f3a862ed5 141 ay_1_0=(ay_1_0+ay_1_1)/2;
hirokimineshita 0:ad6f3a862ed5 142 az_1_0=(az_1_0+az_1_1)/2;
hirokimineshita 0:ad6f3a862ed5 143 gx_0_0-=ax_0_0;
hirokimineshita 0:ad6f3a862ed5 144 gy_0_0-=ay_0_0;
hirokimineshita 0:ad6f3a862ed5 145 gz_0_0-=az_0_0;
hirokimineshita 0:ad6f3a862ed5 146 gx_1_0-=ax_0_1;
hirokimineshita 0:ad6f3a862ed5 147 gy_1_0-=ay_0_1;
hirokimineshita 0:ad6f3a862ed5 148 gz_1_0-=az_0_1;
hirokimineshita 0:ad6f3a862ed5 149
hirokimineshita 0:ad6f3a862ed5 150 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 151 wait_ms(100);
hirokimineshita 0:ad6f3a862ed5 152 }
hirokimineshita 0:ad6f3a862ed5 153
hirokimineshita 0:ad6f3a862ed5 154 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 155 //def or -1 -> don' change value
hirokimineshita 0:ad6f3a862ed5 156 mma7361::GS=new_range;
hirokimineshita 0:ad6f3a862ed5 157 mma7361::range=new_range;
hirokimineshita 0:ad6f3a862ed5 158 int temp[12];
hirokimineshita 0:ad6f3a862ed5 159 temp[0]=ax_0_0;
hirokimineshita 0:ad6f3a862ed5 160 temp[1]=ay_0_0;
hirokimineshita 0:ad6f3a862ed5 161 temp[2]=az_0_0;
hirokimineshita 0:ad6f3a862ed5 162 temp[3]=ax_1_0;
hirokimineshita 0:ad6f3a862ed5 163 temp[4]=ay_1_0;
hirokimineshita 0:ad6f3a862ed5 164 temp[5]=az_1_0;
hirokimineshita 0:ad6f3a862ed5 165 temp[6]=gx_0_0;
hirokimineshita 0:ad6f3a862ed5 166 temp[7]=gy_0_0;
hirokimineshita 0:ad6f3a862ed5 167 temp[8]=gz_0_0;
hirokimineshita 0:ad6f3a862ed5 168 temp[9]=gx_1_0;
hirokimineshita 0:ad6f3a862ed5 169 temp[10]=gy_1_0;
hirokimineshita 0:ad6f3a862ed5 170 temp[11]=gz_1_0;
hirokimineshita 0:ad6f3a862ed5 171 for(int i=0; i<4; i++){
hirokimineshita 0:ad6f3a862ed5 172 for(int j=0; j<3; j++){
hirokimineshita 0:ad6f3a862ed5 173 if(temp[i*3+j]!=def){
hirokimineshita 0:ad6f3a862ed5 174 mma7361::mat[i][j]=temp[i*3+j];
hirokimineshita 0:ad6f3a862ed5 175 }
hirokimineshita 0:ad6f3a862ed5 176 }
hirokimineshita 0:ad6f3a862ed5 177 }
hirokimineshita 0:ad6f3a862ed5 178 }
hirokimineshita 0:ad6f3a862ed5 179
hirokimineshita 0:ad6f3a862ed5 180 void mma7361::set_num(int *new_mat,int new_range){
hirokimineshita 0:ad6f3a862ed5 181 mma7361::GS=new_range;
hirokimineshita 0:ad6f3a862ed5 182 mma7361::range=new_range;
hirokimineshita 0:ad6f3a862ed5 183 for(int i=0; i<4; i++){
hirokimineshita 0:ad6f3a862ed5 184 for(int j=0; j<3; j++){
hirokimineshita 0:ad6f3a862ed5 185 if(new_mat[i*3+j]!=def){
hirokimineshita 0:ad6f3a862ed5 186 mma7361::mat[i][j]=new_mat[i*3+j];
hirokimineshita 0:ad6f3a862ed5 187 }
hirokimineshita 0:ad6f3a862ed5 188 }
hirokimineshita 0:ad6f3a862ed5 189 }
hirokimineshita 0:ad6f3a862ed5 190 }
hirokimineshita 0:ad6f3a862ed5 191
hirokimineshita 0:ad6f3a862ed5 192 void mma7361::set_range(int new_range){
hirokimineshita 0:ad6f3a862ed5 193 mma7361::GS=new_range;
hirokimineshita 0:ad6f3a862ed5 194 mma7361::range=new_range;
hirokimineshita 0:ad6f3a862ed5 195 }
hirokimineshita 0:ad6f3a862ed5 196
hirokimineshita 0:ad6f3a862ed5 197 void mma7361::clear_mat(){
hirokimineshita 0:ad6f3a862ed5 198 mma7361::set_each_num(A00,A10,A20,A01,A11,A21,G00,G10,G20,G01,G11,G21,mma7361::range);
hirokimineshita 0:ad6f3a862ed5 199 }
hirokimineshita 0:ad6f3a862ed5 200
hirokimineshita 0:ad6f3a862ed5 201 int mma7361::read_bit(int axis){
hirokimineshita 0:ad6f3a862ed5 202 int v;
hirokimineshita 0:ad6f3a862ed5 203 switch(axis){
hirokimineshita 0:ad6f3a862ed5 204 case 0:
hirokimineshita 0:ad6f3a862ed5 205 v=mma7361::dx.read_u16();
hirokimineshita 0:ad6f3a862ed5 206 break;
hirokimineshita 0:ad6f3a862ed5 207 case 1:
hirokimineshita 0:ad6f3a862ed5 208 v=mma7361::dy.read_u16();
hirokimineshita 0:ad6f3a862ed5 209 break;
hirokimineshita 0:ad6f3a862ed5 210 case 2:
hirokimineshita 0:ad6f3a862ed5 211 v=mma7361::dz.read_u16();
hirokimineshita 0:ad6f3a862ed5 212 break;
hirokimineshita 0:ad6f3a862ed5 213 default:
hirokimineshita 0:ad6f3a862ed5 214 v=def;
hirokimineshita 0:ad6f3a862ed5 215 break;
hirokimineshita 0:ad6f3a862ed5 216 }
hirokimineshita 0:ad6f3a862ed5 217 return v;
hirokimineshita 0:ad6f3a862ed5 218 }
hirokimineshita 0:ad6f3a862ed5 219
hirokimineshita 0:ad6f3a862ed5 220 int mma7361::read_bit_0(int axis){
hirokimineshita 0:ad6f3a862ed5 221 int v;
hirokimineshita 0:ad6f3a862ed5 222 v=mma7361::read_bit(axis);
hirokimineshita 0:ad6f3a862ed5 223 v-=mat[mma7361::range][axis];
hirokimineshita 0:ad6f3a862ed5 224 return v;
hirokimineshita 0:ad6f3a862ed5 225 }
hirokimineshita 0:ad6f3a862ed5 226
hirokimineshita 0:ad6f3a862ed5 227 float mma7361::read_by_g(int axis){
hirokimineshita 0:ad6f3a862ed5 228 float v;
hirokimineshita 0:ad6f3a862ed5 229 v=mma7361::read_bit_0(axis);
hirokimineshita 0:ad6f3a862ed5 230 v/=mat[mma7361::range+2][axis];
hirokimineshita 0:ad6f3a862ed5 231 return v;
hirokimineshita 0:ad6f3a862ed5 232 }
hirokimineshita 0:ad6f3a862ed5 233
hirokimineshita 0:ad6f3a862ed5 234 void mma7361::low_pass_filter(int *int_bit,int axis){
hirokimineshita 0:ad6f3a862ed5 235 *int_bit=*int_bit*0.9+mma7361::read_bit(axis)*0.1;
hirokimineshita 0:ad6f3a862ed5 236 }
hirokimineshita 0:ad6f3a862ed5 237
hirokimineshita 0:ad6f3a862ed5 238 void mma7361::show_mat(){
hirokimineshita 0:ad6f3a862ed5 239 for(int i=0; i<4; i++){
hirokimineshita 0:ad6f3a862ed5 240 for(int j=0; j<3; j++){
hirokimineshita 0:ad6f3a862ed5 241 mma7361::pcdev.printf("%d ",mat[i][j]);
hirokimineshita 0:ad6f3a862ed5 242 }
hirokimineshita 0:ad6f3a862ed5 243 mma7361::pcdev.printf("\n\r");
hirokimineshita 0:ad6f3a862ed5 244 }
hirokimineshita 0:ad6f3a862ed5 245 }
hirokimineshita 0:ad6f3a862ed5 246
hirokimineshita 0:ad6f3a862ed5 247 float mma7361::bit_to_g(int bit,int axis){
hirokimineshita 0:ad6f3a862ed5 248 float v=0;
hirokimineshita 0:ad6f3a862ed5 249 v=bit-mma7361::mat[mma7361::range][axis];
hirokimineshita 0:ad6f3a862ed5 250 v/=mma7361::mat[mma7361::range+2][axis];
hirokimineshita 0:ad6f3a862ed5 251 return v;
hirokimineshita 0:ad6f3a862ed5 252 }
hirokimineshita 0:ad6f3a862ed5 253
hirokimineshita 0:ad6f3a862ed5 254 void mma7361::set_1st_bit(int *variable,int axis,float direction_G){
hirokimineshita 0:ad6f3a862ed5 255 *variable=mma7361::mat[mma7361::range][axis]+mma7361::mat[mma7361::range+2][axis]*direction_G;
hirokimineshita 0:ad6f3a862ed5 256 }
hirokimineshita 0:ad6f3a862ed5 257
hirokimineshita 0:ad6f3a862ed5 258
hirokimineshita 0:ad6f3a862ed5 259 float mma7361::rotate(int *a_value,int a_axis,int *b_value,int b_axis){
hirokimineshita 0:ad6f3a862ed5 260 float ga,gb,g;
hirokimineshita 0:ad6f3a862ed5 261 ga=mma7361::bit_to_g(*a_value,a_axis);
hirokimineshita 0:ad6f3a862ed5 262 gb=mma7361::bit_to_g(*b_value,b_axis);
hirokimineshita 0:ad6f3a862ed5 263 if(abs(ga)<THHO){
hirokimineshita 0:ad6f3a862ed5 264 ga=0;
hirokimineshita 0:ad6f3a862ed5 265 }
hirokimineshita 0:ad6f3a862ed5 266 if(abs(gb)<THHO){
hirokimineshita 0:ad6f3a862ed5 267 return 0;
hirokimineshita 0:ad6f3a862ed5 268 }else{
hirokimineshita 0:ad6f3a862ed5 269 g=ga/gb;
hirokimineshita 0:ad6f3a862ed5 270 g=atan(g);
hirokimineshita 0:ad6f3a862ed5 271 return g;
hirokimineshita 0:ad6f3a862ed5 272 }
hirokimineshita 0:ad6f3a862ed5 273 }
hirokimineshita 0:ad6f3a862ed5 274
hirokimineshita 0:ad6f3a862ed5 275 float mma7361::rad_to_deg(float rad){
hirokimineshita 0:ad6f3a862ed5 276 float deg;
hirokimineshita 0:ad6f3a862ed5 277 deg=rad*180/PI;
hirokimineshita 0:ad6f3a862ed5 278 return deg;
hirokimineshita 0:ad6f3a862ed5 279 }