3

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)

YUKI
  • 29
  • 1
  • You are using GGA. Why don't you use the RMC? That already includes speed-over-ground, there really is no need to calculate it yourself. It also include course-over-ground. – Clifford Dec 24 '22 at 15:56
  • you mean the ground speed is GPS speed, no need to calculate the pre, current, lat and lon by my self? – YUKI Dec 24 '22 at 15:59
  • 1
    Exactly, and usefully this value will be calculated and filtered over several solutions. If you try to calculate SoG over delta-position over just 0.1 seconds it will be both inaccurate and noisy. I would trust the expertise and experience of the GNSS receiver's firmware developers over anything you might devise from already processed data. – Clifford Dec 24 '22 at 16:05
  • ... note however that SoG is not the same as _velocity_ in 3D space. You could be moving vertically at any speed and SoG will be zero. However in your code you were using only lat/lon and not altitude, so I assume that SoG is in fact what you intended? – Clifford Dec 24 '22 at 16:10
  • the ground speed is a scalar, how to show it as the vecter(Vx, Vy, Vz)，and the location velocity is get from IMU, rather than GPS, am i right? – YUKI Dec 25 '22 at 03:20
  • RMC includes course-over-ground to provide a 2D vector. Given your code only takes into account lat/lon and not altitude I assumed that was what you intended. If not you might mention that in the question. Certainly GPS will give you a very course and noisy short term motion vector. You probably need a sensor fusion approach. I work in marine navigation, and typically ignore altitude and speeds low. You might also mention your intended application and the expected velocity and acceleration expected. Note that RMC SoG is in knots not m/s and CoG in degrees not radians. – Clifford Dec 25 '22 at 10:25

0 Answers0