I am creating a fake NMEA Signal and passing to ArduPilot. Positioning seems to work great, but the velocity continuously drops to 0 before going back up, causing EKF Variance & making it impossible to fly.
I’m currently looking at an approach that avoids this using MavLink like here: Intel RealSense T265 — Copter documentation - however long term I would like to be able to send NMEA in in a way that does not cause issues.
These are global positions, calculated using a VIO and known to be accurate - I can plot them on a map, or even look at ardupilot and see the position is correct, the issue appears to be in the velocity.
Help much appreciated!
NMEA Sentences Look Like:
def lat_lon_to_nmea(self, lat, lon, alt, speed, course):
timestamp = self.get_utc_timestamp()
date = self.get_utc_date()
# Convert latitude to degrees and minutes
lat_deg = int(abs(lat))
lat_min = (abs(lat) - lat_deg) * 60
lat_str = f"{lat_deg:02d}{lat_min:09.6f}"
# Convert longitude to degrees and minutes
lon_deg = int(abs(lon))
lon_min = (abs(lon) - lon_deg) * 60
lon_str = f"{lon_deg:03d}{lon_min:09.6f}"
# Create GGA sentence
gga_msg = pynmea2.GGA(
"GP",
"GGA",
(
timestamp,
lat_str,
"N" if lat >= 0 else "S",
lon_str,
"E" if lon >= 0 else "W",
"1", # Fix quality: 1 = GPS fix
"20", # Number of satellites
".7", # HDOP
f"{alt:.1f}", # Altitude
"M", # Altitude units (meters)
"1", # Height of geoid above WGS84 ellipsoid
"M", # Units (meters)
"", # Time since last DGPS update (empty for GPS)
"", # DGPS reference station id (empty for GPS)
),
)
# Create RMC sentence
rmc_msg = pynmea2.RMC(
"GP",
"RMC",
(
timestamp,
"A", # Status A=active or V=void
lat_str,
"N" if lat >= 0 else "S",
lon_str,
"E" if lon >= 0 else "W",
f"{speed:.2f}", # Speed over ground in knots
f"{course:.2f}", # Course over ground in degrees
date, # Date
"0.0", # Magnetic variation
"", # Magnetic variation direction
"A", # Mode indicator
),
)
return f"{gga_msg.render()}\r\n{rmc_msg.render()}\r\n"
# return f"{rmc_msg.render()}\r\n"
An example from logs: