SIM7600CE位置跟踪动态更新

243 0

前置条件如下:

  • 树莓派4B与SIM7600CE完成物理连接
  • SIM7600CE和GNSS/GPS模块完成物理连接
  • 以下代码在树莓派4B上运行
#!/usr/bin/python
# -*- coding:utf-8 -*-
#sudo make clean && sudo make && sudo ./
# 使用wg84地球坐标系
import base64
import json
import os

import RPi.GPIO as GPIO
import requests
import serial
import time
import math
from decimal import *

ser = serial.Serial('/dev/ttyS0',115200)
ser.flushInput()

power_key = 6
rec_buff = ''
# 临时经度、临时纬度
lng_temp=''
lat_temp=''
# 初始化标志
init_gps=1

# 地球曲率
EARTH_REDIUS = 6378.137

class transfer:
    def __init__(self,key=None):
        self.a=6378245.0
        self.ee=Decimal(0.00669342162296594323)

    def transformLng(self,x,y):
        ret=Decimal()
        ret = 300.0+x+2.0*y+0.1*x*x+0.1*x*y+0.1*math.sqrt(math.fabs(x))
        ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
        ret += (20.0 * math.sin(x * math.pi) + 40.0 * math.sin(x / 3.0 * math.pi)) * 2.0 / 3.0
        ret += (150.0 * math.sin(x / 12.0 * math.pi) + 300.0 * math.sin(x / 30.0* math.pi)) * 2.0 / 3.0
        return ret

    def transformLat(self,x,y):
        ret = Decimal()
        ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y+ 0.2 * math.sqrt(math.fabs(x))
        ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
        ret += (20.0 * math.sin(y * math.pi) + 40.0 * math.sin(y / 3.0 * math.pi)) * 2.0 / 3.0
        ret += (160.0 * math.sin(y / 12.0 * math.pi) + 320 * math.sin(y * math.pi / 30.0)) * 2.0 / 3.0
        return ret

    def transfrom(self,lng,lat):
        dLat = self.transformLat(lng - 105.0, lat - 35.0)
        dLng = self.transformLng(lng - 105.0, lat - 35.0)
        radLat = lat / 180.0 * math.pi
        magic = math.sin(radLat)
        magic = 1 - self.ee * Decimal(magic) * Decimal(magic)
        sqrtMagic = math.sqrt(magic)
        dLat = Decimal((dLat * 180.0)) / ((Decimal(self.a) * (1 - self.ee)) / (Decimal(magic) * Decimal(sqrtMagic)) * Decimal(math.pi))
        dLng = (dLng * 180.0) / (self.a / sqrtMagic * math.cos(radLat) * math.pi)
        mgLat = lat + float(dLat)
        mgLng = lng + dLng
        return mgLng,mgLat


    #gps坐标转换为gcj02坐标系
    def wg84_to_gcj02(self,wg84_lng,wg84_lat):
        dLat=self.transformLat(wg84_lng-105.0,wg84_lat-35.0)
        dLng=self.transformLng(wg84_lng-105.0,wg84_lat-35.0)
        radLat = wg84_lat / 180.0 * math.pi
        magic = math.sin(radLat)
        magic = 1 - self.ee * Decimal(magic) * Decimal(magic)
        sqrtMagic = math.sqrt(magic)
        dLat = Decimal((dLat * 180.0)) / ((Decimal(self.a) * (1 - self.ee)) / (Decimal(magic) * Decimal(sqrtMagic)) * Decimal(math.pi))
        dLng = (dLng * 180.0) / (self.a / sqrtMagic * math.cos(radLat) * math.pi)
        gcj02Lat = wg84_lat + float(dLat)
        gcj02Lng = wg84_lng + dLng
        return gcj02Lng,gcj02Lat



# 角度转rad
def rad(d):
    return d * math.pi / 180.0

# 计算两个GPS坐标间距
def getDistance(lat1, lng1, lat2, lng2):
    radLat1 = rad(lat1)
    radLat2 = rad(lat2)
    a = radLat1 - radLat2
    b = rad(lng1) - rad(lng2)
    s = 2 * math.asin(math.sqrt(math.pow(math.sin(a/2), 2) + math.cos(radLat1) * math.cos(radLat2) * math.pow(math.sin(b/2), 2)))
    s = s * EARTH_REDIUS
    return s

# 对图片byte数组进行str解码
def getByte(path):
    with open(path, 'rb') as f:
        img_byte = base64.b64encode(f.read())
    img_str = img_byte.decode('ascii')
    return img_str

# command:AT+CGPSINFO | back:OK | timeout:1
def send_at(command,back,timeout):
    global lng_temp
    global lat_temp
    global init_gps
    rec_buff = ''
    ser.write((command+'\r\n').encode())
    time.sleep(timeout)
    if ser.inWaiting():
        time.sleep(0.01 )
        rec_buff = ser.read(ser.inWaiting())
    if rec_buff != '':
        if back not in rec_buff.decode():
            print(command + ' ERROR')
            print(command + ' back:\t' + rec_buff.decode())
            return 0
        else:
            str_gps=rec_buff.decode()
            # 示例结果:+CGPSINFO: 3407.462252,N,11658.330599,E,090520,151235.0,38.3,0.0,239.4
            print(rec_buff.decode())
            if len(str_gps)>50:
                if init_gps == 1:
                    # 第一条数据
                    lat_temp=str(float(str_gps[25:27])+float(str_gps[27:36])/60)
                    lng_temp=str(float(str_gps[39:42])+float(str_gps[42:51])/60)
                    init_gps=0
                elif getDistance(float(lat_temp),float(lng_temp),float(str_gps[25:27])+float(str_gps[27:36])/60,float(str_gps[39:42])+float(str_gps[42:51])/60)>0.01:
                    print('01 you have moved 10m **--**--')
                    # 更新当前位置
                    lat_temp = str(float(str_gps[25:27]) + float(str_gps[27:36]) / 60)
                    lng_temp = str(float(str_gps[39:42]) + float(str_gps[42:51]) / 60)
                    print('02 you have moved 10m **--**--')
                    # 坐标系转换
                    tr = transfer()
                    lng_gcj02,lat_gcj02=tr.wg84_to_gcj02(float(lng_temp),float(lat_temp))
                    print('Changed GPS float****')
                    print(str(lng_gcj02)+' '+str(lat_gcj02))
                    # 拍摄图片
                    os.system('raspistill -t 3000 -w 1920 -h 1080 -o temp.jpg -hf -vf')
                    time.sleep(1)
                    print('capture a image****')
                    imgstr=getByte('temp.jpg')
                    url='http://47.95.245.186:8080/Server2_0_war_exploded/Edge_Receiver'
                    data = {'img': imgstr, 'lng': str(lng_gcj02), 'lat': str(lat_gcj02), 'device_id': 'uav_01'}
                    json_mod = json.dumps(data)
                    requests.post(url=url, data=json_mod)
                    print('Upload the data')
            return 1
    else:
        print('GPS is not ready')
        return 0

# 获取GPS位置信息
def get_gps_position():
    print('doing get_gps_postion')
    rec_null = True
    answer = 0
    print('Start GPS session...')
    rec_buff = ''
    send_at('AT+CGPS=1,1','OK',1)
    time.sleep(2)
    while rec_null:
        answer = send_at('AT+CGPSINFO','+CGPSINFO: ',1)
        if 1 == answer:
            answer = 0
            if ',,,,,,' in rec_buff:
                print('GPS is not ready')
                rec_null = False
                time.sleep(1)
        else:
            print('error %d'%answer)
            rec_buff = ''
            send_at('AT+CGPS=0','OK',1)
            return False
        time.sleep(1.5)
# 开启SIM7600X模块--设定启动时间20s
def power_on(power_key):
    print('SIM7600X is starting:')
    GPIO.setmode(GPIO.BCM)
    GPIO.setwarnings(False)
    GPIO.setup(power_key,GPIO.OUT)
    time.sleep(0.1)
    GPIO.output(power_key,GPIO.HIGH)
    time.sleep(2)
    GPIO.output(power_key,GPIO.LOW)
    time.sleep(10)
    ser.flushInput()
    print('SIM7600X is ready')
# 关闭SIM7600X模块--设定关闭时间21s
def power_down(power_key):
    print('SIM7600X is loging off:')
    GPIO.output(power_key,GPIO.HIGH)
    time.sleep(3)
    GPIO.output(power_key,GPIO.LOW)
    time.sleep(18)
    print('Good bye')



# 执行程序
try:
    # 开启SIM7600X
    power_on(power_key)
    # 获取GPS位置数据
    get_gps_position()
    # 关闭SIM7600X
    power_down(power_key)
except:
    if ser != None:
        ser.close()
    power_down(power_key)
    GPIO.cleanup()
if ser != None:
        ser.close()
        GPIO.cleanup()

发表评论

Scroll Up