def __init__(self): self.first_msg_ok = False self.busy = False self.confirmation = 0 self.retries = 0 self.lat = 0 self.lon = 0 self.alt = 0 self.home_alt = 0 self.cmd_lib = command_lib.command_lib() self.sys_id = 0 # reset when receiving first msg self.comp_id = 0 self.mavlink_msg = mavlink_lora_msg() self.command_timer = None self.active_command = "" # topic handlers self.mavlink_msg_pub = rospy.Publisher("/mavlink_tx", mavlink_lora_msg, queue_size=0) self.cmd_retry_fail_pub = rospy.Publisher("/telemetry/cmd_retry_fail", telemetry_cmd_retry_fail, queue_size=0) rospy.Subscriber("/mavlink_pos", mavlink_lora_pos, self.on_mavlink_lora_pos)
def __init__(self, stream_id, stream_rate, stream_start_stop): # save variables self.stream_id = stream_id self.stream_rate = stream_rate self.stream_start_stop = stream_start_stop # initiate variables self.stop = False self.target_sys = 0 self.target_comp = 0 self.first_msg_ok = False self.request_sent = False self.msg = mavlink_lora_msg() # initiate node rospy.init_node(ros_node_name) 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(3)
def __init__(self): self.msg = mavlink_lora_msg() self.request_sent = False # status variables self.batt_volt = 0.0 self.last_heard = 0 self.last_heard_sys_status = 0 self.lat = 0.0 self.lon = 0.0 self.alt = 0.0 self.home_lat = 0.0 self.home_lon = 0.0 self.yaw = 0.0 self.pitch = 0.0 self.roll = 0.0 # launch node rospy.init_node('mavlink_lora_gcs_simple', disable_signals=True) self.mavlink_msg_pub = rospy.Publisher(mavlink_lora_pub_topic, mavlink_lora_msg, queue_size=0) rospy.Subscriber(mavlink_lora_sub_topic, mavlink_lora_msg, self.on_mavlink_msg) rospy.Subscriber(mavlink_lora_status_sub_topic, mavlink_lora_status, self.on_mavlink_lora_status) rospy.Subscriber(mavlink_lora_pos_sub_topic, mavlink_lora_pos, self.on_mavlink_lora_pos) rospy.Subscriber(mavlink_lora_atti_sub_topic, mavlink_lora_attitude, self.on_mavlink_lora_attitude) rospy.Subscriber(mavlink_lora_keypress_sub_topic, Int8, self.on_keypress) self.rate = rospy.Rate(update_interval) rospy.sleep(1) # wait until everything is running
def __init__(self): self.msg = mavlink_lora_msg() self.local_pos_msg = mavlink_lora_local_ned_pos() # status variables # launch node rospy.init_node('local_position_publisher', disable_signals = True) rospy.Subscriber(mavlink_lora_sub_topic, mavlink_lora_msg, self.on_mavlink_msg) self.local_ned_pub = rospy.Publisher(mavlink_lora_local_ned_pos_pub, mavlink_lora_local_ned_pos, queue_size=0) self.rate = rospy.Rate(update_interval) rospy.sleep (1) # wait until everything is running
def __init__(self): self.msg = mavlink_lora_msg() # status variables self.armed = False self.arm_request_sent = False self.disarm_request_sent = False self.mavlink_msg_pub = rospy.Publisher(mavlink_lora_pub_topic, mavlink_lora_msg, queue_size=0) rospy.Subscriber(mavlink_lora_sub_topic, mavlink_lora_msg, self.on_mavlink_msg) self.rate = rospy.Rate(update_interval) rospy.sleep(1) # wait until everything is running
def upload_partial_mission(self, srv): msg = mavlink_lora_mission_partial_list(start_index=4, end_index=4) msg.waypoints.append(self.waypoints[1]) self.partial_mission_upload_pub.publish(msg) mav_msg = mavlink_lora_msg( msg_id=MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, payload_len=6, sys_id=0, comp_id=0) mav_msg.payload = struct.pack('<hhBB', 4, 4, 1, 0) self.mavlink_msg_pub.publish(mav_msg) return TriggerResponse()
def __init__(self, arm_request): pass self.msg = mavlink_lora_msg() # status variables self.arm_request = arm_request self.request_sent = False self.arm_request_sent = False self.disarm_request_sent = False self.stop = False # initiate node rospy.init_node("mavlink_command_node") # change to variable self.mavlink_msg_pub = rospy.Publisher(mavlink_lora_pub_topic, mavlink_lora_msg, queue_size=0) rospy.Subscriber(mavlink_lora_sub_topic, mavlink_lora_msg, self.on_mavlink_arm_msg) self.rate = rospy.Rate(update_interval) rospy.sleep(1) # wait until everything is running
mavlink_lora_sub_topic = '/mavlink_rx' mavlink_lora_pub_topic = '/mavlink_tx' update_interval = 10 # defines MAVLINK_MSG_ID_PARAM_REQUEST_LIST = 21 MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN = 2 MAVLINK_MSG_ID_PARAM_VALUE = 22 # imports import rospy import struct from mavlink_lora.msg import mavlink_lora_msg # variables msg = mavlink_lora_msg() request_sent = False first_msg_ok = False target_sys = 0 # reset by first message target_comp = 0 def on_mavlink_msg(msg): global first_msg_ok global target_sys if first_msg_ok == False: first_msg_ok = True target_sys = msg.sys_id if msg.msg_id == MAVLINK_MSG_ID_PARAM_VALUE: (param_value, param_count, param_index, param_id,
def __init__(self): self.msg = mavlink_lora_msg()
def __init__(self): # initiate variables self.msg = mavlink_lora_msg() self.current_lat = 0 self.current_lon = 0 self.current_alt = 0 self.battery_volt = None self.min_batt_volt = 13.5 self.emergency = False self.emergency_prot_started = False self.battery_history = [] self.battery_average = None self.status = 0 # init GPIO GPIO.setmode(GPIO.BOARD) GPIO.setwarnings(False) GPIO.setup(38, GPIO.OUT) GPIO.setup(40, GPIO.OUT) # initiate node rospy.init_node(ros_node_name) self.rate = rospy.Rate(ros_node_update_interval) # publishers self.msg_to_fc_pub = rospy.Publisher(tx_to_fc_topic, mavlink_lora_msg, queue_size=0) self.msg_to_gc_pub = rospy.Publisher(tx_to_gc_topic, mavlink_lora_msg, queue_size=0) self.mission_pub = rospy.Publisher("/fc" + mission_topic, mavlink_lora_mission_list, queue_size=0) self.land_pub = rospy.Publisher(land_pub_topic, mavlink_lora_command_land, queue_size=0) self.mission_start_pub = rospy.Publisher( mission_start_topic, mavlink_lora_command_start_mission, queue_size=0) self.mission_clear_pub = rospy.Publisher(clear_all_topic, std_msgs.msg.Empty, queue_size=0) # subscribers rospy.Subscriber(rx_from_gc_topic, mavlink_lora_msg, self.incoming_message_from_gc_to_fc) rospy.Subscriber(rx_from_fc_topic, mavlink_lora_msg, self.incoming_message_from_fc_to_gc) rospy.Subscriber(status_sub_topic, mavlink_lora_status, self.status_callback) rospy.Subscriber(pos_sub_topic, mavlink_lora_pos, self.pos_callback) # rospy.Subscriber(mission_ack_topic, mavlink_lora_mission_ack, self.mission_ack) rospy.Subscriber(command_long_receive_topic, mavlink_lora_command_long, self.command_long_callback) rospy.Subscriber(heartbeat_topic_fc, mavlink_lora_heartbeat, self.heartbeat_callback) # wait until everything is running (important) rospy.sleep(3)
def __init__(self): self.msg = mavlink_lora_msg(sys_id=0, comp_id=0) self.target_sys = 1 self.target_comp = 1 self.command = ""
def __init__(self): self.msg = mavlink_lora_msg() self.setpoint_msg = mavlink_lora_local_setpoint() self.request_sent = False self.key_text = 'Unknown' # status variables self.batt_volt = 0.0 self.last_heard = 0 self.last_heard_sys_status = 0 self.lat = 0.0 self.lon = 0.0 self.alt = 0.0 self.home_lat = 0.0 self.home_lon = 0.0 self.yaw = 0.0 self.pitch = 0.0 self.roll = 0.0 self.custom_main_mode = '' self.custom_sub_mode = '' self.local_alt_target = 0 self.local_lat_target = 0 self.local_lon_target = 0 self.local_x = 0.0 self.local_y = 0.0 self.local_z = 0.0 self.local_vx = 0.0 self.local_vy = 0.0 self.local_vz = 0.0 # launch node rospy.init_node('gcs_node', disable_signals=True) self.mavlink_msg_pub = rospy.Publisher(mavlink_lora_pub_topic, mavlink_lora_msg, queue_size=0) rospy.Subscriber(mavlink_lora_sub_topic, mavlink_lora_msg, self.on_mavlink_msg) rospy.Subscriber(mavlink_lora_status_sub_topic, mavlink_lora_status, self.on_mavlink_lora_status) rospy.Subscriber(mavlink_lora_pos_sub_topic, mavlink_lora_pos, self.on_mavlink_lora_pos) rospy.Subscriber(mavlink_lora_atti_sub_topic, mavlink_lora_attitude, self.on_mavlink_lora_attitude) rospy.Subscriber(mavlink_lora_keypress_sub_topic, Int8, self.on_keypress) rospy.Subscriber(mavlink_lora_modes_pub_topic, mavlink_lora_modes, self.modes_sub) rospy.Subscriber(local_target_pub_topic, mavlink_lora_local_setpoint, self.local_target_sub) rospy.Subscriber(mavlink_lora_local_ned_pos_sub_topic, mavlink_lora_local_ned_pos, self.on_mavlink_lora_local_pos) self.modes_pub = rospy.Publisher( mavlink_lora_set_local_target_pub_topic, mavlink_lora_local_setpoint, queue_size=0) self.arm_pub = rospy.Publisher(drone_cmd_handler_arm_sub, Bool, queue_size=0) self.offboard_pub = rospy.Publisher(drone_cmd_handler_offboard_sub, Bool, queue_size=0) self.takeoff_pub = rospy.Publisher(drone_cmd_handler_takeoff_sub, Bool, queue_size=0) self.rate = rospy.Rate(update_interval) rospy.sleep(1) # wait until everything is running self.cmd = DroneCommandHandler()