I-O DATA DEV2 / KANI-GS1_mbed-dev

UD-GS1 デバグ用　３D Acc 動かない

Fork of UDNS1_mbed-dev by I-O DATA DEV2

main.cpp@173:1198e6c31b4a, 2018-09-24 (annotated)

Committer:
hakusan270
Date:
Mon Sep 24 15:11:03 2018 +0000
Revision:
173:1198e6c31b4a
Parent:
172:3aa6f1366c5d 
??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hakusan270 168:bccad2df0b45 1 #include "mbed.h"
hakusan270 172:3aa6f1366c5d 2 #include <string.h>
hakusan270 173:1198e6c31b4a 3 #include <time.h>
hakusan270 173:1198e6c31b4a 4 #include "rtc_api.h"
hakusan270 173:1198e6c31b4a 5 #include "watchdog.h"
hakusan270 170:e086a9b46bd7 6
hakusan270 170:e086a9b46bd7 7 #define ONE_G_ACCELERATION 9.8 //重力加速度
hakusan270 170:e086a9b46bd7 8 #define EXP_BIAS 12
hakusan270 170:e086a9b46bd7 9 #define TTFloat_MIN 0.000245094f
hakusan270 170:e086a9b46bd7 10 #define TTFloat_MAX 1046528.0f
hakusan270 170:e086a9b46bd7 11 #define GRAV 9.8f
hakusan270 170:e086a9b46bd7 12 #define GRATE 413.0f
hakusan270 170:e086a9b46bd7 13
hakusan270 170:e086a9b46bd7 14 SPI STSPI(SPI_MOSI, SPI_MISO, SPI_SCK);
hakusan270 170:e086a9b46bd7 15 DigitalOut STSPICS(PB_9);
hakusan270 172:3aa6f1366c5d 16 Ticker interrput;
hakusan270 171:42f0cccc956f 17 Serial pc(PA_9, PA_10,115200 ); //console
hakusan270 170:e086a9b46bd7 18 Serial pclpwa(PB_10, PB_11,115200);
hakusan270 170:e086a9b46bd7 19 DigitalOut myled(PB_14);
hakusan270 170:e086a9b46bd7 20 DigitalOut ext7(PB_15);
hakusan270 171:42f0cccc956f 21
hakusan270 171:42f0cccc956f 22 DigitalOut wkup(PA_5);
hakusan270 171:42f0cccc956f 23 DigitalOut enabl(PA_6);
hakusan270 171:42f0cccc956f 24 DigitalOut v20v(PA_12);
hakusan270 172:3aa6f1366c5d 25 DigitalIn intout1(PA_7,PullNone);
hakusan270 172:3aa6f1366c5d 26 DigitalIn intout2(PA_4,PullNone);
hakusan270 172:3aa6f1366c5d 27 AnalogIn batlev(PA_2);
hakusan270 172:3aa6f1366c5d 28 InterruptIn event(PA_7);
hakusan270 170:e086a9b46bd7 29
hakusan270 173:1198e6c31b4a 30 Watchdog wd;
hakusan270 173:1198e6c31b4a 31
hakusan270 171:42f0cccc956f 32 char buffer[100];
hakusan270 171:42f0cccc956f 33 uint8_t rxbuffer[128];
hakusan270 172:3aa6f1366c5d 34 uint8_t cmdrxbuffer[80];
hakusan270 173:1198e6c31b4a 35 uint8_t rxCmdStatus[128];
hakusan270 171:42f0cccc956f 36 int rxindex=0;
hakusan270 173:1198e6c31b4a 37 int oldrxindex;
hakusan270 172:3aa6f1366c5d 38 int cmdrxindex=0;
hakusan270 172:3aa6f1366c5d 39 uint8_t rcvf;
hakusan270 173:1198e6c31b4a 40 uint8_t dbgmsgf=1;
hakusan270 172:3aa6f1366c5d 41 uint8_t errorf;
hakusan270 171:42f0cccc956f 42
hakusan270 172:3aa6f1366c5d 43 void intTimer( void );
hakusan270 170:e086a9b46bd7 44 int read3axes(short *tx,short *ty,short *tz);
hakusan270 170:e086a9b46bd7 45 float UDGS1_decodeFloat(unsigned short v);
hakusan270 170:e086a9b46bd7 46 unsigned short UDGS1_encodeFloat(float v);
hakusan270 170:e086a9b46bd7 47 int int_sqrt(unsigned int x);
hakusan270 170:e086a9b46bd7 48 int initLIS3DH();
hakusan270 173:1198e6c31b4a 49 int readTemp(short *tmp);
hakusan270 170:e086a9b46bd7 50
hakusan270 171:42f0cccc956f 51 void rx_Irq(void)
hakusan270 171:42f0cccc956f 52 {
hakusan270 171:42f0cccc956f 53 // read from the peripheral and make sure something is available
hakusan270 171:42f0cccc956f 54 uint8_t c;
hakusan270 171:42f0cccc956f 55 while(pclpwa.readable() ) {
hakusan270 171:42f0cccc956f 56 c = rxbuffer[rxindex] = pclpwa.getc();
hakusan270 173:1198e6c31b4a 57 if (rxindex>127) {
hakusan270 172:3aa6f1366c5d 58 rxbuffer[rxindex+1]=0;
hakusan270 173:1198e6c31b4a 59 if (dbgmsgf) pc.write((const uint8_t *)rxbuffer,rxindex,NULL);
hakusan270 173:1198e6c31b4a 60 memcpy((char*)rxCmdStatus,(char*)rxbuffer , rxindex+1 );
hakusan270 173:1198e6c31b4a 61 oldrxindex = rxindex+1;
hakusan270 171:42f0cccc956f 62 rxindex=0;
hakusan270 172:3aa6f1366c5d 63 rcvf=1;
hakusan270 172:3aa6f1366c5d 64 rxbuffer[rxindex+1]=0;
hakusan270 173:1198e6c31b4a 65 } else
hakusan270 171:42f0cccc956f 66 if (/*c==0x0d ||*/ c==0x0a) {
hakusan270 172:3aa6f1366c5d 67 rxbuffer[rxindex+1]=0;
hakusan270 173:1198e6c31b4a 68 if (dbgmsgf) pc.write((const uint8_t *)rxbuffer,rxindex+1,NULL);
hakusan270 173:1198e6c31b4a 69 strcpy((char*)rxCmdStatus,(char*)rxbuffer );
hakusan270 173:1198e6c31b4a 70 oldrxindex = rxindex+1;
hakusan270 171:42f0cccc956f 71 rxindex=0;
hakusan270 172:3aa6f1366c5d 72 rcvf=1;
hakusan270 172:3aa6f1366c5d 73 if (strstr((char*)rxbuffer,"Invalid")) errorf=1;
hakusan270 172:3aa6f1366c5d 74 if (strstr((char*)rxbuffer,"Busy")) errorf=2;
hakusan270 172:3aa6f1366c5d 75
hakusan270 171:42f0cccc956f 76 } else rxindex++;
hakusan270 171:42f0cccc956f 77 }
hakusan270 172:3aa6f1366c5d 78
hakusan270 171:42f0cccc956f 79 return;
hakusan270 171:42f0cccc956f 80 }
hakusan270 170:e086a9b46bd7 81
hakusan270 172:3aa6f1366c5d 82 int SendLPWAcmd(char *cmd)
hakusan270 172:3aa6f1366c5d 83 {
hakusan270 173:1198e6c31b4a 84 int timeout=30;
hakusan270 173:1198e6c31b4a 85 int i=0;
hakusan270 172:3aa6f1366c5d 86 rcvf=0;
hakusan270 172:3aa6f1366c5d 87 errorf=0;
hakusan270 172:3aa6f1366c5d 88
hakusan270 172:3aa6f1366c5d 89 wait_ms(1);
hakusan270 172:3aa6f1366c5d 90 while(rxindex && timeout--)wait_ms(10);
hakusan270 173:1198e6c31b4a 91 if(timeout==0)pc.printf("timeout 1\n\r");
hakusan270 172:3aa6f1366c5d 92 memset(rxbuffer,0,sizeof(rxbuffer));
hakusan270 172:3aa6f1366c5d 93 rxindex=0;
hakusan270 172:3aa6f1366c5d 94
hakusan270 173:1198e6c31b4a 95 wait_ms(10);
hakusan270 172:3aa6f1366c5d 96 pclpwa.printf(cmd);
hakusan270 172:3aa6f1366c5d 97 pclpwa.printf("\r\n");
hakusan270 173:1198e6c31b4a 98 timeout=500;
hakusan270 172:3aa6f1366c5d 99 while (rcvf==0 && timeout--) wait_ms(10);
hakusan270 173:1198e6c31b4a 100 if(timeout==0)pc.printf("timeout 2\n\r");
hakusan270 172:3aa6f1366c5d 101 if (errorf) {
hakusan270 173:1198e6c31b4a 102 timeout=500;
hakusan270 172:3aa6f1366c5d 103 //retry
hakusan270 172:3aa6f1366c5d 104 wait_ms(10);
hakusan270 172:3aa6f1366c5d 105 rcvf=0;
hakusan270 172:3aa6f1366c5d 106 errorf=0;
hakusan270 173:1198e6c31b4a 107 wait_ms(10);
hakusan270 172:3aa6f1366c5d 108 pclpwa.printf(cmd);
hakusan270 172:3aa6f1366c5d 109 pclpwa.printf("\r\n");
hakusan270 172:3aa6f1366c5d 110 while (rcvf==0 && timeout--) wait_ms(10);
hakusan270 173:1198e6c31b4a 111 if(timeout==0)pc.printf("timeout 3\n\r");
hakusan270 172:3aa6f1366c5d 112 }
hakusan270 172:3aa6f1366c5d 113 wait_ms(10);
hakusan270 172:3aa6f1366c5d 114 return errorf;
hakusan270 172:3aa6f1366c5d 115 }
hakusan270 172:3aa6f1366c5d 116
hakusan270 172:3aa6f1366c5d 117
hakusan270 172:3aa6f1366c5d 118
hakusan270 172:3aa6f1366c5d 119 void cmd_rx_Irq(void)
hakusan270 172:3aa6f1366c5d 120 {
hakusan270 172:3aa6f1366c5d 121 // read from the peripheral and make sure something is available
hakusan270 172:3aa6f1366c5d 122 uint8_t c;
hakusan270 172:3aa6f1366c5d 123 while(pc.readable() ) {
hakusan270 172:3aa6f1366c5d 124 c = cmdrxbuffer[cmdrxindex] = pc.getc();
hakusan270 172:3aa6f1366c5d 125 pc.putc(c);
hakusan270 172:3aa6f1366c5d 126 if (cmdrxindex>80) {
hakusan270 172:3aa6f1366c5d 127 pclpwa.write((const uint8_t *)cmdrxbuffer,cmdrxindex,NULL);
hakusan270 172:3aa6f1366c5d 128 cmdrxindex=0;
hakusan270 172:3aa6f1366c5d 129 } else cmdrxindex++;
hakusan270 172:3aa6f1366c5d 130 if (c==0x0d || c==0x0a) {
hakusan270 172:3aa6f1366c5d 131 pclpwa.write((const uint8_t *)cmdrxbuffer,cmdrxindex,NULL);
hakusan270 172:3aa6f1366c5d 132 pclpwa.printf("\r\n");
hakusan270 172:3aa6f1366c5d 133 cmdrxindex=0;
hakusan270 172:3aa6f1366c5d 134 } else cmdrxindex++;
hakusan270 172:3aa6f1366c5d 135 }
hakusan270 172:3aa6f1366c5d 136 return;
hakusan270 172:3aa6f1366c5d 137 }
hakusan270 172:3aa6f1366c5d 138 void intpin1()
hakusan270 172:3aa6f1366c5d 139 {
hakusan270 172:3aa6f1366c5d 140 pc.printf("@@@@@@@@ INTout1 @@@\n\r");
hakusan270 172:3aa6f1366c5d 141 }
hakusan270 168:bccad2df0b45 142
hakusan270 173:1198e6c31b4a 143 float convertLaloFomMin(float lalo)
hakusan270 173:1198e6c31b4a 144 {
hakusan270 173:1198e6c31b4a 145 int integ;
hakusan270 173:1198e6c31b4a 146 float min;
hakusan270 173:1198e6c31b4a 147 integ = (int)lalo;
hakusan270 173:1198e6c31b4a 148 min = lalo - (float)integ;
hakusan270 173:1198e6c31b4a 149 min = min / 0.60 ;
hakusan270 173:1198e6c31b4a 150 return((float)integ + min);
hakusan270 173:1198e6c31b4a 151 }
hakusan270 173:1198e6c31b4a 152 /* 緯度 24 - 46 */
hakusan270 173:1198e6c31b4a 153 long convertLaInt(float la)
hakusan270 173:1198e6c31b4a 154 {
hakusan270 173:1198e6c31b4a 155 return( (int)((convertLaloFomMin(la) - 24.0f ) / 0.00001049042 ) );
hakusan270 173:1198e6c31b4a 156 }
hakusan270 173:1198e6c31b4a 157
hakusan270 173:1198e6c31b4a 158 /* 経度 122 - 146*/
hakusan270 173:1198e6c31b4a 159
hakusan270 173:1198e6c31b4a 160 long convertLoInt(float lo)
hakusan270 173:1198e6c31b4a 161 {
hakusan270 173:1198e6c31b4a 162 return( (int)((convertLaloFomMin(lo) - 122.0f ) / 0.000011444095 ) );
hakusan270 173:1198e6c31b4a 163 }
hakusan270 173:1198e6c31b4a 164
hakusan270 173:1198e6c31b4a 165 /* 緯度 24 - 46 */
hakusan270 173:1198e6c31b4a 166 float convertLaFloat(int la)
hakusan270 173:1198e6c31b4a 167 {
hakusan270 173:1198e6c31b4a 168 return( 24.0f + 0.00001049042 * (float)la );
hakusan270 173:1198e6c31b4a 169 }
hakusan270 173:1198e6c31b4a 170
hakusan270 173:1198e6c31b4a 171 /* 経度 122 - 146*/
hakusan270 173:1198e6c31b4a 172
hakusan270 173:1198e6c31b4a 173 float convertLoFloat(int lo)
hakusan270 173:1198e6c31b4a 174 {
hakusan270 173:1198e6c31b4a 175 return( 122.0f + 0.000011444095 * (float)lo );
hakusan270 173:1198e6c31b4a 176 }
hakusan270 173:1198e6c31b4a 177
hakusan270 173:1198e6c31b4a 178
hakusan270 173:1198e6c31b4a 179 int getNextTimeSet()
hakusan270 173:1198e6c31b4a 180 {
hakusan270 173:1198e6c31b4a 181 int mode=0;
hakusan270 173:1198e6c31b4a 182 char *p;
hakusan270 173:1198e6c31b4a 183 dbgmsgf=0;
hakusan270 173:1198e6c31b4a 184 memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
hakusan270 173:1198e6c31b4a 185 SendLPWAcmd("< TIME DREM GET");
hakusan270 173:1198e6c31b4a 186 // pc.printf((char*)rxCmdStatus);
hakusan270 173:1198e6c31b4a 187 p = strstr((char*)rxCmdStatus,"TIME DREM GET");
hakusan270 173:1198e6c31b4a 188 if (p) {
hakusan270 173:1198e6c31b4a 189 // pc.printf(p+13);
hakusan270 173:1198e6c31b4a 190 sscanf(p + 14,"%d",&mode);
hakusan270 173:1198e6c31b4a 191 }
hakusan270 173:1198e6c31b4a 192 dbgmsgf=1;
hakusan270 173:1198e6c31b4a 193 return(mode);
hakusan270 173:1198e6c31b4a 194 }
hakusan270 173:1198e6c31b4a 195
hakusan270 173:1198e6c31b4a 196 int getNextTimeSend()
hakusan270 173:1198e6c31b4a 197 {
hakusan270 173:1198e6c31b4a 198 int mode=0;
hakusan270 173:1198e6c31b4a 199 char *p;
hakusan270 173:1198e6c31b4a 200 dbgmsgf=0;
hakusan270 173:1198e6c31b4a 201 memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
hakusan270 173:1198e6c31b4a 202 SendLPWAcmd("< TIME TREM GET");
hakusan270 173:1198e6c31b4a 203 // pc.printf((char*)rxCmdStatus);
hakusan270 173:1198e6c31b4a 204 p = strstr((char*)rxCmdStatus,"TIME TREM GET");
hakusan270 173:1198e6c31b4a 205 if (p) {
hakusan270 173:1198e6c31b4a 206 // pc.printf(p+13);
hakusan270 173:1198e6c31b4a 207 sscanf(p + 14,"%d",&mode);
hakusan270 173:1198e6c31b4a 208 }
hakusan270 173:1198e6c31b4a 209 dbgmsgf=1;
hakusan270 173:1198e6c31b4a 210 return(mode);
hakusan270 173:1198e6c31b4a 211 }
hakusan270 173:1198e6c31b4a 212
hakusan270 173:1198e6c31b4a 213 int setPayload(char *payload) {
hakusan270 173:1198e6c31b4a 214 char buf[60]= {"< TX PLD SET "};
hakusan270 173:1198e6c31b4a 215 char dat[8];
hakusan270 173:1198e6c31b4a 216 int i;
hakusan270 173:1198e6c31b4a 217 int ret;
hakusan270 173:1198e6c31b4a 218 for(i=0;i<16;i++){
hakusan270 173:1198e6c31b4a 219 sprintf(dat,"%02x",payload[i] & 0xff);
hakusan270 173:1198e6c31b4a 220 strcat(buf,dat);
hakusan270 173:1198e6c31b4a 221 }
hakusan270 173:1198e6c31b4a 222 // pc.printf(buf);
hakusan270 173:1198e6c31b4a 223 ret = SendLPWAcmd(buf);
hakusan270 173:1198e6c31b4a 224 if (ret==0) {
hakusan270 173:1198e6c31b4a 225 if (strstr((char*)rxbuffer,"> TX PLD SET")==NULL) ret= -1;
hakusan270 173:1198e6c31b4a 226 }
hakusan270 173:1198e6c31b4a 227 return(ret);
hakusan270 173:1198e6c31b4a 228 }
hakusan270 173:1198e6c31b4a 229
hakusan270 173:1198e6c31b4a 230 int getGNSSmode()
hakusan270 173:1198e6c31b4a 231 {
hakusan270 173:1198e6c31b4a 232 int mode=0;
hakusan270 173:1198e6c31b4a 233 char *p;
hakusan270 173:1198e6c31b4a 234 dbgmsgf=0;
hakusan270 173:1198e6c31b4a 235 memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
hakusan270 173:1198e6c31b4a 236 SendLPWAcmd("< GNSS STT GET");
hakusan270 173:1198e6c31b4a 237 // pc.printf((char*)rxCmdStatus);
hakusan270 173:1198e6c31b4a 238 p = strstr((char*)rxCmdStatus,"GNSS STT GET");
hakusan270 173:1198e6c31b4a 239 if (p) {
hakusan270 173:1198e6c31b4a 240 // pc.printf(p+13);
hakusan270 173:1198e6c31b4a 241 sscanf(p + 13,"%x",&mode);
hakusan270 173:1198e6c31b4a 242 }
hakusan270 173:1198e6c31b4a 243 dbgmsgf=1;
hakusan270 173:1198e6c31b4a 244 return(mode);
hakusan270 173:1198e6c31b4a 245 }
hakusan270 173:1198e6c31b4a 246
hakusan270 173:1198e6c31b4a 247 int getGPStime(time_t *gpst)
hakusan270 173:1198e6c31b4a 248 {
hakusan270 173:1198e6c31b4a 249 int timeout=10;
hakusan270 173:1198e6c31b4a 250 dbgmsgf=0;
hakusan270 173:1198e6c31b4a 251 int ret=0;
hakusan270 173:1198e6c31b4a 252 char *p;
hakusan270 173:1198e6c31b4a 253 memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
hakusan270 173:1198e6c31b4a 254 SendLPWAcmd("< TIME CTIME GET");
hakusan270 173:1198e6c31b4a 255 while(timeout--) {
hakusan270 173:1198e6c31b4a 256 wait_ms(500);
hakusan270 173:1198e6c31b4a 257 p = strstr((char*)rxCmdStatus,"TIME CTIME");
hakusan270 173:1198e6c31b4a 258 if (p) break;
hakusan270 173:1198e6c31b4a 259 }
hakusan270 173:1198e6c31b4a 260 wait_ms(1);
hakusan270 173:1198e6c31b4a 261 p = strstr((char*)rxCmdStatus,"0x");
hakusan270 173:1198e6c31b4a 262 if (p) {
hakusan270 173:1198e6c31b4a 263 sscanf(p,"%x",(int*)gpst);
hakusan270 173:1198e6c31b4a 264 ret=1;
hakusan270 173:1198e6c31b4a 265 }
hakusan270 173:1198e6c31b4a 266 dbgmsgf=1;
hakusan270 173:1198e6c31b4a 267 return(ret);
hakusan270 173:1198e6c31b4a 268 }
hakusan270 173:1198e6c31b4a 269
hakusan270 173:1198e6c31b4a 270
hakusan270 173:1198e6c31b4a 271 int getTimeFromGPS(char *buf, int rtcset){
hakusan270 173:1198e6c31b4a 272 time_t gpstime;
hakusan270 173:1198e6c31b4a 273 time_t tim;
hakusan270 173:1198e6c31b4a 274 int ret;
hakusan270 173:1198e6c31b4a 275 struct tm *now;
hakusan270 173:1198e6c31b4a 276 if ( getGPStime(&gpstime) ) {
hakusan270 173:1198e6c31b4a 277 int gpsw, gpstw;
hakusan270 173:1198e6c31b4a 278 gpsw = gpstime >> 20;
hakusan270 173:1198e6c31b4a 279 gpstw = gpstime & 0xfffff;
hakusan270 173:1198e6c31b4a 280 tim = (gpsw * 7 * 24 * 60 * 60) + gpstw;
hakusan270 173:1198e6c31b4a 281 tim += (10*365*24*60*60 + 24*60*60*7 + 9*60*60);//Tokyo time
hakusan270 173:1198e6c31b4a 282 now = localtime(&tim);
hakusan270 173:1198e6c31b4a 283 if (rtcset) rtc_write(tim);
hakusan270 173:1198e6c31b4a 284
hakusan270 173:1198e6c31b4a 285 ret = strftime(buf, 23,"%Y %m/%d %T",now);
hakusan270 173:1198e6c31b4a 286 return(ret);
hakusan270 173:1198e6c31b4a 287 }
hakusan270 173:1198e6c31b4a 288 return -1;
hakusan270 173:1198e6c31b4a 289 }
hakusan270 173:1198e6c31b4a 290
hakusan270 173:1198e6c31b4a 291
hakusan270 173:1198e6c31b4a 292
hakusan270 173:1198e6c31b4a 293 int getGPSposGGA(float *pLo,float *pLa, float *pHDOP, long *pElev)
hakusan270 173:1198e6c31b4a 294 {
hakusan270 173:1198e6c31b4a 295 int timeout=10;
hakusan270 173:1198e6c31b4a 296 int i,j;
hakusan270 173:1198e6c31b4a 297 dbgmsgf=0;
hakusan270 173:1198e6c31b4a 298 int ret=0;
hakusan270 173:1198e6c31b4a 299 int sum=0;
hakusan270 173:1198e6c31b4a 300 memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
hakusan270 173:1198e6c31b4a 301 SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0001");
hakusan270 173:1198e6c31b4a 302 while(timeout--) {
hakusan270 173:1198e6c31b4a 303 wait_ms(500);
hakusan270 173:1198e6c31b4a 304 if (strstr((char*)rxCmdStatus,"$GPGGA") ) break;
hakusan270 173:1198e6c31b4a 305 }
hakusan270 173:1198e6c31b4a 306 wait_ms(1);
hakusan270 173:1198e6c31b4a 307 if (timeout) {
hakusan270 173:1198e6c31b4a 308 for(i=0,j=0;i<128;i++) {
hakusan270 173:1198e6c31b4a 309 if (rxCmdStatus[i]== ',' ) {
hakusan270 173:1198e6c31b4a 310 j++;
hakusan270 173:1198e6c31b4a 311 if (j==2 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pLo );
hakusan270 173:1198e6c31b4a 312 else if (j==4 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pLa );
hakusan270 173:1198e6c31b4a 313 else if (j==8 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pHDOP );
hakusan270 173:1198e6c31b4a 314 else if (j==9 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%d",pElev );
hakusan270 173:1198e6c31b4a 315 ret=1;
hakusan270 173:1198e6c31b4a 316 } else if (rxCmdStatus[i]== '*' ) {
hakusan270 173:1198e6c31b4a 317 sscanf((char*)rxCmdStatus+i+1, "%x",&sum );
hakusan270 173:1198e6c31b4a 318 int x;
hakusan270 173:1198e6c31b4a 319 uint8_t s = 0;
hakusan270 173:1198e6c31b4a 320 for(x=3;rxCmdStatus[x]!='*';x++) s ^= rxCmdStatus[x];
hakusan270 173:1198e6c31b4a 321 s &= 0xff;
hakusan270 173:1198e6c31b4a 322 // pc.printf("%s %02x, %02x %d\r\n",rxCmdStatus,sum,s & 0xff,sum);
hakusan270 173:1198e6c31b4a 323 if (s != sum){
hakusan270 173:1198e6c31b4a 324 ret = -1;// check sum err
hakusan270 173:1198e6c31b4a 325 pc.printf("sum-err %s %02x, %02x \r\n",rxCmdStatus,sum,s & 0xff);
hakusan270 173:1198e6c31b4a 326 }
hakusan270 173:1198e6c31b4a 327 }
hakusan270 173:1198e6c31b4a 328 }
hakusan270 173:1198e6c31b4a 329
hakusan270 173:1198e6c31b4a 330 SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0000");
hakusan270 173:1198e6c31b4a 331 }
hakusan270 173:1198e6c31b4a 332 dbgmsgf=1;
hakusan270 173:1198e6c31b4a 333 return(ret);
hakusan270 173:1198e6c31b4a 334 }
hakusan270 173:1198e6c31b4a 335
hakusan270 173:1198e6c31b4a 336
hakusan270 173:1198e6c31b4a 337 int getGPSGSA(int *pmode,float *pPDOP)
hakusan270 173:1198e6c31b4a 338 {
hakusan270 173:1198e6c31b4a 339 int timeout=10;
hakusan270 173:1198e6c31b4a 340 int i,j;
hakusan270 173:1198e6c31b4a 341 dbgmsgf=0;
hakusan270 173:1198e6c31b4a 342 int ret=0;
hakusan270 173:1198e6c31b4a 343 int sum=0;
hakusan270 173:1198e6c31b4a 344 memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
hakusan270 173:1198e6c31b4a 345 SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0004");
hakusan270 173:1198e6c31b4a 346 while(timeout--) {
hakusan270 173:1198e6c31b4a 347 wait_ms(500);
hakusan270 173:1198e6c31b4a 348 if (strstr((char*)rxCmdStatus,"GSA") ) break;
hakusan270 173:1198e6c31b4a 349 }
hakusan270 173:1198e6c31b4a 350 wait_ms(1);
hakusan270 173:1198e6c31b4a 351 if (timeout) {
hakusan270 173:1198e6c31b4a 352 for(i=0,j=0;i<128;i++) {
hakusan270 173:1198e6c31b4a 353 if (rxCmdStatus[i]== ',' ) {
hakusan270 173:1198e6c31b4a 354 j++;
hakusan270 173:1198e6c31b4a 355 if (j==2 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%d",pmode );
hakusan270 173:1198e6c31b4a 356 else if (j==16 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pPDOP );
hakusan270 173:1198e6c31b4a 357 ret=1;
hakusan270 173:1198e6c31b4a 358 } else if (rxCmdStatus[i]== '*' ) {
hakusan270 173:1198e6c31b4a 359 sscanf((char*)rxCmdStatus+i+1, "%x",&sum );
hakusan270 173:1198e6c31b4a 360 int x;
hakusan270 173:1198e6c31b4a 361 uint8_t s = 0;
hakusan270 173:1198e6c31b4a 362 for(x=3;rxCmdStatus[x]!='*';x++) s ^= rxCmdStatus[x];
hakusan270 173:1198e6c31b4a 363 s &= 0xff;
hakusan270 173:1198e6c31b4a 364 // pc.printf("%s %02x, %02x %d\r\n",rxCmdStatus,sum,s & 0xff,sum);
hakusan270 173:1198e6c31b4a 365 if (s != sum){
hakusan270 173:1198e6c31b4a 366 ret = -1;// check sum err
hakusan270 173:1198e6c31b4a 367 pc.printf("sum-err %s %02x, %02x \r\n",rxCmdStatus,sum,s & 0xff);
hakusan270 173:1198e6c31b4a 368 }
hakusan270 173:1198e6c31b4a 369 }
hakusan270 173:1198e6c31b4a 370 }
hakusan270 173:1198e6c31b4a 371
hakusan270 173:1198e6c31b4a 372 SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0000");
hakusan270 173:1198e6c31b4a 373 }
hakusan270 173:1198e6c31b4a 374 dbgmsgf=1;
hakusan270 173:1198e6c31b4a 375 return(ret);
hakusan270 173:1198e6c31b4a 376 }
hakusan270 173:1198e6c31b4a 377
hakusan270 173:1198e6c31b4a 378
hakusan270 173:1198e6c31b4a 379
hakusan270 173:1198e6c31b4a 380 int getGPSRMC(char *ptime,char *pdate,float *pLo,float *pLa)
hakusan270 173:1198e6c31b4a 381 {
hakusan270 173:1198e6c31b4a 382 int timeout=10;
hakusan270 173:1198e6c31b4a 383 int i,j;
hakusan270 173:1198e6c31b4a 384 dbgmsgf=0;
hakusan270 173:1198e6c31b4a 385 int ret=0;
hakusan270 173:1198e6c31b4a 386 int sum=0;
hakusan270 173:1198e6c31b4a 387 memset((char*)rxCmdStatus,0,sizeof(rxCmdStatus));
hakusan270 173:1198e6c31b4a 388 SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0020");
hakusan270 173:1198e6c31b4a 389 while(timeout--) {
hakusan270 173:1198e6c31b4a 390 wait_ms(500);
hakusan270 173:1198e6c31b4a 391 if (strstr((char*)rxCmdStatus,"RMC") ) break;
hakusan270 173:1198e6c31b4a 392 }
hakusan270 173:1198e6c31b4a 393 wait_ms(1);
hakusan270 173:1198e6c31b4a 394 if (timeout) {
hakusan270 173:1198e6c31b4a 395 for(i=0,j=0;i<128;i++) {
hakusan270 173:1198e6c31b4a 396 if (rxCmdStatus[i]== ',' ) {
hakusan270 173:1198e6c31b4a 397 j++;
hakusan270 173:1198e6c31b4a 398 if (j==1) memcpy(ptime,(char*)rxCmdStatus+i+1,9 );
hakusan270 173:1198e6c31b4a 399 else if (j==9) memcpy(pdate,(char*)rxCmdStatus+i+1,6 );
hakusan270 173:1198e6c31b4a 400 else if (j==3 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pLo );
hakusan270 173:1198e6c31b4a 401 else if (j==5 && rxCmdStatus[i+2]!=',') sscanf((char*)rxCmdStatus+i+1, "%f",pLa );
hakusan270 173:1198e6c31b4a 402 ret=1;
hakusan270 173:1198e6c31b4a 403 } else if (rxCmdStatus[i]== '*' ) {
hakusan270 173:1198e6c31b4a 404 sscanf((char*)rxCmdStatus+i+1, "%x",&sum );
hakusan270 173:1198e6c31b4a 405 int x;
hakusan270 173:1198e6c31b4a 406 uint8_t s = 0;
hakusan270 173:1198e6c31b4a 407 for(x=3;rxCmdStatus[x]!='*';x++) s ^= rxCmdStatus[x];
hakusan270 173:1198e6c31b4a 408 s &= 0xff;
hakusan270 173:1198e6c31b4a 409 // pc.printf("%s %02x, %02x %d\r\n",rxCmdStatus,sum,s & 0xff,sum);
hakusan270 173:1198e6c31b4a 410 if (s != sum){
hakusan270 173:1198e6c31b4a 411 ret = -1;// check sum err
hakusan270 173:1198e6c31b4a 412 pc.printf("sum-err %s %02x, %02x \r\n",rxCmdStatus,sum,s & 0xff);
hakusan270 173:1198e6c31b4a 413 }
hakusan270 173:1198e6c31b4a 414 }
hakusan270 173:1198e6c31b4a 415 }
hakusan270 173:1198e6c31b4a 416
hakusan270 173:1198e6c31b4a 417 SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0000");
hakusan270 173:1198e6c31b4a 418 }
hakusan270 173:1198e6c31b4a 419 dbgmsgf=1;
hakusan270 173:1198e6c31b4a 420 return(ret);
hakusan270 173:1198e6c31b4a 421 }
hakusan270 173:1198e6c31b4a 422
hakusan270 173:1198e6c31b4a 423
hakusan270 173:1198e6c31b4a 424
hakusan270 170:e086a9b46bd7 425 int main()
hakusan270 170:e086a9b46bd7 426 {
hakusan270 170:e086a9b46bd7 427 int i=0;
hakusan270 170:e086a9b46bd7 428 short x,y,z;
hakusan270 173:1198e6c31b4a 429 char c;
hakusan270 173:1198e6c31b4a 430 int ret;
hakusan270 173:1198e6c31b4a 431 int rtcenb=0;
hakusan270 173:1198e6c31b4a 432 rtc_init();
hakusan270 173:1198e6c31b4a 433 printf("\r\n**** Hello UD-GS1 by mbed ******\r\n");
hakusan270 173:1198e6c31b4a 434
hakusan270 173:1198e6c31b4a 435 long llo = convertLoInt(136.599999f);
hakusan270 173:1198e6c31b4a 436 float flo = convertLoFloat(llo);
hakusan270 173:1198e6c31b4a 437 long lla = convertLaInt(36.599999f);
hakusan270 173:1198e6c31b4a 438 float fla = convertLaFloat(lla);
hakusan270 173:1198e6c31b4a 439 pc.printf("lo=%f %08x la=%f %08x\r\n", flo,llo,fla,lla);
hakusan270 173:1198e6c31b4a 440 llo = convertLoInt(146.0f);
hakusan270 173:1198e6c31b4a 441 flo = convertLoFloat(llo);
hakusan270 173:1198e6c31b4a 442 lla = convertLaInt(46.0f);
hakusan270 173:1198e6c31b4a 443 fla = convertLaFloat(lla);
hakusan270 173:1198e6c31b4a 444 pc.printf("lo=%f %08x la=%f %08x\r\n", flo,llo,fla,lla);
hakusan270 173:1198e6c31b4a 445
hakusan270 173:1198e6c31b4a 446
hakusan270 173:1198e6c31b4a 447 if (wd.WatchdogCausedReset())
hakusan270 173:1198e6c31b4a 448 pc.printf("Watchdog caused reset.\r\n");
hakusan270 173:1198e6c31b4a 449 wd.Configure(30);
hakusan270 173:1198e6c31b4a 450 wd.Service();
hakusan270 172:3aa6f1366c5d 451 // enabl.mode(PullNone);
hakusan270 172:3aa6f1366c5d 452 // wkup.mode(PullNone);
hakusan270 172:3aa6f1366c5d 453 // v20v.mode(PullNone);
hakusan270 172:3aa6f1366c5d 454 // ext7.mode(PullNone);
hakusan270 173:1198e6c31b4a 455 dbgmsgf=1;
hakusan270 172:3aa6f1366c5d 456 enabl = 0;
hakusan270 173:1198e6c31b4a 457 wkup = 1; // Normal mode
hakusan270 173:1198e6c31b4a 458 v20v = 1; //2V off
hakusan270 173:1198e6c31b4a 459 wait_ms(100);
hakusan270 173:1198e6c31b4a 460 enabl = 1; // enable
hakusan270 173:1198e6c31b4a 461 v20v = 0; //2V ON
hakusan270 173:1198e6c31b4a 462 while(pclpwa.readable()) c = pclpwa.getc();
hakusan270 172:3aa6f1366c5d 463 pclpwa.attach(rx_Irq,Serial::RxIrq);
hakusan270 173:1198e6c31b4a 464 pclpwa.set_flow_control(Serial::RTSCTS,PB_1,PB_13);
hakusan270 173:1198e6c31b4a 465 wait_ms(500); // 4sec
hakusan270 173:1198e6c31b4a 466 printf("\r\n**** LPWA ON ******\r\n");
hakusan270 173:1198e6c31b4a 467
hakusan270 173:1198e6c31b4a 468 event.rise (&intpin1);
hakusan270 173:1198e6c31b4a 469 event.enable_irq();
hakusan270 172:3aa6f1366c5d 470 pc.printf("Adc=%d INT1=%d INT2=%d ",(int)(batlev.read()*100.0f) , intout1.read(),intout2.read());
hakusan270 170:e086a9b46bd7 471 initLIS3DH();
hakusan270 170:e086a9b46bd7 472 read3axes(&x,&y,&z);
hakusan270 173:1198e6c31b4a 473 printf("3D acc x=%d y=%d z=%d Temp=%d\r\n",x,y,z,readTemp(NULL));
hakusan270 172:3aa6f1366c5d 474 // interrput.attach(&intTimer, 0.001);
hakusan270 172:3aa6f1366c5d 475 #if 0
hakusan270 172:3aa6f1366c5d 476 SendLPWAcmd("< SYS MODE SET 05");
hakusan270 172:3aa6f1366c5d 477 SendLPWAcmd("< SYS EEPROM GET 0x0410");
hakusan270 172:3aa6f1366c5d 478 SendLPWAcmd("< SYS EEPROM GET 0x0414");
hakusan270 172:3aa6f1366c5d 479 wait(0.1); // 100 ms
hakusan270 172:3aa6f1366c5d 480 SendLPWAcmd("< SYS EEPROM SET 0x0410,0x00000001");
hakusan270 172:3aa6f1366c5d 481 SendLPWAcmd("< SYS EEPROM SET 0x0414,0x00000010");
hakusan270 172:3aa6f1366c5d 482 wait(0.1); // 100 ms
hakusan270 172:3aa6f1366c5d 483 SendLPWAcmd("< SYS EEPROM GET 0x0410");
hakusan270 172:3aa6f1366c5d 484 SendLPWAcmd("< SYS EEPROM GET 0x0414");
hakusan270 172:3aa6f1366c5d 485 while(1);
hakusan270 172:3aa6f1366c5d 486 #endif
hakusan270 173:1198e6c31b4a 487 dbgmsgf=0;
hakusan270 173:1198e6c31b4a 488 ret = SendLPWAcmd("< SYS MODE SET 00");
hakusan270 173:1198e6c31b4a 489 if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
hakusan270 172:3aa6f1366c5d 490 // SendLPWAcmd("< SYS MODE SET_EVT ON");
hakusan270 173:1198e6c31b4a 491 ret = SendLPWAcmd("< SYS VER GET");
hakusan270 173:1198e6c31b4a 492 if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
hakusan270 173:1198e6c31b4a 493 // wait_ms(100);
hakusan270 173:1198e6c31b4a 494 ret = SendLPWAcmd("< GNSS VER GET");
hakusan270 173:1198e6c31b4a 495 if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
hakusan270 173:1198e6c31b4a 496 // wait_ms(100);
hakusan270 173:1198e6c31b4a 497 ret = SendLPWAcmd("< SYS MODE SET 00");
hakusan270 173:1198e6c31b4a 498 if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
hakusan270 172:3aa6f1366c5d 499 // SendLPWAcmd("< SYS TO_WAIT_FETCHING SET ON");
hakusan270 173:1198e6c31b4a 500 ret = SendLPWAcmd("< SYS TO_FETCHING SET ON");
hakusan270 173:1198e6c31b4a 501 if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
hakusan270 173:1198e6c31b4a 502 ret = SendLPWAcmd("< TX PLD SET 0123456789ABCDEF0123456789ABCDEF");
hakusan270 173:1198e6c31b4a 503 if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
hakusan270 172:3aa6f1366c5d 504 // SendLPWAcmd("< GNSS HOST_FW_SENT SET_EVT 0x0001");
hakusan270 173:1198e6c31b4a 505 ret = SendLPWAcmd("< TX PREV_STT SET_EVT ON");
hakusan270 173:1198e6c31b4a 506 if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
hakusan270 173:1198e6c31b4a 507 ret = SendLPWAcmd("< GNSS STT GET");
hakusan270 173:1198e6c31b4a 508 if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
hakusan270 173:1198e6c31b4a 509 char pay[16]={0x00, 0x01, 0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x10,0x20,0x30,0x40,0x50,0x60,0x70};
hakusan270 173:1198e6c31b4a 510 setPayload(pay);
hakusan270 173:1198e6c31b4a 511 if (ret==0) pc.write((const uint8_t *)rxbuffer,oldrxindex+1,NULL);
hakusan270 173:1198e6c31b4a 512 else pc.printf("Set payload error\r\n");
hakusan270 173:1198e6c31b4a 513
hakusan270 173:1198e6c31b4a 514 dbgmsgf=1;
hakusan270 173:1198e6c31b4a 515
hakusan270 173:1198e6c31b4a 516 wd.Configure(20.0f);
hakusan270 173:1198e6c31b4a 517
hakusan270 172:3aa6f1366c5d 518 while (1) {
hakusan270 173:1198e6c31b4a 519 int mode;
hakusan270 173:1198e6c31b4a 520 float lo=0.0f,la=0.0f,HDOP;
hakusan270 173:1198e6c31b4a 521 long elev;
hakusan270 173:1198e6c31b4a 522 wd.Service();
hakusan270 173:1198e6c31b4a 523
hakusan270 173:1198e6c31b4a 524
hakusan270 172:3aa6f1366c5d 525 // printf(" cnt=%d stat= ",i++);
hakusan270 173:1198e6c31b4a 526 //SendLPWAcmd("< GNSS STT GET");
hakusan270 173:1198e6c31b4a 527 mode = getGNSSmode();
hakusan270 173:1198e6c31b4a 528 if (mode & 2 && rtcenb==0 ) {
hakusan270 173:1198e6c31b4a 529 char st[32];
hakusan270 173:1198e6c31b4a 530 if (getTimeFromGPS(st,1)>0) {
hakusan270 173:1198e6c31b4a 531 pc.printf("%s ",st );
hakusan270 173:1198e6c31b4a 532
hakusan270 173:1198e6c31b4a 533 rtcenb=1;
hakusan270 173:1198e6c31b4a 534 }
hakusan270 173:1198e6c31b4a 535 }
hakusan270 173:1198e6c31b4a 536 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());
hakusan270 173:1198e6c31b4a 537 if (mode & 4) {
hakusan270 173:1198e6c31b4a 538 char stime[10]={0};
hakusan270 173:1198e6c31b4a 539 char sdate[8]={0};
hakusan270 173:1198e6c31b4a 540 if (getGPSposGGA( &lo , &la, &HDOP,&elev) >= 0) {
hakusan270 173:1198e6c31b4a 541 pc.printf("Lo=N %f La=E %f HDOP=%f evevation=%d\r\n", lo/100.0f,la/100.0f,HDOP,elev);
hakusan270 173:1198e6c31b4a 542 }
hakusan270 173:1198e6c31b4a 543
hakusan270 173:1198e6c31b4a 544 if ( getGPSGSA(&mode,&HDOP) >= 0)
hakusan270 173:1198e6c31b4a 545 pc.printf("GSA 2D/3D mode=%d PDOP=%f\r\n", mode,HDOP);
hakusan270 173:1198e6c31b4a 546
hakusan270 173:1198e6c31b4a 547 if (getGPSRMC(stime,sdate,&lo , &la) >=0)
hakusan270 173:1198e6c31b4a 548 pc.printf("RMC Lo=N %f La=E %f time %s date=%s\r\n", lo/100.0f,la/100.0f,stime,sdate);
hakusan270 173:1198e6c31b4a 549
hakusan270 173:1198e6c31b4a 550 }
hakusan270 173:1198e6c31b4a 551 if (mode &0x80)
hakusan270 173:1198e6c31b4a 552 pc.printf("next time=%d send=%d ",getNextTimeSet(), getNextTimeSend());
hakusan270 173:1198e6c31b4a 553 if (rtcenb)
hakusan270 173:1198e6c31b4a 554 {
hakusan270 173:1198e6c31b4a 555 time_t tim = rtc_read();
hakusan270 173:1198e6c31b4a 556 struct tm *now;
hakusan270 173:1198e6c31b4a 557 char buf[24]={0};
hakusan270 173:1198e6c31b4a 558 now = localtime(&tim);
hakusan270 173:1198e6c31b4a 559 strftime(buf, 23,"%Y %m/%d %T",now);
hakusan270 173:1198e6c31b4a 560 pc.printf("RTC time=%s\n\r",buf);
hakusan270 173:1198e6c31b4a 561 }
hakusan270 173:1198e6c31b4a 562 pc.printf("Temp =%d ",readTemp(NULL));
hakusan270 173:1198e6c31b4a 563
hakusan270 173:1198e6c31b4a 564
hakusan270 173:1198e6c31b4a 565
hakusan270 172:3aa6f1366c5d 566 wait_ms(4000);
hakusan270 170:e086a9b46bd7 567 // pc.write((const uint8_t *)"ACBD\r\n",6,NULL);
hakusan270 170:e086a9b46bd7 568 // pc.puts("ASDFGREWA\r\n");
hakusan270 171:42f0cccc956f 569 //printf("PC12345ABCD cnt=%d\n\r",i++);
hakusan270 171:42f0cccc956f 570 // pclpwa.printf("< SYS VER GET\r\n");
hakusan270 171:42f0cccc956f 571 // read3axes(&x,&y,&z);
hakusan270 171:42f0cccc956f 572 // printf("3D acc x=%d y=%d z=%d\r\n",x,y,z);
hakusan270 170:e086a9b46bd7 573 }
hakusan270 170:e086a9b46bd7 574 }
hakusan270 170:e086a9b46bd7 575
hakusan270 170:e086a9b46bd7 576
hakusan270 170:e086a9b46bd7 577
hakusan270 168:bccad2df0b45 578
hakusan270 168:bccad2df0b45 579
hakusan270 170:e086a9b46bd7 580 /*************************************************
hakusan270 170:e086a9b46bd7 581 Read a regster from SPI
hakusan270 170:e086a9b46bd7 582 *************************************************/
hakusan270 170:e086a9b46bd7 583 unsigned char spireadReg(unsigned char ad){
hakusan270 170:e086a9b46bd7 584 unsigned char r;
hakusan270 170:e086a9b46bd7 585 STSPICS = 0;
hakusan270 170:e086a9b46bd7 586 wait_us(1);
hakusan270 170:e086a9b46bd7 587 STSPI.write(ad | 0x80);
hakusan270 170:e086a9b46bd7 588 r = STSPI.write(0);
hakusan270 170:e086a9b46bd7 589 wait_us(1);
hakusan270 170:e086a9b46bd7 590 STSPICS = 1;
hakusan270 170:e086a9b46bd7 591 return(r);
hakusan270 170:e086a9b46bd7 592 }
hakusan270 170:e086a9b46bd7 593
hakusan270 170:e086a9b46bd7 594 /*************************************************
hakusan270 170:e086a9b46bd7 595 Read multi regsters from SPI
hakusan270 170:e086a9b46bd7 596 *************************************************/
hakusan270 170:e086a9b46bd7 597 unsigned char spireadRegs(unsigned char ad,unsigned char *p,int bytes){
hakusan270 173:1198e6c31b4a 598 #if 0
hakusan270 170:e086a9b46bd7 599 while(bytes--) {
hakusan270 170:e086a9b46bd7 600 *p = spireadReg( ad );
hakusan270 170:e086a9b46bd7 601 p++;
hakusan270 170:e086a9b46bd7 602 ad++;
hakusan270 170:e086a9b46bd7 603 }
hakusan270 170:e086a9b46bd7 604 #else
hakusan270 170:e086a9b46bd7 605 STSPICS=0;
hakusan270 170:e086a9b46bd7 606 wait_us(1);
hakusan270 172:3aa6f1366c5d 607 STSPI.write(ad | 0x80 );
hakusan270 170:e086a9b46bd7 608 while(bytes--) {
hakusan270 170:e086a9b46bd7 609 *p++ = STSPI.write(0);
hakusan270 170:e086a9b46bd7 610 }
hakusan270 170:e086a9b46bd7 611 wait_us(1);
hakusan270 170:e086a9b46bd7 612 STSPICS=1;
hakusan270 170:e086a9b46bd7 613 #endif
hakusan270 170:e086a9b46bd7 614 return(0);
hakusan270 170:e086a9b46bd7 615 }
hakusan270 170:e086a9b46bd7 616
hakusan270 170:e086a9b46bd7 617 /*************************************************
hakusan270 170:e086a9b46bd7 618 Write a regster via SPI
hakusan270 170:e086a9b46bd7 619 *************************************************/
hakusan270 170:e086a9b46bd7 620 unsigned char spiwriteReg(unsigned char ad,unsigned char val){
hakusan270 170:e086a9b46bd7 621 unsigned char r;
hakusan270 170:e086a9b46bd7 622 STSPICS=0;
hakusan270 170:e086a9b46bd7 623 wait_us(1);
hakusan270 170:e086a9b46bd7 624 STSPI.write(ad);
hakusan270 170:e086a9b46bd7 625 r = STSPI.write(val);
hakusan270 170:e086a9b46bd7 626 wait_us(1);
hakusan270 170:e086a9b46bd7 627 STSPICS=1;
hakusan270 170:e086a9b46bd7 628 return(r);
hakusan270 170:e086a9b46bd7 629 }
hakusan270 170:e086a9b46bd7 630
hakusan270 170:e086a9b46bd7 631
hakusan270 173:1198e6c31b4a 632 /****************************************
hakusan270 173:1198e6c31b4a 633 half float 16bit浮動小数点からfloatに変換
hakusan270 173:1198e6c31b4a 634 SEEEEE TT TTTT TTTT --> SEEEEEEEE E TTT TTTT TTTT TTTT TTTT
hakusan270 173:1198e6c31b4a 635 signed ext
hakusan270 173:1198e6c31b4a 636 float 指数部 bias 127
hakusan270 173:1198e6c31b4a 637 half 指数部 bias 15
hakusan270 173:1198e6c31b4a 638 *****************************************/
hakusan270 173:1198e6c31b4a 639 float UDGS1_16decodeFloat(unsigned short v){
hakusan270 173:1198e6c31b4a 640 volatile float r;
hakusan270 173:1198e6c31b4a 641 int sign = 1;
hakusan270 173:1198e6c31b4a 642 if (v & 0x8000) { sign = -1; v&= 0x7fff;}
hakusan270 173:1198e6c31b4a 643
hakusan270 173:1198e6c31b4a 644 if(v==0) return(0.0f);
hakusan270 173:1198e6c31b4a 645 if((v & 0x7c00) == 0 )
hakusan270 173:1198e6c31b4a 646 *(unsigned long *)&r = ((v & 0x3ff) << (23-10));
hakusan270 173:1198e6c31b4a 647 else
hakusan270 173:1198e6c31b4a 648 *(unsigned long *)&r = ((v & 0x3ff) << (23-10)) | (((v>>10) + (127-15)) << 23) ;
hakusan270 173:1198e6c31b4a 649 return(r * sign);
hakusan270 173:1198e6c31b4a 650 }
hakusan270 173:1198e6c31b4a 651
hakusan270 173:1198e6c31b4a 652 /****************************************
hakusan270 173:1198e6c31b4a 653 float から half 16bit浮動小数点に変換
hakusan270 173:1198e6c31b4a 654 SEEEEE TT TTTT TTTT <-- SEEEEEEEE E TTT TTTT TTTT TTTT TTTT
hakusan270 173:1198e6c31b4a 655 float 指数部 bias 127
hakusan270 173:1198e6c31b4a 656 half 指数部 bias 15
hakusan270 173:1198e6c31b4a 657 *****************************************/
hakusan270 173:1198e6c31b4a 658 unsigned short UDGS1_16encodeFloat(float v){
hakusan270 173:1198e6c31b4a 659 unsigned short sign = 0;
hakusan270 173:1198e6c31b4a 660 if (v < 0.0f) sign = 0x8000;
hakusan270 173:1198e6c31b4a 661
hakusan270 173:1198e6c31b4a 662 register unsigned long tf;
hakusan270 173:1198e6c31b4a 663 register unsigned short exp;
hakusan270 173:1198e6c31b4a 664 if(v <= 0.000000059f)return(0);
hakusan270 173:1198e6c31b4a 665 if(v > 131008.0f)return(0x7fff);
hakusan270 173:1198e6c31b4a 666 tf = *(unsigned long *)&v;
hakusan270 173:1198e6c31b4a 667 exp = ((tf >> 23) & 0xff ) - (127-15);
hakusan270 173:1198e6c31b4a 668 return( ((tf >> 13) & 0x3ff) | (exp << 10) | sign );
hakusan270 173:1198e6c31b4a 669 }
hakusan270 173:1198e6c31b4a 670
hakusan270 170:e086a9b46bd7 671
hakusan270 170:e086a9b46bd7 672 /****************************************
hakusan270 170:e086a9b46bd7 673 tyny 13bit浮動小数点からfloatに変換
hakusan270 170:e086a9b46bd7 674 *****************************************/
hakusan270 170:e086a9b46bd7 675 float UDGS1_decodeFloat(unsigned short v){
hakusan270 170:e086a9b46bd7 676 volatile float r;
hakusan270 170:e086a9b46bd7 677 if(v==0) return(0.0f);
hakusan270 170:e086a9b46bd7 678 *(unsigned long *)&r = ((v & 0xff) << (23-8)) | (((v>>8) + 115) << 23) ;
hakusan270 170:e086a9b46bd7 679 return(r);
hakusan270 170:e086a9b46bd7 680 }
hakusan270 170:e086a9b46bd7 681
hakusan270 170:e086a9b46bd7 682 /****************************************
hakusan270 170:e086a9b46bd7 683 float から tiny 13bit浮動小数点に変換
hakusan270 170:e086a9b46bd7 684 *****************************************/
hakusan270 170:e086a9b46bd7 685 unsigned short UDGS1_encodeFloat(float v){
hakusan270 170:e086a9b46bd7 686
hakusan270 170:e086a9b46bd7 687 register unsigned long tf;
hakusan270 170:e086a9b46bd7 688 register unsigned short exp;
hakusan270 170:e086a9b46bd7 689 if(v <= 0.0000235f)return(0);
hakusan270 170:e086a9b46bd7 690 if(v > 1046528.0f)return(0x1fff);
hakusan270 170:e086a9b46bd7 691 tf = *(unsigned long *)&v;
hakusan270 170:e086a9b46bd7 692 exp = ((tf >> 23) & 0xff ) - (127-EXP_BIAS);
hakusan270 170:e086a9b46bd7 693 return( ((tf >> 15) & 0xff) | (exp << 8) );
hakusan270 170:e086a9b46bd7 694
hakusan270 170:e086a9b46bd7 695 }
hakusan270 170:e086a9b46bd7 696
hakusan270 170:e086a9b46bd7 697 /***************************
hakusan270 170:e086a9b46bd7 698 integer sqrt
hakusan270 170:e086a9b46bd7 699 整数 sqrt
hakusan270 170:e086a9b46bd7 700 ****************************/
hakusan270 170:e086a9b46bd7 701 int int_sqrt(unsigned int x)
hakusan270 168:bccad2df0b45 702 {
hakusan270 170:e086a9b46bd7 703 register int a = 0, c = 0, y = 0, i = 0, t = x;
hakusan270 170:e086a9b46bd7 704 while(t >>= 1){
hakusan270 170:e086a9b46bd7 705 ++i;
hakusan270 170:e086a9b46bd7 706 }
hakusan270 170:e086a9b46bd7 707 for(i += i & 1; i >= 0; i -= 2){
hakusan270 170:e086a9b46bd7 708 c = (y << 1 | 1) <= x >> i;
hakusan270 170:e086a9b46bd7 709 a = a << 1 | c;
hakusan270 170:e086a9b46bd7 710 y = y << 1 | c;
hakusan270 170:e086a9b46bd7 711 x -= c * y << i;
hakusan270 170:e086a9b46bd7 712 y += c;
hakusan270 170:e086a9b46bd7 713 }
hakusan270 170:e086a9b46bd7 714 return a;
hakusan270 168:bccad2df0b45 715 }
hakusan270 170:e086a9b46bd7 716
hakusan270 170:e086a9b46bd7 717
hakusan270 170:e086a9b46bd7 718
hakusan270 170:e086a9b46bd7 719 /*****************************************
hakusan270 170:e086a9b46bd7 720 init LIS3DH ST micro 3-axes accelerometer
hakusan270 170:e086a9b46bd7 721 SPI interface
hakusan270 170:e086a9b46bd7 722 *****************************************/
hakusan270 170:e086a9b46bd7 723 int initLIS3DH()
hakusan270 170:e086a9b46bd7 724 {
hakusan270 170:e086a9b46bd7 725 // STSPI = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK, D6);
hakusan270 170:e086a9b46bd7 726
hakusan270 170:e086a9b46bd7 727 STSPI.format(8,3);
hakusan270 170:e086a9b46bd7 728 STSPI.frequency(1000000);
hakusan270 170:e086a9b46bd7 729 STSPICS=1;
hakusan270 170:e086a9b46bd7 730 wait_us(1);
hakusan270 170:e086a9b46bd7 731 printf("Who am I %02x\r\n", spireadReg(0x0f));
hakusan270 170:e086a9b46bd7 732 spiwriteReg(0x20,0x27);
hakusan270 170:e086a9b46bd7 733 spiwriteReg(0x23,0x98);
hakusan270 170:e086a9b46bd7 734 return (spireadReg(0x0f)== 0x44);
hakusan270 170:e086a9b46bd7 735 }
hakusan270 170:e086a9b46bd7 736
hakusan270 170:e086a9b46bd7 737 /*************************************************
hakusan270 170:e086a9b46bd7 738 read 3-axes from LIS3DH
hakusan270 170:e086a9b46bd7 739 *************************************************/
hakusan270 170:e086a9b46bd7 740 int read3axes(short *tx,short *ty,short *tz)
hakusan270 170:e086a9b46bd7 741 {
hakusan270 170:e086a9b46bd7 742 int timeout=5;
hakusan270 170:e086a9b46bd7 743 unsigned char regs[6];
hakusan270 173:1198e6c31b4a 744
hakusan270 170:e086a9b46bd7 745 while((spireadReg(0x07)&7)==0 && timeout--){wait_us(5);};
hakusan270 170:e086a9b46bd7 746 spireadRegs(0x28,regs,6);// 12bit signed 1G = 413 9.8m/s**2
hakusan270 170:e086a9b46bd7 747 // STSPI.frequency(); //1MHz
hakusan270 170:e086a9b46bd7 748 *tx = (short)(regs[0] | (regs[1]<<8)) >> 4;
hakusan270 170:e086a9b46bd7 749 *ty = (short)(regs[2] | (regs[3]<<8)) >> 4;
hakusan270 170:e086a9b46bd7 750 *tz = (short)(regs[4] | (regs[5]<<8)) >> 4;
hakusan270 170:e086a9b46bd7 751 return(0);
hakusan270 170:e086a9b46bd7 752 }
hakusan270 170:e086a9b46bd7 753
hakusan270 173:1198e6c31b4a 754 /*************************************************
hakusan270 173:1198e6c31b4a 755 read temparature from LIS3DH
hakusan270 173:1198e6c31b4a 756 *************************************************/
hakusan270 173:1198e6c31b4a 757 int readTemp(short *tmp)
hakusan270 173:1198e6c31b4a 758 {
hakusan270 173:1198e6c31b4a 759 int t;
hakusan270 173:1198e6c31b4a 760 t = spireadReg(0x0d);
hakusan270 173:1198e6c31b4a 761 t |= spireadReg(0x0e) << 8;
hakusan270 173:1198e6c31b4a 762 t >>= 4;
hakusan270 173:1198e6c31b4a 763 // pc.printf("tmp %d %04x\r\n", t, t);
hakusan270 173:1198e6c31b4a 764 if (tmp) *tmp = t;
hakusan270 173:1198e6c31b4a 765 return(t);
hakusan270 173:1198e6c31b4a 766 }
hakusan270 173:1198e6c31b4a 767
hakusan270 170:e086a9b46bd7 768