国产美女丝袜高潮白浆,国产精品999永久在线观看,欧美激情亚洲色图

#include <SoftwareSerial.h>
#include <MsTimer2.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
#define NUM 10
#define NUM_XYZ 6
//int16_t ax[NUM], ay[NUM], az[NUM];
//int16_t gx[NUM], gy[NUM], gz[NUM];
int16_t agxyz[NUM_XYZ][NUM];
long Sumagxyz[NUM_XYZ];
long Avgagxyz[NUM_XYZ];
bool blinkState = false;
const int BEEP = 10;
const int KEY_0 = 8;
const int LED = 9;
static unsigned char index = 0;
static unsigned char Send = 3;
static unsigned char Key_State = 0;
static unsigned char Waring = 0;
static unsigned char BEEP_State = 0;
static unsigned char Tmr20msFlag = 0;
static unsigned char gMainLoop100msCnt = 1;
static unsigned char gMainLoop500msCnt = 2;
static unsigned char gMainLoop1000msCnt = 3;
static unsigned char gSchedularType = 0;
static int lastKEY0 = 0;
char nmeaSentence[68];
String last_latitude = "";
String last_longitude = "";
String Set_latitude = "";
String Set_longitude = "";
String g_latitude = "";
String g_longitude = "";
SoftwareSerial mySerial(2, 3); // RX, TX
void flash()
{
Tmr20msFlag = 1;
//Serial.println("20ms");
}
void Task_20ms(void)
{
KeyCheck();
if(Key_State)
{
GetData();
}
}
void Task_100ms(void)
{
if(Key_State)
{
if( (Check()) && ( Waring == 0))
{
Waring = 1;
Send = 0;
}
}
else
{
Waring = 0;
}
if( Waring == 0 )
{
BEEP_State = HIGH;
}
else
{
BEEP_State = !BEEP_State;
}
digitalWrite(BEEP,BEEP_State); //蜂鳴器響
//Serial.println("100ms");
}
void Task_500ms(void)
{
if(Waring == 1)
{
if( Send == 0 )
{
SendGPS0();
Send = 1;
}
else
{
SendGPS1();
Send = 0;
}
}
else
{
//Waring==0;
}
//Serial.println("500ms);
}
void Task_1000ms(void)
{
//Serial.println("1000ms");
}
void GetData(void)
{
int16_t lax, lay, laz;
int16_t lgx, lgy, lgz;
accelgyro.getMotion6(&lax, &lay, &laz, &lgx, &lgy, &lgz);
agxyz[0][index] = lax; agxyz[1][index] = lay, agxyz[2][index] = laz;
agxyz[3][index] = lgx, agxyz[4][index] = lgy, agxyz[5][index] = lgz;
if( index > NUM ) index = 0;
}
bool Check(void)
{
bool reval = 0;
unsigned char i= 0;
unsigned char j= 0;
for( i = 0;i < NUM_XYZ; i++ )
{
Sumagxyz[i] = agxyz[i][0]+agxyz[i][1]+agxyz[i][2]+agxyz[i][3]+agxyz[i][4]+agxyz[i][5]+agxyz[i][6]+agxyz[i][7]+agxyz[i][8]+agxyz[i][9];
Avgagxyz[i] = Sumagxyz[i]/NUM;
Sumagxyz[i] = 0;
for( j = 0;j < NUM; j++ )
{
Sumagxyz[i] = Sumagxyz[i] + (agxyz[i][j] -Avgagxyz[i])*(agxyz[i][j] -Avgagxyz[i]);
}
Avgagxyz[i]= Sumagxyz[i]/NUM;
Avgagxyz[i]= Avgagxyz[i]/10000;;
}
if((Avgagxyz[0]>100)&&(Avgagxyz[1]>100)&&(Avgagxyz[2]>100))
{
reval = 1;
}
return reval;
}
void Task_IDLE(void)
{
KeyCheck();
if(Key_State)
{
GetGPS();
}
else
{
}
//Serial.println("IDLE");
}
void KeyCheck(void)
{
int lKEY0 = digitalRead(KEY_0);
if(( lKEY0 != lastKEY0) && (lKEY0 == 1 ))
{
Key_State = !Key_State;
if(Key_State)
{
digitalWrite(LED,LOW);
Serial.println("Lock");
}
else
{
digitalWrite(LED,HIGH);
Serial.println("UnLock");
}
}
lastKEY0 = lKEY0;
}
String parseGprmcLat(String s)
{
int pLoc = 0; int lEndLoc = 0; int dEndLoc = 0; String lat;
if(s.substring(0,4) == "GPRM")
{
for(int i = 0; i < 5; i++)
{
if(i < 3) { pLoc = s.indexOf(',', pLoc+1); }
if(i == 3) { lEndLoc = s.indexOf(',', pLoc+1); lat = s.substring(pLoc+1, lEndLoc); }
else { dEndLoc = s.indexOf(',', lEndLoc+1); lat = lat + " " + s.substring(lEndLoc+1, dEndLoc); }
}
return lat;
}
}
String parseGprmcLon(String s)
{
int pLoc = 0; int lEndLoc = 0; int dEndLoc = 0; String lon;
if(s.substring(0,4) == "GPRM")
{
for(int i = 0; i < 7; i++)
{
if(i < 5) { pLoc = s.indexOf(',', pLoc+1); }
if(i == 5) { lEndLoc = s.indexOf(',', pLoc+1); lon = s.substring(pLoc+1, lEndLoc); }
else { dEndLoc = s.indexOf(',', lEndLoc+1); lon = lon + " " + s.substring(lEndLoc+1, dEndLoc); }
}
return lon;
}
}
String DataChange(String s)
{
int i=0; int pLoc = 0; int lEndLoc = 0;
String DD=""; String FF=""; String NE="";
String FFo=""; char FF0[20]="";
float n=0.0;
String Output="";
pLoc = s.indexOf('.',0);
lEndLoc = s.indexOf(' ',0);
DD = s.substring(0, pLoc-2);
FF = s.substring(pLoc-2,lEndLoc);
NE = s.substring(lEndLoc+1,lEndLoc+2);
n = FF.toFloat()/60; dtostrf(n,0,10,FF0);
for(i=1;i<10;i++) { FFo += FF0[i]; }
Output = NE+": "+DD+FFo;
return Output;
}
void GetGPS(void)
{
String latitude = ""; //緯度
String longitude = ""; //經度
while (mySerial.available())
{
char c = mySerial.read();
switch(c)
{
case '
mySerial.readBytesUntil('*', nmeaSentence, 67);
latitude = parseGprmcLat(nmeaSentence);
longitude = parseGprmcLon(nmeaSentence);
if(latitude > "") {
//Serial.println("latitude: " + latitude);
//g_latitude = latitude;
g_latitude = DataChange(latitude);
}
if(longitude> "") {
//Serial.println("longitude: " + longitude);
//g_longitude = longitude;
g_longitude = DataChange(longitude);
}
break;
default:
break;
}
}
}
void SendGPS0()
{
if( ( g_latitude != last_latitude ) && ( g_latitude > "" ) )
{
Serial.println("latitude: " + g_latitude );
}
last_latitude = g_latitude;
}
void SendGPS1()
{
if(( g_longitude != last_longitude ) && ( g_longitude > "" ))
{
Serial.println("longitude: " + g_longitude);
}
last_longitude = g_longitude;
}

復制代碼

欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136