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)
if msg_type == "RAW_IMU":
pub_raw_imu.publish(Header(), msg.time_usec, msg.xacc,
msg.yacc, msg.zacc, msg.xgyro, msg.ygyro,
msg.zgyro, msg.xmag, msg.ymag, msg.zmag)
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 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 get_messages(m): '''show incoming mavlink messages''' while True: msg = m.recv_match(blocking=True) if not msg: return 'NULL' if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: return msg
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)
px4_time[0] = msg.time_boot_ms # get the time from px4
local_time[0] = time.time() # get the local start time
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)
if msg_type == "RAW_IMU" :
pub_raw_imu.publish (Header(), msg.time_usec,
msg.xacc, msg.yacc, msg.zacc,
msg.xgyro, msg.ygyro, msg.zgyro,
msg.xmag, msg.ymag, msg.zmag)
def mainloop():
rospy.init_node('Mavlink_Node')
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":
#print "Got GPS Message. SV: %d eph: %d epv: %d" %(msg.satellites_visible,msg.eph,msg.epv)
pub_gps_state.publish(msg.satellites_visible,msg.eph,msg.epv,msg.lat/1e07,msg.lon/1e07,msg.alt/1e03,msg.fix_type)
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 update_device(m):
global WaypointCount
global Current_Yaw_rad
global Current_Pitch_rad
global Current_Roll_rad
global fc_badpacket_counter
global first_attitude_packet
global Initial_Yaw_rad
global my_MissionItems
if (m.protocol == "MAVLINK") or (m.protocol == "APM_MAVLINK"):
msg = m.device.recv_match(blocking=False)
#pub_data_from_fc.publish(str(msg.get_type()))
if m.device == device_remote.device:
if msg:
#print msg
device_remote.printtext(str(msg))
if m.device == device_fc.device:
print msg
#if m == device_gcs.device:
# if msg:
# print msg
if msg:
if msg.get_type() == "BAD_DATA":
fc_badpacket_counter = fc_badpacket_counter + 1
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
elif msg.get_type() == "HEARTBEAT":
if m.device == device_gcs.device:
device_gcs.appenderror(calc_errorcode(SYSTEM_GCS,ERRORTYPE_NOERROR,SEVERITY_INFORMATION,MESSAGE_NOERROR))
#device_gcs.display_errors()
#print "device_gcs.device: {}".format(msg)
if m.device == device_fc.device:
device_gcs.appenderror(calc_errorcode(SYSTEM_GCS,ERRORTYPE_NOERROR,SEVERITY_INFORMATION,MESSAGE_NOERROR))
device_fc.state = msg.system_status
device_fc.mode = msg.base_mode
#print "FC: {}".format(msg)
#fc_state = msg.system_status
if m.device == device_remote.device:
device_remote.printtext("Remote: {}".format(msg))
device_remote.appenderror(calc_errorcode(SYSTEM_REMOTE,ERRORTYPE_NOERROR,SEVERITY_INFORMATION,MESSAGE_NOERROR))
elif msg.get_type() == "ATTITUDE" :
if first_attitude_packet:
Initial_Yaw_rad = msg.yaw
first_attitude_packet = False
Current_Pitch_rad = msg.pitch
Current_Yaw_rad = msg.yaw
Current_Roll_rad = msg.roll
pub_attitude.publish(msg.roll, msg.pitch, msg.yaw)
elif msg.get_type() == "STATUSTEXT":
dumb = 1
#print msg
elif msg.get_type() == "MISSION_CURRENT":
if m == device_fc.device:
dumb = 1
#print "FC: {}".format(msg)
elif msg.get_type() == "MISSION_ITEM":
if m.device == device_gcs.device:
device_gcs.printtext(str(msg))
waypoint_rcv_fsm(m,"NewWP",msg)
elif m.device == device_fc.device:
device_fc.printtext(str(msg))
elif msg.get_type() == "MISSION_ACK":
if m.device == device_gcs.device:
waypoint_rcv_fsm(m,"Finish",msg)
if m.device == device_fc.device:
device_fc.printtext(str(msg))
send_mission_request_list(device_fc.device)
elif msg.get_type() == "GPS_RAW_INT":
dumb = 1
elif msg.get_type() == "RADIO":
dumb = 1
elif msg.get_type() == "AHRS":
dumb = 1
elif msg.get_type() == "HWSTATUS":
dumb = 1
elif msg.get_type() == "SYS_STATUS":
dumb = 1
#if m == fc:
# print msg
elif msg.get_type() == "NAV_CONTROLLER_OUTPUT":
dumb = 1
#if m == fc:
# print msg
elif msg.get_type() == "MEMINFO":
dumb = 1
elif msg.get_type() == "REQUEST_DATA_STREAM":
if m.device == device_remote.device:
device_remote.mav_data_streams.append(msg.req_stream_id)
#device_remote.display_streams()
device_remote.appenderror(calc_errorcode(SYSTEM_REMOTE,ERRORTYPE_NOERROR,SEVERITY_INFORMATION,MESSAGE_NOERROR))
elif msg.get_type() == "GLOBAL_POSITION_INT":
dumb = 1
elif msg.get_type() == "RC_CHANNELS_SCALED":
dumb = 1
elif msg.get_type() == "SERVO_OUTPUT_RAW":
dumb = 1
elif msg.get_type() == "RC_CHANNELS_RAW":
dumb = 1
elif msg.get_type() == "VFR_HUD":
dumb = 1
elif msg.get_type() == "RAW_IMU":
dumb = 1
elif msg.get_type() == "SCALED_PRESSURE":
dumb = 1
elif msg.get_type() == "SENSOR_OFFSETS":
dumb = 1
elif msg.get_type() == "MISSION_REQUEST_LIST":
for wp in my_MissionItems:
wp.display()
if m.device == device_gcs.device:
device_pc.printtext("Going to send these waypoints: ")
device_pc.printtext(str(msg))
try:
if (len(my_MissionItems)>0):
waypoint_xmit_fsm(device_gcs.device,"Start",msg)
else:
waypoint_xmit_fsm(device_gcs.device,"Empty",msg)
except NameError:
dumb = 1
#waypoint_xmit_fsm(device_gcs.device,"Empty",msg)
#print "ERRORORRR"
elif m.device == device_fc.device:
device_fc.printtext(str(msg))
elif msg.get_type() == "MISSION_REQUEST":
if m.device == device_gcs.device:
device_gcs.printtext(str(msg))
waypoint_xmit_fsm(device_gcs.device,"NewWP",msg)
elif m.device == device_fc.device:
waypoint_xmit_fsm(device_fc.device,"NewWP",msg)
elif msg.get_type() == "MISSION_CLEAR_ALL":
if m.device == device_gcs.device:
device_gcs.device.target_system = 0
device_gcs.device.target_component = 0
device_gcs.printtext(str(msg))
my_MissionItems = []
send_ack(device_gcs.device)
elif m.device == device_fc.device:
device_fc.printtext(str(msg))
elif msg.get_type() == "MISSION_COUNT":
if m.device == device_gcs.device:
device_gcs.printtext(str(msg))
device_gcs.device.target_system = 0
device_gcs.device.target_component = 0
WaypointCount = int(msg.count)
my_MissionItems = []
waypoint_rcv_fsm(device_gcs.device,"Start",msg)
elif m.device == device_fc.device:
device_fc.printtext(str(msg))
elif msg.get_type() == "COMMAND_LONG":
if m.device == device_gcs.device:
print msg
device_gcs.printtext(str(msg))
if msg.command == mavlink.MAV_CMD_NAV_LAND:
if device_pc.isarmed():
device_gcs.printtext("MAV_CMD_NAV_LAND")
device_pc.changecommand(msg.command)
print "Need more code here"
else:
tempstr = "not even armed yet"
send_text(device_gcs.device,tempstr)
print tempstr
elif msg.command == mavlink.MAV_CMD_NAV_TAKEOFF:
if device_pc.isarmed():
device_gcs.printtext("MAV_CMD_NAV_TAKEOFF")
device_pc.changecommand(msg.command)
#statemode_fsm(device_fc.device,mavlink.MAV_STATE_ACTIVE,mavlink.MAV_MODE_AUTO_ARMED)
else:
tempstr = "Not even armed yet."
send_text(device_gcs.device,tempstr)
print tempstr
elif msg.command == mavlink.MAV_CMD_OVERRIDE_GOTO:
device_gcs.printtext("MAV_CMD_OVERRIDE_GOTO")
if (str(msg.confirmation) == "1") and(str(msg.param1) == "0.0") and (str(msg.param2) == "2.0"):
if device_pc.isarmed():
#device_pc.changecommand(msg.command)
print "Need more code here"
print "PAUSE"
else:
tempstr = "Not even armed yet."
send_text(device_gcs.device,tempstr)
print tempstr
if (str(msg.confirmation) == "1") and (str(msg.param1) == "1.0") and (str(msg.param2) == "2.0"):
if device_pc.isarmed():
print "Need mroe code here"
else:
tempstr = "Not even armed yet."
send_text(device_gcs.device,tempstr)
print tempstr
if m.device == device_fc.device:
device_fc.printtext(str(msg))
elif msg.get_type() == "SET_MODE":
if m.device == device_gcs.device:
print msg
device_gcs.printtext(str(msg))
print msg.base_mode
print device_pc.isarmed()
if str(msg.base_mode) == "132":
if device_pc.isarmed() == False:
print "trying to arm"
device_pc.armdisarm(True)
send_ack(device_gcs.device)
else:
device_pc.armdisarm(False)
elif str(msg.base_mode) == "4":
if device_pc.isarmed() == True:
print "trying to disarm"
device_pc.armdisarm(False)
send_ack(device_gcs.device)
else:
if device_pc.isarmed() == True:
device_pc.armdisarm(False)
device_pc.changemode(msg.base_mode)
if device_fc.enabled == True:
device_fc.changemode(msg.base_mode)
if m.device == device_remote.device:
if device_pc.isarmed() == True:
device_pc.armdisarm(False)
if msg.custom_mode == APM_STABILIZE:
device_pc.changemode(mavlink.MAV_MODE_STABILIZE_DISARMED)
#device_pc.armdisarm(True)
elif msg.custom_mode == APM_AUTO:
device_pc.changemode(mavlink.MAV_MODE_AUTO_DISARMED)
elif msg.custom_mode == APM_LAND:
device_pc.changecommand(mavlink.MAV_CMD_NAV_LAND)
elif msg.custom_mode == APM_LOITER:
device_pc.changecommand(mavlink.MAV_CMD_NAV_LOITER_UNLIM)
elif msg.custom_mode == APM_GUIDED:
device_pc.changemode(mavlink.MAV_MODE_GUIDED_DISARMED)
else:
if m.device == device_gcs.device:
device_gcs.printtext(str(msg))
if m.device == device_remote.device:
device_remote.printtext(str(msg))
if m.device == device_fc.device:
dumb = 1
#print msg
if msg == "STATUSTEXT":
if m.device == device_fc.device:
if (msg.find("flight plan received")>0):
dumb = 1
device_fc.printtext(str(msg))
if m.device == device_fc.device:
dumb = 1
device_fc.printtext(str(msg))
if m.protocol == "ICARUS":
try:
msg = device_mc.device.readline()
#device_mc.printtext(msg)
if msg[0] == "$" and msg[len(msg)-3]=="*":
#print msg
msg = msg[1:len(msg)-3]
contents = msg.split(",")
if contents[0] == "STA":
if contents[1] == "STATE":
device_mc.state = int(contents[2])
if device_mc.state == mavlink.MAV_STATE_ACTIVE:
device_mc.armed = True
else:
device_mc.armed = False
#print device_mc.state
elif contents[1] == "MODE":
device_mc.cur_mode = int(contents[2])
#print device_mc.mode
elif contents[0] == "ERR":
device_mc.appenderror(contents[1])
elif contents[0] == "CAL":
dumb = 1
elif contents[0] == "CON":
dumb = 1
elif contents[0] == "INF":
dumb = 1
elif contents[0] == "MOTOR":
dumb = 1
elif contents[0] == "NET":
dumb = 1
elif contents[0] == "SEN":
dumb = 1
elif contents[0] == "SRV":
dumb = 1
elif contents[0] == "PWMIN":
print msg
else:
print msg
except:
device_mc.appenderror(calc_errorcode(system=SYSTEM_FLYER_MC,errortype=ERRORTYPE_COMMUNICATION,severity=SEVERITY_CAUTION,message=MESSAGE_DROPPEDPACKETS))
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(struct.pack('>Q', timestamp*1.0e6))
m.write(msg.get_msgbuf().tostring())
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() == "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())
def handle_mavlink(self):
"""Main loop for handling all mavlink communication
Incoming mavlink messages may be from drone, host, or
QGroundControl. This method dispatches and processes
messages separately in each case.
This runs inside its own thread.
"""
#**********************************************************************
# Process mavlink messages (forever)
#**********************************************************************
while True:
#******************************************************************
# Wait (forever) until we receive a mavlink message
#******************************************************************
msg = None
try:
msg = self.connection.recv_match(blocking=True)
except socket.error as e:
print("[AR2MAV]%s: caught socket error while trying to"
" receive mavlink message: %s" %
(self.name, str(e)) )
traceback.print_exc()
time.sleep(1)
#******************************************************************
# If recv_match returns without a message, then try again
# This shouldn't really happen because we've asked to block until
# a message is available
#******************************************************************
if msg is None:
print("[AR2MAV]%s: mavlink unexpectedly returned nothing." %
self.name)
#******************************************************************
# Write out bad data (shouldn't happen either)
#******************************************************************
elif msg.get_type() == "BAD_DATA":
print("[AR2MAV]%s: Received bad data -- ignoring." %
self.name)
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
#******************************************************************
# Process data from drone
#******************************************************************
elif self.connection.last_address == self.drone:
try:
self.process_from_drone(msg)
except socket.error as e:
print("[AR2MAV]%s: caught socket error while processing"
" last message from drone: %s" %
(self.name, str(e)) )
traceback.print_exc()
#******************************************************************
# Process data from host
#******************************************************************
elif self.connection.last_address == self.host:
try:
self.process_from_host(msg)
except socket.error as e:
print("[AR2MAV]%s: caught socket error while processing"
" last message from host: %s" %
(self.name, str(e)) )
traceback.print_exc()
#******************************************************************
# Process data from QGroundControl (we should be able to treat
# this just like data from the host)
#******************************************************************
elif self.qgc and \
self.connection.last_address == (self.host[0], QGC_PORT):
try:
self.process_from_host(msg)
except socket.error as e:
print("[AR2MAV]%s: caught socket error while processing"
" last message from QGroundControl: %s" %
(self.name, str(e)) )
traceback.print_exc()
#******************************************************************
# Ignore data from anyone else
#******************************************************************
else:
if self.verbose > 0:
print("[AR2MAV]%s: Unregistered AUV with IP(MAV): %s" %
(self.name, self.connection.last_address[0]))
def mainloop():
global gps_msg, autonomous_enable
rospy.init_node('casy_rover')
if opts.enable_watchdog:
rospy.Timer(rospy.Duration(1/opts.watchdog_rate), watchdog_timer_cb)
# SEND IF YOU DESIRE A LIST OF ALL PARAMS (TODO: Publish params to a topic)
#master.mav.param_request_list_send(master.target_system, master.target_component)
# Permette di settare un parametro come in mission planner
#master.param_set_send('FS_ACTION', 0, 2)
# Riceve una lista di tutti i parametri disponibili sotto forma di messaggi PARAM_VALUE
#master.param_fetch_all()
r = rospy.Rate(opts.rate)
while not rospy.is_shutdown():
r.sleep()
msg = master.recv_match(blocking=False)
if not msg:
continue
# Parse incoming message
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" :
# If using Autonomous Control safety switch, use specified channel
# to enable or disable autonomous control. Autonomous control
# allows mode, rc, and waypoint controls.
if (opts.enable_autonomous_safety_switch):
if (msg.chan6_raw >= 1500):
autonomous_enable = True
elif (msg.chan6_raw < 1500):
autonomous_enable = False
# Give control back to controller
master.mav.rc_channels_override_send(master.target_system,
master.target_component, 0, 0, 0, 0, 0, 0, 0, 0)
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])
elif 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))
state_msg.armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
state_msg.mode = mavutil.mode_string_v10(msg)
elif msg_type == "VFR_HUD":
pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb)
elif msg_type == "GPS_RAW_INT":
fix = NavSatStatus.STATUS_NO_FIX
if msg.fix_type >=3:
fix=NavSatStatus.STATUS_FIX
header = Header()
header.frame_id = 'base_link'# '/gps'
header.stamp = rospy.Time.now()
#rospy.loginfo("Hdop is %d", msg.eph)
#rospy.loginfo("Vdop is %d", msg.epv)
sigma = math.sqrt((3.04 * msg.eph**2)**2 + 3.57**2)
position_covariance = [0] * 9
position_covariance[0] = sigma #9999
position_covariance[4] = sigma #9999
position_covariance[8] = sigma #9999
pub_gps.publish(NavSatFix(header = header,
latitude = msg.lat/1e07,
longitude = msg.lon/1e07,
altitude = msg.alt/1e03,
position_covariance=position_covariance,
position_covariance_type=NavSatFix.COVARIANCE_TYPE_APPROXIMATED,
status = NavSatStatus(status=fix, service = NavSatStatus.SERVICE_GPS)
))
gps_msg.latitude = msg.lat
gps_msg.longitude = msg.lon
elif msg_type == "LOCAL_POSITION_NED" :
rospy.loginfo("Local Pos: (%f %f %f) , (%f %f %f)" %(msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz))
elif msg_type == "RAW_IMU" :
pub_raw_imu.publish (Header(), msg.time_usec,
msg.xacc, msg.yacc, msg.zacc,
msg.xgyro, msg.ygyro, msg.zgyro,
msg.xmag, msg.ymag, msg.zmag)
elif msg_type == "SYS_STATUS":
status_msg = casy_rover.msg.Status()
status_msg.header.stamp = rospy.Time.now()
status_msg.battery_voltage = msg.voltage_battery
status_msg.battery_current = msg.current_battery
status_msg.battery_remaining = msg.battery_remaining
status_msg.sensors_enabled = msg.onboard_control_sensors_enabled
pub_status.publish(status_msg)
elif msg_type == "GLOBAL_POSITION_INT":
header = Header()
header.stamp = rospy.Time.now()
filtered_pos_msg.header = header
filtered_pos_msg.latitude = msg.lat
filtered_pos_msg.longitude = msg.lon
filtered_pos_msg.altitude = msg.alt
filtered_pos_msg.relative_altitude = msg.relative_alt
filtered_pos_msg.ground_x_speed = msg.vx
filtered_pos_msg.ground_y_speed = msg.vy
filtered_pos_msg.ground_z_speed = msg.vz
filtered_pos_msg.heading = msg.hdg
pub_filtered_pos.publish(filtered_pos_msg)
elif msg_type == "COMMAND_ACK":
rospy.loginfo ("COMMAND_ACK: Command Message ACK with result - " + str(msg.result))
elif msg_type == "STATUSTEXT":
rospy.loginfo ("STATUSTEXT: Status severity is %d. Text Message is %s" %(msg.severity, msg.text))
elif msg_type == "PARAM_VALUE":
rospy.loginfo ("PARAM_VALUE: ID = %s, Value = %d, Type = %d, Count = %d, Index = %d"
%(msg.param_id, msg.param_value, msg.param_type, msg.param_count, msg.param_index))
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()
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F2_A':
print >> f, "F2:T%li:S%s:N%li:E%li:A%li:W%i:a%i:b%i:c%i:d%i:e%i:f%i:g%i:h%i" \
":i%i:c%u:s%i:cpu%u:bmv%i:as%u:wvx%i:wvy%i:wvz%i:ma%i:mb%i:mc%i:svs%i:hd%i:" % \
( msg.sue_time, bstr( msg.sue_status ), msg.sue_latitude, msg.sue_longitude, \
msg.sue_altitude, msg.sue_waypoint_index, \
msg.sue_rmat0, msg.sue_rmat1, msg.sue_rmat2, \
msg.sue_rmat3, msg.sue_rmat4, msg.sue_rmat5, \
msg.sue_rmat6, msg.sue_rmat7, msg.sue_rmat8, \
msg.sue_cog, msg.sue_sog, msg.sue_cpu_load, msg.sue_voltage_milis, \
msg.sue_air_speed_3DIMU, \
msg.sue_estimated_wind_0, msg.sue_estimated_wind_1, msg.sue_estimated_wind_2, \
msg.sue_magFieldEarth0, msg.sue_magFieldEarth1, msg.sue_magFieldEarth2, \
msg.sue_svs, msg.sue_hdop ),
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F2_B':
sys.stdout.softspace=False # This stops a space being inserted between print statements
print >> f, "p1i%i:p2i%i:p3i%i:p4i%i:p5i%i:p6i%i:p7i%i:p8i%i:p9i%i:p10i%i:" \
"p1o%i:p2o%i:p3o%i:p4o%i:p5o%i:p6o%i:p7o%i:p8o%i:p9o%i:p10o%i:" \
"imx%i:imy%i:imz%i:fgs%X:ofc%i:tx%i:ty%i:tz%i:G%d,%d,%d:stk%d:\r\n" % \
( msg.sue_pwm_input_1, msg.sue_pwm_input_2, msg.sue_pwm_input_3, msg.sue_pwm_input_4, msg.sue_pwm_input_5, \
msg.sue_pwm_input_6, msg.sue_pwm_input_7, msg.sue_pwm_input_8, msg.sue_pwm_input_9, msg.sue_pwm_input_10,\
msg.sue_pwm_output_1, msg.sue_pwm_output_2, msg.sue_pwm_output_3, \
msg.sue_pwm_output_4, msg.sue_pwm_output_5, msg.sue_pwm_output_6, \
msg.sue_pwm_output_7, msg.sue_pwm_output_8, msg.sue_pwm_output_9, \
msg.sue_pwm_output_10, msg.sue_imu_location_x, msg.sue_imu_location_y, msg.sue_imu_location_z, \
msg.sue_flags, msg.sue_osc_fails, \
msg.sue_imu_velocity_x, msg.sue_imu_velocity_y, msg.sue_imu_velocity_z, \
msg.sue_waypoint_goal_x, msg.sue_waypoint_goal_y, msg.sue_waypoint_goal_z,\
msg.sue_memory_stack_free ),
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F4' :
print >> f, "F4:R_STAB_A=%i:R_STAB_RD=%i:P_STAB=%i:Y_STAB_R=%i:Y_STAB_A=%i:AIL_NAV=%i:" \
"RUD_NAV=%i:AH_STAB=%i:AH_WP=%i:RACE=%i:\r\n" % \
( msg.sue_ROLL_STABILIZATION_AILERONS, msg.sue_ROLL_STABILIZATION_RUDDER, \
msg.sue_PITCH_STABILIZATION, msg.sue_YAW_STABILIZATION_RUDDER, \
msg.sue_YAW_STABILIZATION_AILERON, msg.sue_AILERON_NAVIGATION, \
msg.sue_RUDDER_NAVIGATION, msg.sue_ALTITUDEHOLD_STABILIZED, \
msg.sue_ALTITUDEHOLD_WAYPOINT, msg.sue_RACING_MODE ),
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F5' :
print >> f, "F5:YAWKP_A=%5.3f:YAWKD_A=%5.3f:ROLLKP=%5.3f:ROLLKD=%5.3f:A_BOOST=%3.1f:\r\n" % \
( msg.sue_YAWKP_AILERON, msg.sue_YAWKD_AILERON, msg.sue_ROLLKP, \
msg.sue_ROLLKD, msg.sue_AILERON_BOOST ),
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F6' :
print >> f, "F6:P_GAIN=%5.3f:P_KD=%5.3f:RUD_E_MIX=%5.3f:ROL_E_MIX=%5.3f:E_BOOST=%3.1f:\r\n" % \
( msg.sue_PITCHGAIN, msg.sue_PITCHKD, msg.sue_RUDDER_ELEV_MIX, \
msg.sue_ROLL_ELEV_MIX, msg.sue_ELEVATOR_BOOST),
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F7' :
print >> f, "F7:Y_KP_R=%5.4f:Y_KD_R=%5.3f:RLKP_RUD=%5.3f:RLKD_RUD=%5.3f:" \
"RUD_BOOST=%5.3f:RTL_PITCH_DN=%5.3f:\r\n" % \
( msg.sue_YAWKP_RUDDER, msg.sue_YAWKD_RUDDER, msg.sue_ROLLKP_RUDDER , \
msg.sue_ROLLKD_RUDDER , msg.sue_RUDDER_BOOST, msg.sue_RTL_PITCH_DOWN),
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F8' :
print >> f, "F8:H_MAX=%6.1f:H_MIN=%6.1f:MIN_THR=%3.2f:MAX_THR=%3.2f:PITCH_MIN_THR=%4.1f:" \
"PITCH_MAX_THR=%4.1f:PITCH_ZERO_THR=%4.1f:\r\n" % \
( msg.sue_HEIGHT_TARGET_MAX, msg.sue_HEIGHT_TARGET_MIN, \
msg.sue_ALT_HOLD_THROTTLE_MIN, msg.sue_ALT_HOLD_THROTTLE_MAX, \
msg.sue_ALT_HOLD_PITCH_MIN, msg.sue_ALT_HOLD_PITCH_MAX, \
msg.sue_ALT_HOLD_PITCH_HIGH),
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F13' :
print >> f, "F13:week%i:origN%li:origE%li:origA%li:\r\n" % \
(msg.sue_week_no, msg.sue_lat_origin, msg.sue_lon_origin, msg.sue_alt_origin),
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F14' :
print "\r\nF14:WIND_EST=%i:GPS_TYPE=%i:DR=%i:BOARD_TYPE=%i:AIRFRAME=%i:RCON=0x%X:TRAP_FLAGS=0x%X:TRAP_SOURCE=0x%lX:ALARMS=%i:" \
"CLOCK=%i:FP=%d:\r\n" % \
( msg.sue_WIND_ESTIMATION, msg.sue_GPS_TYPE, msg.sue_DR, msg.sue_BOARD_TYPE, \
msg.sue_AIRFRAME, msg.sue_RCON, msg.sue_TRAP_FLAGS, msg.sue_TRAP_SOURCE, \
msg.sue_osc_fail_count, msg.sue_CLOCK_CONFIG, msg.sue_FLIGHT_PLAN_TYPE ),
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F15' :
print >> f, "F15:IDA=%s:IDB=%s:\r\n" % \
( msg.sue_ID_VEHICLE_MODEL_NAME, msg.sue_ID_VEHICLE_REGISTRATION ),
elif msg.get_type() == 'SERIAL_UDB_EXTRA_F16' :
print >> f, "F16:IDC=%s:IDD=%s:\r\n" % \
( msg.sue_ID_LEAD_PILOT, msg.sue_ID_DIY_DRONES_URL ),
else :
pass
