How do I get the GPS_velocity?

234 views Asked by At

How to get the previous_lat,lon and current_lat, lon, then caculate the speed each 0.1s. I have some code about cacluate the distance with different lat ,lon. but i don't now how to calcute vehicle itself distance and velocity.

from __future__ import print_function
import time
from dronekit import connect, VehicleMode, LocationGlobalRelative
import gps
import socket
import sys
from pymavkink import mavutil
vehicle = connect('/dev/ttyACM0', wait_ready=True, baud=115200)

def get_location_metres(original_location, dNorth, dEast):
    earth_radius = 6378137.0 #Radius of "spherical" earth
    #Coordinate offsets in radians
    dLat = dNorth/earth_radius
    dLon = dEast/(earth_radius*math.cos(math.pi*original_location.lat/180))

    #New position in decimal degrees
    newlat = original_location.lat + (dLat * 180/math.pi)
    newlon = original_location.lon + (dLon * 180/math.pi)
    if type(original_location) is LocationGlobal:
        targetlocation=LocationGlobal(newlat, newlon,original_location.alt)
    elif type(original_location) is LocationGlobalRelative:
        targetlocation=LocationGlobalRelative(newlat, newlon,original_location.alt)
    else:
        raise Exception("Invalid Location object passed")

def get_distance_metres(aLocation1, aLocation2):
    dlat = aLocation2.lat - aLocation1.lat
    dlong = aLocation2.lon - aLocation1.lon
    return math.sqrt((dlat*dlat) + (dlong*dlong)) * 1.113195e5

other people code Calculate the speed from two longitude and latitude GPS coordinates

def haversine(lon1, lat1, lon2, lat2):
    """
    Calculate the great circle distance in kilometers between two points
    on the earth (specified in decimal degrees)
    """
    # convert decimal degrees to radians
    lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])

    # haversine formula
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
    c = 2 * asin(sqrt(a))
    r = 6371 # Radius of earth in kilometers. Use 3956 for miles. Determines return value units.
    return c * r

prev_data = None

while 1:
    line = ser.readline().decode('UTF-8')
    splitline = line.split(',')

    if splitline[0] == '$GPGGA':
        msg = line
        data = pynmea2.parse(msg)
        if prev_data is not None:
            distance = haversine(data.longitude, data.latitude, prev_data.longitude, prev_data.latitude)
            print('distance', distance)
            print('speed', round(distance*3600, 2))
        prev_data = data

get the previous_lat,lon and current_lat, lon, then caculate the speed each 0.1s. moreover, save the file as csv than compare with vehicle.velocity(get from IMU)

0

There are 0 answers