File content as of revision 172:3aa6f1366c5d:

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

char buffer[100];
uint8_t rxbuffer[128];
uint8_t cmdrxbuffer[80];
int rxindex=0;
int cmdrxindex=0;
uint8_t rcvf;

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

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>120) {
               rxbuffer[rxindex+1]=0;
               pc.write((const uint8_t *)rxbuffer,rxindex,NULL);
               rxindex=0;
               rcvf=1;
               rxbuffer[rxindex+1]=0;
       } else rxindex++;
       if (/*c==0x0d ||*/ c==0x0a) {
            rxbuffer[rxindex+1]=0;
            pc.write((const uint8_t *)rxbuffer,rxindex+1,NULL);
            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=10;
    rcvf=0;
    errorf=0;
    
    wait_ms(1);    
    while(rxindex && timeout--)wait_ms(10);
    
    
    
    memset(rxbuffer,0,sizeof(rxbuffer));
    rxindex=0;
    
    pclpwa.printf(cmd);
    pclpwa.printf("\r\n");
    timeout=1000;
    while (rcvf==0 && timeout--) wait_ms(10);
    if (errorf) {
        //retry
        wait_ms(10);
        rcvf=0;
        errorf=0;
        pclpwa.printf(cmd);
        pclpwa.printf("\r\n");
        while (rcvf==0 && timeout--) wait_ms(10);
    }
    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");   
}

int main()
{
       int i=0;
       short  x,y,z;
       
       
  //     enabl.mode(PullNone);
  //     wkup.mode(PullNone);
  //     v20v.mode(PullNone);
  // ext7.mode(PullNone);
event.rise (&intpin1);
event.enable_irq();
       enabl = 0;
       wkup = 1;    // Normal mode
       v20v = 1;   //2V off   
      
//       pc.attach(cmd_rx_Irq,Serial::RxIrq); 
       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");
       wait_ms(100); 
       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\r\n",x,y,z);
       v20v = 0;   //2V on    
       wait_ms(100);    
       enabl = 1;  // enable
       wait_ms(4000);  // 100 ms
//        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       
       SendLPWAcmd("< SYS MODE SET 00");
//       SendLPWAcmd("< SYS MODE SET_EVT ON");
       SendLPWAcmd("< SYS VER GET");
       wait_ms(100);    
       SendLPWAcmd("< GNSS VER GET");
       wait_ms(100);    
//       SendLPWAcmd("< SYS TO_WAIT_FETCHING SET ON");
       SendLPWAcmd("< SYS TO_FETCHING SET ON");
       SendLPWAcmd("< TX PLD SET 0123456789ABCDEF0123456789ABCDEF");
//       SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0001");
       SendLPWAcmd("< TX PREV_STT SET_EVT ON");
       SendLPWAcmd("< GNSS STT GET");
       while (1) {
          // printf(" cnt=%d   stat= ",i++);
           SendLPWAcmd("< GNSS STT GET");
           pc.printf("Adc=%d INT1=%d INT2=%d ",(int)(batlev.read()*100.0f) , intout1.read(),intout2.read());
           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 1
   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);
}



/****************************************
  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);
}