def compute_camera_poses(proj): log("Setting camera poses (offset from aircraft pose.)") images_node = getNode("/images", True) ref_node = getNode("/config/ned_reference", True) ref_lat = ref_node.getFloat("lat_deg") ref_lon = ref_node.getFloat("lon_deg") ref_alt = ref_node.getFloat("alt_m") body2cam = camera.get_body2cam() for image in proj.image_list: print("camera pose:", image.name) ac_pose_node = image.node.getChild("aircraft_pose", True) #cam_pose_node = images_node.getChild(name + "/camera_pose", True) aircraft_lat = ac_pose_node.getFloat("lat_deg") aircraft_lon = ac_pose_node.getFloat("lon_deg") aircraft_alt = ac_pose_node.getFloat("alt_m") ned2body = [] for i in range(4): ned2body.append(ac_pose_node.getFloatEnum("quat", i)) ned2cam = transformations.quaternion_multiply(ned2body, body2cam) (yaw_rad, pitch_rad, roll_rad) = transformations.euler_from_quaternion(ned2cam, "rzyx") ned = navpy.lla2ned(aircraft_lat, aircraft_lon, aircraft_alt, ref_lat, ref_lon, ref_alt) image.set_camera_pose(ned, yaw_rad * r2d, pitch_rad * r2d, roll_rad * r2d)
def __init__(self, config_node): Task.__init__(self) self.task_node = getNode("/task", True) self.ap_node = getNode("/autopilot", True) self.engine_node = getNode("/controls/engine", True) self.name = config_node.getString("name") self.nickname = config_node.getString("nickname")
def __init__(self, config_node): Task.__init__(self) self.route_node = getNode('/task/route', True) self.ap_node = getNode('/autopilot', True) self.nav_node = getNode("/navigation", True) self.targets_node = getNode('/autopilot/targets', True) self.alt_agl_ft = 0.0 self.speed_kt = 30.0 self.saved_fcs_mode = '' self.saved_nav_mode = '' self.saved_agl_ft = 0.0 self.saved_speed_kt = 0.0 self.name = config_node.getString('name') self.nickname = config_node.getString('nickname') self.coord_path = config_node.getString('coord_path') self.alt_agl_ft = config_node.getFloat('altitude_agl_ft') self.speed_kt = config_node.getFloat('speed_kt') # load a route if included in config tree if control.route.build(config_node): control.route.swap() else: print 'Detected an internal inconsistency in the route' print ' configuration. See earlier errors for details.' quit()
def __init__(self, config_node): Task.__init__(self) self.pos_node = getNode("/position", True) self.home_node = getNode("/task/home", True) self.home_node.setBool("valid", False) self.startup_node = getNode("/task/startup", True) self.gps_node = getNode("/sensors/gps", True) self.name = config_node.getString("name")
def __init__(self, config_node): Task.__init__(self) self.sensors_node = getNode("/sensors", True) self.gps_node = getNode("/sensors/gps", True) self.home_node = getNode("/task/home", True) self.task_node = getNode("/task", True) self.completed = False self.name = config_node.getString("name")
def __init__(self, config_node): Task.__init__(self) self.task_node = getNode("/task", True) self.ap_node = getNode("/autopilot", True) self.engine_node = getNode("/controls/engine", True) self.saved_fcs_mode = "" self.name = config_node.getString("name") self.nickname = config_node.getString("nickname")
def write_configs(): config = getNode("/config", True) file = os.path.join(flight_dir, 'master-config.json') props_json.save(file, config) config = getNode("/config/autopilot", True) file = os.path.join(flight_dir, 'ap-config.json') props_json.save(file, config)
def __init__(self, config_node): Task.__init__(self) self.pos_node = getNode("/position", True) self.home_node = getNode("/task/home", True) self.home_node.setBool("valid", False) self.startup_node = getNode("/task/startup", True) self.gps_node = getNode("/sensors/gps", True) self.name = config_node.getString("name") self.nickname = config_node.getString("nickname")
def __init__(self, config_node): Task.__init__(self) self.sensors_node = getNode("/sensors", True) self.gps_node = getNode("/sensors/gps", True) self.home_node = getNode("/task/home", True) self.task_node = getNode("/task", True) self.completed = False self.name = config_node.getString("name") self.nickname = config_node.getString("nickname")
def __init__(self, config_node): Task.__init__(self) self.name = config_node.getString("name") self.ap_node = getNode("/autopilot", True) self.locks_node = getNode("/autopilot/locks", True) self.last_master_switch = False self.last_fcs_mode = ''
def __init__(self, config_node): Task.__init__(self) self.targets_node = getNode("/autopilot/targets", True) self.config_node = config_node self.engine_node = getNode("/controls/engine", True) self.glide_node = getNode("/task/glide", True) self.pos_node = getNode("/position", True) self.name = config_node.getString("name") self.running = False self.last_enable = True # so we don't start out with a enable event
def __init__(self, config_node): Task.__init__(self) self.imu_node = getNode("/sensors/imu", True) self.config_node = config_node self.excite_node = getNode("/task/excite", True) self.name = config_node.getString("name") self.index = 0 self.start_time = 0.0 self.type = '' self.running = False self.last_trigger = True # so we don't start out with a trigger event
def __init__(self, config_node): Task.__init__(self) self.flight_node = getNode("/controls/flight", True) self.imu_node = getNode("/sensors/imu", True) self.name = config_node.getString("name") if config_node.hasChild("speed_secs"): self.speed_secs = float(config_node.getString("speed_secs")) if self.speed_secs < 1.0: self.speed_secs = 1.0 else: self.speed_secs = 5
def __init__(self, config_node): Task.__init__(self) self.task_node = getNode("/task", True) self.ap_node = getNode("/autopilot", True) self.targets_node = getNode("/autopilot/targets", True) self.imu_node = getNode("/sensors/imu", True) self.flight_node = getNode("/controls/flight", True) self.saved_fcs_mode = "" self.timer = 0.0 self.duration_sec = 60.0 self.name = config_node.getString("name") self.nickname = config_node.getString("nickname") self.duration_sec = config_node.getFloat("duration_sec")
def __init__(self, config_node): Task.__init__(self) self.flight_node = getNode("/controls/flight", True) self.imu_node = getNode("/sensors/imu", True) self.name = config_node.getString("name") self.nickname = config_node.getString("nickname") if config_node.hasChild("speed_secs"): self.speed_secs = float(config_node.getString("speed_secs")) if self.speed_secs < 1.0: self.speed_secs = 1.0 else: self.speed_secs = 5
def __init__(self, config_node): Task.__init__(self) self.status_node = getNode("/status", True) self.task_node = getNode("/task", True) self.remote_link_node = getNode("/comms/remote_link", True) self.remote_link_node.setString("link", "inactive") self.link_state = False self.push_task = "" self.name = config_node.getString("name") self.timeout_sec = config_node.getFloat("timeout_sec") if self.timeout_sec < 1.0: # set a sane default if none provided self.timetout_sec = 60.0 self.action = config_node.getString("action")
def __init__(self, config_node): Task.__init__(self) self.act_node = getNode("/actuators", True) self.gps_node = getNode("/sensors/gps", True) self.task_node = getNode("/task", True) # initial defaults are locked down (this is kind of a big deal!) self.master_safety = True self.safety_on_ground = True self.act_node.setBool("throttle_safety", True) self.name = config_node.getString("name") self.nickname = config_node.getString("nickname") self.safety_on_ground = config_node.getBool("safety_on_ground")
def __init__(self, config_node): Task.__init__(self) self.act_node = getNode("/actuators", True) self.gps_node = getNode("/sensors/gps", True) self.task_node = getNode("/task", True) # initial defaults are locked down (this is kind of a big deal!) self.master_safety = True self.safety_mode = 'on_ground' self.airborne_latch = False self.act_node.setBool("throttle_safety", True) self.name = config_node.getString("name") if config_node.hasChild("safety_mode"): self.safety_mode = config_node.getString('safety_mode')
def __init__(self, config_node): Task.__init__(self) self.status_node = getNode("/status", True) self.task_node = getNode("/task", True) self.remote_link_node = getNode("/comms/remote_link", True) self.remote_link_node.setString("link", "inactive") self.link_state = False self.push_task = "" self.name = config_node.getString("name") self.nickname = config_node.getString("nickname") self.timeout_sec = config_node.getFloat("timeout_sec") if self.timeout_sec < 1.0: # set a sane default if none provided self.timetout_sec = 60.0 self.action = config_node.getString("action")
def __init__(self, switch_node): print "switch: ", switch_node.getChildren(expand=True) self.valid = True if switch_node.hasChild("input_prop"): prop_name = switch_node.getString("input_prop") tmp = prop_name.split('/') input_path = '/'.join(tmp[0:-1]) self.input_node = getNode(input_path, True) self.input_name = tmp[-1] print "input_path:", input_path print "input_name:", self.input_name else: self.valid = False if switch_node.hasChild("output_prop"): prop_name = switch_node.getString("output_prop") tmp = prop_name.split('/') output_path = '/'.join(tmp[0:-1]) self.output_node = getNode(output_path, True) self.output_name = tmp[-1] else: self.valid = False if switch_node.hasChild("output_type"): self.output_type = switch_node.getString("output_type") self.states = 2 else: self.output_type = 'boolean' self.states = 2 self.choices = [] if self.output_type == 'choice': self.states = switch_node.getLen('choice') #print 'found:', self.states, 'choices' if self.states > 0: for i in range(self.states): choice = switch_node.getStringEnum('choice', i) #print ' choice:', choice self.choices.append(choice) else: self.states = 1 self.choices = [ 'switch_config_error' ] if switch_node.hasChild("force_true"): self.force_true = True else: self.force_true = False self.min = -1.0 self.max = 1.0 self.range = self.max - self.min self.step = self.range / self.states
def pack_filter_bin(index): if index >= len(filter_nodes): for i in range(len(filter_nodes), index + 1): path = '/filters/filter[%d]' % i filter_nodes.append(getNode(path, True)) node = filter_nodes[index] buf = struct.pack(filter_v4_fmt, index, node.getFloat("timestamp"), node.getFloat("latitude_deg"), node.getFloat("longitude_deg"), node.getFloat("altitude_m"), int(node.getFloat("vn_ms") * 100), int(node.getFloat("ve_ms") * 100), int(node.getFloat("vd_ms") * 100), int(node.getFloat("roll_deg") * 10), int(node.getFloat("pitch_deg") * 10), int(node.getFloat("heading_deg") * 10), int(round(node.getFloat("p_bias") * 10000.0)), int(round(node.getFloat("q_bias") * 10000.0)), int(round(node.getFloat("r_bias") * 10000.0)), int(round(node.getFloat("ax_bias") * 1000.0)), int(round(node.getFloat("ay_bias") * 1000.0)), int(round(node.getFloat("az_bias") * 1000.0)), remote_link_node.getInt("sequence_num"), 0) return wrap_packet(FILTER_PACKET_V4, buf)
def unpack_filter_v4(buf): result = struct.unpack(filter_v4_fmt, buf) index = result[0] if index >= len(filter_nodes): for i in range(len(filter_nodes), index + 1): path = '/filters/filter[%d]' % i filter_nodes.append(getNode(path, True)) node = filter_nodes[index] node.setFloat("timestamp", result[1]) node.setFloat("latitude_deg", result[2]) node.setFloat("longitude_deg", result[3]) node.setFloat("altitude_m", result[4]) node.setFloat("vn_ms", result[5] / 100.0) node.setFloat("ve_ms", result[6] / 100.0) node.setFloat("vd_ms", result[7] / 100.0) node.setFloat("roll_deg", result[8] / 10.0) node.setFloat("pitch_deg", result[9] / 10.0) node.setFloat("heading_deg", result[10] / 10.0) node.setFloat("p_bias", result[11] / 10000.0) node.setFloat("q_bias", result[12] / 10000.0) node.setFloat("r_bias", result[13] / 10000.0) node.setFloat("ax_bias", result[14] / 1000.0) node.setFloat("ay_bias", result[15] / 1000.0) node.setFloat("az_bias", result[16] / 1000.0) remote_link_node.setInt("sequence_num", result[17]) node.setInt("status", result[18]) return index
def load(self, create=True): if not self.validate_project_dir(): return # load project configuration result = False project_file = os.path.join(self.project_dir, "config.json") config_node = getNode("/config", True) if os.path.isfile(project_file): if props_json.load(project_file, config_node): # fixme: # if 'matcher' in project_dict: # self.matcher_params = project_dict['matcher'] # root.pretty_print() result = True else: print("Notice: unable to load: ", project_file) else: print("Notice: project configuration doesn't exist:", project_file) if not result and create: print("Continuing with an empty project configuration") self.set_defaults() elif not result: print("aborting...") quit()
def pack_airdata_csv(index): airdata_node = getNode('/sensors/airdata[%d]' % index, True) row = dict() row['timestamp'] = '%.4f' % airdata_node.getFloat('timestamp') row['pressure_mbar'] = '%.1f' % airdata_node.getFloat('pressure_mbar') row['temp_C'] = '%.1f' % airdata_node.getFloat('temp_C') row['airspeed_smoothed_kt'] = '%.1f' % vel_node.getFloat( 'airspeed_smoothed_kt') row['altitude_smoothed_m'] = '%.2f' % pos_pressure_node.getFloat( 'altitude_smoothed_m') row['altitude_true_m'] = '%.2f' % pos_combined_node.getFloat( 'altitude_true_m') row['wind_dir_deg'] = '%.1f' % wind_node.getFloat('wind_dir_deg') row['wind_speed_kt'] = '%.1f' % wind_node.getFloat('wind_speed_kt') row['pitot_scale_factor'] = '%.2f' % wind_node.getFloat( 'pitot_scale_factor') row['tecs_error_total'] = '%.2f' % tecs_node.getFloat('error_total') row['tecs_error_diff'] = '%.2f' % tecs_node.getFloat('error_diff') row['status'] = '%d' % airdata_node.getInt('status') keys = [ 'timestamp', 'pressure_mbar', 'temp_C', 'airspeed_smoothed_kt', 'altitude_smoothed_m', 'altitude_true_m', 'wind_dir_deg', 'wind_speed_kt', 'pitot_scale_factor', 'tecs_error_total', 'tecs_error_diff', 'status' ] return row, keys
def unpack_event_v1(buf): #print 'buf len:', len(buf) # unpack without knowing full size (index, timestamp, size) = struct.unpack("<BdB", buf[:10]) #print 'unpack header len:', struct.calcsize("<BdB") #print 'expected size:', size #print 'maybe the message:', buf[10:] message = struct.unpack("%ds" % size, buf[10:]) #print 'message:', timestamp, message[0] #result = struct.unpack(event_v1_fmt, buf) #index = result[0] m = re.match('get: (.*)$', message[0].decode()) if m: (prop, value) = m.group(1).split(',') # print prop, value # absolute path parts = prop.split('/') node_path = '/'.join(parts[0:-1]) if node_path == '': node_path = '/' node = getNode(node_path, True) name = parts[-1] node.setString(name, value) event_node.setFloat("timestamp", timestamp) event_node.setString("message", message[0]) #print 'end of unpack event' return index
def pack_raven_v1(index): global imu_timestamp if index >= len(airdata_nodes): for i in range(len(airdata_nodes),index+1): path = '/sensors/airdata[%d]' % i airdata_nodes.append( getNode(path, True) ) node = airdata_nodes[index] buf = struct.pack(raven_v1_fmt, index, imu_timestamp, node.getIntEnum("pots", 0), node.getIntEnum("pots", 1), node.getIntEnum("pots", 2), node.getIntEnum("pots", 3), node.getIntEnum("pots", 4), node.getIntEnum("pots", 5), node.getIntEnum("pots", 6), node.getIntEnum("pots", 7), node.getIntEnum("pots", 8), node.getIntEnum("pots", 9), node.getFloat("diff_pa"), node.getFloat("pressure_mbar"), node.getFloat("rpm0"), node.getFloat("rpm1"), 0) return buf
def unpack_raven_v1(buf): result = struct.unpack(raven_v1_fmt, buf) index = result[0] if index >= len(airdata_nodes): for i in range(len(airdata_nodes),index+1): path = '/sensors/airdata[%d]' % i airdata_nodes.append( getNode(path, True) ) node = airdata_nodes[index] node.setFloat("timestamp", result[1]) node.setIntEnum("pots", 0, result[2]) node.setIntEnum("pots", 1, result[3]) node.setIntEnum("pots", 2, result[4]) node.setIntEnum("pots", 3, result[5]) node.setIntEnum("pots", 4, result[6]) node.setIntEnum("pots", 5, result[7]) node.setIntEnum("pots", 6, result[8]) node.setIntEnum("pots", 7, result[9]) node.setIntEnum("pots", 8, result[10]) node.setIntEnum("pots", 9, result[11]) node.setFloat("diff_pa", result[12]) node.setFloat("pressure_mbar", result[13]) node.setFloat("rpm0", result[14]) node.setFloat("rpm1", result[15]) node.setInt("status", result[16]) return index
def load(self, create=True): if not self.validate_project_dir(create): return # load project configuration result = False project_file = os.path.join(self.analysis_dir, "config.json") config_node = getNode("/config", True) if os.path.isfile(project_file): if props_json.load(project_file, config_node): # fixme: # if 'matcher' in project_dict: # self.matcher_params = project_dict['matcher'] # root.pretty_print() result = True else: log("Notice: unable to load: ", project_file) else: log("project: project configuration doesn't exist:", project_file) if not result and create: log("Continuing with an empty project configuration") self.set_defaults() elif not result: log("Project load failed, aborting...") quit() # overwrite project_dir with current location (this will get # saved out into the config.json, but projects could relocate # and it's more important to have the actual current location) self.dir_node.setString('project_dir', self.project_dir)
def make_detector(self): detector_node = getNode('/config/detector', True) detector = None if detector_node.getString('detector') == 'SIFT': max_features = detector_node.getInt('sift_max_features') detector = cv2.xfeatures2d.SIFT_create(nfeatures=max_features) elif detector_node.getString('detector') == 'SURF': threshold = detector_node.getFloat('surf_hessian_threshold') nOctaves = detector_node.getInt('surf_noctaves') detector = cv2.xfeatures2d.SURF_create(hessianThreshold=threshold, nOctaves=nOctaves) elif detector_node.getString('detector') == 'ORB': max_features = detector_node.getInt('orb_max_features') grid_size = detector_node.getInt('grid_detect') cells = grid_size * grid_size max_cell_features = int(max_features / cells) detector = cv2.ORB_create(max_cell_features) elif detector_node.getString('detector') == 'Star': maxSize = detector_node.getInt('star_max_size') responseThreshold = detector_node.getInt('star_response_threshold') lineThresholdProjected = detector_node.getInt( 'star_line_threshold_projected') lineThresholdBinarized = detector_node.getInt( 'star_line_threshold_binarized') suppressNonmaxSize = detector_node.getInt( 'star_suppress_nonmax_size') detector = cv2.xfeatures2d.StarDetector_create( maxSize, responseThreshold, lineThresholdProjected, lineThresholdBinarized, suppressNonmaxSize) return detector
def unpack_raven_v1(buf): result = struct.unpack(raven_v1_fmt, buf) index = result[0] if index >= len(airdata_nodes): for i in range(len(airdata_nodes), index + 1): path = '/sensors/airdata[%d]' % i airdata_nodes.append(getNode(path, True)) node = airdata_nodes[index] node.setFloat("timestamp", result[1]) node.setIntEnum("pots", 0, result[2]) node.setIntEnum("pots", 1, result[3]) node.setIntEnum("pots", 2, result[4]) node.setIntEnum("pots", 3, result[5]) node.setIntEnum("pots", 4, result[6]) node.setIntEnum("pots", 5, result[7]) node.setIntEnum("pots", 6, result[8]) node.setIntEnum("pots", 7, result[9]) node.setIntEnum("pots", 8, result[10]) node.setIntEnum("pots", 9, result[11]) node.setFloat("diff_pa", result[12]) node.setFloat("pressure_mbar", result[13]) node.setFloat("rpm0", result[14]) node.setFloat("rpm1", result[15]) node.setInt("status", result[16]) return index
def pack_imu_v3(index): global imu_timestamp if index >= len(imu_nodes): for i in range(len(imu_nodes),index+1): path = '/sensors/imu[%d]' % i imu_nodes.append( getNode(path, True) ) node = imu_nodes[index] imu_timestamp = node.getFloat("timestamp") buf = struct.pack(imu_v3_fmt, index, imu_timestamp, node.getFloat("p_rad_sec"), node.getFloat("q_rad_sec"), node.getFloat("r_rad_sec"), node.getFloat("ax_mps_sec"), node.getFloat("ay_mps_sec"), node.getFloat("az_mps_sec"), node.getFloat("hx"), node.getFloat("hy"), node.getFloat("hz"), int(node.getFloat("temp_C") * 10.0), 0) return buf
def unpack_gps_v4(buf): result = struct.unpack(gps_v4_fmt, buf) index = result[0] if index >= len(gps_nodes): for i in range(len(gps_nodes), index + 1): path = '/sensors/gps[%d]' % i gps_nodes.append(getNode(path, True)) node = gps_nodes[index] node.setFloat("timestamp", result[1]) node.setFloat("latitude_deg", result[2]) node.setFloat("longitude_deg", result[3]) node.setFloat("altitude_m", result[4]) node.setFloat("vn_ms", result[5] / 100.0) node.setFloat("ve_ms", result[6] / 100.0) node.setFloat("vd_ms", result[7] / 100.0) node.setFloat("unix_time_sec", result[8]) node.setInt("satellites", result[9]) node.setFloat('horiz_accuracy_m', result[10] / 100.0) node.setFloat('vert_accuracy_m', result[11] / 100.0) node.setFloat('pdop', result[12] / 100.0) node.setInt('fixType', result[13]) node.setInt("status", 0) return index
def pack_filter_csv(index): filter_node = getNode('/filters/filter[%d]' % index, True) row = dict() row['timestamp'] = '%.4f' % filter_node.getFloat('timestamp') row['latitude_deg'] = '%.10f' % filter_node.getFloat('latitude_deg') row['longitude_deg'] = '%.10f' % filter_node.getFloat('longitude_deg') row['altitude_m'] = '%.2f' % filter_node.getFloat('altitude_m') row['vn_ms'] = '%.4f' % filter_node.getFloat('vn_ms') row['ve_ms'] = '%.4f' % filter_node.getFloat('ve_ms') row['vd_ms'] = '%.4f' % filter_node.getFloat('vd_ms') row['roll_deg'] = '%.2f' % filter_node.getFloat('roll_deg') row['pitch_deg'] = '%.2f' % filter_node.getFloat('pitch_deg') row['heading_deg'] = '%.2f' % filter_node.getFloat('heading_deg') row['p_bias'] = '%.4f' % filter_node.getFloat('p_bias') row['q_bias'] = '%.4f' % filter_node.getFloat('q_bias') row['r_bias'] = '%.4f' % filter_node.getFloat('r_bias') row['ax_bias'] = '%.3f' % filter_node.getFloat('ax_bias') row['ay_bias'] = '%.3f' % filter_node.getFloat('ay_bias') row['az_bias'] = '%.3f' % filter_node.getFloat('az_bias') row['status'] = '%d' % filter_node.getInt('status') keys = [ 'timestamp', 'latitude_deg', 'longitude_deg', 'altitude_m', 'vn_ms', 've_ms', 'vd_ms', 'roll_deg', 'pitch_deg', 'heading_deg', 'p_bias', 'q_bias', 'r_bias', 'ax_bias', 'ay_bias', 'az_bias', 'status' ] return row, keys
def __init__(self, config_node): Task.__init__(self) self.flight_node = getNode("/controls/flight", True) self.imu_node = getNode("/sensors/imu", True) self.act_node = getNode("/actuators/actuator", True) self.chirp_node = getNode("/task/chirp", True) self.name = config_node.getString("name") self.nickname = config_node.getString("nickname") self.start_time = 0.0 self.k = 0.0 # rad/sec is hz*2*pi if config_node.hasChild("freq_start_rad_sec"): self.freq_start = float(config_node.getString("freq_start_rad_sec")) if self.freq_start < 0.1: self.freq_start = 0.1 if self.freq_start > 100.0: self.freq_start = 100.0 else: self.freq_start = 6.2830 self.chirp_node.setFloat("freq_start_rad_sec", self.freq_start) if config_node.hasChild("freq_end_rad_sec"): self.freq_end = float(config_node.getString("freq_end_rad_sec")) if self.freq_end < 0.1: self.freq_end = 0.1 if self.freq_end > 100.0: self.freq_end = 100.0 else: self.freq_end = 62.83 self.chirp_node.setFloat("freq_end_rad_sec", self.freq_end) if config_node.hasChild("duration_sec"): self.dur_sec = float(config_node.getString("duration_sec")) if self.dur_sec < 1: self.dur_sec = 1 if self.dur_sec > 100: self.dur_sec = 100 else: self.dur_sec = 20 self.chirp_node.setFloat("duration_sec", self.dur_sec) if config_node.hasChild("amplitude"): self.amplitude = float(config_node.getString("amplitude")) if self.amplitude < 0.001: self.amplitude = 0.001 if self.amplitude > 1: self.amplitude = 1 else: self.amplitude = 0.1 self.chirp_node.setFloat("amplitude", self.amplitude) if config_node.hasChild("inject"): self.inject = config_node.getString("inject") else: self.inject = "aileron" self.chirp_node.setString("inject", self.inject) self.last_trigger = True # so we don't start out with a trigger event self.running = False
def __init__(self, switch_node): print "switch: ", switch_node.getChildren(expand=True) self.valid = True if switch_node.hasChild("input_prop"): prop_name = switch_node.getString("input_prop") tmp = prop_name.split('/') input_path = '/'.join(tmp[0:-1]) self.input_node = getNode(input_path, True) self.input_name = tmp[-1] print "input_path:", input_path print "input_name:", self.input_name else: self.valid = False if switch_node.hasChild("output_prop"): prop_name = switch_node.getString("output_prop") tmp = prop_name.split('/') output_path = '/'.join(tmp[0:-1]) self.output_node = getNode(output_path, True) self.output_name = tmp[-1] else: self.valid = False self.output_type = 'boolean' self.states = 2 if switch_node.hasChild("output_type"): self.output_type = switch_node.getString("output_type") self.choices = [] if self.output_type == 'choice': self.states = switch_node.getLen('choice') #print 'found:', self.states, 'choices' if self.states > 0: for i in range(self.states): choice = switch_node.getStringEnum('choice', i) #print ' choice:', choice self.choices.append(choice) else: self.states = 1 self.choices = ['switch_config_error'] if switch_node.hasChild("force_true"): self.force_true = True else: self.force_true = False self.min = -1.0 self.max = 1.0 self.range = self.max - self.min self.step = self.range / self.states
def __init__(self, config_node): Task.__init__(self) self.imu_node = getNode("/sensors/imu", True) self.chirp_node = getNode("/task/chirp", True) self.signal_node = getNode("/controls/signal", True) self.name = config_node.getString("name") self.nickname = config_node.getString("nickname") self.start_time = 0.0 self.k = 0.0 # rad/sec is hz*2*pi if config_node.hasChild("freq_start_rad_sec"): self.freq_start = float( config_node.getString("freq_start_rad_sec")) if self.freq_start < 0.1: self.freq_start = 0.1 if self.freq_start > 100.0: self.freq_start = 100.0 else: self.freq_start = 6.2830 self.chirp_node.setFloat("freq_start_rad_sec", self.freq_start) if config_node.hasChild("freq_end_rad_sec"): self.freq_end = float(config_node.getString("freq_end_rad_sec")) if self.freq_end < 0.1: self.freq_end = 0.1 if self.freq_end > 100.0: self.freq_end = 100.0 else: self.freq_end = 62.83 self.chirp_node.setFloat("freq_end_rad_sec", self.freq_end) if config_node.hasChild("duration_sec"): self.dur_sec = float(config_node.getString("duration_sec")) if self.dur_sec < 1: self.dur_sec = 1 if self.dur_sec > 100: self.dur_sec = 100 else: self.dur_sec = 20 self.chirp_node.setFloat("duration_sec", self.dur_sec) if config_node.hasChild("amplitude"): self.amplitude = float(config_node.getString("amplitude")) if self.amplitude < 0.001: self.amplitude = 0.001 if self.amplitude > 1: self.amplitude = 1 else: self.amplitude = 0.1 self.chirp_node.setFloat("amplitude", self.amplitude) if config_node.hasChild("inject"): self.inject = config_node.getString("inject") else: self.inject = "aileron" self.signal_node.setString("inject", self.inject) self.last_trigger = True # so we don't start out with a trigger event self.running = False
def compute_ned_reference_lla(self): # requires images to have their location computed/loaded lon_sum = 0.0 lat_sum = 0.0 count = 0 images_node = getNode("/images", True) for name in images_node.getChildren(): image_node = images_node.getChild(name, True) pose_node = image_node.getChild('aircraft_pose', True) if pose_node.hasChild('lon_deg') and pose_node.hasChild('lat_deg'): lon_sum += pose_node.getFloat('lon_deg') lat_sum += pose_node.getFloat('lat_deg') count += 1 ned_node = getNode('/config/ned_reference', True) ned_node.setFloat('lat_deg', lat_sum / count) ned_node.setFloat('lon_deg', lon_sum / count) ned_node.setFloat('alt_m', 0.0)
def init(): global enable_file global enable_udp global logging_node logging_node = getNode("/config/logging", True) global log_path global udp_host global udp_port log_path = logging_node.getString('path') udp_host = logging_node.getString('hostname') udp_port = logging_node.getInt('port') if log_path != '': if init_file_logging(): enable_file = True # success # fixme: # events->open(flight_dir.c_str()) # events->log("Log", "Start") if udp_host != '' and udp_port > 0: if init_udp_logging(): enable_udp = True global act_skip global act_count global airdata_skip global airdata_count global ap_skip global ap_count global filter_skip global filter_count global gps_skip global gps_count global health_skip global health_count global imu_skip global imu_count global pilot_skip global pilot_count act_skip = logging_node.getInt("actuator_skip") act_count = random.randint(0, act_skip) airdata_skip = logging_node.getInt("airdata_skip") airdata_count = random.randint(0, airdata_skip) ap_skip = logging_node.getInt("autopilot_skip") ap_count = random.randint(0, ap_skip) filter_skip = logging_node.getInt("filter_skip") filter_count = random.randint(0, filter_skip) gps_skip = logging_node.getInt("gps_skip") gps_count = random.randint(0, gps_skip) health_skip = logging_node.getInt("health_skip") health_count = random.randint(0, health_skip) imu_skip = logging_node.getInt("imu_skip") imu_count = random.randint(0, imu_skip) pilot_skip = logging_node.getInt("pilot_skip") pilot_count = random.randint(0, pilot_skip) return True
def __init__(self, config_node): Task.__init__(self) self.pos_node = getNode("/position", True) self.home_node = getNode("/task/home", True) self.circle_node = getNode("/task/circle", True) self.ap_node = getNode("/autopilot", True) self.targets_node = getNode("/autopilot/targets", True) self.nav_node = getNode("/navigation", True) self.tecs_node = getNode("/config/autopilot/TECS", True) self.t = 0.0 self.direction = "left" self.radius_m = 100.0 self.name = config_node.getString("name") if config_node.hasChild("direction"): # only override the default if a value is given self.direction = config_node.getString("direction") if config_node.hasChild("radius_m"): # only override the default if a value is given self.radius_m = config_node.getFloat("radius_m") # estimate the distance of the route (assuming loop_t is set correctly!) steps = 1000 self.step = loop_t / float(steps) t1 = 0 t2 = t1 + self.step self.loop_dist = 0.0 while t2 <= loop_t + 0.5*self.step: self.loop_dist += distance_t2t(t1, t2) t1 = t2 t2 += self.step print('route distance is approximately:', self.loop_dist)
def __init__(self): self.ap_node = getNode("/autopilot/targets", True) self.targets_node = getNode("/autopilot/targets", True) self.missions_node = getNode("/config/mission", True) self.pos_node = getNode("/position", True) self.task_node = getNode("/task", True) self.preflight_node = getNode("/task/preflight", True) self.circle_standby_node = getNode("/task/circle/standby", True) self.home_node = getNode("/task/home", True) self.wind_node = getNode("/filters/wind", True) self.global_tasks = [] self.seq_tasks = [] self.standby_tasks = [] self.last_master_switch = False
def __init__(self, config_node): Task.__init__(self) self.pos_node = getNode("/position", True) self.vel_node = getNode("/velocity", True) self.task_node = getNode("/task", True) self.status_node = getNode("/status", True) self.is_airborne = False self.off_alt_agl_ft = 0.0 self.off_airspeed_kt = 0.0 self.on_alt_agl_ft = 0.0 self.on_airspeed_kt = 0.0 self.name = config_node.getString("name") self.nickname = config_node.getString("nickname") self.off_alt_agl_ft = config_node.getFloat("off_alt_agl_ft") self.off_airspeed_kt = config_node.getFloat("off_airspeed_kt") self.on_alt_agl_ft = config_node.getFloat("on_alt_agl_ft") self.on_airspeed_kt = config_node.getFloat("on_airspeed_kt") self.flight_accum = 0.0 self.flight_start = 0.0
def pack_gps_text(index, delim=','): gps_node = getNode('/sensors/gps[%d]' % index, True) data = [ '%.4f' % gps_node.getFloat('timestamp'), '%.10f' % gps_node.getFloat('latitude_deg'), '%.10f' % gps_node.getFloat('longitude_deg'), '%.2f' % gps_node.getFloat('altitude_m'), '%.4f' % gps_node.getFloat('vn_ms'), '%.4f' % gps_node.getFloat('ve_ms'), '%.4f' % gps_node.getFloat('vd_ms'), '%.3f' % gps_node.getFloat('unix_time_sec'), '%d' % gps_node.getInt('satellites'), '%d' % gps_node.getInt('status') ] return delim.join(data)
def __init__(self): self.targets_node = getNode("/autopilot/targets", True) self.missions_node = getNode("/config/mission", True) self.pos_node = getNode("/position", True) self.task_node = getNode("/task", True) self.circle_node = getNode("/task/circle", True) self.home_node = getNode("/task/home", True) self.wind_node = getNode("/filters/wind", True) self.global_tasks = [] self.seq_tasks = [] self.standby_tasks = []
def pack_act_text(index, delim=','): act_node = getNode('/actuators/actuator[%d]' % index, True) data = [ '%.4f' % act_node.getFloat('timestamp'), '%.3f' % act_node.getFloat('aileron'), '%.3f' % act_node.getFloat('elevator'), '%.3f' % act_node.getFloat('throttle'), '%.3f' % act_node.getFloat('rudder'), '%.3f' % act_node.getFloat('channel5'), '%.3f' % act_node.getFloat('flaps'), '%.3f' % act_node.getFloat('channel7'), '%.3f' % act_node.getFloat('channel8'), '%d' % act_node.getInt('status') ] return delim.join(data)
def pack_pilot_text(index, delim=','): pilot_node = getNode('/sensors/pilot_input[%d]' % index, True) data = [ '%.4f' % pilot_node.getFloat('timestamp'), '%.3f' % pilot_node.getFloatEnum('channel', 0), '%.3f' % pilot_node.getFloatEnum('channel', 1), '%.3f' % pilot_node.getFloatEnum('channel', 2), '%.3f' % pilot_node.getFloatEnum('channel', 3), '%.3f' % pilot_node.getFloatEnum('channel', 4), '%.3f' % pilot_node.getFloatEnum('channel', 5), '%.3f' % pilot_node.getFloatEnum('channel', 6), '%.3f' % pilot_node.getFloatEnum('channel', 7), '%d' % pilot_node.getInt('status') ] return delim.join(data)
def __init__(self, config_node): Task.__init__(self) self.home_node = getNode("/task/home", True) self.route_node = getNode("/task/route", True) self.ap_node = getNode("/autopilot", True) self.targets_node = getNode("/autopilot/targets", True) self.alt_agl_ft = 0.0 self.speed_kt = 30.0 self.last_lon = 0.0 self.last_lat = 0.0 self.last_az = 0.0 self.saved_fcs_mode = "" self.saved_agl_ft = 0.0 self.saved_speed_kt = 0.0 self.name = config_node.getString("name") self.nickname = config_node.getString("nickname") self.coord_path = config_node.getString("coord_path") self.alt_agl_ft = config_node.getFloat("altitude_agl_ft") self.speed_kt = config_node.getFloat("speed_kt")
def pack_airdata_text(index, delim=','): airdata_node = getNode('/sensors/airdata[%d]' % index, True) data = [ '%.4f' % airdata_node.getFloat('timestamp'), '%.1f' % airdata_node.getFloat('pressure_mbar'), '%.1f' % airdata_node.getFloat('temp_degC'), '%.1f' % vel_node.getFloat('airspeed_smoothed_kt'), '%.2f' % pos_pressure_node.getFloat('altitude_smoothed_m'), '%.2f' % pos_combined_node.getFloat('altitude_true_m'), '%.2f' % (vel_node.getFloat('pressure_vertical_speed_fps')*60.0), '%.1f' % wind_node.getFloat('wind_dir_deg'), '%.1f' % wind_node.getFloat('wind_speed_kt'), '%.2f' % wind_node.getFloat('pitot_scale_factor'), '%d' % airdata_node.getInt('status') ] return delim.join(data)
def pack_filter_text(index, delim=','): filter_node = getNode('/filters/filter[%d]' % index, True) data = [ '%.4f' % filter_node.getFloat('timestamp'), '%.10f' % filter_node.getFloat('latitude_deg'), '%.10f' % filter_node.getFloat('longitude_deg'), '%.2f' % filter_node.getFloat('altitude_m'), '%.4f' % filter_node.getFloat('vn_ms'), '%.4f' % filter_node.getFloat('ve_ms'), '%.4f' % filter_node.getFloat('vd_ms'), '%.2f' % filter_node.getFloat('roll_deg'), '%.2f' % filter_node.getFloat('pitch_deg'), '%.2f' % filter_node.getFloat('heading_deg'), '%d' % filter_node.getInt('status') ] return delim.join(data)
def pack_imu_text(index, delim=','): imu_node = getNode('/sensors/imu[%d]' % index, True) data = [ '%.4f' % imu_node.getFloat('timestamp'), '%.4f' % imu_node.getFloat('p_rad_sec'), '%.4f' % imu_node.getFloat('q_rad_sec'), '%.4f' % imu_node.getFloat('r_rad_sec'), '%.4f' % imu_node.getFloat('ax_mps_sec'), '%.4f' % imu_node.getFloat('ay_mps_sec'), '%.4f' % imu_node.getFloat('az_mps_sec'), '%.3f' % imu_node.getFloat('hx'), '%.3f' % imu_node.getFloat('hy'), '%.3f' % imu_node.getFloat('hz'), '%.1f' % imu_node.getFloat('temp_C'), '%d' % imu_node.getInt('status') ] return delim.join(data)
def __init__(self, config_node): Task.__init__(self) self.ap_node = getNode("/autopilot", True) self.task_node = getNode("/task", True) self.pos_node = getNode("/position", True) self.vel_node = getNode("/velocity", True) self.orient_node = getNode("/orientation", True) self.targets_node = getNode("/autopilot/targets", True) self.imu_node = getNode("/sensors/imu", True) self.flight_node = getNode("/controls/flight", True) self.engine_node = getNode("/controls/engine", True) self.complete_agl_ft = 150.0 self.mission_agl_ft = 300.0 self.target_speed_kt = 25.0 self.roll_gain = 0.5 self.roll_limit = 5.0 self.rudder_enable = False self.rudder_gain = 1.0 self.rudder_max = 1.0 self.control_limit = 1.0 self.flaps = 0.0 self.last_ap_master = False self.relhdg = 0.0 self.name = config_node.getString("name") self.nickname = config_node.getString("nickname") self.completion_agl_ft = config_node.getFloat("completion_agl_ft") self.mission_agl_ft = config_node.getFloat("mission_agl_ft") self.target_speed_kt = config_node.getFloat("speed_kt") self.roll_gain = config_node.getFloat("roll_gain") self.roll_limit = config_node.getFloat("roll_limit") self.rudder_enable = config_node.getBool("rudder_enable") self.rudder_gain = config_node.getFloat("rudder_gain") self.rudder_max = config_node.getFloat("rudder_max") self.flaps = config_node.getFloat("flaps")
def __init__(self, sock): asynchat.async_chat.__init__(self, sock=sock) self.set_terminator('\n') self.buffer = [] self.path = '/' self.prompt = True self.imu_node = getNode("/sensors/imu", True) self.targets_node = getNode("/autopilot/targets", True) self.filter_node = getNode("/filters/filter", True) self.act_node = getNode("/actuators/actuator", True) self.vel_node = getNode("/velocity", True) self.pos_comb_node = getNode("/position/combined", True)
def pack_gps_v2(index): if index >= len(gps_nodes): for i in range(len(gps_nodes),index+1): path = '/sensors/gps[%d]' % i gps_nodes.append( getNode(path, True) ) node = gps_nodes[index] buf = struct.pack(gps_v2_fmt, index, node.getFloat("timestamp"), node.getFloat("latitude_deg"), node.getFloat("longitude_deg"), node.getFloat("altitude_m"), int(node.getFloat("vn_ms") * 100), int(node.getFloat("ve_ms") * 100), int(node.getFloat("vd_ms") * 100), node.getFloat("unix_time_sec"), node.getInt("satellites"), 0) return buf
def pack_raven_text(index, delim=','): node = getNode('/sensors/airdata[%d]' % index, True) data = [ '%.4f' % node.getFloat('timestamp'), '%d' % node.getIntEnum('pots', 0), '%d' % node.getIntEnum('pots', 1), '%d' % node.getIntEnum('pots', 2), '%d' % node.getIntEnum('pots', 3), '%d' % node.getIntEnum('pots', 4), '%d' % node.getIntEnum('pots', 5), '%d' % node.getIntEnum('pots', 6), '%d' % node.getIntEnum('pots', 7), '%d' % node.getIntEnum('pots', 8), '%d' % node.getIntEnum('pots', 9), '%.4f' % node.getFloat('diff_pa'), '%.4f' % node.getFloat('pressure_mbar'), '%.2f' % node.getFloat('rpm0'), '%.2f' % node.getFloat('rpm1'), '%d' % node.getInt('status') ] return delim.join(data)
def pack_airdata_v5(index): if index >= len(airdata_nodes): for i in range(len(airdata_nodes),index+1): path = '/sensors/airdata[%d]' % i airdata_nodes.append( getNode(path, True) ) node = airdata_nodes[index] buf = struct.pack(airdata_v5_fmt, index, node.getFloat("timestamp"), int(node.getFloat("pressure_mbar") * 10.0), int(node.getFloat("temp_degC") * 100.0), int(vel_node.getFloat("airspeed_smoothed_kt") * 100.0), pos_pressure_node.getFloat("altitude_smoothed_m"), pos_combined_node.getFloat("altitude_true_m"), int(vel_node.getFloat("pressure_vertical_speed_fps") * 60 * 10), int(wind_node.getFloat("wind_dir_deg") * 100), int(wind_node.getFloat("wind_speed_kt") * 4), int(wind_node.getFloat("pitot_scale_factor") * 100), node.getInt("status")) return buf
def unpack_gps_v2(buf): result = struct.unpack(gps_v2_fmt, buf) index = result[0] if index >= len(gps_nodes): for i in range(len(gps_nodes),index+1): path = '/sensors/gps[%d]' % i gps_nodes.append( getNode(path, True) ) node = gps_nodes[index] node.setFloat("timestamp", result[1]) node.setFloat("latitude_deg", result[2]) node.setFloat("longitude_deg", result[3]) node.setFloat("altitude_m", result[4]) node.setFloat("vn_ms", result[5] / 100.0) node.setFloat("ve_ms", result[6] / 100.0) node.setFloat("vd_ms", result[7] / 100.0) node.setFloat("unix_time_sec", result[8]) node.setInt("satellites", result[9]) node.setInt("status", result[10]) return index
def pack_pilot_v2(index): if index >= len(pilot_nodes): for i in range(len(pilot_nodes),index+1): path = '/sensors/pilot_input[%d]' % i node = getNode(path, True) node.setLen("channel", NUM_ACTUATORS, 0.0) pilot_nodes.append( node ) node = pilot_nodes[index] buf = struct.pack(pilot_v2_fmt, index, node.getFloat("timestamp"), int(node.getFloatEnum("channel", 0) * 20000), int(node.getFloatEnum("channel", 1) * 20000), int(node.getFloatEnum("channel", 2) * 20000), int(node.getFloatEnum("channel", 3) * 20000), int(node.getFloatEnum("channel", 4) * 20000), int(node.getFloatEnum("channel", 5) * 20000), int(node.getFloatEnum("channel", 6) * 20000), int(node.getFloatEnum("channel", 7) * 20000), 0) return buf
def unpack_act_v2(buf): result = struct.unpack(act_v2_fmt, buf) index = result[0] if index >= len(airdata_nodes): for i in range(len(airdata_nodes),index+1): path = '/sensors/airdata[%d]' % i airdata_nodes.append( getNode(path, True) ) node = airdata_nodes[index] node.setFloat("timestamp", result[1]) node.setFloat("aileron", result[2] / 20000.0) node.setFloat("elevator", result[3] / 20000.0) node.setFloat("throttle", result[4] / 60000.0) node.setFloat("rudder", result[5] / 20000.0) node.setFloat("channel5", result[6] / 20000.0) node.setFloat("flaps", result[7] / 20000.0) node.setFloat("channel7", result[8] / 20000.0) node.setFloat("channel8", result[9] / 20000.0) node.setInt("status", result[10]) return index
def pack_filter_v2(index): if index >= len(filter_nodes): for i in range(len(filter_nodes),index+1): path = '/filters/filter[%d]' % i filter_nodes.append( getNode(path, True) ) node = filter_nodes[index] buf = struct.pack(filter_v2_fmt, index, node.getFloat("timestamp"), node.getFloat("latitude_deg"), node.getFloat("longitude_deg"), node.getFloat("altitude_m"), int(node.getFloat("vn_ms") * 100), int(node.getFloat("ve_ms") * 100), int(node.getFloat("vd_ms") * 100), int(node.getFloat("roll_deg") * 10), int(node.getFloat("pitch_deg") * 10), int(node.getFloat("heading_deg") * 10), remote_link_node.getInt("sequence_num"), 0) return buf