def fcs_export(self, filename, dtype = None):
"""Export to an FCS file
"""
# TODO: remove this quick hack
#fcs.write(filename, self._data, self._metadata)
#return
data = self.panda.values.T
#data = np.nan_to_num(data).T
data = data.astype('<f')
metadata = self._metadata
metadata['$PAR'] = str(len(self.columns))
for j, name in enumerate(self.columns):
if '$P{}S'.format(j+1) not in metadata:
metadata['$P{}S'.format(j+1)] = name
if '$P{}N'.format(j+1) not in metadata:
metadata['$P{}N'.format(j+1)] = 'p{}'.format(j+1)
metadata['$TOT'] = str(data.shape[0])
fcs.write(filename, data, metadata)
def tick(io0, io1):
if not fcs._fcs:
raise RuntimeError("Please call fcs.init()")
fcs.write(0, io0)
fcs.write(1, io1)
fcs._fcs.fcs_board_tick()
fcs._fcs.fcs_ahrs_tick()
fcs.read(1, 1023)
fcs.read(2, 1023)
fcs.read(3, 1023)
fcs.read(4, 1023)
sensor_health = fcs.get_sensor_health()
try:
return plog.ParameterLog.deserialize(fcs.read(0, 1023)), sensor_health
except Exception:
return None, None
def tick(lat=None, lon=None, alt=None, velocity=None, attitude=None,
angular_velocity=None, wind_velocity=None, measurement_input=None):
"""
Runs the FCS control and comms tasks with the state data provided as
though it came from the AHRS, and returns the control output.
"""
if not fcs._fcs:
raise RuntimeError("Please call fcs.init()")
estimate_log = plog.ParameterLog(
log_type=plog.LogType.FCS_LOG_TYPE_ESTIMATE)
estimate_log.append(
plog.DataParameter(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_ESTIMATED_POSITION_LLA,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=32,
values=[
int(lat * (2**31 - 1) / math.pi),
int(lon * (2**31 - 1) / math.pi),
int(alt * 1e2)
]
)
)
estimate_log.append(
plog.DataParameter(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_ESTIMATED_VELOCITY_NED,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=16,
values=map(lambda x: int(x * 1e2), velocity)
)
)
estimate_log.append(
plog.DataParameter(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_ESTIMATED_ATTITUDE_Q,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=16,
values=map(lambda x: int(x * (2**15 - 1)), attitude)
)
)
estimate_log.append(
plog.DataParameter(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_ESTIMATED_ANGULAR_VELOCITY_XYZ,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=16,
values=map(lambda x: int(x * (2**15 - 1) / math.pi * 0.25), angular_velocity)
)
)
estimate_log.append(
plog.DataParameter(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=16,
values=map(lambda x: int(x * 1e2), wind_velocity)
)
)
estimate_log.append(
plog.DataParameter(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_AHRS_STATUS,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=16,
values=[0, 0]
)
)
fcs.write(3, estimate_log.serialize())
fcs.write(1, measurement_input)
#print binascii.b2a_hex(estimate_log.serialize())
fcs._fcs.fcs_board_tick()
fcs._fcs.fcs_ahrs_tick()
fcs._fcs.fcs_control_tick()
# Read out ignored streams
fcs.read(0, 1023)
fcs.read(1, 1023)
fcs.read(2, 1023)
sys.stderr.write(fcs.read(4, 1023))
try:
control_log = plog.ParameterLog.deserialize(fcs.read(3, 1023))
# print control_log
control_param = control_log.find_by(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT)
path = control_log.find_by(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_NAV_PATH_ID).values[0]
refp = control_log.find_by(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_KEY_VALUE)
cycles, obj_val, errors, resets = control_log.find_by(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS).values
last_control = (
map(lambda x: float(x) / float(2**16), control_param.values),
plog.extract_waypoint(refp.value),
obj_val, cycles, errors, resets, control_log.tick, path
)
return last_control
except Exception:
return ([0.0, 0.5, 0.5], {}, 0, 0, 0, 0, 0, 0xFFFF)
#! /opt/local/bin/python
import fcs
data, metadata, analysis, meta_analysis = fcs.read('test.fcs')
fcs.write('test_write.fcs',data, metadata)
def tick(lat=None,
lon=None,
alt=None,
velocity=None,
attitude=None,
angular_velocity=None,
wind_velocity=None,
measurement_input=None):
"""
Runs the FCS control and comms tasks with the state data provided as
though it came from the AHRS, and returns the control output.
"""
if not fcs._fcs:
raise RuntimeError("Please call fcs.init()")
estimate_log = plog.ParameterLog(
log_type=plog.LogType.FCS_LOG_TYPE_ESTIMATE)
estimate_log.append(
plog.DataParameter(device_id=0,
parameter_type=plog.ParameterType.
FCS_PARAMETER_ESTIMATED_POSITION_LLA,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=32,
values=[
int(lat * (2**31 - 1) / math.pi),
int(lon * (2**31 - 1) / math.pi),
int(alt * 1e2)
]))
estimate_log.append(
plog.DataParameter(device_id=0,
parameter_type=plog.ParameterType.
FCS_PARAMETER_ESTIMATED_VELOCITY_NED,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=16,
values=map(lambda x: int(x * 1e2), velocity)))
estimate_log.append(
plog.DataParameter(device_id=0,
parameter_type=plog.ParameterType.
FCS_PARAMETER_ESTIMATED_ATTITUDE_Q,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=16,
values=map(lambda x: int(x * (2**15 - 1)),
attitude)))
estimate_log.append(
plog.DataParameter(device_id=0,
parameter_type=plog.ParameterType.
FCS_PARAMETER_ESTIMATED_ANGULAR_VELOCITY_XYZ,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=16,
values=map(
lambda x: int(x * (2**15 - 1) / math.pi * 0.25),
angular_velocity)))
estimate_log.append(
plog.DataParameter(device_id=0,
parameter_type=plog.ParameterType.
FCS_PARAMETER_ESTIMATED_WIND_VELOCITY_NED,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=16,
values=map(lambda x: int(x * 1e2), wind_velocity)))
estimate_log.append(
plog.DataParameter(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_AHRS_STATUS,
value_type=plog.ValueType.FCS_VALUE_SIGNED,
value_precision=16,
values=[0, 0]))
fcs.write(3, estimate_log.serialize())
fcs.write(1, measurement_input)
#print binascii.b2a_hex(estimate_log.serialize())
fcs._fcs.fcs_board_tick()
fcs._fcs.fcs_ahrs_tick()
fcs._fcs.fcs_control_tick()
# Read out ignored streams
fcs.read(0, 1023)
fcs.read(1, 1023)
fcs.read(2, 1023)
sys.stderr.write(fcs.read(4, 1023))
try:
control_log = plog.ParameterLog.deserialize(fcs.read(3, 1023))
# print control_log
control_param = control_log.find_by(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_CONTROL_SETPOINT)
path = control_log.find_by(device_id=0,
parameter_type=plog.ParameterType.
FCS_PARAMETER_NAV_PATH_ID).values[0]
refp = control_log.find_by(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_KEY_VALUE)
cycles, obj_val, errors, resets = control_log.find_by(
device_id=0,
parameter_type=plog.ParameterType.FCS_PARAMETER_CONTROL_STATUS
).values
last_control = (map(lambda x: float(x) / float(2**16),
control_param.values),
plog.extract_waypoint(refp.value), obj_val, cycles,
errors, resets, control_log.tick, path)
return last_control
except Exception:
return ([0.0, 0.5, 0.5], {}, 0, 0, 0, 0, 0, 0xFFFF)
