def __init__(self, target_sys, target_comp):
# initiate variables
self.stop = False
self.first_msg_ok = False
self.request_sent = False
self.mission_count = 0
self.mission_id_next = 0
self.mi = mission_lib()
self.sys_id = 0 # reset when receiving first msg
self.comp_id = 0
self.mission_list = mavlink_lora_mission_list()
self.download_timer = None
self.retries = 0
# initiate node
rospy.init_node(ros_node_name)
self.mission_list_pub = rospy.Publisher("/telemetry/new_mission",
mavlink_lora_mission_list,
queue_size=0)
self.mavlink_msg_pub = rospy.Publisher(mavlink_lora_tx_pub,
mavlink_lora_msg,
queue_size=0)
rospy.Subscriber(mavlink_lora_rx_sub, mavlink_lora_msg,
self.on_mavlink_msg)
self.rate = rospy.Rate(ros_node_update_interval)
# wait until everything is running (important)
rospy.sleep(1)
def land_takeoff(self, current_lat, current_lon):
# Upload mission
self.lat = int(current_lat * 10000000)
self.lon = int(current_lon * 10000000)
if self.lat == 0 and self.lon == 0:
print("No GPS Mission not uploaded")
return
seq = 0
missionlist = mavlink_lora_mission_list()
takeoff = mavlink_lora_mission_item_int()
takeoff.target_system = 0
takeoff.target_component = 0
takeoff.seq = seq
takeoff.frame = 6 # global pos, relative alt_int
takeoff.command = 21
takeoff.x = self.lat
takeoff.y = self.lon
takeoff.z = 5
takeoff.param1 = 5
takeoff.current = 1
takeoff.autocontinue = 1
missionlist.waypoints.append(takeoff)
seq += 1
landing = mavlink_lora_mission_item_int()
landing.target_system = 0
landing.target_component = 0
landing.seq = seq
landing.frame = 6 # global pos, relative alt_int
landing.command = 21
landing.param1 = 20 # abort alt
landing.param2 = 0 # precision landing. 0 = normal landing
landing.x = self.lat
landing.y = self.lon
landing.z = 32
landing.autocontinue = 0
seq += 1
takeoff = mavlink_lora_mission_item_int()
takeoff.target_system = 0
takeoff.target_component = 0
takeoff.seq = seq
takeoff.frame = 6 # global pos, relative alt_int
takeoff.command = 22
takeoff.x = self.lat
takeoff.y = self.lon
takeoff.z = self.static_alt
takeoff.param1 = 5
takeoff.current = 1
takeoff.autocontinue = 1
missionlist.waypoints.append(takeoff)
seq += 1
missionlist.waypoints.append(landing)
self.mavlink_msg_pub.publish(missionlist)
def __init__(self):
self.request_sent = False
# status variables
self.lat = 0.0
self.lon = 0.0
self.alt = 0.0
self.local_lat = 0.0
self.local_lon = 0.0
self.local_alt = -5.0
self.mission_list = mavlink_lora_mission_list()
self.valid_mission = False
self.upload_timer = None
rospy.sleep(1) # wait until everything is running
# Service handlers
self.mission_upload_service = rospy.Service("/mission/upload",
Trigger,
self.upload_mission,
buff_size=10)
self.mission_upload_partial_service = rospy.Service(
"/mission/partial",
Trigger,
self.upload_partial_mission,
buff_size=10)
self.mission_upload_from_file_service = rospy.Service(
"/mission/upload_from_file",
UploadFromFile,
self.upload_from_file,
buff_size=10)
# Topic handlers
self.mission_upload_pub = rospy.Publisher(
"mavlink_interface/mission/mavlink_upload_mission",
mavlink_lora_mission_list,
queue_size=0)
self.partial_mission_upload_pub = rospy.Publisher(
"mavlink_interface/mission/mavlink_upload_partial_mission",
mavlink_lora_mission_partial_list,
queue_size=0)
self.mavlink_msg_pub = rospy.Publisher("/mavlink_tx",
mavlink_lora_msg,
queue_size=0)
rospy.Subscriber("/downloaded_mission", mavlink_lora_mission_list,
self.on_mission_list)
rospy.Subscriber(mavlink_lora_sub_topic, mavlink_lora_msg,
self.on_mavlink_msg)
self.rate = rospy.Rate(update_interval)
def __init__(self):
# initiate variables
self.stop = False
self.first_msg_ok = False
self.request_sent = False
self.busy = False
self.mission_count = 0
self.mission_id_next = 0
self.mi = mission_lib()
self.sys_id = 0 # reset when receiving first msg
self.comp_id = 0
self.active_mission_item = 0
self.active_mission_length = 0
self.mission_list_down = mavlink_lora_mission_list()
self.mission_list_up = mavlink_lora_mission_list()
self.active_mission = mavlink_lora_mission_list(
waypoints=[mavlink_lora_mission_item_int()])
# self.active_mission.waypoints.append(mavlink_lora_mission_item_int())
rospack = rospkg.RosPack()
package_path = rospack.get_path("telemetry")
self.plan_path = package_path + "/plans/"
self.download_timer = None
self.retries = 0
# Topic handlers
self.mavlink_msg_pub = rospy.Publisher("/mavlink_tx",
mavlink_lora_msg,
queue_size=0)
self.mission_upload_pub = rospy.Publisher(
"/mavlink_interface/mission/mavlink_upload_mission",
mavlink_lora_mission_list,
queue_size=0)
self.mission_info_pub = rospy.Publisher("/telemetry/mission_info",
telemetry_mission_info,
queue_size=0)
def upload_mission(self, current_lat, current_lon):
self.lat = int(current_lat * 10000000)
self.lon = int(current_lon * 10000000)
if self.lat == 0 and self.lon == 0:
print("No GPS Mission not uploaded")
return
seq = 0
missionlist = mavlink_lora_mission_list()
# Takeoff
takeoff = mavlink_lora_mission_item_int()
takeoff.target_system = 0
takeoff.target_component = 0
takeoff.seq = seq
takeoff.frame = 6 # global pos, relative alt_int
takeoff.command = 22
takeoff.x = self.lat
takeoff.y = self.lon
takeoff.z = self.static_alt
takeoff.param1 = 5
takeoff.current = 1
takeoff.autocontinue = 1
missionlist.waypoints.append(takeoff)
for i in range(len(self.way_alt)):
seq = i + 1
wp = self.set_waypoints(self.way_lat[i], self.way_lon[i],
self.way_alt[i], seq)
missionlist.waypoints.append(wp)
landing = mavlink_lora_mission_item_int()
landing.target_system = 0
landing.target_component = 0
landing.seq = seq + 1
landing.frame = 6 # global pos, relative alt_int
landing.command = 21
landing.param1 = 5 # abort alt
landing.param2 = 0 # precision landing. 0 = normal landing
landing.x = self.lat
landing.y = self.lon
landing.z = 10
landing.autocontinue = 0
missionlist.waypoints.append(landing)
self.mavlink_msg_pub.publish(missionlist)
def import_plan(filename, target_sys=1, target_comp=0):
with open(filename) as file:
data = json.load(file)
mission_list = mavlink_lora_mission_list()
for itr, waypoint in enumerate(data['mission']['items']):
curr = 0
if itr == 0:
curr = 1
# convert all None-types to nan
params = [float('nan') if param == None else param for param in waypoint['params'] ]
command = waypoint['command']
if command in SUPPORTED_GLOBAL_FRAMES:
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
elif command in SUPPORTED_MISSION_FRAMES:
frame = MAV_FRAME_MISSION
else:
rospy.logwarn("Mission item ignored. Not supported on PX4 stack.")
continue
mission_item = mavlink_lora_mission_item_int(
param1=params[0],
param2=params[1],
param3=params[2],
param4=params[3],
x=int(waypoint['params'][4]*1e7),
y=int(waypoint['params'][5]*1e7),
z=waypoint['params'][6],
seq=waypoint['doJumpId'],
command=command,
current=curr,
autocontinue=1,
target_system=target_sys,
target_component=target_comp,
frame=frame
)
mission_list.waypoints.append(mission_item)
return mission_list
def gps_to_mavlink(self, gps_list):
sequence_number = 1
ml_list = mavlink_lora_mission_list()
for itr, waypoint in enumerate(gps_list):
current = 0
if itr == 0:
current = 1
mission_item = mavlink_lora_mission_item_int(
param1=0, # hold time in seconds
param2=WP_ACCEPTANCE_RADIUS, # acceptance radius [m]
param3=0, # orbit CW or CCW
param4=float('nan'),
x=int(waypoint.latitude * 1e7),
y=int(waypoint.longitude * 1e7),
z=waypoint.altitude,
seq=sequence_number,
command=MAV_CMD_NAV_WAYPOINT,
current=current,
autocontinue=1,
target_system=self.id,
target_component=0,
frame=MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)
sequence_number += 1
ml_list.waypoints.append(mission_item)
if self.loiter:
# set last waypoint to a loiter command
ml_list.waypoints[-1].command = MAV_CMD_NAV_LOITER_UNLIM
ml_list.waypoints[-1].param2 = 0
ml_list.waypoints[-1].autocontinue = 0
else:
# set last waypoint to a landing command
ml_list.waypoints[-1].command = MAV_CMD_NAV_LAND
ml_list.waypoints[-1].param1 = 0 # abort alt
ml_list.waypoints[-1].param2 = 2 # precision land
ml_list.waypoints[-1].z = 0
ml_list.waypoints[-1].autocontinue = 0
ml_list.header.stamp = rospy.Time.now()
return ml_list
def update_mission(self, current_lat, current_lon, current_alt, sim):
# Put uav in loiter mode
self.send_mavlink_set_mode(1, 4, 3)
# Clear mission
# msg = std_msgs.msg.Empty()
# self.mavlink_clear_mission.publish(msg)
# Upload mission
self.lat = int(current_lat * 10000000)
self.lon = int(current_lon * 10000000)
if self.lat == 0 and self.lon == 0:
print("No GPS Mission not uploaded")
return
seq = 0
missionlist = mavlink_lora_mission_list()
# if sim:
# print("Added speed points x2")
# speed = mavlink_lora_mission_item_int()
# speed.target_system = 0
# speed.target_component = 0
# speed.seq = seq # Sequence in the list. Starts from 0 and every item increments by one
# speed.frame = 2 # mission command frame
# speed.command = 178 # change speed id
# speed.param1 = 0 # air_speed
# speed.param2 = 4 # m/s
# speed.param3 = -1 # no change
# speed.param4 = 0 # absolute or relative. relative = 1
# speed.autocontinue = 1 # automatic continue to next waypoint when this is reached
# missionlist.waypoints.append(speed)
# seq += 1
#
# speed1 = mavlink_lora_mission_item_int()
# speed1.target_system = 0
# speed1.target_component = 0
# speed1.seq = seq # Sequence in the list. Starts from 0 and every item increments by one
# speed1.frame = 2 # mission command frame
# speed1.command = 178 # change speed id
# speed1.param1 = 0 # air_speed
# speed1.param2 = 5 # m/s
# speed1.param3 = -1 # no change
# speed1.param4 = 0 # absolute or relative. relative = 1
# speed1.autocontinue = 1 # automatic continue to next waypoint when this is reached
# missionlist.waypoints.append(speed1)
# seq += 1
# Add a Takeoff point
print("Added TakeOff")
takeoff = mavlink_lora_mission_item_int()
takeoff.target_system = 0
takeoff.target_component = 0
takeoff.seq = seq
takeoff.frame = 6 # global pos, relative alt_int
takeoff.command = 22
takeoff.x = self.lat
takeoff.y = self.lon
takeoff.z = self.static_alt
takeoff.param1 = 5
takeoff.current = 1
takeoff.autocontinue = 1
missionlist.waypoints.append(takeoff)
seq += 1
for i in range(len(self.way_alt)):
wp = self.set_waypoints(self.way_lat[i], self.way_lon[i],
self.way_alt[i], seq)
missionlist.waypoints.append(wp)
seq += 1
landing = mavlink_lora_mission_item_int()
landing.target_system = 0
landing.target_component = 0
landing.seq = seq
landing.frame = 6 # global pos, relative alt_int
landing.command = 21
landing.param1 = 20 # abort alt
landing.param2 = 0 # precision landing. 0 = normal landing
landing.x = self.way_lat[len(self.way_lat) - 1]
landing.y = self.way_lon[len(self.way_lon) - 1]
landing.z = 32
landing.autocontinue = 0
missionlist.waypoints.append(landing)
self.mavlink_msg_pub.publish(missionlist)
def __init__(self, drone_id=1):
# All variables relating to the status and information of the drone
self.id = drone_id
self.up_time = 0
self.gps_timestamp = 0
self.fsm_state = State.GROUNDED
self.new_mission = False
self.upload_done = False
self.upload_failed = False
self.cmd_try_again = False
self.speed_ack = False
self.loiter = False
self.active = False
self.wait = False
self.hold = False
self.upload_retries = 0
self.mission_id = 0
self.pending_mission_gps = []
self.active_waypoint_gps = GPS()
self.active_mission_gps = []
self.active_mission_ml = mavlink_lora_mission_list()
self.active_mission_len = 0
self.active_mission_idx = 0
self.dist_to_goal = -1
self.holding_waypoint = GPS()
# the drone starts on the ground ( land means ground for now. Change later ) TODO
self.gcs_status = DroneInfo.Land
# from mission info
# self.active_sub_waypoint_idx = 0
# self.active_sub_mission_len = 0
# self.active_sub_waypoint = mavlink_lora_mission_item_int()
# from heartbeat status
self.main_mode = ""
self.sub_mode = ""
self.autopilot = ""
self.type = ""
self.state = ""
# from mav mode
self.armed = False
self.manual_input = False
self.hil_simulation = False
self.stabilized_mode = False
self.guided_mode = False
self.auto_mode = False
self.test_mode = False
self.custom_mode = False
# from statustext
self.statustext = deque([], maxlen=BUFFER_SIZE)
self.severity = deque([], maxlen=BUFFER_SIZE)
# from vfr hud
self.ground_speed = 0
self.climb_rate = 0
self.throttle = 0
# from home position
self.home_position = GPS()
# from drone attitude
self.roll = 0
self.pitch = 0
self.yaw = 0
# from drone status
self.battery_voltage = 0
self.battery_SOC = 0
self.cpu_load = 0
self.msg_sent_gcs = 0
self.msg_received_gcs = 0
self.msg_dropped_gcs = 0
self.msg_dropped_uas = 0
self.last_heard = rospy.Time().now()
# from drone pos
self.latitude = 0
self.longitude = 0
self.absolute_alt = 0
self.relative_alt = 0
self.heading = 0
self.clear_mission_pub = rospy.Publisher(
"/mavlink_interface/mission/mavlink_clear_all",
Empty,
queue_size=0)
self.upload_mission_pub = rospy.Publisher("/telemetry/new_mission",
mavlink_lora_mission_list,
queue_size=0)
self.set_current_mission_pub = rospy.Publisher(
"/telemetry/mission_set_current", Int16, queue_size=10)
# TODO add drone id to services
self.arm = rospy.ServiceProxy("/telemetry/arm_drone", Trigger)
self.disarm = rospy.ServiceProxy("/telemetry/disarm_drone", Trigger)
self.takeoff = rospy.ServiceProxy("/telemetry/takeoff_drone",
TakeoffDrone)
self.land = rospy.ServiceProxy("/telemetry/land_drone", LandDrone)
self.set_mode = rospy.ServiceProxy("/telemetry/set_mode", SetMode)
self.set_home = rospy.ServiceProxy("/telemetry/set_home", SetHome)
self.change_speed = rospy.ServiceProxy("/telemetry/change_speed",
ChangeSpeed)
self.return_to_home = rospy.ServiceProxy("/telemetry/return_home",
Trigger)
self.reposition = rospy.ServiceProxy("/telemetry/goto_waypoint",
GotoWaypoint)
self.upload_mission = rospy.ServiceProxy("/telemetry/upload_mission",
UploadMission)
# class that handles the missions manually while an upload is in progress
self.manual_mission = manual_mission.ManualMission(
target_sys=self.id,
target_comp=0,
reposition_handle=self.reposition)
# pubs
mavlink_msg_pub = rospy.Publisher(mavlink_lora_pub_topic,
mavlink_lora_mission_list,
queue_size=0)
# subs
mavlink_mission_ack_sub = rospy.Subscriber("mavlink_interface/mission/ack",
mavlink_lora_mission_ack,
on_ack_received_callback)
# wait until everything is running
rospy.sleep(1)
# make 4-5 waypoints and save in list.
missionlist = mavlink_lora_mission_list()
# To see what the different settings mean see: https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_WAYPOINT
# scroll down and find the ids for each waypoint and you can see what it means and what it can do
# CHANGE SPEED
speed = mavlink_lora_mission_item_int()
speed.target_system = 0
speed.target_component = 0
speed.seq = 0 # Sequence in the list. Starts from 0 and every item increments by one
speed.frame = 2 # mission command frame
speed.command = 178 # change speed id
speed.param1 = 0 # air_speed
speed.param2 = 5 # m/s
speed.param3 = -1 # no change
speed.param4 = 0 # absolute or relative. relative = 1
def upload_mission(self, srv):
msg = mavlink_lora_mission_list(header=Header(stamp=rospy.Time.now()),
waypoints=self.waypoints)
self.mission_upload_pub.publish(msg)
return TriggerResponse()
