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)