Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
raise NotAchievedException("Breached fence unexpectedly (%u)" %
(m.breach_status))
self.mavproxy.send('fence disable\n')
self.mavproxy.expect("fence disabled")
self.assert_fence_sys_status(True, False, True)
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_NONE)
self.assert_fence_sys_status(False, False, True)
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL)
self.assert_fence_sys_status(True, False, True)
self.mavproxy.send("fence clear\n")
self.mavproxy.expect("fence removed")
if self.get_parameter("FENCE_TOTAL") != 0:
raise NotAchievedException("Expected zero points remaining")
self.assert_fence_sys_status(False, False, True)
self.progress("Trying to enable fence with no points")
self.do_fence_enable(want_result=mavutil.mavlink.MAV_RESULT_FAILED)
# test a rather unfortunate behaviour:
self.progress("Killing a live fence with fence-clear")
self.load_fence("CMAC-fence.txt")
self.set_parameter("FENCE_ACTION", mavutil.mavlink.FENCE_ACTION_RTL)
self.do_fence_enable()
self.assert_fence_sys_status(True, True, True)
self.mavproxy.send("fence clear\n")
self.mavproxy.expect("fence removed")
if self.get_parameter("FENCE_TOTAL") != 0:
raise NotAchievedException("Expected zero points remaining")
self.assert_fence_sys_status(False, False, True)
except Exception as e:
self.progress("Exception caught:")
self.progress(self.get_exception_stacktrace(e))
def process_mavlink(slave):
'''process packets from MAVLink slaves, forwarding to the master'''
try:
buf = slave.recv()
except socket.error:
return
try:
global mavversion
if slave.first_byte and mavversion is None:
slave.auto_mavlink_version(buf)
msgs = slave.mav.parse_buffer(buf)
except mavutil.mavlink.MAVError as e:
mpstate.console.error("Bad MAVLink slave message from %s: %s" % (slave.address, e.message))
return
if msgs is None:
return
if mpstate.settings.mavfwd and not mpstate.status.setup_mode:
for m in msgs:
mbuf = m.get_msgbuf()
mpstate.master().write(mbuf)
if mpstate.logqueue:
usec = int(time.time() * 1.0e6)
mpstate.logqueue.put(bytearray(struct.pack('>Q', usec) + m.get_msgbuf()))
if mpstate.status.watch:
for msg_type in mpstate.status.watch:
if fnmatch.fnmatch(m.get_type().upper(), msg_type.upper()):
mpstate.console.writeln('> '+ str(m))
break
if self.dist > self.radius:
if self.fresh == True:
self.say("GCS position set as home")
self.fresh = False
else:
message = "GCS moved "
message2 = message + "%.0f" % self.dist + "meters"
self.say("%s: %s" % (self.name,message2))
message2_enc = message2.encode(bytes)
self.master.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_NOTICE, message2)
self.console.writeln("home position updated")
self.master.mav.command_int_send(
self.settings.target_system, self.settings.target_component,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
mavutil.mavlink.MAV_CMD_DO_SET_HOME,
1, # (1, set current location as home)
0, # move on
0, # param1
0, # param2
0, # param3
0, # param4
int(self.lat*1e7), # param5
int(self.lon*1e7), # param6
0) # param7
self.lath = self.lat
self.lonh = self.lon
#print data
self.console.writeln("%s: %s %s GNSS Quality %s Sats %s "% (self.name,self.lat,self.lon,msg.gps_qual,msg.num_sats))
# set the fail radius down to let us see everything on a reasonable scale:
vehicle.parameters["AVD_F_DIST_XY"] = 20
vehicle.parameters["AVD_F_DIST_Z"] = 10
# set the time horizon down to let us see everything on a reasonable scale:
vehicle.parameters["AVD_F_TIME"] = 10
vehicle.parameters["AVD_W_ACTION"] = mavutil.mavlink.MAV_COLLISION_ACTION_REPORT
vehicle.parameters["AVD_W_RCVRY"] = 1
vehicle.parameters["AVD_F_RCVRY"] = 1
if args.resolution == "RTL":
vehicle.parameters["AVD_F_ACTION"] = mavutil.mavlink.MAV_COLLISION_ACTION_RTL
elif args.resolution == "PERPENDICULAR":
vehicle.parameters["AVD_F_ACTION"] = mavutil.mavlink.MAV_COLLISION_ACTION_MOVE_PERPENDICULAR
elif args.resolution == "TCAS":
vehicle.parameters["AVD_F_ACTION"] = mavutil.mavlink.MAV_COLLISION_ACTION_TCAS
print("%d: Allowing time for parameter write" % (i,))
time.sleep(2)
print("%d: Stop" % (i,))
sitls[i][0].stop()
vehicle.disconnect()
def enterPhotoMode(self):
# switch into photo mode if we aren't already in it
self.shotmgr.goproManager.sendGoProCommand(mavutil.mavlink.GOPRO_COMMAND_CAPTURE_MODE, (CAPTURE_MODE_PHOTO, 0 ,0 , 0))
def cmd_land(self, args):
'''auto land commands'''
if len(args) < 1:
self.master.mav.command_long_send(self.settings.target_system,
0,
mavutil.mavlink.MAV_CMD_DO_LAND_START,
0, 0, 0, 0, 0, 0, 0, 0)
elif args[0] == 'abort':
self.master.mav.command_long_send(self.settings.target_system,
0,
mavutil.mavlink.MAV_CMD_DO_GO_AROUND,
0, 0, 0, 0, 0, 0, 0, 0)
else:
print("Usage: land [abort]")
yaw_cd = int(mavextra.wrap_360(math.degrees(yaw)) * 100)
if yaw_cd == 0:
# the yaw extension to GPS_INPUT uses 0 as no yaw support
yaw_cd = 36000
if args.debug > 0:
print("Pose: [%.3f, %.3f, %.3f] [%.2f %.2f %.2f]" % (
pos_ned[0], pos_ned[1], pos_ned[2],
math.degrees(roll), math.degrees(pitch), math.degrees(yaw)))
time_us = int(now * 1.0e6)
if now - last_origin_send > 1 and not args.gps_only:
# send a heartbeat msg
mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, 0)
# send origin at 1Hz
mav.mav.set_gps_global_origin_send(args.target_sysid,
int(origin[0]*1.0e7), int(origin[1]*1.0e7), int(origin[2]*1.0e3),
time_us)
last_origin_send = now
if args.gps_rate > 0 and now_ms - last_gps_send_ms > gps_period_ms:
'''send GPS data at the specified rate, trying to align on the given period'''
if last_gps_pos is None:
last_gps_pos = pos_ned
gps_lat, gps_lon = mavextra.gps_offset(origin[0], origin[1], pos_ned[1], pos_ned[0])
gps_alt = origin[2] - pos_ned[2]
gps_dt = now - last_gps_send
gps_vel = [ (pos_ned[0]-last_gps_pos[0])/gps_dt,
def colors_for_severity(self, severity):
severity_colors = {
# tuple is (fg, bg) (as in "white on red")
mavutil.mavlink.MAV_SEVERITY_EMERGENCY: ('white', 'red'),
mavutil.mavlink.MAV_SEVERITY_ALERT: ('white', 'red'),
mavutil.mavlink.MAV_SEVERITY_CRITICAL: ('white', 'red'),
mavutil.mavlink.MAV_SEVERITY_ERROR: ('black', 'orange'),
mavutil.mavlink.MAV_SEVERITY_WARNING: ('black', 'orange'),
mavutil.mavlink.MAV_SEVERITY_NOTICE: ('black', 'yellow'),
mavutil.mavlink.MAV_SEVERITY_INFO: ('white', 'green'),
mavutil.mavlink.MAV_SEVERITY_DEBUG: ('white', 'green'),
}
try:
return severity_colors[severity]
except Exception as e:
print("Exception: %s" % str(e))
return ('white', 'red')
if enum_name == "MAV_FRAME":
if enum_value == 0:
return "Abs"
elif enum_value == 1:
return "Local"
elif enum_value == 2:
return "Mission"
elif enum_value == 3:
return "Rel"
elif enum_value == 4:
return "Local ENU"
elif enum_value == 5:
return "Global (INT)"
elif enum_value == 10:
return "AGL"
ret = mavutil.mavlink.enums[enum_name][enum_value].name
ret = ret[len(enum_name)+1:]
return ret