File content as of revision 171:42f0cccc956f:

#include "mbed.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);

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);



char buffer[100];
uint8_t rxbuffer[128];
int rxindex=0;


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();

void rx_Irq(void)
{
    // read from the peripheral and make sure something is available
    int i=120;
    uint8_t c;
    while(pclpwa.readable() ) {
       c = rxbuffer[rxindex] = pclpwa.getc();
       if (rxindex>120) {
               pc.write((const uint8_t *)rxbuffer,rxindex,NULL);
               rxindex=0;
       } else rxindex++;
       if (/*c==0x0d ||*/ c==0x0a) {
            pc.write((const uint8_t *)rxbuffer,rxindex+1,NULL);
            rxindex=0;
       } else rxindex++;
    }
   
    return;
}


int main()
{
       int i=0;
       short  x,y,z;
       enabl=0;
       wkup=0; 
       v20v=1;      
      
        pclpwa.attach(rx_Irq,Serial::RxIrq); 
        pclpwa.set_flow_control(Serial::RTSCTS,PB_1,PB_13);
       printf("\r\n**** Hello UD-GS1 by mbed ******\r\n");
       initLIS3DH();
       read3axes(&x,&y,&z);
       printf("3D acc x=%d y=%d z=%d\r\n",x,y,z);
       v20v=0;      
 //      wait(1.0); // 1000 ms
       wkup=1; 
       enabl=1;
       wait(3.0); // 100 ms
       pclpwa.printf("< SYS MODE SET 00\r\n");
       wait(0.2);
       pclpwa.printf("< SYS VER GET\r\n");
       wait(2.0);
       pclpwa.printf("< GNSS VER GET\r\n");
       wait(2.0);
       pclpwa.printf("< SYS TO_WAIT_FETCHING SET ON\r\n");
       wait(2.0);

       pclpwa.printf("< GNSS HOST_FW_SENT SET_EVT 0x0001\r\n");
       wait(2.7);
       pclpwa.printf("< GNSS STT GET\r\n");
       wait(2.7);
while (1) {
        ext7=1;
        myled =0;
        wait(0.7); // 700 ms
        ext7=0;
        myled =1;
       pclpwa.printf("< GNSS STT GET\r\n");
       wait(2.7);
        wait(1.7); // 1700 ms
     //  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 1
   while(bytes--) {
      *p = spireadReg( ad );
      p++;
      ad++;
   }    
#else   
   STSPICS=0;
    wait_us(1);
    STSPI.write(ad | 0xc0 );  
   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);
}



/****************************************
  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];
//    STSPI.frequency(1000);
    
    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);
}