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 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 on_mavlink_msg(self, msg):
if self.first_msg_ok == False:
self.first_msg_ok = True
self.sys_id = msg.sys_id
self.mi.set_target(self.sys_id, self.comp_id)
if msg.msg_id == MAVLINK_MSG_ID_MISSION_ITEM:
# stop the timeout
self.download_timer.shutdown()
# add the item to the list
mission_item = self.mi.unpack_mission_item(msg.payload)
print(mission_item)
# check if the waypoint is valid
if mission_item[8] != 530:
mission_item_msg = mavlink_lora_mission_item_int(
param1=mission_item[0],
param2=mission_item[1],
param3=mission_item[2],
param4=mission_item[3],
x=int(mission_item[4] * 1e7),
y=int(mission_item[5] * 1e7),
z=mission_item[6],
seq=mission_item[7],
command=mission_item[8],
current=mission_item[9],
autocontinue=mission_item[10],
target_system=self.sys_id,
target_component=self.comp_id,
frame=MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)
self.mission_list.waypoints.append(mission_item_msg)
if self.mission_id_next < self.mission_count - 1:
self.mission_id_next += 1
# if self.mission_id_next == 10:
# self.mission_id_next += 2
print("Requesting item #%d" % self.mission_id_next)
self.send_mavlink_mission_req(self.mission_id_next)
else:
self.mission_list.header.stamp = rospy.Time.now()
self.mission_list_pub.publish(self.mission_list)
self.stop = True
elif msg.msg_id == MAVLINK_MSG_ID_MISSION_COUNT:
# stop the timeout
self.download_timer.shutdown()
self.mission_count = self.mi.unpack_mission_count(msg.payload)
print("Mission count: %d" % self.mission_count)
if self.mission_count > 0:
self.mission_id_next = 0
self.send_mavlink_mission_req(self.mission_id_next)
else:
self.stop = True
def set_waypoints(self, lat, lon, alt, sequ):
way = mavlink_lora_mission_item_int()
way.target_system = 0
way.target_component = 0
way.seq = sequ
way.frame = 6
way.command = 16
way.param1 = 0
way.param2 = 5
way.param3 = 0
way.x = lat
way.y = lon
way.z = alt
way.autocontinue = 1
return way
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 __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 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)
# 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
speed.autocontinue = 1 # automatic continue to next waypoint when this is reached
speed2 = mavlink_lora_mission_item_int()
speed2.target_system = 0
speed2.target_component = 0
speed2.seq = 1
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 20+ waypoints and save in list.
missionlist = mavlink_lora_mission_list()
# CHANGE SPEED
speed = mavlink_lora_mission_item_int()
speed.target_system = 0
speed.target_component = 0
speed.seq = 0
speed.frame = 2 # mission command frame
speed.command = 178
speed.param1 = 5 # air_speed
speed.param2 = 5 # m/s
speed.param3 = -1 # no change
speed.param4 = 0 # abosulte or relative. relative = 1
speed.autocontinue = 1
speed2 = mavlink_lora_mission_item_int()
speed2.target_system = 0
speed2.target_component = 0
speed2.seq = 1
# # TAKEOFF waypoint
# way1 = mavlink_lora_mission_item_int()
# way1.target_system = 0
# way1.target_component = 0
# way1.seq = 2
# way1.frame = 6 #global pos, relative alt_int
# way1.command = 22
# way1.x = home_lat * 10000000
# way1.y = home_lon * 10000000
# way1.z = 20
# way1.param1 = 5
# way1.current = 1
# way1.autocontinue = 1
# WAYPOINT 1
way2 = mavlink_lora_mission_item_int()
way2.target_system = 0
way2.target_component = 0
way2.seq = 0
way2.frame = 6 #global pos, relative alt_int
way2.command = 16
way2.param1 = 0 # hold time
way2.param2 = 5 # acceptance radius in m
way2.param3 = 0 # pass though waypoint, no trajectory control
way2.x = 55.4720010 * 10000000
way2.y = 10.4164463 * 10000000
way2.z = 20
way2.autocontinue = 1
print("published single waypoint")
mavlink_msg_pub.publish(way2)
def on_mavlink_msg(self, msg):
if self.first_msg_ok == False:
self.first_msg_ok = True
self.sys_id = msg.sys_id
self.mi.set_target(self.sys_id, self.comp_id)
if msg.msg_id == MAVLINK_MSG_ID_MISSION_ITEM:
# stop the timeout
self.download_timer.shutdown()
# add the item to the list
mission_item = self.mi.unpack_mission_item(msg.payload)
# print(mission_item)
# check if the waypoint is valid
if mission_item[8] != 530:
mission_item_msg = mavlink_lora_mission_item_int(
param1=mission_item[0],
param2=mission_item[1],
param3=mission_item[2],
param4=mission_item[3],
x=int(mission_item[4] * 1e7),
y=int(mission_item[5] * 1e7),
z=mission_item[6],
seq=mission_item[7],
command=mission_item[8],
current=mission_item[9],
autocontinue=mission_item[10],
target_system=self.sys_id,
target_component=self.comp_id,
frame=MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)
self.mission_list_down.waypoints.append(mission_item_msg)
if self.mission_id_next < self.mission_count - 1:
self.mission_id_next += 1
# if self.mission_id_next == 10:
# self.mission_id_next += 2
rospy.loginfo("Requesting item #%d" % self.mission_id_next)
self.send_mavlink_mission_req(self.mission_id_next)
else:
# finished download
rospy.loginfo("Download has finished.")
self.mission_list_down.header.stamp = rospy.Time.now()
self.mission_id_next = 0
self.busy = False
self.active_mission_length = len(
self.mission_list_down.waypoints)
self.active_mission = self.mission_list_down
self.mi.pack_mission_ack(MAV_MISSION_ACCEPTED)
self.mavlink_msg_pub.publish(self.mi.msg)
# self.mission_list_pub.publish(self.mission_list_down)
elif msg.msg_id == MAVLINK_MSG_ID_MISSION_COUNT:
# stop the timeout
self.download_timer.shutdown()
self.mission_count = self.mi.unpack_mission_count(msg.payload)
rospy.loginfo("Mission count: %d" % self.mission_count)
if self.mission_count > 0:
self.mission_id_next = 0
rospy.loginfo("Requesting item #%d" % self.mission_id_next)
self.send_mavlink_mission_req(self.mission_id_next)
else:
self.busy = False
# elif msg.msg_id == MAVLINK_MSG_ID_MISSION_ACK:
# self.busy = False
# self.active_mission_length = len(self.mission_list_up.waypoints)
# self.active_mission = self.mission_list_up
elif msg.msg_id == MAVLINK_MSG_ID_MISSION_CURRENT:
self.active_mission_item = self.mi.unpack_mission_current(
msg.payload)
try:
current = self.active_mission.waypoints[
self.active_mission_item]
except IndexError:
current = mavlink_lora_mission_item_int()
msg = telemetry_mission_info(
system_id=msg.sys_id,
component_id=msg.comp_id,
timestamp=rospy.Time.now(),
active_waypoint_idx=self.active_mission_item,
active_mission_len=self.active_mission_length,
current_item=current)
self.mission_info_pub.publish(msg)