Hello,
I have 2 copters in Gazebo simulation: 1 with GPS and 2 without GPS.
I am trying to transmit GPS data as external GPS from the 1 to the 2 drone.
As far as I am aware, I am well able to transmit and receive GPS messages (see GPSOutput and GPSInput below), but the problem lies in its content: I somehow transmit the wrong data in the message which makes Drone 2 autopilot to think it’s unhealthy.
GPSOutput, 1 drone
import socket
import json
from pymavlink import mavutil
from MAVProxy.modules.lib import mp_module
class GPSOutput(mp_module.MPModule):
def init(self, mpstate):
super().init(mpstate, “gps_output”)
self.mpstate.console.write(“GPS Output Module Loaded\n”)
# Setup UDP socket for sending data to the second drone
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.gps_input_address = ("127.0.0.1", 25100) # Address of the second drone
# Flag to track if the drone is airborne
self.in_air = False
def mavlink_packet(self, m):
"""
Process incoming packets. Forward GPS_RAW_INT with altitude control.
"""
packet_type = m.get_type()
# Track whether the drone has taken off
if packet_type == "GLOBAL_POSITION_INT":
if m.relative_alt > 0:
self.in_air = True
elif m.relative_alt == 0:
self.in_air = False
# Forward GPS data
elif packet_type == "GPS_RAW_INT":
try:
# Convert data to JSON format
data = {
'time_usec': m.time_usec,
'gps_id': 0,
'ignore_flags': mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_ALT if self.in_air else 0,
'time_week_ms': 0,
'time_week': 0,
'fix_type': m.fix_type,
'lat': m.lat,
'lon': m.lon,
'alt': m.alt / 1000.0, # Convert from mm to meters
'hdop': m.eph / 100.0, # Convert eph to meters
'vdop': m.epv / 100.0, # Convert epv to meters
'vn': 0.0, # Velocities can be refined if needed
've': 0.0,
'vd': 0.0,
'speed_accuracy': 0.1,
'horiz_accuracy': m.h_acc / 1000.0, # Convert to meters
'vert_accuracy': m.v_acc / 1000.0, # Convert to meters
'satellites_visible': m.satellites_visible
}
# Convert data to JSON string
json_data = json.dumps(data)
# Send data via UDP
self.sock.sendto(json_data.encode('utf-8'), self.gps_input_address)
except Exception as e:
self.mpstate.console.write(f"Error forwarding GPS_RAW_INT: {e}\n")
def init(mpstate):
“”"
Initialize the module.
“”"
return GPSOutput(mpstate)
GPSInput (a default plugin), 2 drone
#!/usr/bin/env python
'''
support for GPS_INPUT message
'''
import socket, errno
import json
from pymavlink import mavutil
from MAVProxy.modules.lib import mp_module
class GPSInputModule(mp_module.MPModule):
IGNORE_FLAG_ALL = (mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_ALT |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_HDOP |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VDOP |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_HORIZ |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VEL_VERT |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_HORIZONTAL_ACCURACY |
mavutil.mavlink.GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY)
def __init__(self, mpstate):
super(GPSInputModule, self).__init__(mpstate, "GPSInput", "GPS_INPUT message support")
self.add_command('GPSInput.port', self.cmd_port, 'Port selection', ['<25100>'])
self.data = {
'time_usec' : 0, # (uint64_t) Timestamp (micros since boot or Unix epoch)
'gps_id' : 0, # (uint8_t) ID of the GPS for multiple GPS inputs
'ignore_flags' : self.IGNORE_FLAG_ALL, # (uint16_t) Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum). All other fields must be provided.
'time_week_ms' : 0, # (uint32_t) GPS time (milliseconds from start of GPS week)
'time_week' : 0, # (uint16_t) GPS week number
'fix_type' : 0, # (uint8_t) 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK
'lat' : 0, # (int32_t) Latitude (WGS84), in degrees * 1E7
'lon' : 0, # (int32_t) Longitude (WGS84), in degrees * 1E7
'alt' : 0, # (float) Altitude (AMSL, not WGS84), in m (positive for up)
'hdop' : 0, # (float) GPS HDOP horizontal dilution of position in m
'vdop' : 0, # (float) GPS VDOP vertical dilution of position in m
'vn' : 0, # (float) GPS velocity in m/s in NORTH direction in earth-fixed NED frame
've' : 0, # (float) GPS velocity in m/s in EAST direction in earth-fixed NED frame
'vd' : 0, # (float) GPS velocity in m/s in DOWN direction in earth-fixed NED frame
'speed_accuracy' : 0, # (float) GPS speed accuracy in m/s
'horiz_accuracy' : 0, # (float) GPS horizontal accuracy in m
'vert_accuracy' : 0, # (float) GPS vertical accuracy in m
'satellites_visible' : 0 # (uint8_t) Number of satellites visible.
}
self.BUFFER_SIZE = 4096
self.ip="127.0.0.1"
self.portnum = 25100
self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.port.bind((self.ip, self.portnum))
self.port.setblocking(0)
mavutil.set_close_on_exec(self.port.fileno())
print("Listening for GPS Input packets on UDP://%s:%s" % (self.ip, self.portnum))
def idle_task(self):
'''called in idle time'''
try:
datagram = self.port.recvfrom(self.BUFFER_SIZE)
data = json.loads(datagram[0])
except socket.error as e:
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
return
raise
for key in data.keys():
self.data[key] = data[key]
try:
self.master.mav.gps_input_send(
self.data['time_usec'],
self.data['gps_id'],
self.data['ignore_flags'],
self.data['time_week_ms'],
self.data['time_week'],
self.data['fix_type'],
self.data['lat'],
self.data['lon'],
self.data['alt'],
self.data['hdop'],
self.data['vdop'],
self.data['vn'],
self.data['ve'],
self.data['vd'],
self.data['speed_accuracy'],
self.data['horiz_accuracy'],
self.data['vert_accuracy'],
self.data['satellites_visible'])
except Exception as e:
print("GPS Input Failed:", e)
def cmd_port(self, args):
'handle port selection'
if len(args) != 1:
print("Usage: port <number>")
return
self.port.close()
self.portnum = int(args[0])
self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.port.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.port.bind((self.ip, self.portnum))
self.port.setblocking(0)
mavutil.set_close_on_exec(self.port.fileno())
print("Listening for GPS INPUT packets on UDP://%s:%s" % (self.ip, self.portnum))
def init(mpstate):
'''initialise module'''
return GPSInputModule(mpstate)
1 drone status
GLOBAL_POSITION_INT {time_boot_ms : 83476, lat : -353632172, lon : 1491652374, alt : 583990, relative_alt : 0, vx : 0, vy : 0, vz : 0, hdg : 8999}
GPS_GLOBAL_ORIGIN {latitude : -353632621, longitude : 1491652374, altitude : 584000, time_usec : 2503974}
GPS_RAW_INT {time_usec : 83384000, fix_type : 6, lat : -353632122, lon : 1491652423, alt : 583990, eph : 121, epv : 200, vel : 0, cog : 22800, satellites_visible : 10, alt_ellipsoid : 583990, h_acc : 300, v_acc : 300, vel_acc : 0, hdg_acc : 0, yaw : 0}
2 drone status
GLOBAL_POSITION_INT {time_boot_ms : 81803, lat : -353632125, lon : 1491652419, alt : 583990, relative_alt : 0, vx : -3, vy : -3, vz : 0, hdg : 9047}
GPS_GLOBAL_ORIGIN {latitude : -353632122, longitude : 1491652423, altitude : 583990, time_usec : 21013502}
GPS_RAW_INT {time_usec : 81737000, fix_type : 6, lat : -353632122, lon : 1491652423, alt : 583990, eph : 121, epv : 200, vel : 0, cog : 0, satellites_visible : 10, alt_ellipsoid : 0, h_acc : 300, v_acc : 300, vel_acc : 100, hdg_acc : 0, yaw : 0}
What is important, this code worked before, I guess I just somehow messed it up.
How should I correct the message in GPSOutput? Which fields need to be changed? Is there a way to collect the exact reason of the mistake from logs?