def __init__(self, mpstate): from pymavlink import mavparm super(TrackerModule, self).__init__(mpstate, "tracker", "antenna tracker control module") self.connection = None self.tracker_param = mavparm.MAVParmDict() self.pstate = ParamState(self.tracker_param, self.logdir, self.vehicle_name, 'tracker.parm') self.tracker_settings = mp_settings.MPSettings( [ ('port', str, "/dev/ttyUSB0"), ('baudrate', int, 57600), ('debug', int, 0) ] ) self.add_command('tracker', self.cmd_tracker, "antenna tracker control module", ['<start|arm|disarm|level|mode|position|calpress|mode>', 'set (TRACKERSETTING)', 'param <set|show|fetch|help> (TRACKERPARAMETER)', 'param (TRACKERSETTING)']) self.add_completion_function('(TRACKERSETTING)', self.tracker_settings.completion) self.add_completion_function('(TRACKERPARAMETER)', self.complete_parameter)
class TrackerModule(mp_module.MPModule): def __init__(self, mpstate): from pymavlink import mavparm super(TrackerModule, self).__init__(mpstate, "tracker", "antenna tracker control module") self.connection = None self.tracker_param = mavparm.MAVParmDict() self.pstate = ParamState(self.tracker_param, self.logdir, self.vehicle_name, 'tracker.parm') self.tracker_settings = mp_settings.MPSettings( [ ('port', str, "/dev/ttyUSB0"), ('baudrate', int, 57600), ('debug', int, 0) ] ) self.add_command('tracker', self.cmd_tracker, "antenna tracker control module", ['<start|arm|disarm|level|mode|position|calpress|mode>', 'set (TRACKERSETTING)', 'param <set|show|fetch|help> (TRACKERPARAMETER)', 'param (TRACKERSETTING)']) self.add_completion_function('(TRACKERSETTING)', self.tracker_settings.completion) self.add_completion_function('(TRACKERPARAMETER)', self.complete_parameter) def complete_parameter(self, text): '''complete a tracker parameter''' return self.tracker_param.keys() def find_connection(self): '''find an antenna tracker connection if possible''' if self.connection is not None: return self.connection for m in self.mpstate.mav_master: if 'HEARTBEAT' in m.messages: if m.messages['HEARTBEAT'].type == mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER: return m return None def cmd_tracker(self, args): '''tracker command parser''' usage = "usage: tracker <start|set|arm|disarm|level|param|mode|position> [options]" if len(args) == 0: print(usage) return if args[0] == "start": self.cmd_tracker_start() elif args[0] == "set": self.tracker_settings.command(args[1:]) elif args[0] == 'arm': self.cmd_tracker_arm() elif args[0] == 'disarm': self.cmd_tracker_disarm() elif args[0] == 'level': self.cmd_tracker_level() elif args[0] == 'param': self.cmd_tracker_param(args[1:]) elif args[0] == 'mode': self.cmd_tracker_mode(args[1:]) elif args[0] == 'position': self.cmd_tracker_position(args[1:]) elif args[0] == 'calpress': self.cmd_tracker_calpress(args[1:]) else: print(usage) def cmd_tracker_position(self, args): '''tracker manual positioning commands''' connection = self.find_connection() if not connection: print("No antenna tracker found") return positions = [0, 0, 0, 0, 0] # x, y, z, r, buttons. only position[0] (yaw) and position[1] (pitch) are currently used for i in range(0, 4): if len(args) > i: positions[i] = int(args[i]) # default values are 0 connection.mav.manual_control_send(connection.target_system, positions[0], positions[1], positions[2], positions[3], positions[4]) def cmd_tracker_calpress(self, args): '''calibrate barometer on tracker''' connection = self.find_connection() if not connection: print("No antenna tracker found") return connection.calibrate_pressure() def cmd_tracker_mode(self, args): '''set arbitrary mode''' connection = self.find_connection() if not connection: print("No antenna tracker found") return mode_mapping = connection.mode_mapping() if mode_mapping is None: print('No mode mapping available') return if len(args) != 1: print('Available modes: ', mode_mapping.keys()) return mode = args[0].upper() if mode not in mode_mapping: print('Unknown mode %s: ' % mode) return connection.set_mode(mode_mapping[mode]) def mavlink_packet(self, m): '''handle an incoming mavlink packet from the master vehicle. Relay it to the tracker if it is a GLOBAL_POSITION_INT''' if m.get_type() in ['GLOBAL_POSITION_INT', 'SCALED_PRESSURE']: connection = self.find_connection() if not connection: return if m.get_srcSystem() != connection.target_system: connection.mav.send(m) def idle_task(self): '''called in idle time''' if not self.connection: return # check for a mavlink message from the tracker m = self.connection.recv_msg() if m is None: return if self.tracker_settings.debug: print(m) self.pstate.handle_mavlink_packet(self.connection, m) self.pstate.fetch_check(self.connection) if self.module('map') is None: return if m.get_type() == 'GLOBAL_POSITION_INT': (self.lat, self.lon, self.heading) = (m.lat*1.0e-7, m.lon*1.0e-7, m.hdg*0.01) if self.lat != 0 or self.lon != 0: self.module('map').create_vehicle_icon('AntennaTracker', 'red', follow=False, vehicle_type='antenna') self.mpstate.map.set_position('AntennaTracker', (self.lat, self.lon), rotation=self.heading) def cmd_tracker_start(self): if self.tracker_settings.port == None: print("tracker port not set") return if self.connection is not None: self.connection.close() self.connection = None print("Closed old connection") print("connecting to tracker %s at %d" % (self.tracker_settings.port, self.tracker_settings.baudrate)) m = mavutil.mavlink_connection(self.tracker_settings.port, autoreconnect=True, source_system=self.settings.source_system, baud=self.tracker_settings.baudrate) m.mav.srcComponent = self.settings.source_component if self.logdir: m.setup_logfile(os.path.join(self.logdir, 'tracker.tlog')) self.connection = m def cmd_tracker_arm(self): '''Enable the servos in the tracker so the antenna will move''' if not self.connection: print("tracker not connected") return self.connection.arducopter_arm() def cmd_tracker_disarm(self): '''Disable the servos in the tracker so the antenna will not move''' if not self.connection: print("tracker not connected") return self.connection.arducopter_disarm() def cmd_tracker_level(self): '''Calibrate the accelerometers. Disarm and move the antenna level first''' if not self.connection: print("tracker not connected") return self.connection.calibrate_level() def cmd_tracker_param(self, args): '''Parameter commands''' if not self.connection: print("tracker not connected") return self.pstate.handle_command(self.connection, self.mpstate, args)
class TrackerModule(mp_module.MPModule): def __init__(self, mpstate): from pymavlink import mavparm super(TrackerModule, self).__init__(mpstate, "tracker", "antenna tracker control module") self.connection = None self.tracker_param = mavparm.MAVParmDict() self.pstate = ParamState(self.tracker_param, self.logdir, self.vehicle_name, 'tracker.parm') self.tracker_settings = mp_settings.MPSettings([ ('port', str, "/dev/ttyUSB0"), ('baudrate', int, 57600), ('debug', int, 0) ]) self.add_command( 'tracker', self.cmd_tracker, "antenna tracker control module", [ '<start|arm|disarm|level|mode|position|calpress|mode>', 'set (TRACKERSETTING)', 'param <set|show|fetch|help> (TRACKERPARAMETER)', 'param (TRACKERSETTING)' ]) self.add_completion_function('(TRACKERSETTING)', self.tracker_settings.completion) self.add_completion_function('(TRACKERPARAMETER)', self.complete_parameter) def complete_parameter(self, text): '''complete a tracker parameter''' return self.tracker_param.keys() def find_connection(self): '''find an antenna tracker connection if possible''' if self.connection is not None: return self.connection for m in self.mpstate.mav_master: if 'HEARTBEAT' in m.messages: if m.messages[ 'HEARTBEAT'].type == mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER: return m return None def cmd_tracker(self, args): '''tracker command parser''' usage = "usage: tracker <start|set|arm|disarm|level|param|mode|position> [options]" if len(args) == 0: print(usage) return if args[0] == "start": self.cmd_tracker_start() elif args[0] == "set": self.tracker_settings.command(args[1:]) elif args[0] == 'arm': self.cmd_tracker_arm() elif args[0] == 'disarm': self.cmd_tracker_disarm() elif args[0] == 'level': self.cmd_tracker_level() elif args[0] == 'param': self.cmd_tracker_param(args[1:]) elif args[0] == 'mode': self.cmd_tracker_mode(args[1:]) elif args[0] == 'position': self.cmd_tracker_position(args[1:]) elif args[0] == 'calpress': self.cmd_tracker_calpress(args[1:]) else: print(usage) def cmd_tracker_position(self, args): '''tracker manual positioning commands''' connection = self.find_connection() if not connection: print("No antenna tracker found") return positions = [ 0, 0, 0, 0, 0 ] # x, y, z, r, buttons. only position[0] (yaw) and position[1] (pitch) are currently used for i in range(0, 4): if len(args) > i: positions[i] = int(args[i]) # default values are 0 connection.mav.manual_control_send(connection.target_system, positions[0], positions[1], positions[2], positions[3], positions[4]) def cmd_tracker_calpress(self, args): '''calibrate barometer on tracker''' connection = self.find_connection() if not connection: print("No antenna tracker found") return connection.calibrate_pressure() def cmd_tracker_mode(self, args): '''set arbitrary mode''' connection = self.find_connection() if not connection: print("No antenna tracker found") return mode_mapping = connection.mode_mapping() if mode_mapping is None: print('No mode mapping available') return if len(args) != 1: print('Available modes: ', mode_mapping.keys()) return mode = args[0].upper() if mode not in mode_mapping: print('Unknown mode %s: ' % mode) return connection.set_mode(mode_mapping[mode]) def mavlink_packet(self, m): '''handle an incoming mavlink packet from the master vehicle. Relay it to the tracker if it is a GLOBAL_POSITION_INT''' if m.get_type() in ['GLOBAL_POSITION_INT', 'SCALED_PRESSURE']: connection = self.find_connection() if not connection: return if m.get_srcSystem() != connection.target_system: connection.mav.send(m) def idle_task(self): '''called in idle time''' if not self.connection: return # check for a mavlink message from the tracker m = self.connection.recv_msg() if m is None: return if self.tracker_settings.debug: print(m) self.pstate.handle_mavlink_packet(self.connection, m) self.pstate.fetch_check(self.connection) if self.module('map') is None: return if m.get_type() == 'GLOBAL_POSITION_INT': (self.lat, self.lon, self.heading) = (m.lat * 1.0e-7, m.lon * 1.0e-7, m.hdg * 0.01) if self.lat != 0 or self.lon != 0: self.module('map').create_vehicle_icon('AntennaTracker', 'red', follow=False, vehicle_type='antenna') self.mpstate.map.set_position('AntennaTracker', (self.lat, self.lon), rotation=self.heading) def cmd_tracker_start(self): if self.tracker_settings.port == None: print("tracker port not set") return print("connecting to tracker %s at %d" % (self.tracker_settings.port, self.tracker_settings.baudrate)) m = mavutil.mavlink_connection( self.tracker_settings.port, autoreconnect=True, source_system=self.settings.source_system, baud=self.tracker_settings.baudrate) m.mav.srcComponent = self.settings.source_component if self.logdir: m.setup_logfile(os.path.join(self.logdir, 'tracker.tlog')) self.connection = m def cmd_tracker_arm(self): '''Enable the servos in the tracker so the antenna will move''' if not self.connection: print("tracker not connected") return self.connection.arducopter_arm() def cmd_tracker_disarm(self): '''Disable the servos in the tracker so the antenna will not move''' if not self.connection: print("tracker not connected") return self.connection.arducopter_disarm() def cmd_tracker_level(self): '''Calibrate the accelerometers. Disarm and move the antenna level first''' if not self.connection: print("tracker not connected") return self.connection.calibrate_level() def cmd_tracker_param(self, args): '''Parameter commands''' if not self.connection: print("tracker not connected") return self.pstate.handle_command(self.connection, self.mpstate, args)