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.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.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
