I am working on an outdoor non-GPS flight and using a Jetson Orin Nano to calculate movement using vision. I want to send fake GPS data to a Pixhawk 6C running ArduPilot. I tried using MAVLink GPS_INPUT (GPS_TYPE=14) and NMEA (GPS_TYPE=5), both of which successfully delivered the GPS data. However, Mission Planner shows GPS2: 3D fix in red. How can I resolve this issue? Can the drone perform a mission in this state?
This is the python code that sends the gps data.
import time
from pymavlink import mavutil
from datetime import datetime
# Define the GPS epoch (January 6, 1980)
GPS_EPOCH = datetime(1980, 1, 6)
# Define the connection to the Pixhawk (change '/dev/ttyTHS1' to your UART port and '57600' to your baud rate)
connection = mavutil.mavlink_connection('/dev/ttyTHS1', baud=57600)
def get_gps_time():
# Calculate the GPS week number and milliseconds into the week
now = datetime.utcnow()
delta = now - GPS_EPOCH
gps_week = delta.days // 7
gps_week_ms = ((delta.days % 7) * 24 * 60 * 60 + delta.seconds) * 1000 + int(delta.microseconds / 1000)
return gps_week, gps_week_ms
def send_gps_input():
# Define the GPS data as per your format
gps_week, gps_week_ms = get_gps_time()
data = {
'time_usec': int(time.time() * 1e6), # Timestamp (microseconds since system boot)
'gps_id': 0, # ID of the GPS for multiple GPS inputs
'ignore_flags': 8, # Flags indicating which fields to ignore
'time_week_ms': gps_week_ms, # GPS time (milliseconds from start of GPS week)
'time_week': gps_week, # GPS week number
'fix_type': 3, # 3D fix
'lat': 254100000, # Latitude (degrees * 1E7)
'lon': 1212100000, # Longitude (degrees * 1E7)
'alt': 60, # Altitude (meters above mean sea level)
'hdop': 1.0, # Horizontal dilution of position
'vdop': 1.0, # Vertical dilution of position
'vn': 0, # GPS velocity in m/s in NORTH direction in earth-fixed NED frame
've': 0, # GPS velocity in m/s in EAST direction in earth-fixed NED frame
'vd': 0, # GPS velocity in m/s in DOWN direction in earth-fixed NED frame
'speed_accuracy': 0, # GPS speed accuracy in m/s
'horiz_accuracy': 0, # GPS horizontal accuracy in m
'vert_accuracy': 0, # GPS vertical accuracy in m
'satellites_visible': 7 # Number of satellites visible
}
# Send the GPS_INPUT MAVLink message using the data
connection.mav.gps_input_send(
data['time_usec'], # Timestamp (microseconds since boot)
data['gps_id'], # GPS ID (for multiple GPS systems)
data['ignore_flags'], # Flags indicating which fields to ignore
data['time_week_ms'], # GPS week time (milliseconds)
data['time_week'], # GPS week number
data['fix_type'], # Fix type (0-1: no fix, 2: 2D fix, 3: 3D fix)
data['lat'], # Latitude (degrees * 1E7)
data['lon'], # Longitude (degrees * 1E7)
data['alt'], # Altitude (meters above mean sea level)
float(data['hdop']), # Horizontal dilution of precision
float(data['vdop']), # Vertical dilution of precision
float(data['vn']), # GPS velocity in N direction (m/s)
float(data['ve']), # GPS velocity in E direction (m/s)
float(data['vd']), # GPS velocity in D direction (m/s)
float(data['speed_accuracy']), # Speed accuracy (m/s)
float(data['horiz_accuracy']), # Horizontal accuracy (meters)
float(data['vert_accuracy']), # Vertical accuracy (meters)
data['satellites_visible'] # Number of visible satellites
)
# Main function to send GPS data at regular intervals
def main():
while True:
# Send the GPS_INPUT message
send_gps_input()
# Print debug information
print("GPS_INPUT data sent.")
# Sleep for 0.05 seconds to send data at 20 Hz
time.sleep(0.05)
if __name__ == "__main__":
try:
main()
except KeyboardInterrupt:
print("Script interrupted. Exiting...")