Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def wait_for_position_estimator(self, cf):
logger.info('Waiting for estimator to find stable position...')
self.cfStatus = (
'Waiting for estimator to find stable position... '
'(QTM needs to be connected and providing data)'
)
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(cf, log_config) as log:
for log_entry in log:
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
def makeLog(self):
""" Makes a log. All ative children are added """
#print " -> Making new Log with %d ms interval"
if self.lg:
#print " ---> Previous Log detected, removing first"
self.lg.delete()
self.fm = FreqMonitor(window=max(10, int(1.2*self.hzTarget)))
self.lg = LogConfig(self.name, int(round(1000./self.hzTarget)))
#print " ---> Adding children to new log"
for x in range(self.childCount()):
c = self.child(x)
if c.isChecked():
name = c.log.group+"."+c.log.name
self.lg.add_variable(name, c.log.ctype)
#print " --- --> Adding child[%d]: [%s] to new log"%(x, name)
#print " ---> Checking log with TOC"
self.treeWidget().cf.log.add_config(self.lg)
if self.lg.valid:
#print " --- --> PASS"
self.lg.data_received_cb.add_callback(self.logDataCB)
self.lg.started_cb.add_callback(self.logStartedCB)
self.lg.error_cb.add_callback(self.treeWidget().sig_logError.emit)
self.lg.error_cb.add_callback(self.errorLog)
log_vel.add_variable('kalman.statePY', 'float')
log_vel.add_variable('kalman.statePZ', 'float')
try:
self._scf.cf.log.add_config(log_vel)
# This callback will receive the data
log_vel.data_received_cb.add_callback(self._cf_callback_vel)
# This callback will be called on errors
log_vel.error_cb.add_callback(self._cf_callback_error)
# Start the logging
log_vel.start()
except KeyError as e:
print('Could not start velocity log configuration,' '{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add velocity log config, bad configuration.')
log_att = LogConfig(name='Attitude', period_in_ms=500)
log_att.add_variable('stabilizer.roll', 'float')
log_att.add_variable('stabilizer.pitch', 'float')
log_att.add_variable('stabilizer.yaw', 'float')
try:
self._scf.cf.log.add_config(log_att)
# This callback will receive the data
log_att.data_received_cb.add_callback(self._cf_callback_att)
# This callback will be called on errors
log_att.error_cb.add_callback(self._cf_callback_error)
# Start the logging
log_att.start()
except KeyError as e:
print('Could not start attitude log configuration,' '{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add attitude log config, bad configuration.')
lg.add_variable("stabilizer.pitch", "float")
lg.add_variable("stabilizer.yaw", "float")
lg.add_variable("stabilizer.thrust", "uint16_t")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._imu_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
# MOTOR
lg = LogConfig("Motors", Config().get("ui_update_period"))
lg.add_variable("motor.m1")
lg.add_variable("motor.m2")
lg.add_variable("motor.m3")
lg.add_variable("motor.m4")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._motor_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def _connected(self):
self.uiState = UIState.CONNECTED
self._update_ui_state()
Config().set("link_uri", str(self._selected_interface))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
lg.add_variable("pm.state", "int8_t")
try:
self.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
mems = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)
if len(mems) > 0:
mems[0].write_data(self._led_write_done)
def connected(self, linkURI):
# IMU & THRUST
lg = LogConfig("Stabilizer", Config().get("ui_update_period"))
lg.add_variable("stabilizer.roll", "float")
lg.add_variable("stabilizer.pitch", "float")
lg.add_variable("stabilizer.yaw", "float")
lg.add_variable("stabilizer.thrust", "uint16_t")
try:
self.helper.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self._imu_data_signal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
# MOTOR
def _set_available_sensors(self, name, available):
logger.info("[%s]: %s", name, available)
available = eval(available)
self.actualHeight.setEnabled(True)
self.helper.inputDeviceReader.set_alt_hold_available(available)
if not self.logBaro:
# The sensor is available, set up the logging
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable(LOG_NAME_ESTIMATED_Z, "float")
try:
self.helper.cf.log.add_config(self.logBaro)
self.logBaro.data_received_cb.add_callback(
self._baro_data_signal.emit)
self.logBaro.error_cb.add_callback(
self._log_error_signal.emit)
self.logBaro.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))