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)