def get_mavlink(m): # Get position, heading, and velocity msg = m.recv_match(type='GLOBAL_POSITION_INT', blocking=True) if not msg: return if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: #Message is valid params['lat'] = msg.lat / 10000000 params['lon'] = msg.lon / 10000000 params['hae'] = msg.alt / 1000 # TODO: Convert to HAE from MSL params['course'] = msg.hdg / 100 # TODO: Check mag/true params['speed'] = math.sqrt((msg.vx)**2 + (msg.vy)**2) / 100 # Get battery remaining msg = m.recv_match(type='SYS_STATUS', blocking=False) if not msg: return if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: #Message is valid params['remarks'] = 'Battery Remaining: ' + str( msg.battery_remaining) + '%'
def mavlink_message_loop(master, socket, plane_location): # wait for the heartbeat msg to find the system ID wait_heartbeat(master) request_data_streams(master, args.rate) feed_watchdog() wd_t = threading.Thread(target=mavlink_watchdog, args=(master,)) wd_t.daemon = True wd_t.start() '''show incoming mavlink messages''' while True: msg = master.recv_match(blocking=True) if not msg: return if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() elif msg.get_type() == "GLOBAL_POSITION_INT": plane_location.alt_asl_meters = msg.alt / 1000.0 plane_location.alt_agl_meters = msg.relative_alt / 1000.0 plane_location.lat_degrees = msg.lat / 10000000.0 plane_location.lon_degrees = msg.lon / 10000000.0 server_send_msg(socket, plane_location) feed_watchdog() elif msg.get_type() == "ATTITUDE": plane_location.roll_degrees = msg.roll * 180.0 / math.pi plane_location.pitch_degrees = msg.pitch * 180.0 / math.pi plane_location.yaw_degrees = msg.yaw * 180.0 / math.pi server_send_msg(socket, plane_location) feed_watchdog()
def handleMissionTransmission(self, m, wp): while True: mavlinkMSG = mavlink.MAVLink(m) msg = m.recv_match(blocking=True) if not msg: return msgID = msg.get_msgId() if msgID == mavlink.MAVLINK_MSG_ID_MISSION_REQUEST: print("New mission request object for: %u" % msg.seq) missionIndex = msg.seq mavlinkMSG.send(wp.wp(missionIndex)) if missionIndex == wp.count() - 1: print "I am done transmitting for this aircraft" break elif msgID == mavlink.MAVLINK_MSG_ID_BAD_DATA: if (mavutil.all_printable(msg.data)): sys.stdout.write(msg.data) sys.stdout.flush() elif msgID == mavlink.MAVLINK_MSG_ID_GLOBAL_POSITION_INT: aircraftLatitude = msg.lat aircraftLongitude = msg.lon else: print(msg)
def run(self): global radar_param '''show incoming mavlink messages''' i = 0 while not self.stop_event.is_set(): msg = self.m.recv_match(blocking=True) if not msg: return if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: msgid = msg.get_msgId() currenttime = monotonic.monotonic() c = self.count[msgid] p = self.period[msgid] l = self.last[msgid] d = currenttime - l if (self.last[msgid] != 0): self.maxp[msgid] = d if d > self.maxp[ msgid] else self.maxp[msgid] self.minp[msgid] = d if ( (d < self.minp[msgid]) or (self.minp[msgid] == 0)) else self.minp[msgid] self.period[msgid] = ((c * p) + d) / (c + 1) ++self.count[msgid] self.last[msgid] = currenttime print(i, msg, self.period[msgid]) i += 1 if msg.get_msgId() == mavlink.MAVLINK_MSG_ID_RADAR_PARAM: radar_param.set(msg)
def point_reached(self, blocking=False): point_reached = self.__mavlink_socket.recv_match( type='MISSION_ITEM_REACHED', blocking=blocking, timeout=self.__ack_timeout) if not point_reached: return False if point_reached.get_type() == "BAD_DATA": if mavutil.all_printable(point_reached.data): sys.stdout.write(point_reached.data) sys.stdout.flush() return False else: point_id = point_reached.seq if self.__prev_point_id is None: self.__prev_point_id = point_id new_point = True elif point_id > self.__prev_point_id: self.__prev_point_id = point_id new_point = True else: new_point = False if new_point: if self.__logger: print("point reached, id: ", point_id) return True else: return False
def read_pressure(mav_obj): """ Read accelerometer readings until taxis is exhausted. There will only be output once the total time has elapsed. """ pub = rospy.Publisher('/depth', FluidPressure, queue_size=10) rospy.init_node('externalpressure') rate = rospy.Rate(10) msg_type = 'SCALED_PRESSURE2' msg = mav_obj.recv_match(blocking=True) # flush out old data if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() while not rospy.is_shutdown(): msg = mav_obj.recv_match(type=msg_type, blocking=True) h = Header() h.stamp = rospy.Time.now() depth_m = (msg.press_abs - 1014.25) / 100 fp = FluidPressure(header=h, fluid_pressure=depth_m, variance=0) pub.publish(fp) rate.sleep()
def read_messages(mav_obj): """ Read accelerometer readings until taxis is exhausted. There will only be output once the total time has elapsed. """ msg = mav_obj.recv_match(blocking=True) if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): # flush out all bad data msgs at start sys.stdout.write(msg.data) sys.stdout.flush() else: print(msg) i = 0 while i < num_points: msg = mav_obj.recv_match(blocking=True) # if something catches your interest, pull out that msg type #msg = mav_obj.recv_match(type='GLOBAL_POSITION_INT', blocking=True) #msg = mav_obj.recv_match(type='SCALED_IMU2', blocking=True) #msg = mav_obj.recv_match(type='RAW_IMU', blocking=True) # uncomment if statement for parameters #if not(msg.get_msgId() == -1) and msg.name == 'PARAM_VALUE': #print(msg) print(msg) i += 1
def read_loop(m): while (True): # grab a mavlink message msg = m.recv_match(blocking=True) if not msg: return mdata = msg.to_dict() #print(mdata) # handle the message based on its type msg_type = msg.get_type() if msg_type == "BAD_DATA": if mavutil.all_printable(msg.data): print("Bad Data") #sys.stdout.write(msg.data) #sys.stdout.flush() """ elif msg_type == "RC_CHANNELS_RAW": handle_rc_raw(msg) elif msg_type == "HEARTBEAT": handle_heartbeat(msg) elif msg_type == "VFR_HUD": handle_hud(msg) print() elif msg_type == "ATTITUDE": handle_attitude(msg) print() """ if msg_type == "GLOBAL_POSITION_INT": print("Latitude : ", float(msg.lat) / 10000000) print("Longitude: ", float(msg.lon) / 10000000) print("Altitude : ", float(msg.alt) / 1000) print()
def read_loop(m): while(True): # grab a mavlink message msg = m.recv_match(blocking=False) if not msg: #print("returning!") continue # handle the message based on its type msg_type = msg.get_type() if msg_type == "BAD_DATA": print("inside bad_data") if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() elif msg_type == "RC_CHANNELS_RAW": print("INSIDE RC_CHANNELS RAW") handle_rc_raw(msg) elif msg_type == "HEARTBEAT": print("INSIDE HEARTBEAT") handle_heartbeat(msg) elif msg_type == "VFR_HUD": print("inside VFR HUD") handle_hud(msg) elif msg_type == "ATTITUDE": print("ATTITUDE") handle_attitude(msg)
def run(self): # continuous loop that monitors the information coming from the quadcopter while 1: # stores message from the quadcopter msg = self.master.recv_match(blocking=False) if msg: # stores the message type msg_type = msg.get_type() #print msg_type # debugging to check which data streams are operational # checks if the message contains information if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: # add in new messages to monitor below if msg_type == "ATTITUDE": # publishes information to the rostopic self.pub_attitude.publish(msg.roll * 180 / 3.1415, msg.pitch * 180 / 3.1415, msg.yaw * 180 / 3.1415, msg.rollspeed, msg.pitchspeed, msg.yawspeed) #upadtes imu values self.imu_yaw = msg.yaw * 180 / 3.14159 self.imu_roll = msg.roll * 180 / 3.14159 self.imu_pitch = msg.pitch * 180 / 3.14159 # updates the channel data elif msg_type == "RC_CHANNELS_RAW": self.ch1 = msg.chan1_raw self.ch2 = msg.chan2_raw self.ch3 = msg.chan3_raw self.ch4 = msg.chan4_raw self.ch5 = msg.chan5_raw self.ch6 = msg.chan6_raw self.ch7 = msg.chan7_raw self.ch8 = msg.chan8_raw #checks if channel 5 is below the threshold and gives back control to the transmitter if self.ch5 < 1400: if not self.transmitter_control: if self.have_control: # informs via terminal print "You no longer have control, Transmitter has taken over control" self.transmitter_control = True self.have_control = False # changes the control message on the ui ui.control_toggle_button.setText("Take Over Control") self.disconnect_pwm_channels() else: # turns off transmitter control so that the program is able to take over when requested self.transmitter_control = False time.sleep(0.01)
def validate_msg(msg): if msg == None: return False elif not msg or msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() return False else: return True
def show_messages(m): '''show incoming mavlink messages''' while True: msg = m.recv_match(blocking=True) if not msg: return if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: print(msg)
def run(self): while self.command_exit is False: m = self.master.recv_match(blocking=True) if not m: return if m.get_type() == "BAD_DATA": if mavutil.all_printable(m.data): sys.stdout.write(m.data) sys.stdout.flush() self.emit(SIGNAL("messageReceived"), m, self.conn) print "Returning from thread loop"
def show_messages(m): """show incoming mavlink messages""" while True: msg = m.recv_match(blocking=True) if not msg: return if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: print(msg)
def read_thread(self): while (self.running): msg = self.m.recv_match(blocking=True, timeout=1) if not msg: return if msg.get_type() == "BAD_DATA": print("bad data") if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: if self.cb: self.cb(msg)
def process_messages(self): """ This runs continuously. The mavutil.recv_match() function will call mavutil.post_message() any time a new message is received, and will notify all functions in the master.message_hooks list. """ while True: msg = self.master.recv_match(blocking=True) if not msg: return if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush()
def __ack_receive_point(self, blocking=False, timeout=None): if timeout is None: timeout = self.__ack_timeout ack = self.__mavlink_socket.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=blocking, timeout=timeout) if not ack: return False if ack.get_type() == "BAD_DATA": if mavutil.all_printable(ack.data): sys.stdout.write(ack.data) sys.stdout.flush() return False else: return True
def get_dist_sensor_data(self, blocking=False): dist_sensor_data = self.__mavlink_socket.recv_match(type='DISTANCE_SENSOR', blocking=blocking, timeout=self.__ack_timeout) if not dist_sensor_data: return if dist_sensor_data.get_type() == "BAD_DATA": if mavutil.all_printable(dist_sensor_data.data): sys.stdout.write(dist_sensor_data.data) sys.stdout.flush() return else: curr_distance = float(dist_sensor_data.current_distance)/100 # cm to m if self.__logger: print("get dist sensor data: %5.2f m" % curr_distance) return curr_distance
def get_local_position(self, blocking=False): position = self.__mavlink_socket.recv_match(type='POSITION_TARGET_LOCAL_NED', blocking=blocking, timeout=self.__ack_timeout) if not position: return if position.get_type() == "BAD_DATA": if mavutil.all_printable(position.data): sys.stdout.write(position.data) sys.stdout.flush() else: if self.__logger: print("X: {x}, Y: {y}, Z: {z}, YAW: {yaw}".format(x=position.x, y=position.y, z=-position.z, yaw=position.yaw)) return position
def run(self): global radar_param '''show incoming mavlink messages''' i = 0 while not self.stop_event.is_set(): msg = self.m.recv_match(blocking=True) if not msg: return if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: print(i, msg) i += 1 if msg.get_msgId() == mavlink.MAVLINK_MSG_ID_RADAR_PARAM: radar_param.set(msg)
def read_loop(self): while(True and not self.shutdownFlag): # grab a mavlink message msg = self.master.recv_match(blocking=False) if not msg: continue # handle the message based on its type msg_type = msg.get_type() if msg_type == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() elif msg_type == "HEARTBEAT": self.handle_heartbeat(msg) messagesFormated = {} for k, v in self.master.messages.iteritems(): if k == "MAV": continue if k == "PARAM_VALUE": continue messagesFormated[k] = v.to_dict() #global messages #messages = jsonpickle.encode(messagesFormated, unpicklable=False) #global params #params = jsonpickle.encode(self.master.params, unpicklable=False) formattedData = {} formattedData['params'] = self.master.params formattedData['messages'] = messagesFormated formattedData = jsonpickle.encode(formattedData, unpicklable=False) currentTime = time.time() if currentTime - self.time > 0.5: self.time = currentTime try: requests.post("http://127.0.0.1/autoPilotData", data=formattedData) except: print("Unexpected error:", sys.exc_info()) pass
def mainloop(): rospy.init_node('roscopter') while not rospy.is_shutdown(): rospy.sleep(0.001) msg = master.recv_match(blocking=False) if not msg: continue #print msg.get_type() if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: msg_type = msg.get_type() ''' if msg_type == "RC_CHANNELS_RAW" : pub_rc.publish([msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw, msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw]) ''' if msg_type == "HEARTBEAT": pub_state.publish(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED, msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED, mavutil.mode_string_v10(msg)) ''' if msg_type == "VFR_HUD": pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb) ''' if msg_type == "GPS_RAW_INT": fix = NavSatStatus.STATUS_NO_FIX if msg.fix_type >=3: fix=NavSatStatus.STATUS_FIX pub_gps.publish(NavSatFix(latitude = msg.lat/1e07, longitude = msg.lon/1e07, altitude = msg.alt/1e03, status = NavSatStatus(status=fix, service = NavSatStatus.SERVICE_GPS) )) #pub.publish(String("MSG: %s"%msg)) if msg_type == "ATTITUDE" : pub_attitude.publish(msg.roll, msg.pitch, msg.yaw, msg.rollspeed, msg.pitchspeed, msg.yawspeed) if msg_type == "LOCAL_POSITION_NED" : print "Local Pos: (%f %f %f) , (%f %f %f)" %(msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz) '''
def send_message(m): msg = m.recv_match(type='SYS_STATUS', blocking=True) if not msg: return if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): print("Bad data") sys.stdout.write(msg.data) sys.stdout.flush() else: # Message is valid # m.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, # mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0) m.param_set_send("WP_RADIUS", 101, mavlink1.MAV_PARAM_TYPE_REAL32) m.param_set_send("COPTER_MODE", 0) # Use the attribute # print('Mode: %s' % msg.mode) print("Message sent")
def handle_mission_transmission(self, waypoint_loader): self.in_mission = True while True: msg = self.mav_connection.recv_match(blocking=True) if not msg: return msg_id = msg.get_msgId() if msg_id == mavlink.MAVLINK_MSG_ID_MISSION_REQUEST: print("New mission request object for: %u" % msg.seq) mission_index = msg.seq self.mav_link.send(waypoint_loader.wp(mission_index)) if mission_index == waypoint_loader.count() - 1: print("I am done transmitting for this aircraft") break elif msg_id == mavlink.MAVLINK_MSG_ID_BAD_DATA: if mavutil.all_printable(msg.data): print('Bad data for mission', msg.data) else: print(msg) self.in_mission = False
def read_loop(m): messages = ['DEBUG_VECT', 'HEARTBEAT', 'LOCAL_POSITION_NED'] while (True): for i in messages: # grab a mavlink message msg = m.recv_match(type=i, blocking=True) if msg: # handle the message based on its type msg_type = msg.get_type() if msg_type == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: print("%s: %s" % (i, msg)) else: print("No message yet") time.sleep(1)
def mainloop(): rospy.init_node("roscopter") while not rospy.is_shutdown(): rospy.sleep(0.001) msg = master.recv_match(blocking=False) if not msg: continue if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: msg_type = msg.get_type() if msg_type == "RC_CHANNELS_RAW": global X0 # x->roll, y->pitch, z->throttle x_desired = remap(msg.chan1_raw, roll_radio_min, roll_radio_max, x_min, x_max) y_desired = remap(msg.chan2_raw, pitch_radio_min, pitch_radio_max, y_min, y_max) if msg.chan3_raw < throttle_radio_mid: z_desired = remap(msg.chan3_raw, throttle_radio_min, throttle_radio_mid, z_min, z_mid) else: z_desired = remap(msg.chan3_raw, throttle_radio_mid, throttle_radio_max, z_mid, z_max) X0[0], X0[1], X0[2] = x_desired, y_desired, z_desired
def read_loop(m): while (True): # grab a mavlink message msg = m.recv_match() if not msg: return # handle the message based on its type msg_type = msg.get_type() if msg_type == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() elif msg_type == "RC_CHANNELS_RAW": handle_rc_raw(msg) elif msg_type == "HEARTBEAT": handle_heartbeat(msg) elif msg_type == "VFR_HUD": handle_hud(msg) elif msg_type == "ATTITUDE": handle_attitude(msg)
def master_callback(self, m, master): '''process mavlink message m on master, sending any messages to recipients''' if getattr(m, '_timestamp', None) is None: master.post_message(m) self.status.counters['MasterIn'][master.linknum] += 1 mtype = m.get_type() # and log them if mtype not in dataPackets and self.mpstate.logqueue: # put link number in bottom 2 bits, so we can analyse packet # delay in saved logs usec = self.get_usec() usec = (usec & ~3) | master.linknum self.mpstate.logqueue.put( str(struct.pack('>Q', usec) + m.get_msgbuf())) # keep the last message of each type around self.status.msgs[m.get_type()] = m if not m.get_type() in self.status.msg_count: self.status.msg_count[m.get_type()] = 0 self.status.msg_count[m.get_type()] += 1 if m.get_srcComponent( ) == mavutil.mavlink.MAV_COMP_ID_GIMBAL and m.get_type( ) == 'HEARTBEAT': # silence gimbal heartbeat packets for now return if getattr(m, 'time_boot_ms', None) is not None: # update link_delayed attribute self.handle_msec_timestamp(m, master) if mtype in activityPackets: if master.linkerror: master.linkerror = False self.say("link %u OK" % (master.linknum + 1)) self.status.last_message = time.time() master.last_message = self.status.last_message if master.link_delayed: # don't process delayed packets that cause double reporting if mtype in delayedPackets: return if mtype == 'HEARTBEAT' and m.get_srcSystem() != 255: if self.settings.target_system == 0 and self.settings.target_system != m.get_srcSystem( ): self.settings.target_system = m.get_srcSystem() self.say("online system %u" % self.settings.target_system, 'message') if self.status.heartbeat_error: self.status.heartbeat_error = False self.say("heartbeat OK") if master.linkerror: master.linkerror = False self.say("link %u OK" % (master.linknum + 1)) self.status.last_heartbeat = time.time() master.last_heartbeat = self.status.last_heartbeat armed = self.master.motors_armed() if armed != self.status.armed: self.status.armed = armed if armed: self.say("ARMED") else: self.say("DISARMED") if master.flightmode != self.status.flightmode and time.time( ) > self.status.last_mode_announce + 2: self.status.flightmode = master.flightmode self.status.last_mode_announce = time.time() sysid = self.mav_param.get('SYSID_THISMAV') if sysid is None: sysid = "" else: sysid = str(int(sysid)) + ":" if self.mpstate.functions.input_handler is None: self.mpstate.rl.set_prompt(sysid + self.status.flightmode + "> ") self.say("Mode " + self.status.flightmode) if m.type == mavutil.mavlink.MAV_TYPE_FIXED_WING: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif m.type in [ mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT, mavutil.mavlink.MAV_TYPE_SUBMARINE ]: self.mpstate.vehicle_type = 'rover' self.mpstate.vehicle_name = 'APMrover2' elif m.type in [ mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER ]: self.mpstate.vehicle_type = 'copter' self.mpstate.vehicle_name = 'ArduCopter' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.mpstate.vehicle_type = 'antenna' self.mpstate.vehicle_name = 'AntennaTracker' elif mtype == 'STATUSTEXT': if m.text != self.status.last_apm_msg or time.time( ) > self.status.last_apm_msg_time + 2: self.mpstate.console.writeln("APM: %s" % m.text, bg='red') self.status.last_apm_msg = m.text self.status.last_apm_msg_time = time.time() elif mtype == "VFR_HUD": have_gps_lock = False if 'GPS_RAW' in self.status.msgs and self.status.msgs[ 'GPS_RAW'].fix_type == 2: have_gps_lock = True elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs[ 'GPS_RAW_INT'].fix_type == 3: have_gps_lock = True if have_gps_lock and not self.status.have_gps_lock and m.alt != 0: self.say("GPS lock at %u meters" % m.alt, priority='notification') self.status.have_gps_lock = True elif mtype == "GPS_RAW": if self.status.have_gps_lock: if m.fix_type != 2 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type == 2 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type == 2: self.status.last_gps_lock = time.time() elif mtype == "GPS_RAW_INT": if self.status.have_gps_lock: if m.fix_type < 3 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type >= 3 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type >= 3: self.status.last_gps_lock = time.time() elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout: rounded_dist = int(m.wp_dist / self.mpstate.settings.distreadout ) * self.mpstate.settings.distreadout if math.fabs(rounded_dist - self.status.last_distance_announce ) >= self.mpstate.settings.distreadout: if rounded_dist != 0: self.say("%u" % rounded_dist, priority="progress") self.status.last_distance_announce = rounded_dist elif mtype == "GLOBAL_POSITION_INT": self.report_altitude(m.relative_alt * 0.001) elif mtype == "COMPASSMOT_STATUS": print(m) elif mtype == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable( m.data): self.mpstate.console.write(str(m.data), bg='red') elif mtype in ["COMMAND_ACK", "MISSION_ACK"]: self.mpstate.console.writeln("Got MAVLink msg: %s" % m) if mtype == "COMMAND_ACK" and m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION: if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: self.say("Calibrated") else: #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass if self.status.watch is not None: if fnmatch.fnmatch(m.get_type().upper(), self.status.watch.upper()): self.mpstate.console.writeln('< ' + str(m)) # don't pass along bad data if mtype != 'BAD_DATA': # pass messages along to listeners, except for REQUEST_DATA_STREAM, which # would lead a conflict in stream rate setting between mavproxy and the other # GCS if self.mpstate.settings.mavfwd_rate or mtype != 'REQUEST_DATA_STREAM': if not mtype in self.no_fwd_types: for r in self.mpstate.mav_outputs: r.write(m.get_msgbuf()) # pass to modules for (mod, pm) in self.mpstate.modules: if not hasattr(mod, 'mavlink_packet'): continue try: mod.mavlink_packet(m) except Exception as msg: if self.mpstate.settings.moddebug == 1: print(msg) elif self.mpstate.settings.moddebug > 1: exc_type, exc_value, exc_traceback = sys.exc_info() traceback.print_exception(exc_type, exc_value, exc_traceback, limit=2, file=sys.stdout)
def master_callback(self, m, master): '''process mavlink message m on master, sending any messages to recipients''' if getattr(m, '_timestamp', None) is None: master.post_message(m) self.status.counters['MasterIn'][master.linknum] += 1 mtype = m.get_type() # and log them if mtype not in dataPackets and self.mpstate.logqueue: # put link number in bottom 2 bits, so we can analyse packet # delay in saved logs usec = self.get_usec() usec = (usec & ~3) | master.linknum self.mpstate.logqueue.put(str(struct.pack('>Q', usec) + m.get_msgbuf())) # keep the last message of each type around self.status.msgs[m.get_type()] = m if not m.get_type() in self.status.msg_count: self.status.msg_count[m.get_type()] = 0 self.status.msg_count[m.get_type()] += 1 if m.get_srcComponent() == mavutil.mavlink.MAV_COMP_ID_GIMBAL and m.get_type() == 'HEARTBEAT': # silence gimbal heartbeat packets for now return if getattr(m, 'time_boot_ms', None) is not None: # update link_delayed attribute self.handle_msec_timestamp(m, master) if mtype in activityPackets: if master.linkerror: master.linkerror = False self.say("link %u OK" % (master.linknum+1)) self.status.last_message = time.time() master.last_message = self.status.last_message if master.link_delayed: # don't process delayed packets that cause double reporting if mtype in delayedPackets: return if mtype == 'HEARTBEAT' and m.get_srcSystem() != 255: if self.settings.target_system == -1 and self.settings.target_system != m.get_srcSystem(): self.settings.target_system = m.get_srcSystem() self.say("online system %u" % self.settings.target_system,'message') if self.status.heartbeat_error: self.status.heartbeat_error = False self.say("heartbeat OK") if master.linkerror: master.linkerror = False self.say("link %u OK" % (master.linknum+1)) self.status.last_heartbeat = time.time() master.last_heartbeat = self.status.last_heartbeat armed = self.master.motors_armed() if armed != self.status.armed: self.status.armed = armed if armed: self.say("ARMED") else: self.say("DISARMED") if master.flightmode != self.status.flightmode and time.time() > self.status.last_mode_announce + 2: self.status.flightmode = master.flightmode self.status.last_mode_announce = time.time() self.mpstate.rl.set_prompt(self.status.flightmode + "> ") self.say("Mode " + self.status.flightmode) if m.type == mavutil.mavlink.MAV_TYPE_FIXED_WING: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT, mavutil.mavlink.MAV_TYPE_SUBMARINE]: self.mpstate.vehicle_type = 'rover' self.mpstate.vehicle_name = 'APMrover2' elif m.type in [mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER]: self.mpstate.vehicle_type = 'copter' self.mpstate.vehicle_name = 'ArduCopter' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.mpstate.vehicle_type = 'antenna' self.mpstate.vehicle_name = 'AntennaTracker' elif mtype == 'STATUSTEXT': if m.text != self.status.last_apm_msg or time.time() > self.status.last_apm_msg_time+2: self.mpstate.console.writeln("APM: %s" % m.text, bg='red') self.status.last_apm_msg = m.text self.status.last_apm_msg_time = time.time() elif mtype == "VFR_HUD": have_gps_lock = False if 'GPS_RAW' in self.status.msgs and self.status.msgs['GPS_RAW'].fix_type == 2: have_gps_lock = True elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs['GPS_RAW_INT'].fix_type == 3: have_gps_lock = True if have_gps_lock and not self.status.have_gps_lock and m.alt != 0: self.say("GPS lock at %u meters" % m.alt, priority='notification') self.status.have_gps_lock = True elif mtype == "GPS_RAW": if self.status.have_gps_lock: if m.fix_type != 2 and not self.status.lost_gps_lock and (time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type == 2 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type == 2: self.status.last_gps_lock = time.time() elif mtype == "GPS_RAW_INT": if self.status.have_gps_lock: if m.fix_type < 3 and not self.status.lost_gps_lock and (time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type >= 3 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type >= 3: self.status.last_gps_lock = time.time() elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout: rounded_dist = int(m.wp_dist/self.mpstate.settings.distreadout)*self.mpstate.settings.distreadout if math.fabs(rounded_dist - self.status.last_distance_announce) >= self.mpstate.settings.distreadout: if rounded_dist != 0: self.say("%u" % rounded_dist, priority="progress") self.status.last_distance_announce = rounded_dist elif mtype == "GLOBAL_POSITION_INT": self.report_altitude(m.relative_alt*0.001) elif mtype == "COMPASSMOT_STATUS": print(m) elif mtype == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable(m.data): self.mpstate.console.write(str(m.data), bg='red') elif mtype in [ "COMMAND_ACK", "MISSION_ACK" ]: self.mpstate.console.writeln("Got MAVLink msg: %s" % m) if mtype == "COMMAND_ACK" and m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION: if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: self.say("Calibrated") else: #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass if self.status.watch is not None: if fnmatch.fnmatch(m.get_type().upper(), self.status.watch.upper()): self.mpstate.console.writeln(m) # don't pass along bad data if mtype != 'BAD_DATA': # pass messages along to listeners, except for REQUEST_DATA_STREAM, which # would lead a conflict in stream rate setting between mavproxy and the other # GCS if self.mpstate.settings.mavfwd_rate or mtype != 'REQUEST_DATA_STREAM': if not mtype in self.no_fwd_types: for r in self.mpstate.mav_outputs: r.write(m.get_msgbuf()) # pass to modules for (mod,pm) in self.mpstate.modules: if not hasattr(mod, 'mavlink_packet'): continue try: mod.mavlink_packet(m) except Exception as msg: if self.mpstate.settings.moddebug == 1: print(msg) elif self.mpstate.settings.moddebug > 1: exc_type, exc_value, exc_traceback = sys.exc_info() traceback.print_exception(exc_type, exc_value, exc_traceback, limit=2, file=sys.stdout)
def __update_state(self, mtype, m, master): """Update our model state based on the received message""" if mtype == 'HEARTBEAT' and m.get_srcSystem() != 255: if (self.status.target_system != m.get_srcSystem() or self.status.target_component != m.get_srcComponent()): self.status.target_system = m.get_srcSystem() self.status.target_component = m.get_srcComponent() self.say( "online system %u component %u" % (self.status.target_system, self.status.target_component), 'message') if self.status.heartbeat_error: self.status.heartbeat_error = False self.say("heartbeat OK") if master.linkerror: master.linkerror = False self.say("link %u OK" % (master.linknum + 1)) self.status.last_heartbeat = time.time() master.last_heartbeat = self.status.last_heartbeat armed = self.master.motors_armed() if armed != self.status.armed: self.status.armed = armed if armed: self.say("ARMED") else: self.say("DISARMED") if master.flightmode != self.status.flightmode and time.time( ) > self.status.last_mode_announce + 2: self.status.flightmode = master.flightmode self.status.last_mode_announce = time.time() self.mpstate.rl.set_prompt(self.status.flightmode + "> ") self.say("Mode " + self.status.flightmode) if mtype == mavutil.mavlink.MAV_TYPE_FIXED_WING: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif mtype in [ mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT, mavutil.mavlink.MAV_TYPE_SUBMARINE ]: self.mpstate.vehicle_type = 'rover' self.mpstate.vehicle_name = 'APMrover2' elif mtype in [ mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER ]: self.mpstate.vehicle_type = 'copter' self.mpstate.vehicle_name = 'ArduCopter' elif mtype in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.mpstate.vehicle_type = 'antenna' self.mpstate.vehicle_name = 'AntennaTracker' elif mtype == 'STATUSTEXT': if m.text != self.status.last_apm_msg or time.time( ) > self.status.last_apm_msg_time + 2: self.mpstate.console.writeln("APM: %s" % m.text, bg='red') self.status.last_apm_msg = m.text self.status.last_apm_msg_time = time.time() elif mtype == "VFR_HUD": have_gps_lock = False if 'GPS_RAW' in self.status.msgs and self.status.msgs[ 'GPS_RAW'].fix_type == 2: have_gps_lock = True elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs[ 'GPS_RAW_INT'].fix_type == 3: have_gps_lock = True if have_gps_lock and not self.status.have_gps_lock and m.alt != 0: self.say("GPS lock at %u meters" % m.alt, priority='notification') self.status.have_gps_lock = True elif mtype == "GPS_RAW": if self.status.have_gps_lock: if m.fix_type != 2 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type == 2 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type == 2: self.status.last_gps_lock = time.time() elif mtype == "GPS_RAW_INT": if self.status.have_gps_lock: if m.fix_type < 3 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type >= 3 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type >= 3: self.status.last_gps_lock = time.time() elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout: rounded_dist = int(m.wp_dist / self.mpstate.settings.distreadout ) * self.mpstate.settings.distreadout if math.fabs(rounded_dist - self.status.last_distance_announce ) >= self.mpstate.settings.distreadout: if rounded_dist != 0: self.say("%u" % rounded_dist, priority="progress") self.status.last_distance_announce = rounded_dist elif mtype == "GLOBAL_POSITION_INT": self.report_altitude(m.relative_alt * 0.001) elif mtype == "COMPASSMOT_STATUS": print(m) elif mtype == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable( m.data): self.mpstate.console.write(str(m.data), bg='red') elif mtype == "MISSION_ACK": self.mpstate.console.writeln("Got MAVLink msg: %s" % m) elif mtype == "COMMAND_ACK" and m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION: if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: self.say("Calibrated") else: #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass
def master_msg_handling(self, m, master): '''link message handling for an upstream link''' if self.settings.target_system != 0 and m.get_srcSystem( ) != self.settings.target_system: # don't process messages not from our target if m.get_type() == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable( m.data): out = m.data if type(m.data) == bytearray: out = m.data.decode('ascii') self.mpstate.console.write(out, bg='red') return if self.settings.target_system != 0 and master.target_system != self.settings.target_system: # keep the pymavlink level target system aligned with the MAVProxy setting master.target_system = self.settings.target_system if self.settings.target_component != 0 and master.target_component != self.settings.target_component: # keep the pymavlink level target component aligned with the MAVProxy setting print("change target_component %u" % self.settings.target_component) master.target_component = self.settings.target_component mtype = m.get_type() if (mtype == 'HEARTBEAT' or mtype == 'HIGH_LATENCY2') and m.type != mavutil.mavlink.MAV_TYPE_GCS: if self.settings.target_system == 0 and self.settings.target_system != m.get_srcSystem( ): self.settings.target_system = m.get_srcSystem() self.say("online system %u" % self.settings.target_system, 'message') for mav in self.mpstate.mav_master: mav.target_system = self.settings.target_system if self.status.heartbeat_error: self.status.heartbeat_error = False self.say("heartbeat OK") if master.linkerror: master.linkerror = False self.say("link %s OK" % (self.link_label(master))) self.status.last_heartbeat = time.time() master.last_heartbeat = self.status.last_heartbeat armed = self.master.motors_armed() if armed != self.status.armed: self.status.armed = armed if armed: self.say("ARMED") else: self.say("DISARMED") if master.flightmode != self.status.flightmode: self.status.flightmode = master.flightmode if self.mpstate.functions.input_handler is None: self.set_prompt(self.status.flightmode + "> ") if master.flightmode != self.status.last_mode_announced and time.time( ) > self.status.last_mode_announce + 2: self.status.last_mode_announce = time.time() self.status.last_mode_announced = master.flightmode self.say("Mode " + self.status.flightmode) if m.type == mavutil.mavlink.MAV_TYPE_FIXED_WING: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif m.type in [ mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT ]: self.mpstate.vehicle_type = 'rover' self.mpstate.vehicle_name = 'APMrover2' elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]: self.mpstate.vehicle_type = 'sub' self.mpstate.vehicle_name = 'ArduSub' elif m.type in [ mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER, mavutil.mavlink.MAV_TYPE_DODECAROTOR ]: self.mpstate.vehicle_type = 'copter' self.mpstate.vehicle_name = 'ArduCopter' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.mpstate.vehicle_type = 'antenna' self.mpstate.vehicle_name = 'AntennaTracker' elif mtype == 'STATUSTEXT': class PendingText(object): def __init__(self): self.expected_count = None self.severity = None self.chunks = {} self.start_time = time.time() self.last_chunk_time = time.time() def add_chunk(self, m): # m is a statustext message self.severity = m.severity self.last_chunk_time = time.time() if hasattr(m, 'chunk_seq'): # mavlink extensions are present. chunk_seq = m.chunk_seq mid = m.id else: # Note that m.id may still exist! It will # contain the value 253, STATUSTEXT's mavlink # message id. Thus our reliance on the # presence of chunk_seq. chunk_seq = 0 mid = 0 self.chunks[chunk_seq] = m.text if len(m.text) != 50 or mid == 0: self.expected_count = chunk_seq + 1 def complete(self): return (self.expected_count is not None and self.expected_count == len(self.chunks)) def accumulated_statustext(self): next_expected_chunk = 0 out = "" for chunk_seq in sorted(self.chunks.keys()): if chunk_seq != next_expected_chunk: out += " ... " next_expected_chunk = chunk_seq out += self.chunks[chunk_seq] next_expected_chunk += 1 return out key = "%s.%s" % (m.get_srcSystem(), m.get_srcComponent()) if key not in self.status.statustexts_by_sysidcompid: self.status.statustexts_by_sysidcompid[key] = {} if hasattr(m, 'chunk_seq'): mid = m.id else: # m.id will have the value of 253, STATUSTEXT mavlink id mid = 0 if mid not in self.status.statustexts_by_sysidcompid[key]: self.status.statustexts_by_sysidcompid[key][mid] = PendingText( ) pending = self.status.statustexts_by_sysidcompid[key][mid] pending.add_chunk(m) if pending.complete(): # we have all of the chunks! self.emit_accumulated_statustext(key, mid, pending) elif mtype == "VFR_HUD": have_gps_lock = False if 'GPS_RAW' in self.status.msgs and self.status.msgs[ 'GPS_RAW'].fix_type == 2: have_gps_lock = True elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs[ 'GPS_RAW_INT'].fix_type == 3: have_gps_lock = True if have_gps_lock and not self.status.have_gps_lock and m.alt != 0: self.say("GPS lock at %u meters" % m.alt, priority='notification') self.status.have_gps_lock = True elif mtype == "GPS_RAW": if self.status.have_gps_lock: if m.fix_type != 2 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type == 2 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type == 2: self.status.last_gps_lock = time.time() elif mtype == "GPS_RAW_INT": if self.status.have_gps_lock: if m.fix_type < 3 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type >= 3 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type >= 3: self.status.last_gps_lock = time.time() elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout: rounded_dist = int(m.wp_dist / self.mpstate.settings.distreadout ) * self.mpstate.settings.distreadout if math.fabs(rounded_dist - self.status.last_distance_announce ) >= self.mpstate.settings.distreadout: if rounded_dist != 0: self.say("%u" % rounded_dist, priority="progress") self.status.last_distance_announce = rounded_dist elif mtype == "GLOBAL_POSITION_INT": self.report_altitude(m.relative_alt * 0.001) elif mtype == "COMPASSMOT_STATUS": print(m) elif mtype == "SIMSTATE": self.mpstate.is_sitl = True elif mtype == "ATTITUDE": att_time = m.time_boot_ms * 0.001 self.mpstate.attitude_time_s = max(self.mpstate.attitude_time_s, att_time) if self.mpstate.attitude_time_s - att_time > 120: # cope with wrap self.mpstate.attitude_time_s = att_time elif mtype == "COMMAND_ACK": try: cmd = mavutil.mavlink.enums["MAV_CMD"][m.command].name cmd = cmd[8:] res = mavutil.mavlink.enums["MAV_RESULT"][m.result].name res = res[11:] self.mpstate.console.writeln("Got COMMAND_ACK: %s: %s" % (cmd, res)) except Exception: self.mpstate.console.writeln("Got MAVLink msg: %s" % m) if m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION: if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: self.say("Calibrated") elif m.result == mavutil.mavlink.MAV_RESULT_FAILED: self.say("Calibration failed") elif m.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: self.say("Calibration unsupported") elif m.result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: self.say("Calibration temporarily rejected") else: self.say("Calibration response (%u)" % m.result) elif mtype == "MISSION_ACK": try: t = mavutil.mavlink.enums["MAV_MISSION_TYPE"][ m.mission_type].name t = t[12:] res = mavutil.mavlink.enums["MAV_MISSION_RESULT"][m.type].name res = res[12:] self.mpstate.console.writeln("Got MISSION_ACK: %s: %s" % (t, res)) except Exception as e: self.mpstate.console.writeln("Got MAVLink msg: %s" % m) else: #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass if self.status.watch is not None: for msg_type in self.status.watch: if fnmatch.fnmatch(mtype.upper(), msg_type.upper()): self.mpstate.console.writeln('< ' + str(m)) break
device = '/dev/tty.usbmodem1' streamrate = 15 from pymavlink import mavutil import sys # create a mavlink serial instance master = mavutil.mavlink_connection(device) print("waiting for heartbeat") master.wait_heartbeat() print("Heartbeat from APM (system %u component %u)" % (master.target_system, master.target_system)) print("Sending all stream request for rate %u" % streamrate) for i in range(0, 3): print "Sending..." master.mav.request_data_stream_send(master.target_system, master.target_component, mavutil.mavlink.MAV_DATA_STREAM_ALL, streamrate, 1) print("Receiving messages") while True: msg = master.recv_match(type='ATTITUDE', blocking=True) if not msg: break if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: print(msg)
def next_message(self): '''called as each msg is ready''' msg = self.msg if msg is None: self.paused = True if self.paused: self.root.after(100, self.next_message) return try: speed = float(self.playback.get()) except: speed = 0.0 timestamp = getattr(msg, '_timestamp') now = time.strftime("%H:%M:%S", time.localtime(timestamp)) self.clock.configure(text=now) if speed == 0.0: self.root.after(200, self.next_message) else: self.root.after( int(1000 * (timestamp - self.last_timestamp) / speed), self.next_message) self.last_timestamp = timestamp while True: self.msg = self.mlog.recv_match(condition=args.condition) if self.msg is None and self.mlog.f.tell() > self.filesize - 10: self.paused = True return if self.msg is not None and self.msg.get_type() != "BAD_DATA": break pos = float(self.mlog.f.tell()) / self.filesize self.slider.set(pos) self.filepos = self.slider.get() if msg.get_type() != "BAD_DATA": for m in self.mout: m.write(msg.get_msgbuf()) if msg.get_type() == "GPS_RAW": self.fdm.set('latitude', msg.lat, units='degrees') self.fdm.set('longitude', msg.lon, units='degrees') if args.gpsalt: self.fdm.set('altitude', msg.alt, units='meters') if msg.get_type() == "GPS_RAW_INT": self.fdm.set('latitude', msg.lat / 1.0e7, units='degrees') self.fdm.set('longitude', msg.lon / 1.0e7, units='degrees') if args.gpsalt: self.fdm.set('altitude', msg.alt / 1.0e3, units='meters') if msg.get_type() == "VFR_HUD": if not args.gpsalt: self.fdm.set('altitude', msg.alt, units='meters') self.fdm.set('num_engines', 1) self.fdm.set('vcas', msg.airspeed, units='mps') if msg.get_type() == "ATTITUDE": self.fdm.set('phi', msg.roll, units='radians') self.fdm.set('theta', msg.pitch, units='radians') self.fdm.set('psi', msg.yaw, units='radians') self.fdm.set('phidot', msg.rollspeed, units='rps') self.fdm.set('thetadot', msg.pitchspeed, units='rps') self.fdm.set('psidot', msg.yawspeed, units='rps') if msg.get_type() == "RC_CHANNELS_SCALED": self.fdm.set("right_aileron", msg.chan1_scaled * 0.0001) self.fdm.set("left_aileron", -msg.chan1_scaled * 0.0001) self.fdm.set("rudder", msg.chan4_scaled * 0.0001) self.fdm.set("elevator", msg.chan2_scaled * 0.0001) self.fdm.set('rpm', msg.chan3_scaled * 0.01) if msg.get_type() == 'STATUSTEXT': print("APM: %s" % msg.text) if msg.get_type() == 'SYS_STATUS': self.flightmode.configure(text=self.mlog.flightmode) if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() if self.fdm.get('latitude') != 0: for f in self.fgout: f.write(self.fdm.pack())
def master_msg_handling(self, m, master): '''link message handling for an upstream link''' if self.settings.target_system != 0 and m.get_srcSystem() != self.settings.target_system: # don't process messages not from our target if m.get_type() == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable(m.data): self.mpstate.console.write(str(m.data), bg='red') return if self.settings.target_system != 0 and master.target_system != self.settings.target_system: # keep the pymavlink level target system aligned with the MAVProxy setting master.target_system = self.settings.target_system if self.settings.target_component != 0 and master.target_component != self.settings.target_component: # keep the pymavlink level target component aligned with the MAVProxy setting print("change target_component %u" % self.settings.target_component) master.target_component = self.settings.target_component mtype = m.get_type() if mtype == 'HEARTBEAT' and m.type != mavutil.mavlink.MAV_TYPE_GCS: if self.settings.target_system == 0 and self.settings.target_system != m.get_srcSystem(): self.settings.target_system = m.get_srcSystem() self.say("online system %u" % self.settings.target_system,'message') for mav in self.mpstate.mav_master: mav.target_system = self.settings.target_system if self.status.heartbeat_error: self.status.heartbeat_error = False self.say("heartbeat OK") if master.linkerror: master.linkerror = False self.say("link %s OK" % (self.link_label(master))) self.status.last_heartbeat = time.time() master.last_heartbeat = self.status.last_heartbeat armed = self.master.motors_armed() if armed != self.status.armed: self.status.armed = armed if armed: self.say("ARMED") else: self.say("DISARMED") if master.flightmode != self.status.flightmode: self.status.flightmode = master.flightmode if self.mpstate.functions.input_handler is None: self.set_prompt(self.status.flightmode + "> ") if master.flightmode != self.status.last_mode_announced and time.time() > self.status.last_mode_announce + 2: self.status.last_mode_announce = time.time() self.status.last_mode_announced = master.flightmode self.say("Mode " + self.status.flightmode) if m.type == mavutil.mavlink.MAV_TYPE_FIXED_WING: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT]: self.mpstate.vehicle_type = 'rover' self.mpstate.vehicle_name = 'APMrover2' elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]: self.mpstate.vehicle_type = 'sub' self.mpstate.vehicle_name = 'ArduSub' elif m.type in [mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER, mavutil.mavlink.MAV_TYPE_DODECAROTOR]: self.mpstate.vehicle_type = 'copter' self.mpstate.vehicle_name = 'ArduCopter' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.mpstate.vehicle_type = 'antenna' self.mpstate.vehicle_name = 'AntennaTracker' elif mtype == 'STATUSTEXT': if m.text != self.status.last_apm_msg or time.time() > self.status.last_apm_msg_time+2: (fg, bg) = self.colors_for_severity(m.severity) self.mpstate.console.writeln("APM: %s" % mp_util.null_term(m.text), bg=bg, fg=fg) self.status.last_apm_msg = m.text self.status.last_apm_msg_time = time.time() elif mtype == "VFR_HUD": have_gps_lock = False if 'GPS_RAW' in self.status.msgs and self.status.msgs['GPS_RAW'].fix_type == 2: have_gps_lock = True elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs['GPS_RAW_INT'].fix_type == 3: have_gps_lock = True if have_gps_lock and not self.status.have_gps_lock and m.alt != 0: self.say("GPS lock at %u meters" % m.alt, priority='notification') self.status.have_gps_lock = True elif mtype == "GPS_RAW": if self.status.have_gps_lock: if m.fix_type != 2 and not self.status.lost_gps_lock and (time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type == 2 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type == 2: self.status.last_gps_lock = time.time() elif mtype == "GPS_RAW_INT": if self.status.have_gps_lock: if m.fix_type < 3 and not self.status.lost_gps_lock and (time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type >= 3 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type >= 3: self.status.last_gps_lock = time.time() elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout: rounded_dist = int(m.wp_dist/self.mpstate.settings.distreadout)*self.mpstate.settings.distreadout if math.fabs(rounded_dist - self.status.last_distance_announce) >= self.mpstate.settings.distreadout: if rounded_dist != 0: self.say("%u" % rounded_dist, priority="progress") self.status.last_distance_announce = rounded_dist elif mtype == "GLOBAL_POSITION_INT": self.report_altitude(m.relative_alt*0.001) elif mtype == "COMPASSMOT_STATUS": print(m) elif mtype == "SIMSTATE": self.mpstate.is_sitl = True elif mtype == "ATTITUDE": att_time = m.time_boot_ms * 0.001 self.mpstate.attitude_time_s = max(self.mpstate.attitude_time_s, att_time) if self.mpstate.attitude_time_s - att_time > 120: # cope with wrap self.mpstate.attitude_time_s = att_time elif mtype in [ "COMMAND_ACK", "MISSION_ACK" ]: self.mpstate.console.writeln("Got MAVLink msg: %s" % m) if mtype == "COMMAND_ACK" and m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION: if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: self.say("Calibrated") elif m.result == mavutil.mavlink.MAV_RESULT_FAILED: self.say("Calibration failed") elif m.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: self.say("Calibration unsupported") elif m.result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: self.say("Calibration temporarily rejected") else: self.say("Calibration response (%u)" % m.result) else: #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass if self.status.watch is not None: for msg_type in self.status.watch: if fnmatch.fnmatch(mtype.upper(), msg_type.upper()): self.mpstate.console.writeln('< '+ str(m)) break
def loop(m): #definisanje promenljivih pre petlje mav = at.MAVLink(m) xaxis_output = 0 yaxis_output = 0 zaxis_output = 0 xraxis_output = 0 yraxis_output = 0 zraxis_output = 0 counter = 0 tracker_types = [ 'BOOSTING', 'MIL', 'KCF', 'TLD', 'MEDIANFLOW', 'GOTURN' ] #Biramo algoritam za pracenje objekta, a objekat pravimo pomocu BoundingBox-a tracker_type = tracker_types[2] if int(minor_ver) < 3: tracker = cv2.Tracker_create(tracker_type) else: if tracker_type == 'BOOSTING': tracker = cv2.TrackerBoosting_create() if tracker_type == 'MIL': tracker = cv2.TrackerMIL_create() if tracker_type == 'KCF': tracker = cv2.TrackerKCF_create() if tracker_type == 'TLD': tracker = cv2.TrackerTLD_create() if tracker_type == 'MEDIANFLOW': tracker = cv2.TrackerMedianFlow_create() if tracker_type == 'GOTURN': tracker = cv2.TrackerGOTURN_create() # Read video video = cv2.VideoCapture( "udp://127.0.0.1:10000" ) #cita se video iz memorije, prethodno mora biti pokrenut gopro fajl da cuva lajv strim na toj memoriskoj lokaciji # Exit if video not opened. if not video.isOpened(): print("Could not open video") #sys.exit() # Read first frame. ok, frame = video.read() if not ok: print('Cannot read video file') sys.exit() # Define an initial bounding box bbox = (287, 23, 86, 320) # Uncomment the line below to select a different bounding box bbox = cv2.selectROI(frame, False) # Initialize tracker with first frame and bounding box ok = tracker.init(frame, bbox) #end of new part a = 0.5 xaxis_output = 0 yaxis_output = 0 zaxis_output = 0 while (True): #new part # Read a new frame xaxis_outputNEW = 0 yaxis_outputNEW = 0 zaxis_outputNEW = 0 xraxis_output = 0 yraxis_output = 0 zraxis_output = 0 ok, frame = video.read() if not ok: # U slucaju da ima problem sa citanjem lajv strima da preskoci iteraciju obrade i pokusa opet continue # Start timer timer = cv2.getTickCount() # Update tracker ok, bbox = tracker.update(frame) # Calculate Frames per second (FPS) fps = cv2.getTickFrequency() / (cv2.getTickCount() - timer) # Draw bounding box if ok: # Tracking success p1 = (int(bbox[0]), int(bbox[1])) p2 = (int(bbox[0] + bbox[2]), int(bbox[1] + bbox[3])) cv2.rectangle(frame, p1, p2, (255, 0, 0), 2, 1) else: # Tracking failure cv2.putText(frame, "Tracking failure detected", (100, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2) # Display tracker type on frame cv2.putText(frame, tracker_type + " Tracker", (100, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2) # Display FPS on frame cv2.putText(frame, "FPS : " + str(int(fps)), (100, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (50, 170, 50), 2) # Display result cv2.imshow("Tracking", frame) # Exit if ESC pressed k = cv2.waitKey(1) & 0xff #provera tastera pritisnutog na tastaturi if k == 27: break if k == 112: #Menjamo Bounding Box i biramo novi objekat sa screenshot-a lajv strima koji zelimo da pratimo if tracker_type == 'BOOSTING': tracker = cv2.TrackerBoosting_create() if tracker_type == 'MIL': tracker = cv2.TrackerMIL_create() if tracker_type == 'KCF': tracker = cv2.TrackerKCF_create() if tracker_type == 'TLD': tracker = cv2.TrackerTLD_create() if tracker_type == 'MEDIANFLOW': tracker = cv2.TrackerMedianFlow_create() if tracker_type == 'GOTURN': tracker = cv2.TrackerGOTURN_create() ok, frame = video.read() bbox = cv2.selectROI(frame, False) ok = tracker.init(frame, bbox) ok, bbox = tracker.update(frame) #end of new part # grab a mavlink message msg = m.recv_match(blocking=False) if not msg: #discard this message continue # handle the message based on its type msg_type = msg.get_type() if msg_type == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() elif msg_type == "HEARTBEAT": handle_heartbeat(msg) elif msg_type == "ATTITUDE": handle_attitude(msg) # if msg.roll > 0 and xaxis_output < 32757: # xaxis_output += 10 # elif msg.roll < 0 and xaxis_output > -32757: # xaxis_output -= 10 if k == 119: #forward picture_mode || xaxis_output = 32757 xaxis_outputNEW = 32757 if k == 115: #backwards xaxis_outputNEW = -32757 if k == 97: #left yaxis_outputNEW = 32757 if k == 100: #right yaxis_outputNEW = -32757 if k == 113: #up zaxis_outputNEW = -32757 if k == 101: #down zaxis_outputNEW = 32757 if k == 108: #take a picture ok, frame = video.read() cv2.imwrite("frame%d.jpg" % counter, frame) print(bbox) if ( bbox[0] + (bbox[2] / 2) ) > 210: #gledamo da li je BoundingBox previse desno i u slucaju da jeste kazemo dronu da ide brze desno da odrzi boundingbox u centru print('too far right') zraxis_output = 12000 # zraxis_output = -(((bbox[0] - 240)/160)*(-32760)) Pokusaj skaliranja koji nije bio optimizovan da dron sto je BoundingBox dalje od centra da ga jacim intenzitetom vraca ka centru # if zraxis_output < -32760 : # zraxis_output = -32760 # if zraxis_output > 0 : # zraxis_output = 0 elif (bbox[0] + (bbox[2] / 2)) < 190: print('too far left') zraxis_output = -12000 # zraxis_output = -(32760 - (32760*(bbox[0]))/160) # if zraxis_output > 32760 : # zraxis_output = 32760 # if zraxis_output < 0 : # zraxis_output = 0 if (bbox[1] + (bbox[3] / 2)) > 110: print('too far down') yraxis_output = -8000 # yraxis_output = 32760 - 32760*(200-bbox[1])/50 #check the sign # if yraxis_output > 32760 : # yraxis_output = 32760 # if yraxis_output < 0 : # yraxis_output = 0 elif (bbox[1] + (bbox[3] / 2)) < 90: print('too far up') yraxis_output = 8000 # yraxis_output = -32760 + (bbox[1]*32760)/50 #check the sign # if yraxis_output < -32760 : # yraxis_output = -32760 # if yraxis_output > 0 : # yraxis_output = 0 if bbox[1] == 0: #Ako nema nikakav BoundingBox ili ako se izgubi da ne radi nista yraxis_output = 0 if bbox[0] == 0: zraxis_output = 0 xaxis_output = xaxis_output * a + xaxis_outputNEW * ( 1 - a ) #Skaliranje kretanja drona na komande tako da sto duze drzis dugme sto se vise dron ubrzava yaxis_output = yaxis_output * a + yaxis_outputNEW * (1 - a) zaxis_output = zaxis_output * a + zaxis_outputNEW * (1 - a) # print("y axis: ", yraxis_output) print("z axis: ", zraxis_output) # print() mav.setpoint_6dof_send(int(xaxis_output), int(yaxis_output), int(zaxis_output), int(xraxis_output), int(yraxis_output), int(zraxis_output), counter) counter += 1
def __update_state(self, mtype, m, master): """Update our model state based on the received message""" if mtype == 'HEARTBEAT' and m.get_srcSystem() != 255: if self.settings.target_system == 0 and self.settings.target_system != m.get_srcSystem(): self.settings.target_system = m.get_srcSystem() self.say("online system %u" % self.settings.target_system,'message') if self.status.heartbeat_error: self.status.heartbeat_error = False self.say("heartbeat OK") if master.linkerror: master.linkerror = False self.say("link %u OK" % (master.linknum+1)) self.status.last_heartbeat = time.time() master.last_heartbeat = self.status.last_heartbeat armed = self.master.motors_armed() if armed != self.status.armed: self.status.armed = armed if armed: self.say("ARMED") else: self.say("DISARMED") if master.flightmode != self.status.flightmode and time.time() > self.status.last_mode_announce + 2: self.status.flightmode = master.flightmode self.status.last_mode_announce = time.time() self.mpstate.rl.set_prompt(self.status.flightmode + "> ") self.say("Mode " + self.status.flightmode) if mtype == mavutil.mavlink.MAV_TYPE_FIXED_WING: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif mtype in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT, mavutil.mavlink.MAV_TYPE_SUBMARINE]: self.mpstate.vehicle_type = 'rover' self.mpstate.vehicle_name = 'APMrover2' elif mtype in [mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER]: self.mpstate.vehicle_type = 'copter' self.mpstate.vehicle_name = 'ArduCopter' elif mtype in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.mpstate.vehicle_type = 'antenna' self.mpstate.vehicle_name = 'AntennaTracker' elif mtype == 'STATUSTEXT': if m.text != self.status.last_apm_msg or time.time() > self.status.last_apm_msg_time+2: self.mpstate.console.writeln("APM: %s" % m.text, bg='red') self.status.last_apm_msg = m.text self.status.last_apm_msg_time = time.time() elif mtype == "VFR_HUD": have_gps_lock = False if 'GPS_RAW' in self.status.msgs and self.status.msgs['GPS_RAW'].fix_type == 2: have_gps_lock = True elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs['GPS_RAW_INT'].fix_type == 3: have_gps_lock = True if have_gps_lock and not self.status.have_gps_lock and m.alt != 0: self.say("GPS lock at %u meters" % m.alt, priority='notification') self.status.have_gps_lock = True elif mtype == "GPS_RAW": if self.status.have_gps_lock: if m.fix_type != 2 and not self.status.lost_gps_lock and (time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type == 2 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type == 2: self.status.last_gps_lock = time.time() elif mtype == "GPS_RAW_INT": if self.status.have_gps_lock: if m.fix_type < 3 and not self.status.lost_gps_lock and (time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type >= 3 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type >= 3: self.status.last_gps_lock = time.time() elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout: rounded_dist = int(m.wp_dist/self.mpstate.settings.distreadout)*self.mpstate.settings.distreadout if math.fabs(rounded_dist - self.status.last_distance_announce) >= self.mpstate.settings.distreadout: if rounded_dist != 0: self.say("%u" % rounded_dist, priority="progress") self.status.last_distance_announce = rounded_dist elif mtype == "GLOBAL_POSITION_INT": self.report_altitude(m.relative_alt*0.001) elif mtype == "COMPASSMOT_STATUS": print(m) elif mtype == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable(m.data): self.mpstate.console.write(str(m.data), bg='red') elif mtype == "MISSION_ACK": self.mpstate.console.writeln("Got MAVLink msg: %s" % m) elif mtype == "COMMAND_ACK" and m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION: if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: self.say("Calibrated") else: #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass
def handle_bad_data(self, msg): if mavutil.all_printable(msg.data): sys.stderr.write(msg.data) sys.stderr.flush()
def master_msg_handling(self, m, master): '''link message handling for an upstream link''' if self.settings.target_system != 0 and m.get_srcSystem( ) != self.settings.target_system: # don't process messages not from our target if m.get_type() == "BAD_DATA": if self.mpstate.settings.shownoise and mavutil.all_printable( m.data): out = m.data if type(m.data) == bytearray: out = m.data.decode('ascii') self.mpstate.console.write(out, bg='red') return if self.settings.target_system != 0 and master.target_system != self.settings.target_system: # keep the pymavlink level target system aligned with the MAVProxy setting master.target_system = self.settings.target_system if self.settings.target_component != 0 and master.target_component != self.settings.target_component: # keep the pymavlink level target component aligned with the MAVProxy setting print("change target_component %u" % self.settings.target_component) master.target_component = self.settings.target_component mtype = m.get_type() if mtype == 'HEARTBEAT' and m.type != mavutil.mavlink.MAV_TYPE_GCS: if self.settings.target_system == 0 and self.settings.target_system != m.get_srcSystem( ): self.settings.target_system = m.get_srcSystem() self.say("online system %u" % self.settings.target_system, 'message') for mav in self.mpstate.mav_master: mav.target_system = self.settings.target_system if self.status.heartbeat_error: self.status.heartbeat_error = False self.say("heartbeat OK") if master.linkerror: master.linkerror = False self.say("link %s OK" % (self.link_label(master))) self.status.last_heartbeat = time.time() master.last_heartbeat = self.status.last_heartbeat armed = self.master.motors_armed() if armed != self.status.armed: self.status.armed = armed if armed: self.say("ARMED") else: self.say("DISARMED") if master.flightmode != self.status.flightmode: self.status.flightmode = master.flightmode if self.mpstate.functions.input_handler is None: self.set_prompt(self.status.flightmode + "> ") if master.flightmode != self.status.last_mode_announced and time.time( ) > self.status.last_mode_announce + 2: self.status.last_mode_announce = time.time() self.status.last_mode_announced = master.flightmode self.say("Mode " + self.status.flightmode) if m.type == mavutil.mavlink.MAV_TYPE_FIXED_WING: self.mpstate.vehicle_type = 'plane' self.mpstate.vehicle_name = 'ArduPlane' elif m.type in [ mavutil.mavlink.MAV_TYPE_GROUND_ROVER, mavutil.mavlink.MAV_TYPE_SURFACE_BOAT ]: self.mpstate.vehicle_type = 'rover' self.mpstate.vehicle_name = 'APMrover2' elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]: self.mpstate.vehicle_type = 'sub' self.mpstate.vehicle_name = 'ArduSub' elif m.type in [ mavutil.mavlink.MAV_TYPE_QUADROTOR, mavutil.mavlink.MAV_TYPE_COAXIAL, mavutil.mavlink.MAV_TYPE_HEXAROTOR, mavutil.mavlink.MAV_TYPE_OCTOROTOR, mavutil.mavlink.MAV_TYPE_TRICOPTER, mavutil.mavlink.MAV_TYPE_HELICOPTER, mavutil.mavlink.MAV_TYPE_DODECAROTOR ]: self.mpstate.vehicle_type = 'copter' self.mpstate.vehicle_name = 'ArduCopter' elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]: self.mpstate.vehicle_type = 'antenna' self.mpstate.vehicle_name = 'AntennaTracker' elif mtype == 'STATUSTEXT': if m.text != self.status.last_apm_msg or time.time( ) > self.status.last_apm_msg_time + 2: (fg, bg) = self.colors_for_severity(m.severity) self.mpstate.console.writeln("APM: %s" % mp_util.null_term(m.text), bg=bg, fg=fg) awsprint("APM: %s" % mp_util.null_term(m.text), bg=bg, fg=fg) self.status.last_apm_msg = m.text self.status.last_apm_msg_time = time.time() elif mtype == "VFR_HUD": have_gps_lock = False if 'GPS_RAW' in self.status.msgs and self.status.msgs[ 'GPS_RAW'].fix_type == 2: have_gps_lock = True elif 'GPS_RAW_INT' in self.status.msgs and self.status.msgs[ 'GPS_RAW_INT'].fix_type == 3: have_gps_lock = True if have_gps_lock and not self.status.have_gps_lock and m.alt != 0: self.say("GPS lock at %u meters" % m.alt, priority='notification') self.status.have_gps_lock = True elif mtype == "GPS_RAW": if self.status.have_gps_lock: if m.fix_type != 2 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type == 2 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type == 2: self.status.last_gps_lock = time.time() elif mtype == "GPS_RAW_INT": if self.status.have_gps_lock: if m.fix_type < 3 and not self.status.lost_gps_lock and ( time.time() - self.status.last_gps_lock) > 3: self.say("GPS fix lost") self.status.lost_gps_lock = True if m.fix_type >= 3 and self.status.lost_gps_lock: self.say("GPS OK") self.status.lost_gps_lock = False if m.fix_type >= 3: self.status.last_gps_lock = time.time() elif mtype == "NAV_CONTROLLER_OUTPUT" and self.status.flightmode == "AUTO" and self.mpstate.settings.distreadout: rounded_dist = int(m.wp_dist / self.mpstate.settings.distreadout ) * self.mpstate.settings.distreadout if math.fabs(rounded_dist - self.status.last_distance_announce ) >= self.mpstate.settings.distreadout: if rounded_dist != 0: self.say("%u" % rounded_dist, priority="progress") self.status.last_distance_announce = rounded_dist elif mtype == "GLOBAL_POSITION_INT": self.report_altitude(m.relative_alt * 0.001) elif mtype == "COMPASSMOT_STATUS": print(m) elif mtype == "SIMSTATE": self.mpstate.is_sitl = True elif mtype == "ATTITUDE": att_time = m.time_boot_ms * 0.001 self.mpstate.attitude_time_s = max(self.mpstate.attitude_time_s, att_time) if self.mpstate.attitude_time_s - att_time > 120: # cope with wrap self.mpstate.attitude_time_s = att_time elif mtype in ["COMMAND_ACK", "MISSION_ACK"]: self.mpstate.console.writeln("Got MAVLink msg: %s" % m) awsprint("Got MAVLink msg: %s" % m) if mtype == "COMMAND_ACK" and m.command == mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION: if m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: self.say("Calibrated") elif m.result == mavutil.mavlink.MAV_RESULT_FAILED: self.say("Calibration failed") elif m.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: self.say("Calibration unsupported") elif m.result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: self.say("Calibration temporarily rejected") else: self.say("Calibration response (%u)" % m.result) else: #self.mpstate.console.writeln("Got MAVLink msg: %s" % m) pass if self.status.watch is not None: for msg_type in self.status.watch: if fnmatch.fnmatch(mtype.upper(), msg_type.upper()): self.mpstate.console.writeln('< ' + str(m)) break
def mainloop(): print("Waiting") global hz_counter hz_counter=1; trate = 1.0/rate first_time_imu=True first_time_pos=True earliercmdtime = 0 ## Set home values from Vicon/Kinect data ## rospy.sleep(.1) set_home() ## Integration and differentiation error initialized ## I_ex=0.0; I_ey=0.0; I_ez=0.0 D_ex=0.0; D_ey=0.0; D_ez=0.0 new_err_x=0.0; new_err_y=0.0; new_err_z=0.0; ## IMU values initialized ## imu_roll=0.0; imu_pitch=0.0; first_yaw=0.0; imu_yaw=0.0; first_roll=0.0; first_pitch=0.0 ## Position values initialised ## pos_x=0.0; pos_y=0.0; pos_z=0.0 while not rospy.is_shutdown(): # start loop #continue if float(time.time())-trate >= earliercmdtime: earliercmdtime = time.time() hz_counter =1; if connect_APM: # enable these lines if you want to obtain the data from the arducopter on-the-fly # msg = master.recv_match(blocking=False) # if not msg: # continue if 0:#msg: if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: msg_type = msg.get_type() if msg_type == "ATTITUDE" : pub_attitude.publish(msg.roll*180/3.1415, msg.pitch*180/3.1415, msg.yaw*180/3.1415, msg.rollspeed, msg.pitchspeed, msg.yawspeed) if first_time_imu: first_time_imu=False first_yaw = msg.yaw first_roll = msg.roll first_pitch = msg.pitch imu_yaw = msg.yaw imu_roll = msg.roll imu_pitch = msg.pitch else: imu_yaw = msg.yaw imu_roll = msg.roll imu_pitch = msg.pitch ## Update position values ## (old_pos_x,old_pos_y,old_pos_z,pos_x,pos_y,pos_z) = update_pos_values(pos_x,pos_y,pos_z,vicon_x,vicon_y,vicon_z,first_time_pos) (err_x,err_y,err_z) = calc_pos_error(home_x,home_y,home_z,pos_x,pos_y,pos_z) # home - pos (d_x, d_y, d_z) = calc_pos_error(old_pos_x,old_pos_y,old_pos_z,pos_x,pos_y,pos_z) ## Toggle for first time through loop ## if first_time_pos: first_time_pos=False ## PID controller (roll+pitch and thrust, yaw later) ## I_ex = I_ex + err_x*trate I_ey = I_ey + err_y*trate I_ez = I_ez + err_z*trate V_x = d_x*trate # units of m/s V_y = d_y*trate # units of m/s V_z = d_z*trate # units of m/s ## Cap the integration error as well after certain value ## if (I_ex) > I_ex_MAX: I_ex = I_ex_MAX #* I_ex / abs(I_ex) if (I_ex) < -I_ex_MAX: I_ex = -I_ex_MAX #* I_ex / abs(I_ex) if (I_ey) > I_ey_MAX: I_ey = I_ey_MAX #* I_ey / abs(I_ey) if (I_ey) < -I_ey_MAX: I_ey = -I_ey_MAX #* I_ey / abs(I_ey) if (I_ez) > I_ez_MAX: I_ez = I_ez_MAX #* I_ez / abs(I_ez) if (I_ez) < I_ez_MAX: I_ez = -I_ez_MAX #* I_ez / abs(I_ez) p_pitch = pitch_P * err_x i_pitch = pitch_I * I_ex d_pitch = pitch_D * V_x p_roll = roll_P * err_y i_roll = roll_I * I_ey d_roll = roll_D * V_y p_th = thrust_P * err_z i_th = thrust_I * (I_ez) d_th = thrust_D * V_z ## Calculate PID values to send sendroll = (p_roll + i_roll + d_roll) # sendpitch = (p_pitch + i_pitch + d_pitch) ###### neg pitch === hexacopter will move forward ###### pos pitch === hexacopter will move backward ###### neg roll === hexacopter will move left ###### pos roll === hexacopter will move right sendthrust = p_th + i_th + d_th sendyaw = yaw_P*(vicon_yaw - home_yaw) #0 #-imu_yaw ## Cap the roll/pitch/yaw/thrust values ## if (sendroll) > roll_MAX: sendroll = roll_MAX #* sendroll / abs(sendroll) if (sendroll) < -roll_MAX: sendroll = -roll_MAX #* sendroll / abs(sendroll) if (sendpitch) > pitch_MAX: sendpitch = pitch_MAX #* sendpitch / abs(sendpitch) if (sendpitch) < -pitch_MAX: sendpitch = -pitch_MAX #* sendpitch / abs(sendpitch) if sendthrust > thrust_MAX: sendthrust = thrust_MAX if sendthrust < 0: sendthrust = 0 ## Send values to APM ## if connect_APM: master.mav.set_roll_pitch_yaw_thrust_send(master.target_system, master.target_component, sendroll, sendpitch, sendyaw, sendthrust) send_odometry.transform.translation.x = sendroll * 180/3.14159 send_odometry.transform.translation.y = sendpitch * 180/3.14159 send_odometry.transform.translation.z = sendyaw * 180/3.14159 ## Print some output to display for debugging ## # stdscr.addstr(0, 0, "Position controller is running...") # if connect_APM: # stdscr.addstr(1, 0, "Connection to APM established...") # else: # stdscr.addstr(1, 0, "******Warning: No connection to APM!*************") # stdscr.addstr(3, 0, "The gains are:") # stdscr.addstr(4, 0, " P I D") # stdscr.addstr(5, 0, "Roll %.3f %.3f %.3f" % (roll_P, roll_I, roll_D)) # stdscr.addstr(6, 0, "Pitch %.3f %.3f %.3f" % (pitch_P, pitch_I, pitch_D)) # stdscr.addstr(7, 0, "Yaw %.3f %.3f %.3f" % (yaw_P, yaw_I, yaw_D)) # stdscr.addstr(8, 0, "Thrust %.3f %.3f %.3f" % (thrust_P, thrust_I, thrust_D)) # stdscr.addstr(10, 0, "Home position is: (%+.3f,%+.3f,%+.3f) m" % (home_x, home_y, home_z)) # stdscr.addstr(11, 0, "Current position is: (%+.3f,%+.3f,%+.3f) m" % (pos_x, pos_y, pos_z)) # stdscr.addstr(12, 0, "Position error is: (%+.3f,%+.3f,%+.3f) m" % (err_x, err_y, err_z)) # if connect_APM: # stdscr.addstr(14, 0, "The IMU values are (%+.3f,%+.3f,%+.3f) deg " % (imu_roll/deg2pi, imu_pitch/deg2pi, imu_yaw/deg2pi)) # else: # stdscr.addstr(14, 0, "The IMU values are not available!") # stdscr.addstr(15, 0, "The Vicon values are (%+.3f,%+.3f,%+.3f) deg " % (vicon_roll/deg2pi,vicon_pitch/deg2pi,vicon_yaw/deg2pi)) # stdscr.addstr(17, 0, "Sending control commands to APM:") # stdscr.addstr(18, 0, "Roll (deg) Pitch (deg) Yaw (deg) Thrust (PWM)") # stdscr.addstr(19, 0, "%+.3f %+.3f %+.3f %+.3f" % (sendroll/deg2pi, sendpitch/deg2pi, sendyaw/deg2pi, sendthrust)) # stdscr.addstr(20, 0, " \n") # stdscr.refresh() # else: # hz_counter =0; #stdscr.addstr(22, 0, " The rate of command is not correct") pub_odom .publish(send_odometry)
def monitorMessages(self, gps, sensors, status, mavCommandList): self.gps = gps self.sensors = sensors self.status = status msgTypes = [] while True: msg = self.master.recv_msg() if msg: t = msg.get_type() if t not in msgTypes: # print(t) msgTypes.append(t) if t == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() elif t == "HEARTBEAT": self.log(msg) self.updateStatus(msg) elif t == "SYS_STATUS": self.log(msg) self.updateStatus(msg) elif t == "BATTERY_STATUS": self.log(msg) self.updateStatus(msg) elif t == "ATTITUDE_TARGET": # self.log(msg) pass elif t == "ATTITUDE": # roll/pitch/yaw #self.log(msg) pass elif t == "HOME_POSITION": self.updateStatus(msg) #self.log(msg) elif t == "ALTITUDE": #self.log(msg) pass elif t == "LOCAL_POSITION_NED": #self.log(msg) pass elif t == "GPS_RAW_INT": # Should use not raw_int pass #self.log(msg) #self.updateGPS(msg, gps) elif t == "GLOBAL_POSITION_INT": #self.log(msg) self.updateGPS(msg) elif t == "VFR_HUD": self.log(msg) self.updateSensors(msg) elif t == "STATUSTEXT": self.log(msg) print(msg.text) elif t == "EXTENDED_SYS_STATE": pass elif t == "HIGHRES_IMU": pass elif t == "LOCAL_POSITION_NED": pass elif t == "POSITION_TARGET_GLOBAL_INT" or t == "TERRAIN_REPORT" or t=="POWER_STATUS" or t=="RAW_IMU" or t=="SYSTEM_TIME" \ or t=="MISSION_CURRENT" or t=="SCALED_IMU2" or t=="SCALED_PRESSURE" : pass elif t == "PARAM_VALUE": self.log(msg) if t in self.savedParams: self.savedParams[msg.param_id].value = msg.param_value else: self.savedParams[msg.param_id] = Parameter(msg.param_value, msg.param_type) if msg.param_index == msg.param_count - 1: print("Got all parameters?") else: self.log(msg) pass # If we have a command, send it if mavCommandList.qsize() > 0: print("Sending command") mavCommand = mavCommandList.get() self.sendCommand(mavCommand) else: time.sleep(0.1)
def next_message(self): '''called as each msg is ready''' msg = self.msg if msg is None: self.paused = True if self.paused: self.root.after(100, self.next_message) return speed = float(self.playback.get()) timestamp = getattr(msg, '_timestamp') now = time.strftime("%H:%M:%S", time.localtime(timestamp)) self.clock.configure(text=now) if speed == 0.0: self.root.after(200, self.next_message) else: self.root.after(int(1000*(timestamp - self.last_timestamp) / speed), self.next_message) self.last_timestamp = timestamp while True: self.msg = self.mlog.recv_match(condition=opts.condition) if self.msg is None and self.mlog.f.tell() > self.filesize - 10: self.paused = True return if self.msg is not None and self.msg.get_type() != "BAD_DATA": break pos = float(self.mlog.f.tell()) / self.filesize self.slider.set(pos) self.filepos = self.slider.get() if msg.get_type() != "BAD_DATA": for m in self.mout: m.write(msg.get_msgbuf()) if msg.get_type() == "GPS_RAW": self.fdm.set('latitude', msg.lat, units='degrees') self.fdm.set('longitude', msg.lon, units='degrees') if opts.gpsalt: self.fdm.set('altitude', msg.alt, units='meters') if msg.get_type() == "GPS_RAW_INT": self.fdm.set('latitude', msg.lat/1.0e7, units='degrees') self.fdm.set('longitude', msg.lon/1.0e7, units='degrees') if opts.gpsalt: self.fdm.set('altitude', msg.alt/1.0e3, units='meters') if msg.get_type() == "VFR_HUD": if not opts.gpsalt: self.fdm.set('altitude', msg.alt, units='meters') self.fdm.set('num_engines', 1) self.fdm.set('vcas', msg.airspeed, units='mps') if msg.get_type() == "ATTITUDE": self.fdm.set('phi', msg.roll, units='radians') self.fdm.set('theta', msg.pitch, units='radians') self.fdm.set('psi', msg.yaw, units='radians') self.fdm.set('phidot', msg.rollspeed, units='rps') self.fdm.set('thetadot', msg.pitchspeed, units='rps') self.fdm.set('psidot', msg.yawspeed, units='rps') if msg.get_type() == "RC_CHANNELS_SCALED": self.fdm.set("right_aileron", msg.chan1_scaled*0.0001) self.fdm.set("left_aileron", -msg.chan1_scaled*0.0001) self.fdm.set("rudder", msg.chan4_scaled*0.0001) self.fdm.set("elevator", msg.chan2_scaled*0.0001) self.fdm.set('rpm', msg.chan3_scaled*0.01) if msg.get_type() == 'STATUSTEXT': print("APM: %s" % msg.text) if msg.get_type() == 'SYS_STATUS': self.flightmode.configure(text=self.mlog.flightmode) if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() if self.fdm.get('latitude') != 0: for f in self.fgout: f.write(self.fdm.pack())