IMU-and-GPS-Fusion-for-Inertial-Navigation/gps.py at main · heydan98/IMU-and-GPS-Fusion-for-Inertial-Navigation · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
#!/usr/bin/env python
import serial
import numpy as np
import time
# import wiringpi as wp
import RPi.GPIO as GPIO
import sys
# import mysql.connector

import string

import pynmea2
# import mariadb

# # mydb = mysql.connector.connect(
# mydb = mariadb.connect(
#   host="localhost",
#   user="admin_rasp2",
#   password="1",
#   database="admin"
# )

# mycursor = mydb.cursor()


pps=18

port="/dev/ttyS0"
ser=serial.Serial(port, baudrate=9600 ,   parity=serial.PARITY_NONE,

  stopbits=serial.STOPBITS_ONE,

  bytesize=serial.EIGHTBITS,)

ser.write(b"$PUBX,40,GLL,0,0,0,0*5D\r\n")

def PPS_interrupt(channel):
  print("PPS...")


GPIO.setmode(GPIO.BCM)
GPIO.setup(pps, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(pps, GPIO.FALLING,
        callback=PPS_interrupt, bouncetime=100)


while True:
      #port="/dev/ttyAMA0"
  # try:
    #ser=serial.Serial(port, baudrate=9600, timeout=0.5)

  try:
    dataout = pynmea2.NMEAStreamReader()
    newdata=ser.readline()
    # print(50*"-")
    # print(newdata)

    # print(newdata[0:6] == "$GPRMC")
    if (newdata[0:6] == b"$GPGGA"):
            newmsg=pynmea2.parse(newdata.decode("utf-8"))
            lat=newmsg.latitude
            lat_deg= lat*np.pi//180
            alt = float(newmsg.altitude)


            lng=newmsg.longitude
            lng_deg=lng*np.pi/180
            write_command = f"update gps set kinhdo={lng}, vido={lat}, docao={alt}, goc=80;"
            print(write_command)
            # mycursor.execute(write_command)
            # mydb.commit()
            utc_time = newmsg.timestamp
            print(utc_time)
    if (newdata[0:6] == b"$GPVTG"):
            # new = str(newdata).split(',')
            # print(new)
            newmsg=pynmea2.parse(newdata.decode("utf-8"))
            speed = newmsg.spd_over_grnd_kmph
            if speed < 1.5 : speed = 0
            speed = speed / 3.6
            write_command = f"update gps set vantoc={speed};"
            print(write_command)
            # mycursor.execute(write_command)
            # mydb.commit()
            # print(speed)
    # if (newdata[0:6] == b"$GPRMC"):
    #         newmsg=pynmea2.parse(newdata.decode("utf-8"))
    #         utc_time = newmsg.timestamp
    #         print(type(utc_time))

  except :
      port="/dev/ttyS0"
      ser=serial.Serial(port, baudrate=9600 ,   parity=serial.PARITY_NONE,

        stopbits=serial.STOPBITS_ONE,

        bytesize=serial.EIGHTBITS,)
      continue
  # except KeyboardInterrupt:
  #   print("stop")
  #   break