main.cpp

00001 #include "mbed.h"
00002 //UD-GS4  H3LIS331DL sensor test program
00003 // i2c SPI switch by #define USESPI
00004 
00005 /* UD-GS2 H/W define 
00006  PIO_SWin    PB_4 
00007  PIO_wkup    PA_4 
00008  PIO_enable  PB_0
00009  PIO_intout1 PB_2
00010  PIO_led     PB_5
00011  PIO_v20v    PC_13
00012  PIO_v18v    PH_0
00013  PIO_intout2 PH_1
00014  PIO_spics   PB_12
00015  PIO_battryMonEn   PA_5
00016  PB_7 SDA
00017  PB_6 SCL
00018  */
00019 
00020 //I2C               i2cacc(p_sda, p_scl)
00021 //#include "H3LIS331DL.h"
00022 //DigitalIn sdaDummy(D14, PullUp);
00023 //DigitalIn sclDummy(D15, PullUp);
00024 //H3LIS331DL  h3dacc(PB_7,PB_6);
00025 //H3LIS331DL  h3dacc(D14,D15);
00026 //キャリブレーション　仮値
00027 #define OFFSET_X   12
00028 #define OFFSET_Y   22
00029 #define OFFSET_Z   -7
00030 
00031 //SPI (PinName mosi, PinName miso, PinName sclk, PinName ssel=NC)
00032 SPI STSPI(PA_7, PA_6, PA_5);
00033 DigitalOut  STSPICS(PB_6);
00034 
00035 //             SDA SCL
00036 I2C i2c(D14,D15);                          //NUCLEO pin assign
00037 //RawSerial         pc(PA_9, PA_10,115200); //console UART UD-GS4
00038 RawSerial          pc(PA_2, PA_3,115200 ); //NUCLEO pin assign
00039 
00040 Ticker    interrput;
00041 //Ticker    interrupt;
00042 //DigitalOut led(LED1);
00043 //int initLIS3DH();
00044 
00045 
00046 int initLIS331();
00047 int read3axes331(short *tx,short *ty,short *tz);
00048 
00049 
00050 /***************************
00051   integer sqrt
00052     整数 sqrt √演算を整数で　ライブラリより速い
00053 ****************************/
00054 int int_sqrt(unsigned int x)
00055 {
00056   register int a = 0, c = 0, y = 0, i = 0, t = x;
00057   while(t >>= 1){
00058     ++i;
00059   }
00060   for(i += i & 1; i >= 0; i -= 2){
00061     c = (y << 1 | 1) <= x >> i;
00062     a = a << 1 | c;
00063     y = y << 1 | c;
00064     x -= c * y << i;
00065     y += c;
00066   }
00067   return a;
00068 }
00069 
00070 //#define PRINTMAX
00071 
00072 long max_g=0;
00073 long now_g;
00074 int  update;
00075 // SPIはtimer割り込みが使える 
00076 void timer()
00077 {
00078     
00079     short tx=0,ty=0,tz=0;
00080     long  scr;
00081     static long ax,ay,az,as;
00082     static  int cnt;
00083     
00084     read3axes331(&tx,&ty,&tz);
00085    //キャリブレーションの補正
00086     tx += OFFSET_X;
00087     ty += OFFSET_Y;
00088     tz += OFFSET_Z;
00089 //    led = !led;
00090 
00091     //スカラー値
00092     scr = int_sqrt( tx*tx  + ty*ty + tz*tz);
00093     now_g = scr;
00094     
00095     ax+= tx;
00096     ay+= ty;
00097     az+= tz;
00098     as+= scr;
00099     cnt++;
00100 
00101 #ifdef PRINTMAX
00102     if ( max_g < scr) {
00103         max_g = scr;
00104         update=1;
00105         pc.printf("x:%d y:%d,z:%d scaler = %d  max g=%2.2fG \r\n",tx,ty,tz,scr,  (float)scr/11.0f);
00106     }  
00107 #else    
00108      pc.printf("x:%d\ty:%d\tz:%d\tscaler=%d\t%2.2fG \r\n",tx,ty,tz,scr,  (float)scr/11.0f);
00109 #endif
00110     //16回平均
00111     if ((cnt & 0x0f)== 0) {
00112          float asc = (float)as / 11.0f /16.0f;
00113          pc.printf("avarage x:%d\ty:%d\tz:%d\tscaler=%d\t%2.2fG \r\n",ax/16,ay/16,az/16,as/16,  asc );
00114         as=ax=ay=az=0;
00115     }
00116 
00117 
00118 
00119 }
00120 
00121 
00122 int main()
00123 {
00124     long gg;
00125     int i=0;
00126     int ret = 1;
00127     short tx=0,ty=0,tz=0;
00128      pc.printf("\nSTM32 Start \r\n");
00129 
00130      
00131 #if 1 //i2c 
00132     while(ret) {
00133       ret = initLIS331();
00134       pc.printf(" init LIS331DL %d\r\n",ret);
00135       wait_ms(100);
00136     }
00137     while(1)
00138     {
00139         timer();
00140          wait_ms(20);
00141     }
00142 
00143 #endif
00144 #if 0//SPI     
00145     while(ret) {
00146        ret = initLIS331();
00147        pc.printf(" init LIS331DL %d\r\n",ret);
00148        wait_ms(100);
00149     }
00150     interrput.attach_us(&timer, 11650  );// 20ms
00151 
00152     while(1)
00153     {
00154         gg = now_g;
00155         if (update) {//maxが見つかったラ表示
00156             update=0;
00157             pc.printf("max g =%2.2f G \r\n", (float)max_g / 50.0f );
00158         } else if (gg > 50) pc.printf("now g =%2.2f G \r\n", (float)gg / 50.0f );
00159     //    led = !led;
00160         wait_ms(20);
00161     }
00162 #endif    
00163 }