def __init__(self, mpstate):
super(RCModule, self).__init__(mpstate, "rc", "rc command handling", public = True)
self.override = [ 0 ] * 8
self.last_override = [ 0 ] * 8
self.override_counter = 0
self.add_command('rc', self.cmd_rc, "RC input control", ['<1|2|3|4|5|6|7|8|all>'])
self.add_command('switch', self.cmd_switch, "flight mode switch control", ['<0|1|2|3|4|5|6>'])
if self.sitl_output:
self.override_period = mavutil.periodic_event(20)
else:
self.override_period = mavutil.periodic_event(1)
def __init__(self, mpstate):
super(SetRefModule, self).__init__(mpstate, "setref", "Sets IMU 0 reference point",public=True)
self.override = [ 0 ] * 16
self.last_override = [ 0 ] * 16
self.override_counter = 0
self.add_command('setref', self.cmd_ref, "Set IMU reference point", [''])
self.add_command('print_ref', self.print_ref, "print IMU reference point", [''])
self.waiting_for_command = True
if self.sitl_output:
self.override_period = mavutil.periodic_event(20)
else:
self.override_period = mavutil.periodic_event(1)
def init(_mpstate):
'''initialise module'''
global mpstate
mpstate = _mpstate
mpstate.rc_state = rc_state()
state = mpstate.rc_state
mpstate.command_map['rc'] = (cmd_rc, "RC input control")
mpstate.command_map['switch'] = (cmd_switch, "flight mode switch control")
mpstate.completions['rc'] = ['<1|2|3|4|5|6|7|8|all>']
mpstate.completions['switch'] = ['<0|1|2|3|4|5|6>']
if mpstate.sitl_output:
state.override_period = mavutil.periodic_event(20)
else:
state.override_period = mavutil.periodic_event(1)
def __init__(self):
self.gps = None
self.msgs = {}
self.msg_count = {}
self.sock = 0
self.counters = {'MasterIn' : [], 'MasterOut' : 0, 'FGearIn' : 0, 'FGearOut' : 0, 'Slave' : 0}
self.setup_mode = opts.setup
self.mav_error = 0
self.altitude = 0
self.last_altitude_announce = 0.0
self.last_distance_announce = 0.0
self.exit = False
self.flightmode = 'MAV'
self.last_mode_announce = 0
self.logdir = None
self.last_heartbeat = 0
self.last_message = 0
self.heartbeat_error = False
self.last_apm_msg = None
self.last_apm_msg_time = 0
self.highest_msec = 0
self.have_gps_lock = False
self.lost_gps_lock = False
self.last_gps_lock = 0
self.watch = None
self.last_streamrate1 = -1
self.last_streamrate2 = -1
self.last_seq = 0
self.armed = False
self.current_depth = 0
self.xcenter = 0
self.ycenter = 0
self.circle_depth = 0
self.isCircle = 0
self.depth_stream = collections.deque([0]*5, 5)
self.sock_failure_data = False
self.socket_open = False
self.pwm_val = 0
self.auto = False
# integrate rc module
self.override = [ 0 ] * 16
self.last_override = [ 0 ] * 16
self.override_counter = 0
self.sitl_output = False
self.disarm_flag = False
self.auto_t_started = False
if self.sitl_output:
self.override_period = mavutil.periodic_event(20)
else:
self.override_period = mavutil.periodic_event(1)
def __init__(self, mpstate):
super(CUAVModule, self).__init__(mpstate, "CUAV", "CUAV checks")
self.console.set_status('RPM', 'RPM: --', row=8, fg='black')
self.console.set_status('RFind', 'RFind: --', row=8, fg='black')
self.console.set_status('Button', 'Button: --', row=8, fg='black')
self.console.set_status('ICE', 'ICE: --', row=8, fg='black')
self.rate_period = mavutil.periodic_event(1.0/15)
self.button_remaining = None
self.button_change = None
self.last_button_update = time.time()
self.last_target_update = time.time()
self.button_change_recv_time = 0
self.button_announce_time = 0
self.last_rpm_update = 0
self.last_rpm_value = None
self.last_rpm_announce = 0
self.showLandingZone = 0
self.showJoeZone = True
self.target = None
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.cuav_settings = MPSettings(
[ MPSetting('rpm_threshold', int, 6000, 'RPM Threshold'),
MPSetting('wind_speed', float, 0, 'wind speed (m/s)'),
MPSetting('wind_direction', float, 0, 'wind direction (degrees)') ])
self.add_completion_function('(CUAVCHECKSETTING)', self.cuav_settings.completion)
self.add_command('cuavcheck', self.cmd_cuavcheck,
'cuav check control',
['set (CUAVCHECKSETTING)'])
#make the initial map menu
if mp_util.has_wxpython:
self.menu = MPMenuSubMenu('UAV Challenge', items=[MPMenuCheckbox('Show Landing Zone', 'Show Landing Zone', '# cuavcheck toggleLandingZone'), MPMenuCheckbox('Show Joe Zone', 'Show Joe Zone', '# cuavcheck toggleJoeZone')])
self.module('map').add_menu(self.menu)
def __init__(self, master, sysID=1, compoID=250):
if master == "/dev/ttyACM0":
master = detect_pixhawk()
print "Pixhawk Master is", master
baud = 115200
reboot = False
else:
baud = 57600
reboot = True
self.conn = mavutil.mavlink_connection(device=master, baud=baud,
autoreconnect=True)
if reboot:
logging.info("Reboot pixhawk !")
self.conn.reboot_autopilot()
time.sleep(0.5)
self.sysID = sysID
self.compoID = compoID
self.heartbeat_period = mavutil.periodic_event(1)
self.heartbeat_period.frequency = 1.0
self.recv_msg_count = 0
self.bad_data_count = 0
self.param_names = {}
for i in range(1, 8):
self.param_names["RC%d_MIN" % i] = None
self.param_names["RC%d_MAX" % i] = None
self.initialize()
def __init__(self, mav_param, observer):
self.mav_param_set = set()
self.mav_param_count = 0
self.param_period = mavutil.periodic_event(1)
self.fetch_one = 0
self.mav_param = mav_param
self.observer = observer
def __init__(self, mpstate):
super(BatteryModule, self).__init__(mpstate, "battery", "battery commands")
self.add_command('bat', self.cmd_bat, "show battery information")
self.last_battery_announce = 0
self.last_battery_announce_time = 0
self.last_battery_cell_announce_time = 0
self.battery_level = -1
self.voltage_level = -1
self.current_battery = -1
self.battery2_voltage = -1
self.per_cell = 0
self.servo_voltage = -1
self.high_servo_voltage = -1
self.last_servo_warn_time = 0
self.last_vcc_warn_time = 0
self.settings.append(
MPSetting('battwarn', int, 1, 'Battery Warning Time', tab='Battery'))
self.settings.append(
MPSetting('batwarncell', float, 3.7, 'Battery cell Warning level'))
self.settings.append(
MPSetting('servowarn', float, 4.3, 'Servo voltage warning level'))
self.settings.append(
MPSetting('vccwarn', float, 4.3, 'Vcc voltage warning level'))
self.settings.append(MPSetting('numcells', int, 0, range=(0,10), increment=1))
self.battery_period = mavutil.periodic_event(5)
def __init__(self):
self.wp_op = None
self.wp_save_filename = None
self.wploader = mavwp.MAVWPLoader()
self.loading_waypoints = False
self.loading_waypoint_lasttime = time.time()
self.last_waypoint = 0
self.wp_period = mavutil.periodic_event(0.5)
def __init__(self, mpstate):
super(ADSBModule, self).__init__(mpstate, "adsb", "ADS-B data support")
self.threat_vehicles = {}
self.active_threat_ids = [] # holds all threat ids the vehicle is evading
self.add_command('adsb', self.cmd_ADSB, ["adsb control",
"<status>",
"set (ADSBSETTING)"])
self.ADSB_settings = mp_settings.MPSettings([("timeout", int, 10), # seconds
("threat_radius", int, 200), # meters
("show_threat_radius", bool, False),
# threat_radius_clear = threat_radius*threat_radius_clear_multiplier
("threat_radius_clear_multiplier", int, 2),
("show_threat_radius_clear", bool, False)])
self.threat_detection_timer = mavutil.periodic_event(2)
self.threat_timeout_timer = mavutil.periodic_event(2)
def __init__(self, mav_param, logdir, vehicle_name, parm_file):
self.mav_param_set = set()
self.mav_param_count = 0
self.param_period = mavutil.periodic_event(1)
self.fetch_one = 0
self.mav_param = mav_param
self.logdir = logdir
self.vehicle_name = vehicle_name
self.parm_file = parm_file
def __init__(self, mpstate):
super(WPModule, self).__init__(mpstate, "wp", "waypoint handling", public=True)
self.wp_op = None
self.wp_save_filename = None
self.wploader = mavwp.MAVWPLoader()
self.loading_waypoints = False
self.loading_waypoint_lasttime = time.time()
self.last_waypoint = 0
self.wp_period = mavutil.periodic_event(0.5)
self.undo_wp = None
self.undo_type = None
self.undo_wp_idx = -1
self.add_command(
"wp",
self.cmd_wp,
"waypoint management",
["<list|clear|move|remove|loop|set|undo|movemulti|param>", "<load|update|save|show> (FILENAME)"],
)
if self.continue_mode and self.logdir != None:
waytxt = os.path.join(mpstate.status.logdir, "way.txt")
if os.path.exists(waytxt):
self.wploader.load(waytxt)
print("Loaded waypoints from %s" % waytxt)
if mp_util.has_wxpython:
self.menu_added_console = False
self.menu_added_map = False
self.menu = MPMenuSubMenu(
"Mission",
items=[
MPMenuItem("Clear", "Clear", "# wp clear"),
MPMenuItem("List", "List", "# wp list"),
MPMenuItem(
"Load",
"Load",
"# wp load ",
handler=MPMenuCallFileDialog(flags=wx.FD_OPEN, title="Mission Load", wildcard="*.txt"),
),
MPMenuItem(
"Save",
"Save",
"# wp save ",
handler=MPMenuCallFileDialog(
flags=wx.FD_SAVE | wx.FD_OVERWRITE_PROMPT, title="Mission Save", wildcard="*.txt"
),
),
MPMenuItem(
"Draw",
"Draw",
"# wp draw ",
handler=MPMenuCallTextDialog(title="Mission Altitude (m)", default=100),
),
MPMenuItem("Undo", "Undo", "# wp undo"),
MPMenuItem("Loop", "Loop", "# wp loop"),
],
)
def __init__(self, mpstate):
super(CUAVModule, self).__init__(mpstate, "CUAV", "CUAV checks", public=True)
self.console.set_status('RPM', 'RPM: --', row=8, fg='black')
self.console.set_status('RFind', 'RFind: --', row=8, fg='black')
self.console.set_status('Button', 'Button: --', row=8, fg='black')
self.console.set_status('ICE', 'ICE: --', row=8, fg='black')
self.console.set_status('FuelPump', 'FuelPump: --', row=8, fg='black')
self.console.set_status('DNFZ', 'DNFZ -- --', row=6, fg='black')
self.rate_period = mavutil.periodic_event(1.0/15)
self.button_remaining = None
self.button_change = None
self.last_button_update = time.time()
self.last_target_update = time.time()
self.button_change_recv_time = 0
self.button_announce_time = 0
self.fuel_change = None
self.last_fuel_update = time.time()
self.fuel_change_recv_time = 0
self.last_rpm_update = 0
self.last_rpm_value = None
self.last_rpm_announce = 0
self.showLandingZone = 0
self.showJoeZone = True
self.target = None
self.last_recall_check = 0
self.is_armed = False
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.cuav_settings = MPSettings(
[ MPSetting('rpm_threshold', int, 6000, 'RPM Threshold'),
MPSetting('wind_speed', float, 0, 'wind speed (m/s)'),
MPSetting('wind_direction', float, 0, 'wind direction (degrees)'),
MPSetting('button_pin', float, 0, 'button pin'),
MPSetting('fuel_pin', float, 1, 'fuel pin'),
MPSetting('wp_center', int, 2, 'center search USER number'),
MPSetting('wp_start', int, 1, 'start search USER number'),
MPSetting('wp_end', int, 3, 'end search USER number'),
MPSetting('wp_land',int, 4, 'landing start USER number'),
MPSetting('wp_recall', int, 5, 'recall Kraken USER number'),
MPSetting('wp_release', int, 6, 'release Kraken USER number'),
MPSetting('qnh_max_err', int, 50, 'maximum QNH error') ])
self.add_completion_function('(CUAVCHECKSETTING)', self.cuav_settings.completion)
self.add_command('cuavcheck', self.cmd_cuavcheck,
'cuav check control',
['checkparams',
'movetarget',
'resettarget',
'showJoeZone',
'set (CUAVCHECKSETTING)'])
#make the initial map menu
if mp_util.has_wxpython and self.module('map'):
self.menu = MPMenuSubMenu('UAV Challenge', items=[MPMenuCheckbox('Show Landing Zone', 'Show Landing Zone', '# cuavcheck toggleLandingZone'), MPMenuCheckbox('Show Joe Zone', 'Show Joe Zone', '# cuavcheck toggleJoeZone')])
self.module('map').add_menu(self.menu)
def __init__(self, mpstate):
super(ParamModule, self).__init__(mpstate, "param", "parameter handling", public = True)
self.mav_param_set = set()
self.mav_param_count = 0
self.param_period = mavutil.periodic_event(1)
self.fetch_one = 0
self.add_command('param', self.cmd_param, "parameter handling",
["<fetch|download>",
"<set|show|fetch|help> (PARAMETER)",
"<load|save> (FILENAME)"])
if self.continue_mode and self.logdir != None:
parmfile = os.path.join(self.logdir, 'mav.parm')
if os.path.exists(parmfile):
mpstate.mav_param.load(parmfile)
def __init__(self, mav_param, logdir, vehicle_name, parm_file):
self.mav_param_set = set()
self.mav_param_count = 0
self.param_period = mavutil.periodic_event(1)
self.fetch_one = dict()
self.mav_param = mav_param
self.logdir = logdir
self.vehicle_name = vehicle_name
self.parm_file = parm_file
self.fetch_set = None
self.xml_filepath = None
self.new_sysid_timestamp = time.time()
self.autopilot_type_by_sysid = {}
self.param_types = {}
def __init__(self, mpstate):
super(BatteryModule, self).__init__(mpstate, "battery", "battery commands")
self.add_command('bat', self.cmd_bat, "show battery information")
self.last_battery_announce = 0
self.last_battery_announce_time = 0
self.last_battery_cell_announce_time = 0
self.battery_level = -1
self.voltage_level = -1
self.per_cell = 0
self.settings.append(
MPSetting('battwarn', int, 1, 'Battery Warning Time'))
self.settings.append(
MPSetting('batwarncell', float, 3.7, 'Battery cell Warning level'))
self.settings.append(MPSetting('numcells', int, 0, range=(0,10), increment=1))
self.battery_period = mavutil.periodic_event(0.3)
def __init__(self, mpstate):
super(WPModule, self).__init__(mpstate, "wp", "waypoint handling", public = True)
self.wp_op = None
self.wp_save_filename = None
self.wploader = mavwp.MAVWPLoader()
self.loading_waypoints = False
self.loading_waypoint_lasttime = time.time()
self.last_waypoint = 0
self.wp_period = mavutil.periodic_event(0.5)
self.add_command('wp', self.cmd_wp, 'waypoint management', ["<list|clear>",
"<load|update|save> (FILENAME)"])
if self.continue_mode and self.logdir != None:
waytxt = os.path.join(mpstate.status.logdir, 'way.txt')
if os.path.exists(waytxt):
self.wploader.load(waytxt)
print("Loaded waypoints from %s" % waytxt)
def __init__(self, mpstate):
super(WPModule, self).__init__(mpstate, "wp", "waypoint handling", public = True)
self.wp_op = None
self.wp_requested = {}
self.wp_received = {}
self.wp_save_filename = None
self.wploader_by_sysid = {}
self.loading_waypoints = False
self.loading_waypoint_lasttime = time.time()
self.last_waypoint = 0
self.wp_period = mavutil.periodic_event(0.5)
self.undo_wp = None
self.undo_type = None
self.undo_wp_idx = -1
self.wploader.expected_count = 0
self.add_command('wp', self.cmd_wp, 'waypoint management',
["<list|clear|move|remove|loop|set|undo|movemulti|changealt|param|status>",
"<load|update|save|savecsv|show> (FILENAME)"])
if self.continue_mode and self.logdir is not None:
waytxt = os.path.join(mpstate.status.logdir, 'way.txt')
if os.path.exists(waytxt):
self.wploader.load(waytxt)
print("Loaded waypoints from %s" % waytxt)
self.menu_added_console = False
self.menu_added_map = False
if mp_util.has_wxpython:
self.menu = MPMenuSubMenu('Mission',
items=[MPMenuItem('Editor', 'Editor', '# wp editor'),
MPMenuItem('Clear', 'Clear', '# wp clear'),
MPMenuItem('List', 'List', '# wp list'),
MPMenuItem('Load', 'Load', '# wp load ',
handler=MPMenuCallFileDialog(flags=('open',),
title='Mission Load',
wildcard='*.txt')),
MPMenuItem('Save', 'Save', '# wp save ',
handler=MPMenuCallFileDialog(flags=('save', 'overwrite_prompt'),
title='Mission Save',
wildcard='*.txt')),
MPMenuItem('Draw', 'Draw', '# wp draw ',
handler=MPMenuCallTextDialog(title='Mission Altitude (m)',
default=100)),
MPMenuItem('Undo', 'Undo', '# wp undo'),
MPMenuItem('Loop', 'Loop', '# wp loop'),
MPMenuItem('Add NoFly', 'Loop', '# wp noflyadd')])
def connectionMade(self):
self.host = self.transport.getHost()
print "Starting MAVLink via {0}".format(self.host)
self.mav.set_callback(self.master_callback,self)
if hasattr(self.mav, 'set_send_callback'):
self.mav.set_send_callback(self.master_send_callback, self)
self.linkerror = False
self.link_delayed = False
self.last_heartbeat = 0
self.last_message = 0
self.highest_msec = 0
#0.1 s send heartbeat
self.wait_heartbeat()
self.msg_period = mavutil.periodic_event(1.0/15)
self.connect_set_up()
#self.WIRE_PROTOCOL_VERSION = "1.0"
self.fd = self.transport.fileno()
self.l.start(0.1)
def main():
# Create a Joystick object
js = JoyStick()
# GCS Handling
global in_emergcs;
# create a mavlink serial instance
master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate, source_system=opts.SOURCE_SYSTEM)
#master.mav.command_long_send(master.target_system, master.target_component, mavutil.mavlink.MAV_CMD_DO_SWITCH_GCS, 0, 0, 255, 0, 0, 0, 0, 0,)
# wait for the heartbeat msg to find the system ID
wait_heartbeat(master)
print("Got HeartBeat")
# Set the GCS_SYSID so that we handle lower packet com
#mavparam = mavparm.MAVParmDict()
#mavparam.mavset(master, SYSID_MYGCS, 255, retries=3)
# Create loop timing
js_period = mavutil.periodic_event(opts.rate)
while True:
# Sanity check to make sure that the traffic we want to be flowing is...
# See note at bottom for messages
read_messages(master)
# This should trigger at 60Hz and send rc override
js_input = js.get_rc_override()
handle_GCS_change(master, js_input[8:])
if in_emergcs and js_period.trigger():
# Send RC commands over mavlink
master.mav.rc_channels_override_send(master.target_system, master.target_component, *js_input[:8])
def __init__(self, mpstate):
super(WPModule, self).__init__(mpstate, "wp", "waypoint handling", public = True)
self.wp_op = None
self.wp_save_filename = None
self.wploader = mavwp.MAVWPLoader()
self.loading_waypoints = False
self.loading_waypoint_lasttime = time.time()
self.last_waypoint = 0
self.wp_period = mavutil.periodic_event(0.5)
self.add_command('wp', self.cmd_wp, 'waypoint management',
["<list|clear|move|remove|loop|set>",
"<load|update|save> (FILENAME)"])
if self.continue_mode and self.logdir != None:
waytxt = os.path.join(mpstate.status.logdir, 'way.txt')
if os.path.exists(waytxt):
self.wploader.load(waytxt)
print("Loaded waypoints from %s" % waytxt)
self.menu_added_console = False
self.menu_added_map = False
from MAVProxy.modules.lib.mp_menu import *
self.menu = MPMenuSubMenu('Mission',
items=[MPMenuItem('Clear', 'Clear', '# wp clear'),
MPMenuItem('List', 'List', '# wp list'),
MPMenuItem('Load', 'Load', '# wp load ',
handler=MPMenuCallFileDialog(flags=wx.FD_OPEN,
title='Mission Load',
wildcard='*.txt')),
MPMenuItem('Save', 'Save', '# wp save ',
handler=MPMenuCallFileDialog(flags=wx.FD_SAVE|wx.FD_OVERWRITE_PROMPT,
title='Mission Save',
wildcard='*.txt')),
MPMenuItem('Draw', 'Draw', '# wp draw ',
handler=MPMenuCallTextDialog(title='Mission Altitude (m)',
default=100)),
MPMenuItem('Loop', 'Loop', '# wp loop')])
context = zmq.Context()
socket_sub = context.socket(zmq.SUB)
socket_sub.connect('tcp://%s:%d' % config.zmq_pub_unreal_proxy)
socket_sub.setsockopt(zmq.SUBSCRIBE, config.topic_unreal_state)
socket_sub.setsockopt(zmq.SUBSCRIBE,
config.topic_unreal_drone_rgb_camera % drone_num)
mav1 = mavutil.mavlink_connection('udp:127.0.0.1:%d' % (14551 + 0 * 10))
print("Waiting for HEARTBEAT")
mav1.wait_heartbeat()
print("Heartbeat from APM (system %u component %u)" %
(mav1.target_system, mav1.target_component))
event = mavutil.periodic_event(0.3)
event1hz = mavutil.periodic_event(1)
freq = 30
pub_position_event = mavutil.periodic_event(freq)
def set_rcs(rc1, rc2, rc3, rc4):
global mav1
values = [1500] * 8
values[0] = rc1
values[1] = rc2
values[2] = rc3
values[3] = rc4
mav1.mav.rc_channels_override_send(mav1.target_system,
mav1.target_component, *values)
mav1.recv_match(type='SYS_STATUS',
condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4',
blocking=True)
mav2.recv_match(type='SYS_STATUS',
condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4',
blocking=True)
print("Setting declination")
mav1.mav.param_set_send(mav1.target_system, mav1.target_component,
'COMPASS_DEC', radians(12.33))
mav2.mav.param_set_send(mav2.target_system, mav2.target_component,
'COMPASS_DEC', radians(12.33))
set_attitude(1060, 1160)
event = mavutil.periodic_event(30)
pevent = mavutil.periodic_event(0.3)
rc3_min = 1060
rc3_max = 1850
rc4_min = 1080
rc4_max = 1500
rc3 = rc3_min
rc4 = 1160
delta3 = 2
delta4 = 1
use_pitch = 1
MAV_ACTION_CALIBRATE_GYRO = 17
mav1.mav.action_send(mav1.target_system, mav1.target_component,
MAV_ACTION_CALIBRATE_GYRO)
mav2.mav.action_send(mav2.target_system, mav2.target_component,
def __init__(self):
self.mav_param_set = set()
self.mav_param_count = 0
self.param_period = mavutil.periodic_event(1)
self.fetch_one = 0
def __init__(self, mpstate):
super(CUAVModule, self).__init__(mpstate, "CUAV", "CUAV checks")
self.console.set_status('Bottle', 'Bottle: --', row=8, fg='green')
self.rate_period = mavutil.periodic_event(1.0/15)
def __init__(self, mpstate):
super(MyPiModule, self).__init__(mpstate, "MyPiModule", "my commands")
# my cmd
self.add_command('mybat', self.cmd_mybat, "my battery information")
self.add_command('myshut', self.cmd_myshutdown, "to shutdown")
self.add_command('myreboot', self.cmd_myreboot, "to reboot")
self.add_command('myrtl', self.cmd_myrtl, "change flight mode to RTL")
# my settings
self.settings.append(MPSetting('mytimebat', int, 5, 'Battery Interval Time sec', tab='my'))
self.settings.append(MPSetting('mytimerc', int, 5, 'RC Interval Time sec'))
self.settings.append(MPSetting('mydebug', bool, False, 'Debug'))
self.settings.append(MPSetting('myminvolt', int, 10000, 'Minimum battery voltage before shutdown'))
self.settings.append(MPSetting('myminremain', int, 10, 'Minimum battery remaining before shutdown'))
self.settings.append(MPSetting('mydelayinit', int, 30, 'Delay before shutdown or reboot'))
self.settings.append(MPSetting('myrcvideo', int, 6, 'Radio channel to change video on/off'))
self.settings.append(MPSetting('myrcwlan0', int, 8, 'Radio channel to change wlan0 on/off'))
self.settings.append(MPSetting('myrcyaw', int, 4, 'Radio channel to reboot/shutdown'))
self.settings.append(MPSetting('myrcroll', int, 1, 'Radio channel to reboot/shutdown'))
self.myversion = "1.4"
# stats
self.VFR_HUD = 0
self.SYS_STATUS = 0
self.HEARTBEAT = 0
self.RC_CHANNELS_RAW = 0
self.battery_period_trigger = 0
self.start_time = time.time()
### battery low :
self.shutdown_by_lowbat = False
self.shutdown_by_lowbat_time = 0
### ByRadio resquested
self.shutdown_by_radio = False
self.shutdown_by_radio_time = 0
self.reboot_by_radio = False
self.reboot_by_radio_time = 0
### ByCmd resquested
self.shutdown_by_cmd = False
self.shutdown_by_cmd_time = 0
self.reboot_by_cmd = False
self.reboot_by_cmd_time = 0
# default values
self.armed = False
self.mystate = 0
self.myvolt = 0
self.mythrottle = 0
self.mycurrent = 0
self.myremaining = 0
self.myrcraw = [0,0,0,0,0,0,0,0,0]
self.wlan0_up = False
self.video_on = True
self.rtl_on = False
self.last_battery_check_time = time.time()
self.last_rc_check_time = time.time()
self.battery_period = mavutil.periodic_event(5)
self.FORMAT = '%Y-%m-%d %H:%M:%S'
#self.FORMAT2 = '%Hh%Mm%Ss'
self.mycountermessage = 0
# default to servo range of 1000 to 1700
#self.RC1_MIN = self.get_mav_param('RC1_MIN', 0)
#self.RC1_MAX = self.get_mav_param('RC1_MAX', 0)
self.RC_low_mark = [1200,1200,1200,1200,1200,1200,1200,1200,1200]
self.RC_high_mark = [1700,1700,1700,1700,1700,1700,1700,1700,1700]
self.myseverity = 0
self.mytext = "nulltext"
self.myip = "0.0.0.0"
# to send statustext
self.master2 = mavutil.mavlink_connection("udp:127.0.0.1:14550", input=False, dialect="common")
def __init__(self, mpstate):
super(Autopilotmodule, self).__init__(mpstate, "autopilot", "autopilot command ", public=True)
self.add_command('autopilot', self.cmd_ap, "Autopilot input control")
self.add_command('current_imu', self.print_imu, "prints out current IMU data", [ '' ])
self.add_command('open_socket', self.cmd_sock, "connect to socket", ['sockno'])
self.add_command('close_socket', self.close_sock, "close socket", ['sockno'])
self.add_command('current_depth', self.cmd_depth, "get current object depth")
self.add_command('kill', self.cmd_kill, "sets rc values to 0")
# camera parameters
self.cam = None
self.most_recent = None
self.CAPTURE_WIDTH = 640.0
self.CAPTURE_HEIGHT = 480.0
self.HALF_CAPTURE_WIDTH = self.CAPTURE_WIDTH/2
self.HALF_CAPTURE_HEIGHT = self.CAPTURE_HEIGHT/2
#create cross-correlation templates
self.col_temp = np.ones((self.CAPTURE_HEIGHT,1), np.uint8) #ROWS by 1 array
self.row_temp = np.ones((1,self.CAPTURE_WIDTH), np.uint8) #1 by COLS array
# for keeping track of vision loop speed
self.t = 0
# classifier output
self.target_in_frame = False
self.last_frames = collections.deque([0]*10, 10)
self.frame = 0
# RC radio data trims
self.ch1_trim = 1500
self.ch2_trim = 1500
# Initialize PID coefficients
self.x_Ap = 0.7
self.x_Ai = 0.05
self.x_Ad = 0.6
self.y_Ap = 0.8
self.y_Ai = 0.05
self.y_Ad = 0.6
# initialize errors
self.x_error, self.y_error = 0, 0
self.old_x_error, self.old_y_error = 0, 0
self.xcenter = 0
self.ycenter = 0
# initialize error accumulators
self.x_sigma = collections.deque([0,0,0,0], 4)
self.y_sigma = collections.deque([0,0,0,0], 4)
# initialize deltas
self.x_delta = 0
self.y_delta = 0
# initialize timer
self.t = time.time()
self.dt = 0.5
# flag for pid first output behavior
self.delta_flag = True
self.sigma_flag = True
mpstate = None
# socket variables
self.sock_option = False
self.auto = False
# set initial PWM values for movement states
self.pwm_val = 1400
self.hover_pwm_val = 1530
# Depth values from realsense
self.depth = 0
self.last_depth = collections.deque([0]*10, 10) # for summing last ten depth samples
self.target_altitude, self.depth = 0, 0
self.waiting_for_command = True
# override commands for motors
self.override = [ 0 ] * 16
self.last_override = [ 0 ] * 16
self.override_counter = 0
# consistant IMU data update
self.check_imu_counter = 0
#AHRS Filtering algorithm variables
self.recipNorm = 0
self.beta = 0.1
self.sampleFreq = 512.0 # sample frequency in Hz
self.q0 = 1.0
self.q1 = 0.0
self.q2 = 0.0
self.q3 = 0.0 # quaternion of sensor frame relative to auxiliary frame
self.port = 0
self.s0, self.s1, self.s2, self.s3 = 0, 0, 0, 0
self.qDot1, self.qDot2, self.qDot3, self.qDot4 = 0, 0, 0, 0
self.hx, self.hy = 0, 0
self._2q0mx, self._2q0my, self._2q0mz, self._2q1mx, self._2bx, self._2bz, self._4bx, self._4bz, self._2q0, self._2q1, self._2q2, self._2q3, self._2q0q2, self._2q2q3, self.q0q0, self.q0q1, self.q0q2, self.q0q3, self.q1q1, self.q1q2, self.q1q3, self.q2q2, self.q2q3, self.q3q3 = 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
if self.sitl_output:
self.override_period = mavutil.periodic_event(20)
else:
self.override_period = mavutil.periodic_event(1)
def __init__(self, mpstate):
super(CUAVModule, self).__init__(mpstate,
"CUAV",
"CUAV checks",
public=True)
self.console.set_status('RPM', 'RPM: --', row=8, fg='black')
self.console.set_status('RFind', 'RFind: --', row=8, fg='black')
self.console.set_status('Button', 'Button: --', row=8, fg='black')
self.console.set_status('ICE', 'ICE: --', row=8, fg='black')
self.console.set_status('FuelPump', 'FuelPump: --', row=8, fg='black')
self.console.set_status('DNFZ', 'DNFZ -- --', row=6, fg='black')
self.rate_period = mavutil.periodic_event(1.0 / 15)
self.button_remaining = None
self.button_change = None
self.last_button_update = time.time()
self.last_target_update = time.time()
self.button_change_recv_time = 0
self.button_announce_time = 0
self.fuel_change = None
self.last_fuel_update = time.time()
self.fuel_change_recv_time = 0
self.last_rpm_update = 0
self.last_rpm_value = None
self.last_rpm_announce = 0
self.showLandingZone = 0
self.showJoeZone = True
self.target = None
self.last_recall_check = 0
self.is_armed = False
from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting
self.cuav_settings = MPSettings([
MPSetting('rpm_threshold', int, 6000, 'RPM Threshold'),
MPSetting('wind_speed', float, 0, 'wind speed (m/s)'),
MPSetting('wind_direction', float, 0, 'wind direction (degrees)'),
MPSetting('button_pin', float, 0, 'button pin'),
MPSetting('fuel_pin', float, 1, 'fuel pin'),
MPSetting('wp_center', int, 2, 'center search USER number'),
MPSetting('wp_start', int, 1, 'start search USER number'),
MPSetting('wp_end', int, 3, 'end search USER number'),
MPSetting('wp_land', int, 4, 'landing start USER number'),
MPSetting('wp_recall', int, 5, 'recall Kraken USER number'),
MPSetting('wp_release', int, 6, 'release Kraken USER number'),
MPSetting('qnh_max_err', int, 50, 'maximum QNH error')
])
self.add_completion_function('(CUAVCHECKSETTING)',
self.cuav_settings.completion)
self.add_command('cuavcheck', self.cmd_cuavcheck, 'cuav check control',
[
'checkparams', 'movetarget', 'resettarget',
'showJoeZone', 'set (CUAVCHECKSETTING)'
])
#make the initial map menu
if mp_util.has_wxpython and self.module('map'):
self.menu = MPMenuSubMenu(
'UAV Challenge',
items=[
MPMenuCheckbox('Show Landing Zone', 'Show Landing Zone',
'# cuavcheck toggleLandingZone'),
MPMenuCheckbox('Show Joe Zone', 'Show Joe Zone',
'# cuavcheck toggleJoeZone')
])
self.module('map').add_menu(self.menu)
def initMAVProxy():
from optparse import OptionParser
parser = OptionParser("mavproxy.py [options]")
parser.add_option("--master", dest="master", action='append',
metavar="DEVICE[,BAUD]", help="MAVLink master port and optional baud rate",
default=[])
parser.add_option("--out", dest="output", action='append',
metavar="DEVICE[,BAUD]", help="MAVLink output port and optional baud rate",
default=[])
parser.add_option("--baudrate", dest="baudrate", type='int',
help="default serial baud rate", default=115200)
parser.add_option("--sitl", dest="sitl", default=None, help="SITL output port")
parser.add_option("--streamrate",dest="streamrate", default=4, type='int',
help="MAVLink stream rate")
parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int',
default=255, help='MAVLink source system for this GCS')
parser.add_option("--target-system", dest='TARGET_SYSTEM', type='int',
default=1, help='MAVLink target master system')
parser.add_option("--target-component", dest='TARGET_COMPONENT', type='int',
default=1, help='MAVLink target master component')
parser.add_option("--logfile", dest="logfile", help="MAVLink master logfile",
default='mav.tlog')
parser.add_option("-a", "--append-log", dest="append_log", help="Append to log files",
action='store_true', default=False)
parser.add_option("--quadcopter", dest="quadcopter", help="use quadcopter controls",
action='store_true', default=False)
parser.add_option("--setup", dest="setup", help="start in setup mode",
action='store_true', default=False)
parser.add_option("--nodtr", dest="nodtr", help="disable DTR drop on close",
action='store_true', default=False)
parser.add_option("--show-errors", dest="show_errors", help="show MAVLink error packets",
action='store_true', default=False)
parser.add_option("--speech", dest="speech", help="use text to speach",
action='store_true', default=False)
parser.add_option("--num-cells", dest="num_cells", help="number of LiPo battery cells",
type='int', default=0)
parser.add_option("--aircraft", dest="aircraft", help="aircraft name", default=None)
parser.add_option("--cmd", dest="cmd", help="initial commands", default=None)
parser.add_option("--console", action='store_true', help="use GUI console")
parser.add_option("--map", action='store_true', help="load map module")
parser.add_option(
'--load-module',
action='append',
default=[],
help='Load the specified module. Can be used multiple times, or with a comma separated list')
parser.add_option("--mav09", action='store_true', default=False, help="Use MAVLink protocol 0.9")
parser.add_option("--auto-protocol", action='store_true', default=False, help="Auto detect MAVLink protocol version")
parser.add_option("--nowait", action='store_true', default=False, help="don't wait for HEARTBEAT on startup")
parser.add_option("--continue", dest='continue_mode', action='store_true', default=False, help="continue logs")
parser.add_option("--dialect", default="ardupilotmega", help="MAVLink dialect")
parser.add_option("--rtscts", action='store_true', help="enable hardware RTS/CTS flow control")
(opts, args) = parser.parse_args()
mavproxy.opts=opts
if opts.mav09:
os.environ['MAVLINK09'] = '1'
from pymavlink import mavutil, mavparm
mavutil.set_dialect(opts.dialect)
# global mavproxy state
mpstate = MPState()
mavproxy.mpstate=mpstate
mpstate.status.exit = False
mpstate.command_map = command_map
mpstate.continue_mode = opts.continue_mode
if opts.speech:
# start the speech-dispatcher early, so it doesn't inherit any ports from
# modules/mavutil
load_module('speech')
if not opts.master:
serial_list = mavutil.auto_detect_serial(preferred_list=['*FTDI*',"*Arduino_Mega_2560*", "*3D_Robotics*", "*USB_to_UART*"])
if len(serial_list) == 1:
opts.master = [serial_list[0].device]
else:
print('''
Please choose a MAVLink master with --master
For example:
--master=com14
--master=/dev/ttyUSB0
--master=127.0.0.1:14550
Auto-detected serial ports are:
''')
for port in serial_list:
print("%s" % port)
sys.exit(1)
# container for status information
mpstate.status.target_system = opts.TARGET_SYSTEM
mpstate.status.target_component = opts.TARGET_COMPONENT
mpstate.mav_master = []
# open master link
for mdev in opts.master:
if ',' in mdev and not os.path.exists(mdev):
port, baud = mdev.split(',')
else:
port, baud = mdev, opts.baudrate
m = mavutil.mavlink_connection(port, autoreconnect=True, baud=int(baud))
m.mav.set_callback(master_callback, m)
if hasattr(m.mav, 'set_send_callback'):
m.mav.set_send_callback(master_send_callback, m)
if opts.rtscts:
m.set_rtscts(True)
m.linknum = len(mpstate.mav_master)
m.linkerror = False
m.link_delayed = False
m.last_heartbeat = 0
m.last_message = 0
m.highest_msec = 0
mpstate.mav_master.append(m)
mpstate.status.counters['MasterIn'].append(0)
# log all packets from the master, for later replay
open_logs()
# open any mavlink UDP ports
for p in opts.output:
if ',' in p and not os.path.exists(p):
port, baud = p.split(',')
else:
port, baud = p, opts.baudrate
mpstate.mav_outputs.append(mavutil.mavlink_connection(port, baud=int(baud), input=False))
if opts.sitl:
mpstate.sitl_output = mavutil.mavudp(opts.sitl, input=False)
mpstate.settings.numcells = opts.num_cells
mpstate.settings.streamrate = opts.streamrate
mpstate.settings.streamrate2 = opts.streamrate
mavproxy.msg_period = mavutil.periodic_event(1.0/15)
mavproxy.heartbeat_period = mavutil.periodic_event(1)
mavproxy.battery_period = mavutil.periodic_event(0.1)
mavproxy.heartbeat_check_period = mavutil.periodic_event(0.33)
mpstate.rl = rline.rline("MAV> ", mpstate)
if opts.setup:
mpstate.rl.set_prompt("")
if 'HOME' in os.environ and not opts.setup:
start_script = os.path.join(os.environ['HOME'], ".mavinit.scr")
if os.path.exists(start_script):
run_script(start_script)
if opts.aircraft is not None:
start_script = os.path.join(opts.aircraft, "mavinit.scr")
if os.path.exists(start_script):
run_script(start_script)
else:
print("no script %s" % start_script)
if not opts.setup:
# some core functionality is in modules
standard_modules = ['log','rally','fence','param','relay',
'tuneopt','arm','mode','calibration','rc','wp','auxopt','quadcontrols','test']
for m in standard_modules:
load_module(m, quiet=True)
if opts.console:
process_stdin('module load console')
if opts.map:
process_stdin('module load map')
for module in opts.load_module:
modlist = module.split(',')
for mod in modlist:
process_stdin('module load %s' % mod)
if opts.cmd is not None:
cmds = opts.cmd.split(';')
for c in cmds:
process_stdin(c)
# run main loop as a thread
mpstate.status.thread = threading.Thread(target=main_loop)
mpstate.status.thread.daemon = True
mpstate.status.thread.start()
print("Heartbeat from APM (system %u component %u)" % (mav2.target_system, mav2.target_system))
print("Waiting for MANUAL mode")
mav1.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True)
mav2.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True)
print("Setting declination")
mav1.mav.param_set_send(mav1.target_system, mav1.target_component,
'COMPASS_DEC', radians(12.33))
mav2.mav.param_set_send(mav2.target_system, mav2.target_component,
'COMPASS_DEC', radians(12.33))
set_attitude(1060, 1160)
event = mavutil.periodic_event(30)
pevent = mavutil.periodic_event(0.3)
rc3_min = 1060
rc3_max = 1850
rc4_min = 1080
rc4_max = 1500
rc3 = rc3_min
rc4 = 1160
delta3 = 2
delta4 = 1
use_pitch = 1
MAV_ACTION_CALIBRATE_GYRO = 17
mav1.mav.action_send(mav1.target_system, mav1.target_component, MAV_ACTION_CALIBRATE_GYRO)
mav2.mav.action_send(mav2.target_system, mav2.target_component, MAV_ACTION_CALIBRATE_GYRO)
# log all packets from the master, for later replay
open_logs()
# open any mavlink UDP ports
for p in opts.output:
mpstate.mav_outputs.append(mavutil.mavlink_connection(p, baud=opts.baudrate, input=False))
if opts.sitl:
mpstate.sitl_output = mavutil.mavudp(opts.sitl, input=False)
mpstate.settings.numcells = opts.num_cells
mpstate.settings.streamrate = opts.streamrate
mpstate.settings.streamrate2 = opts.streamrate
msg_period = mavutil.periodic_event(1.0/15)
heartbeat_period = mavutil.periodic_event(1)
battery_period = mavutil.periodic_event(0.1)
heartbeat_check_period = mavutil.periodic_event(0.33)
mpstate.rl = rline.rline("MAV> ", mpstate)
if opts.setup:
mpstate.rl.set_prompt("")
if 'HOME' in os.environ and not opts.setup:
start_script = os.path.join(os.environ['HOME'], ".mavinit.scr")
if os.path.exists(start_script):
run_script(start_script)
if opts.aircraft is not None:
start_script = os.path.join(opts.aircraft, "mavinit.scr")
def __init__(self, period, cmd):
self.period = period
self.cmd = cmd
self.event = mavutil.periodic_event(1.0/period)
