File content as of revision 173:1198e6c31b4a:

#include "mbed.h"
#include <string.h>
#include <time.h>
#include "rtc_api.h"
#include "watchdog.h"

#define ONE_G_ACCELERATION 9.8     //重力加速度
#define EXP_BIAS           12  
#define TTFloat_MIN   0.000245094f 
#define TTFloat_MAX   1046528.0f 
#define GRAV 9.8f
#define GRATE  413.0f 

SPI STSPI(SPI_MOSI, SPI_MISO, SPI_SCK);
DigitalOut STSPICS(PB_9);
Ticker      interrput;
Serial pc(PA_9, PA_10,115200 ); //console
Serial pclpwa(PB_10, PB_11,115200); 
DigitalOut myled(PB_14);
DigitalOut ext7(PB_15);

DigitalOut wkup(PA_5);
DigitalOut enabl(PA_6);
DigitalOut v20v(PA_12);
DigitalIn  intout1(PA_7,PullNone);
DigitalIn  intout2(PA_4,PullNone);
AnalogIn   batlev(PA_2);
InterruptIn event(PA_7);

Watchdog wd;

char buffer[100];
uint8_t rxbuffer[128];
uint8_t cmdrxbuffer[80];
uint8_t rxCmdStatus[128];
int rxindex=0;
int oldrxindex;
int cmdrxindex=0;
uint8_t rcvf;
uint8_t dbgmsgf=1;
uint8_t errorf;

void intTimer( void );   
int read3axes(short *tx,short *ty,short *tz);
float UDGS1_decodeFloat(unsigned short v);
unsigned short UDGS1_encodeFloat(float v);
int int_sqrt(unsigned int x);
int initLIS3DH();
int readTemp(short *tmp);

void rx_Irq(void)
{
    // read from the peripheral and make sure something is available
    uint8_t c;
    while(pclpwa.readable() ) {
       c = rxbuffer[rxindex] = pclpwa.getc();
       if (rxindex>127) {
               rxbuffer[rxindex+1]=0;
               if (dbgmsgf) pc.write((const uint8_t *)rxbuffer,rxindex,NULL);
               memcpy((char*)rxCmdStatus,(char*)rxbuffer , rxindex+1 );
               oldrxindex = rxindex+1;
               rxindex=0;
               rcvf=1;
               rxbuffer[rxindex+1]=0;
       } else
       if (/*c==0x0d ||*/ c==0x0a) {
            rxbuffer[rxindex+1]=0;
            if (dbgmsgf) pc.write((const uint8_t *)rxbuffer,rxindex+1,NULL);
            strcpy((char*)rxCmdStatus,(char*)rxbuffer );
            oldrxindex = rxindex+1;
            rxindex=0;
            rcvf=1;
            if (strstr((char*)rxbuffer,"Invalid")) errorf=1;
            if (strstr((char*)rxbuffer,"Busy")) errorf=2;
            
       } else rxindex++;
    }

    return;
}

int SendLPWAcmd(char *cmd)
{
    int timeout=30;
    int i=0;
    rcvf=0;
    errorf=0;
    
    wait_ms(1);    
    while(rxindex && timeout--)wait_ms(10);
    if(timeout==0)pc.printf("timeout 1\n\r");
    memset(rxbuffer,0,sizeof(rxbuffer));
    rxindex=0;
    
    wait_ms(10);
    pclpwa.printf(cmd);
    pclpwa.printf("\r\n");
    timeout=500;
    while (rcvf==0 && timeout--) wait_ms(10);
    if(timeout==0)pc.printf("timeout 2\n\r");
    if (errorf) {
         timeout=500;
        //retry
        wait_ms(10);
        rcvf=0;
        errorf=0;
        wait_ms(10);
        pclpwa.printf(cmd);
        pclpwa.printf("\r\n");
        while (rcvf==0 && timeout--) wait_ms(10);
        if(timeout==0)pc.printf("timeout 3\n\r");
    }
    wait_ms(10);    
    return errorf;
}



void cmd_rx_Irq(void)
{
    // read from the peripheral and make sure something is available
    uint8_t c;
    while(pc.readable() ) {
       c = cmdrxbuffer[cmdrxindex] = pc.getc();
       pc.putc(c);
       if (cmdrxindex>80) {
               pclpwa.write((const uint8_t *)cmdrxbuffer,cmdrxindex,NULL);
               cmdrxindex=0;
       } else cmdrxindex++;
       if (c==0x0d || c==0x0a) {
            pclpwa.write((const uint8_t *)cmdrxbuffer,cmdrxindex,NULL);
            pclpwa.printf("\r\n");
            cmdrxindex=0;
       } else cmdrxindex++;
    }
    return;
}
void intpin1()
{
 pc.printf("@@@@@@@@ INTout1 @@@\n\r");   
}

float convertLaloFomMin(float lalo)
{
    int integ;
    float min;
    integ = (int)lalo;
    min = lalo - (float)integ;
    min = min / 0.60 ;
    return((float)integ + min);   
}
/*  緯度 24 - 46 */
long convertLaInt(float la)
{
   return( (int)((convertLaloFomMin(la) - 24.0f ) / 0.00001049042 ) ); 
}

/*  経度 122 - 146*/

long convertLoInt(float lo)
{
   return( (int)((convertLaloFomMin(lo) - 122.0f ) / 0.000011444095 ) ); 
}    

/*  緯度 24 - 46 */
float convertLaFloat(int la)
{
   return( 24.0f + 0.00001049042 * (float)la ); 
}

/*  経度 122 - 146*/

float convertLoFloat(int lo)
{
   return( 122.0f + 0.000011444095 * (float)lo  ); 
}    


int getNextTimeSet()
{
    int mode=0;
    char *p;
    dbgmsgf=0; 
    memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus)); 
    SendLPWAcmd("< TIME DREM GET");
//    pc.printf((char*)rxCmdStatus);
    p = strstr((char*)rxCmdStatus,"TIME DREM GET");
    if (p) {
//        pc.printf(p+13);
        sscanf(p + 14,"%d",&mode);            
    }   
    dbgmsgf=1;  
    return(mode);
}

int getNextTimeSend()
{
    int mode=0;
    char *p;
    dbgmsgf=0; 
    memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus)); 
    SendLPWAcmd("< TIME TREM GET");
//    pc.printf((char*)rxCmdStatus);
    p = strstr((char*)rxCmdStatus,"TIME TREM GET");
    if (p) {
//        pc.printf(p+13);
        sscanf(p + 14,"%d",&mode);            
    }   
    dbgmsgf=1;  
    return(mode);
}

int setPayload(char *payload) {
   char buf[60]= {"< TX PLD SET "};
   char dat[8];
   int i;  
   int ret; 
   for(i=0;i<16;i++){
        sprintf(dat,"%02x",payload[i] & 0xff);
        strcat(buf,dat); 
   } 
 // pc.printf(buf);    
   ret = SendLPWAcmd(buf);
   if (ret==0) {
       if (strstr((char*)rxbuffer,"> TX PLD SET")==NULL) ret= -1;
  }
  return(ret);
}

int getGNSSmode()
{
    int mode=0;
    char *p;
    dbgmsgf=0; 
    memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus)); 
    SendLPWAcmd("< GNSS STT GET");
//    pc.printf((char*)rxCmdStatus);
    p = strstr((char*)rxCmdStatus,"GNSS STT GET");
    if (p) {
//        pc.printf(p+13);
        sscanf(p + 13,"%x",&mode);            
    }   
    dbgmsgf=1;  
    return(mode);
}

int getGPStime(time_t *gpst)
{
    int timeout=10;
    dbgmsgf=0;  
    int   ret=0;
    char  *p;
    memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
    SendLPWAcmd("< TIME CTIME GET");
    while(timeout--) {
       wait_ms(500);
       p = strstr((char*)rxCmdStatus,"TIME CTIME");
       if (p) break;
    }
    wait_ms(1);
     p = strstr((char*)rxCmdStatus,"0x");
    if (p) {
        sscanf(p,"%x",(int*)gpst);
        ret=1;
    }
    dbgmsgf=1;  
    return(ret);
}


int getTimeFromGPS(char *buf, int rtcset){
    time_t gpstime;
    time_t tim;
    int ret;
    struct tm *now;
    if ( getGPStime(&gpstime) ) {
       int gpsw, gpstw;
        gpsw = gpstime >> 20;
        gpstw = gpstime & 0xfffff;
        tim = (gpsw * 7 * 24 * 60 * 60) + gpstw;
        tim += (10*365*24*60*60 + 24*60*60*7 + 9*60*60);//Tokyo time
        now = localtime(&tim);
        if (rtcset) rtc_write(tim);
        
         ret = strftime(buf, 23,"%Y %m/%d %T",now);
        return(ret);
    }
    return -1;
}



int getGPSposGGA(float *pLo,float *pLa, float *pHDOP, long *pElev)
{
    int timeout=10;
    int i,j;
    dbgmsgf=0;  
    int ret=0;
    int sum=0;
    memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
    SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0001");
    while(timeout--) {
       wait_ms(500);
       if (strstr((char*)rxCmdStatus,"$GPGGA") ) break;
    }
    wait_ms(1);
    if (timeout) {
      for(i=0,j=0;i<128;i++) {
        if (rxCmdStatus[i]== ',' ) {
            j++;
            if (j==2 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pLo );
            else if (j==4 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pLa );
            else if (j==8 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pHDOP );
            else if (j==9 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%d",pElev );
            ret=1;
        } else if (rxCmdStatus[i]== '*' ) {
             sscanf((char*)rxCmdStatus+i+1, "%x",&sum );
             int x;
              uint8_t s = 0;
             for(x=3;rxCmdStatus[x]!='*';x++) s ^= rxCmdStatus[x];
             s &= 0xff;
           //  pc.printf("%s %02x, %02x %d\r\n",rxCmdStatus,sum,s & 0xff,sum);
           if (s != sum){
                ret = -1;// check sum err
                pc.printf("sum-err %s %02x, %02x \r\n",rxCmdStatus,sum,s & 0xff);
           }
         }
    }
    
     SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0000");
    }
    dbgmsgf=1;  
     return(ret);
}


int getGPSGSA(int *pmode,float *pPDOP)
{
    int timeout=10;
    int i,j;
    dbgmsgf=0;  
    int ret=0;
    int sum=0;
    memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
    SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0004");
    while(timeout--) {
       wait_ms(500);
       if (strstr((char*)rxCmdStatus,"GSA") ) break;
    }
    wait_ms(1);
    if (timeout) {
      for(i=0,j=0;i<128;i++) {
        if (rxCmdStatus[i]== ',' ) {
            j++;
            if (j==2 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%d",pmode );
            else if (j==16 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pPDOP );
            ret=1;
        } else if (rxCmdStatus[i]== '*' ) {
             sscanf((char*)rxCmdStatus+i+1, "%x",&sum );
             int x;
              uint8_t s = 0;
             for(x=3;rxCmdStatus[x]!='*';x++) s ^= rxCmdStatus[x];
             s &= 0xff;
           //  pc.printf("%s %02x, %02x %d\r\n",rxCmdStatus,sum,s & 0xff,sum);
           if (s != sum){
                ret = -1;// check sum err
                pc.printf("sum-err %s %02x, %02x \r\n",rxCmdStatus,sum,s & 0xff);
           }
         }
    }
    
     SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0000");
    }
    dbgmsgf=1;  
     return(ret);
}



int getGPSRMC(char *ptime,char *pdate,float *pLo,float *pLa)
{
    int timeout=10;
    int i,j;
    dbgmsgf=0;  
    int ret=0;
    int sum=0;
    memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
    SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0020");
    while(timeout--) {
       wait_ms(500);
       if (strstr((char*)rxCmdStatus,"RMC") ) break;
    }
    wait_ms(1);
    if (timeout) {
      for(i=0,j=0;i<128;i++) {
        if (rxCmdStatus[i]== ',' ) {
            j++;
            if (j==1) memcpy(ptime,(char*)rxCmdStatus+i+1,9 );
            else if (j==9) memcpy(pdate,(char*)rxCmdStatus+i+1,6 );
            else if (j==3 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pLo );
            else if (j==5 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pLa );
            ret=1;
        } else if (rxCmdStatus[i]== '*' ) {
             sscanf((char*)rxCmdStatus+i+1, "%x",&sum );
             int x;
              uint8_t s = 0;
             for(x=3;rxCmdStatus[x]!='*';x++) s ^= rxCmdStatus[x];
             s &= 0xff;
           //  pc.printf("%s %02x, %02x %d\r\n",rxCmdStatus,sum,s & 0xff,sum);
           if (s != sum){
                ret = -1;// check sum err
                pc.printf("sum-err %s %02x, %02x \r\n",rxCmdStatus,sum,s & 0xff);
           }
         }
    }
    
     SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0000");
    }
    dbgmsgf=1;  
     return(ret);
}



int main()
{
       int i=0;
       short  x,y,z;
       char c;
       int ret;
       int rtcenb=0;
        rtc_init();
        printf("\r\n**** Hello UD-GS1 by mbed ******\r\n");
        
        long llo = convertLoInt(136.599999f);
        float flo =  convertLoFloat(llo);
        long lla = convertLaInt(36.599999f);
        float fla = convertLaFloat(lla);
        pc.printf("lo=%f %08x la=%f %08x\r\n", flo,llo,fla,lla);         
        llo = convertLoInt(146.0f);
        flo =  convertLoFloat(llo);
        lla = convertLaInt(46.0f);
        fla = convertLaFloat(lla);
        pc.printf("lo=%f %08x la=%f %08x\r\n", flo,llo,fla,lla);         
        
        
        if (wd.WatchdogCausedReset())
            pc.printf("Watchdog caused reset.\r\n");
        wd.Configure(30); 
        wd.Service(); 
  //     enabl.mode(PullNone);
  //     wkup.mode(PullNone);
  //     v20v.mode(PullNone);
  // ext7.mode(PullNone);
       dbgmsgf=1;  
       enabl = 0;
       wkup =  1;    // Normal mode
       v20v =  1;   //2V off   
       wait_ms(100);    
       enabl = 1;  // enable
       v20v =  0;   //2V ON   
       while(pclpwa.readable()) c = pclpwa.getc();       
       pclpwa.attach(rx_Irq,Serial::RxIrq); 
       pclpwa.set_flow_control(Serial::RTSCTS,PB_1,PB_13); 
       wait_ms(500);  // 4sec 
       printf("\r\n**** LPWA ON  ******\r\n");
       
       event.rise (&intpin1);
       event.enable_irq();
       pc.printf("Adc=%d INT1=%d INT2=%d ",(int)(batlev.read()*100.0f) , intout1.read(),intout2.read());
       initLIS3DH();
       read3axes(&x,&y,&z);
       printf("3D acc x=%d y=%d z=%d Temp=%d\r\n",x,y,z,readTemp(NULL));
//        interrput.attach(&intTimer, 0.001);
#if 0       
 SendLPWAcmd("< SYS MODE SET 05");
 SendLPWAcmd("< SYS EEPROM GET 0x0410");
 SendLPWAcmd("< SYS EEPROM GET 0x0414");
 wait(0.1);  // 100 ms
 SendLPWAcmd("< SYS EEPROM SET 0x0410,0x00000001");
 SendLPWAcmd("< SYS EEPROM SET 0x0414,0x00000010");
 wait(0.1);  // 100 ms
 SendLPWAcmd("< SYS EEPROM GET 0x0410");
 SendLPWAcmd("< SYS EEPROM GET 0x0414");
 while(1);       
#endif       
       dbgmsgf=0;  
       ret = SendLPWAcmd("< SYS MODE SET 00");
       if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
//       SendLPWAcmd("< SYS MODE SET_EVT ON");
       ret = SendLPWAcmd("< SYS VER GET");
       if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
//       wait_ms(100);    
       ret = SendLPWAcmd("< GNSS VER GET");
       if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
//       wait_ms(100);    
       ret = SendLPWAcmd("< SYS MODE SET 00");
       if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
//       SendLPWAcmd("< SYS TO_WAIT_FETCHING SET ON");
       ret = SendLPWAcmd("< SYS TO_FETCHING SET ON");
       if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
       ret = SendLPWAcmd("< TX PLD SET 0123456789ABCDEF0123456789ABCDEF");
       if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
//       SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0001");
       ret = SendLPWAcmd("< TX PREV_STT SET_EVT ON");
       if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
       ret = SendLPWAcmd("< GNSS STT GET");
       if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
        char pay[16]={0x00, 0x01, 0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x10,0x20,0x30,0x40,0x50,0x60,0x70};
        setPayload(pay);
       if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
       else pc.printf("Set payload error\r\n");

       dbgmsgf=1;  

       wd.Configure(20.0f); 

       while (1) {
          int mode;
          float lo=0.0f,la=0.0f,HDOP;
          long elev;
          wd.Service(); 

          
          // printf(" cnt=%d   stat= ",i++);
           //SendLPWAcmd("< GNSS STT GET");
           mode = getGNSSmode();
           if (mode & 2  && rtcenb==0 ) {
               char st[32];
               if (getTimeFromGPS(st,1)>0) {
                    pc.printf("%s ",st );
            
                   rtcenb=1;
               }
           }
           pc.printf("Adc=%d INT1=%d INT2=%d mode=%02x  next time=%d\r\n",(int)(batlev.read()*100.0f) , intout1.read(),intout2.read(), mode ,getNextTimeSet());
           if (mode & 4) {
               char stime[10]={0};
               char sdate[8]={0};
               if (getGPSposGGA( &lo , &la, &HDOP,&elev) >= 0) {
                   pc.printf("Lo=N %f La=E %f HDOP=%f  evevation=%d\r\n", lo/100.0f,la/100.0f,HDOP,elev); 
               }
               
                if ( getGPSGSA(&mode,&HDOP) >= 0)
                   pc.printf("GSA 2D/3D mode=%d PDOP=%f\r\n", mode,HDOP); 

                if (getGPSRMC(stime,sdate,&lo , &la) >=0) 
                   pc.printf("RMC Lo=N %f La=E %f time %s date=%s\r\n", lo/100.0f,la/100.0f,stime,sdate); 
               
           }
           if (mode &0x80)    
               pc.printf("next time=%d  send=%d ",getNextTimeSet(), getNextTimeSend());
if (rtcenb)
{
           time_t tim = rtc_read();
           struct tm *now;
           char buf[24]={0};
           now = localtime(&tim);
           strftime(buf, 23,"%Y %m/%d %T",now);
           pc.printf("RTC time=%s\n\r",buf);
}
           pc.printf("Temp =%d ",readTemp(NULL));


           
           wait_ms(4000);
     //  pc.write((const uint8_t *)"ACBD\r\n",6,NULL);
     //  pc.puts("ASDFGREWA\r\n");
       //printf("PC12345ABCD cnt=%d\n\r",i++);
//         pclpwa.printf("< SYS VER GET\r\n");
//       read3axes(&x,&y,&z);
//       printf("3D acc x=%d y=%d z=%d\r\n",x,y,z);
    }
}





/*************************************************
  Read a regster from SPI
*************************************************/
unsigned char spireadReg(unsigned char ad){
    unsigned char r;
    STSPICS = 0;
    wait_us(1);
    STSPI.write(ad | 0x80);  
    r = STSPI.write(0);
    wait_us(1);
    STSPICS = 1;
    return(r);
}

/*************************************************
  Read multi regsters from SPI
*************************************************/
unsigned char spireadRegs(unsigned char ad,unsigned char *p,int bytes){
#if 0
   while(bytes--) {
      *p = spireadReg( ad );
      p++;
      ad++;
   }    
#else   
   STSPICS=0;
    wait_us(1);
    STSPI.write(ad | 0x80 );  
   while(bytes--) {
      *p++ = STSPI.write(0);
   }
   wait_us(1);
   STSPICS=1;
#endif  
    return(0);
}

/*************************************************
  Write a regster via SPI
*************************************************/
unsigned char spiwriteReg(unsigned char ad,unsigned char val){
    unsigned char r;
    STSPICS=0;
    wait_us(1);
    STSPI.write(ad);  
    r = STSPI.write(val);
    wait_us(1);
    STSPICS=1;
    return(r);
}


/****************************************
  half float 16bit浮動小数点からfloatに変換
  SEEEEE TT TTTT TTTT  --> SEEEEEEEE E TTT TTTT TTTT TTTT TTTT
  signed ext 
  float 指数部 bias 127
  half  指数部 bias 15
*****************************************/
float UDGS1_16decodeFloat(unsigned short v){
    volatile float r;
    int sign = 1;
    if (v & 0x8000) { sign = -1; v&= 0x7fff;}

    if(v==0) return(0.0f);
    if((v & 0x7c00) == 0 ) 
        *(unsigned long *)&r = ((v & 0x3ff) << (23-10));
    else 
        *(unsigned long *)&r = ((v & 0x3ff) << (23-10)) | (((v>>10) + (127-15)) << 23) ;  
    return(r * sign);
}

/****************************************
  float から half 16bit浮動小数点に変換
  SEEEEE TT TTTT TTTT  <-- SEEEEEEEE E TTT TTTT TTTT TTTT TTTT
  float 指数部 bias 127
  half  指数部 bias 15
*****************************************/
unsigned short UDGS1_16encodeFloat(float v){
    unsigned short sign = 0;
    if (v < 0.0f) sign = 0x8000;
  
    register unsigned long tf;
    register unsigned short exp;
    if(v <= 0.000000059f)return(0);
    if(v > 131008.0f)return(0x7fff);
    tf = *(unsigned long *)&v;
    exp = ((tf >> 23) & 0xff ) - (127-15);
    return( ((tf >> 13) & 0x3ff) | (exp << 10) | sign );
}


/****************************************
  tyny 13bit浮動小数点からfloatに変換
*****************************************/
float UDGS1_decodeFloat(unsigned short v){
    volatile float r;
    if(v==0) return(0.0f);
    *(unsigned long *)&r = ((v & 0xff) << (23-8)) | (((v>>8) + 115) << 23) ;  
    return(r);
}

/****************************************
  float から tiny 13bit浮動小数点に変換
*****************************************/
unsigned short UDGS1_encodeFloat(float v){
  
    register unsigned long tf;
    register unsigned short exp;
    if(v <= 0.0000235f)return(0);
    if(v > 1046528.0f)return(0x1fff);
    tf = *(unsigned long *)&v;
    exp = ((tf >> 23) & 0xff ) - (127-EXP_BIAS);
    return( ((tf >> 15) & 0xff) | (exp << 8) );
  
}

/***************************
  integer sqrt
    整数 sqrt
****************************/
int int_sqrt(unsigned int x)
{
  register int a = 0, c = 0, y = 0, i = 0, t = x;
  while(t >>= 1){
    ++i;
  }
  for(i += i & 1; i >= 0; i -= 2){
    c = (y << 1 | 1) <= x >> i;
    a = a << 1 | c;
    y = y << 1 | c;
    x -= c * y << i;
    y += c;
  }
  return a;
}



/*****************************************
  init LIS3DH ST micro 3-axes accelerometer
   SPI interface 
*****************************************/
int initLIS3DH()
{
//    STSPI = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK, D6);
   
    STSPI.format(8,3);
    STSPI.frequency(1000000);
    STSPICS=1;
    wait_us(1);
    printf("Who am I %02x\r\n", spireadReg(0x0f));
    spiwriteReg(0x20,0x27);  
    spiwriteReg(0x23,0x98);  
    return (spireadReg(0x0f)== 0x44);
}

/*************************************************
  read 3-axes from LIS3DH
*************************************************/
int read3axes(short *tx,short *ty,short *tz)
{
    int timeout=5;
    unsigned char regs[6];
   
    while((spireadReg(0x07)&7)==0 && timeout--){wait_us(5);};
    spireadRegs(0x28,regs,6);// 12bit signed   1G = 413 9.8m/s**2
//    STSPI.frequency(); //1MHz
    *tx = (short)(regs[0] | (regs[1]<<8)) >> 4;
    *ty = (short)(regs[2] | (regs[3]<<8)) >> 4;
    *tz = (short)(regs[4] | (regs[5]<<8)) >> 4;
    return(0);
}

/*************************************************
  read temparature from LIS3DH
*************************************************/
int readTemp(short *tmp)
{
    int t;
    t = spireadReg(0x0d);
    t |=  spireadReg(0x0e) << 8;
    t >>= 4;
//    pc.printf("tmp %d %04x\r\n", t, t);
    if (tmp) *tmp = t;
    return(t);
}