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 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 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())
