[Python] How do I get the GPS_velocity?

How to get the previous_lat,lon and current_lat, lon, then calculate the speed each 0.1s.

I have some code about calculate the distance with different lat, lon.

But I don't now how to calculate 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's 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 calculate the speed each 0.1s. moreover, save the file as csv than compare with vehicle.velocity (get from IMU)

Continue reading...