def main(name): sm = generate_map(name) opcd = OPCD_Interface(sm['opcd_ctrl'], name) global THIS_SYS_ID THIS_SYS_ID = opcd.get('id') key = opcd.get('psk') crypt.init(key) mhist = MessageHistory(60) out_socket = sm['out'] in_socket = sm['in'] aci = Interface('/dev/ttyACM0') acr = ACIReader(aci, out_socket, mhist) acr.start() # read from SCL in socket and send data via NRF while True: data = loads(in_socket.recv()) if len(data) == 2: msg = [data[0], THIS_SYS_ID, data[1]] elif len(data) > 2: msg = [data[0], THIS_SYS_ID] + data[1:] else: continue crypt_data = crypt.encrypt(dumps(msg)) mhist.append(crypt_data) aci.send(crypt_data)
def main(name): sm = generate_map(name) opcd = OPCD_Interface(sm['opcd_ctrl']) platform = opcd.get('platform') device = opcd.get(platform + '.nrf_serial') global THIS_SYS_ID THIS_SYS_ID = opcd.get('aircomm.id') key = opcd.get('aircomm.psk') crypt.init(key) mhist = MessageHistory(60) out_socket = sm['aircomm_out'] in_socket = sm['aircomm_in'] aci = Interface(device) acr = ACIReader(aci, out_socket, mhist) acr.start() # read from SCL in socket and send data via NRF while True: data = loads(in_socket.recv()) if len(data) == 2: msg = [data[0], THIS_SYS_ID, data[1]] elif len(data) > 2: msg = [data[0], THIS_SYS_ID] + data[1:] else: continue crypt_data = crypt.encrypt(dumps(msg)) mhist.append(crypt_data) aci.send(crypt_data)
def __init__(self, name): map = generate_map(name) self.ctrl_socket = map['ctrl'] self.monitor_socket = map['mon'] self.opcd = OPCD_Interface(map['opcd_ctrl'], 'powerman') bus = SMBus(self.opcd.get('gpio_i2c_bus')) self.gpio_mosfet = GPIO_Bank(bus, self.opcd.get('gpio_i2c_address')) self.power_pin = self.opcd.get('gpio_power_pin') self.cells = self.opcd.get('battery_cells') self.low_cell_voltage_idle = self.opcd.get( 'battery_low_cell_voltage_idle') self.low_cell_voltage_load = self.opcd.get( 'battery_low_cell_voltage_load') self.battery_current_treshold = self.opcd.get( 'battery_current_treshold') self.capacity = self.opcd.get('battery_capacity') self.low_battery_voltage_idle = self.cells * self.low_cell_voltage_idle self.low_battery_voltage_load = self.cells * self.low_cell_voltage_load self.critical = False #self.gpio_mosfet.write() self.warning_started = False # start threads: self.standing = True self.adc_thread = start_daemon_thread(self.adc_reader) self.request_thread = start_daemon_thread(self.request_handler)
def optimize(rate, samples, prefix, names, mse_indices): gates = generate_map('optimizer') opcd = OPCD_Interface(gates['opcd_ctrl']) bb = gates['blackbox'] vec_best = [opcd.get(prefix + n) for n in names] vec = vec_best mse_min = sys.float_info.max best_log = [] try: for _ in range(5): vec = map(lambda x: x * uniform(1.0 - rate, 1.0 + rate), vec) # send new params to opcd: for i, n in zip(range(len(names)), names): opcd.set(prefix + n, vec[i]) # read data from autopilot and compute fitness: mse = 0.0 for _ in range(samples): array = loads(bb.recv()) for i in mse_indices: mse += array[i]**2 mse /= samples # evaluate mse: if mse < mse_min: best_log.append(mse) opcd.persist() mse_min = mse vec_best = copy(vec) else: # we did not improve; # use best vector as search starting point vec = copy(vec_best) except: for i, n in zip(range(len(names)), names): opcd.set(prefix + n, vec_best[i]) return best_log
def optimize(rate, samples, prefix, names, mse_indices): gates = generate_map('optimizer') opcd = OPCD_Interface(gates['opcd_ctrl']) bb = gates['blackbox'] vec_best = [ opcd.get(prefix + n) for n in names] vec = vec_best mse_min = sys.float_info.max best_log = [] try: for _ in range(5): vec = map(lambda x: x * uniform(1.0 - rate, 1.0 + rate), vec) # send new params to opcd: for i, n in zip(range(len(names)), names): opcd.set(prefix + n, vec[i]) # read data from autopilot and compute fitness: mse = 0.0 for _ in range(samples): array = loads(bb.recv()) for i in mse_indices: mse += array[i] ** 2 mse /= samples # evaluate mse: if mse < mse_min: best_log.append(mse) opcd.persist() mse_min = mse vec_best = copy(vec) else: # we did not improve; # use best vector as search starting point vec = copy(vec_best) except: for i, n in zip(range(len(names)), names): opcd.set(prefix + n, vec_best[i]) return best_log
def main(name): map = generate_map(name) socket = map['power'] opcd = OPCD_Interface(map['opcd_ctrl'], 'pi_quad') voltage_adc = ADS1x15_ADC(opcd.get('voltage_channel')) #current_adc = ADS1x15_ADC(opcd.get('current_channel')) voltage_lambda = eval(opcd.get('adc_to_voltage')) #current_lambda = eval(opcd.get('adc_to_current')) while True: sleep(0.2) voltage = voltage_lambda(voltage_adc.read()) #current = current_lambda(current_adc.read()) state = [voltage, # 0 [V] 0.4] # 1 [A] socket.send(dumps(state))
def __init__(self, name): # set-up logger: logfile = user_data_dir() + sep + 'PowerMan.log' log_config(filename = logfile, level = DEBUG, format = '%(asctime)s - %(levelname)s: %(message)s') # initialized and load config: log_info('powerman starting up') map = generate_map(name) self.ctrl_socket = map['ctrl'] self.monitor_socket = map['mon'] self.opcd = OPCD_Interface(map['opcd_ctrl'], 'powerman') bus = SMBus(self.opcd.get('gpio_i2c_bus')) self.gpio_mosfet = GPIO_Bank(bus, self.opcd.get('gpio_i2c_address')) self.power_pin = self.opcd.get('gpio_power_pin') self.cells = self.opcd.get('battery_cells') self.low_cell_voltage = self.opcd.get('battery_low_cell_voltage') self.capacity = self.opcd.get('battery_capacity') self.low_battery_voltage = self.cells * self.low_cell_voltage self.critical = False self.gpio_mosfet.write() self.warning_started = False # start threads: self.standing = True self.adc_thread = start_daemon_thread(self.adc_reader) self.emitter_thread = start_daemon_thread(self.power_state_emitter) self.request_thread = start_daemon_thread(self.request_handler) log_info('powerman running')
def main(name): map = generate_map(name) socket = map['power'] opcd = OPCD_Interface(map['opcd_ctrl'], 'overo_quad') voltage_adc = TWL4030_MADC(opcd.get('voltage_channel')) current_adc = TWL4030_MADC(opcd.get('current_channel')) voltage_lambda = eval(opcd.get('adc_to_voltage')) current_lambda = eval(opcd.get('adc_to_current')) while True: try: sleep(0.2) voltage = voltage_lambda(voltage_adc.read()) current = current_lambda(current_adc.read()) state = [voltage, # 0 [V] current] # 1 [A] socket.send(dumps(state)) except: pass
def __init__(self, dispatcher): Thread.__init__(self) self.dispatcher = dispatcher self.opcd_interface = OPCD_Interface( generate_map('mavlink')['opcd_ctrl']) self.param_map = {} self.param_name_map = {} list = self.opcd_interface.get('') c = 0 type_map = {float: MAVLINK_TYPE_FLOAT_T, long: MAVLINK_TYPE_INT32_T} cast_map = {float: float, long: int} for name, val in list: try: type = type_map[val.__class__] self.param_map[c] = name, type, cast_map[val.__class__] self.param_name_map[c] = c, type, cast_map[val.__class__] c += 1 except Exception, e: print str(e)
def main(name): map = generate_map(name) socket = map['power'] opcd = OPCD_Interface(map['opcd_ctrl'], 'overo_quad') voltage_adc = TWL4030_MADC(opcd.get('voltage_channel')) current_adc = TWL4030_MADC(opcd.get('current_channel')) voltage_lambda = eval(opcd.get('adc_to_voltage')) current_lambda = eval(opcd.get('adc_to_current')) while True: try: sleep(0.2) voltage = voltage_lambda(voltage_adc.read()) current = current_lambda(current_adc.read()) state = [ voltage, # 0 [V] current ] # 1 [A] socket.send(dumps(state)) except: pass
def main(name): sm = generate_map(name) opcd = OPCD_Interface(sm['opcd_ctrl'], name) sys_id = opcd.get('id') out_socket = sm['out'] in_socket = sm['in'] aci = ACI(sys_id, '/dev/ttyACM0') acr = ACIReader(aci, out_socket) acr.start() # read from SCL in socket and send data via NRF while True: try: msg = AirComm() raw = self.in_socket.recv() msg.ParseFromString(raw) aci.send(msg.addr, msg.type, msg.data) except: sleep(0.1)
def main(name): sm = generate_map(name) opcd = OPCD_Interface(sm['opcd_ctrl']) platform = opcd.get('platform') device = opcd.get(platform + '.nrf_serial') global THIS_SYS_ID THIS_SYS_ID = opcd.get('aircomm.id') key = opcd.get('aircomm.psk') crypt.init(key) mhist = MessageHistory(60) out_socket = sm['aircomm_out'] in_socket = sm['aircomm_in'] aci = ZMQ_Interface() acr = ACIReader(aci, out_socket, mhist) acr.start() # read from SCL in socket and send data via NRF while True: data = loads(in_socket.recv()) if len(data) == 2: msg = [data[0], THIS_SYS_ID, data[1]] elif len(data) > 2: msg = [data[0], THIS_SYS_ID] + data[1:] else: continue crypt_data = crypt.encrypt(dumps(msg)) mhist.append(crypt_data) aci.send(crypt_data)
def __init__(self, dispatcher): Thread.__init__(self) self.dispatcher = dispatcher self.opcd_interface = OPCD_Interface(generate_map('mavlink')['opcd_ctrl']) self.param_map = {} self.param_name_map = {} list = self.opcd_interface.get('') c = 0 type_map = {float: MAVLINK_TYPE_FLOAT_T, long: MAVLINK_TYPE_INT32_T} cast_map = {float: float, long: int} for name, val in list: try: type = type_map[val.__class__] self.param_map[c] = name, type, cast_map[val.__class__] self.param_name_map[c] = c, type, cast_map[val.__class__] c += 1 except Exception, e: print str(e)
def __init__(self, name): map = generate_map(name) self.ctrl_socket = map['ctrl'] self.monitor_socket = map['mon'] self.opcd = OPCD_Interface(map['opcd_ctrl'], 'powerman') bus = SMBus(self.opcd.get('gpio_i2c_bus')) self.gpio_mosfet = GPIO_Bank(bus, self.opcd.get('gpio_i2c_address')) self.power_pin = self.opcd.get('gpio_power_pin') self.cells = self.opcd.get('battery_cells') self.low_cell_voltage_idle = self.opcd.get('battery_low_cell_voltage_idle') self.low_cell_voltage_load = self.opcd.get('battery_low_cell_voltage_load') self.battery_current_treshold = self.opcd.get('battery_current_treshold') self.capacity = self.opcd.get('battery_capacity') self.low_battery_voltage_idle = self.cells * self.low_cell_voltage_idle self.low_battery_voltage_load = self.cells * self.low_cell_voltage_load self.critical = False #self.gpio_mosfet.write() self.warning_started = False # start threads: self.standing = True self.adc_thread = start_daemon_thread(self.adc_reader) self.request_thread = start_daemon_thread(self.request_handler)
def main(name): map = generate_map(name) power_socket = map['power'] powerman_socket = map['powerman'] opcd = OPCD_Interface(map['opcd_ctrl']) hysteresis = Hysteresis(opcd.get('powerman.low_voltage_hysteresis')) cells = opcd.get('powerman.battery_cells') low_cell_voltage_idle = opcd.get('powerman.battery_low_cell_voltage_idle') low_cell_voltage_load = opcd.get('powerman.battery_low_cell_voltage_load') battery_current_treshold = opcd.get('powerman.battery_current_treshold') capacity = opcd.get('powerman.battery_capacity') vmin = 13.2 vmax = 16.4 voltage, current = loads(power_socket.recv()) batt = min(1.0, max(0.0, (voltage - vmin) / (vmax - vmin))) consumed = (1.0 - batt) * capacity low_battery_voltage_idle = cells * low_cell_voltage_idle low_battery_voltage_load = cells * low_cell_voltage_load time_prev = time() critical = False while True: voltage, current = loads(power_socket.recv()) consumed += current / 3600.0 * (time() - time_prev) time_prev = time() if voltage < low_battery_voltage_idle and current < battery_current_treshold or voltage < low_battery_voltage_load and current >= battery_current_treshold: critical = hysteresis.set() else: hysteresis.reset() remaining = capacity - consumed if remaining < 0: remaining = 0 if critical: warning() state = [ voltage, # 0 [V] current, # 1 [A] remaining, # 2 [Ah] critical ] # 3 [Bool] powerman_socket.send(dumps(state))
def main(name): map = generate_map(name) power_socket = map['power'] powerman_socket = map['powerman'] opcd = OPCD_Interface(map['opcd_ctrl']) hysteresis = Hysteresis(opcd.get('powerman.low_voltage_hysteresis')) cells = opcd.get('powerman.battery_cells') low_cell_voltage_idle = opcd.get('powerman.battery_low_cell_voltage_idle') low_cell_voltage_load = opcd.get('powerman.battery_low_cell_voltage_load') battery_current_treshold = opcd.get('powerman.battery_current_treshold') capacity = opcd.get('powerman.battery_capacity') vmin = 13.2 vmax = 16.4 voltage, current = loads(power_socket.recv()) batt = min(1.0, max(0.0, (voltage - vmin) / (vmax - vmin))) consumed = (1.0 - batt) * capacity low_battery_voltage_idle = cells * low_cell_voltage_idle low_battery_voltage_load = cells * low_cell_voltage_load time_prev = time() critical = False while True: voltage, current = loads(power_socket.recv()) consumed += current / 3600.0 * (time() - time_prev) time_prev = time() if voltage < low_battery_voltage_idle and current < battery_current_treshold or voltage < low_battery_voltage_load and current >= battery_current_treshold: critical = hysteresis.set() else: hysteresis.reset() remaining = capacity - consumed if remaining < 0: remaining = 0 if critical: warning() state = [voltage, # 0 [V] current, # 1 [A] remaining, # 2 [Ah] critical] # 3 [Bool] powerman_socket.send(dumps(state))
class PowerMan: def __init__(self, name): map = generate_map(name) self.ctrl_socket = map['ctrl'] self.monitor_socket = map['mon'] self.opcd = OPCD_Interface(map['opcd_ctrl'], 'powerman') bus = SMBus(self.opcd.get('gpio_i2c_bus')) self.gpio_mosfet = GPIO_Bank(bus, self.opcd.get('gpio_i2c_address')) self.power_pin = self.opcd.get('gpio_power_pin') self.cells = self.opcd.get('battery_cells') self.low_cell_voltage_idle = self.opcd.get('battery_low_cell_voltage_idle') self.low_cell_voltage_load = self.opcd.get('battery_low_cell_voltage_load') self.battery_current_treshold = self.opcd.get('battery_current_treshold') self.capacity = self.opcd.get('battery_capacity') self.low_battery_voltage_idle = self.cells * self.low_cell_voltage_idle self.low_battery_voltage_load = self.cells * self.low_cell_voltage_load self.critical = False #self.gpio_mosfet.write() self.warning_started = False # start threads: self.standing = True self.adc_thread = start_daemon_thread(self.adc_reader) self.request_thread = start_daemon_thread(self.request_handler) def battery_warning(self): # do something in order to indicate a low battery: msg = 'CRITICAL WARNING: SYSTEM BATTERY VOLTAGE IS LOW; IMMEDIATE SHUTDOWN REQUIRED OR SYSTEM WILL BE DAMAGED' system('echo "%s" | wall' % msg) beeper_enabled = self.opcd.get('beeper_enabled') while True: if beeper_enabled: self.gpio_mosfet.set_gpio(5, False) sleep(0.1) self.gpio_mosfet.set_gpio(5, True) sleep(0.1) else: sleep(1.0) def adc_reader(self): voltage_adc = ADC(self.opcd.get('voltage_adc')) current_adc = ADC(self.opcd.get('current_adc')) voltage_lambda = eval(self.opcd.get('adc_2_voltage')) current_lambda = eval(self.opcd.get('adc_2_current')) self.consumed = 0.0 vmin = 13.2 vmax = 16.4 self.voltage = voltage_lambda(voltage_adc.read()) batt = min(1.0, max(0.0, (self.voltage - vmin) / (vmax - vmin))) self.consumed = (1.0 - batt) * self.capacity hysteresis = Hysteresis(self.opcd.get('low_voltage_hysteresis')) time_prev = time() while True: sleep(0.2) try: self.voltage = voltage_lambda(voltage_adc.read()) self.current = current_lambda(current_adc.read()) self.consumed += self.current / (3600 * (time() - time_prev)) time_prev = time() if self.voltage < self.low_battery_voltage_idle and self.current < self.battery_current_treshold or self.voltage < self.low_battery_voltage_load and self.current >= self.battery_current_treshold: self.critical = hysteresis.set() else: hysteresis.reset() if self.critical: if not self.warning_started: self.warning_started = True start_daemon_thread(self.battery_warning) voltage = self.voltage current = self.current capacity = self.capacity consumed = self.consumed remaining = self.capacity - self.consumed if remaining < 0: remaining = 0 critical = self.critical state = [self.voltage, # 0 [V] self.current, # 1 [A] remaining, # 2 [Ah] self.critical] # 3 [Bool] except Exception, e: continue self.monitor_socket.send(dumps(state))
Copyright (C) 2014 Tobias Simon, Ilmenau University of Technology This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. """ from sys import stdin from scl import generate_map from opcd_interface import OPCD_Interface opcd = OPCD_Interface(generate_map('opcd_shell')['ctrl']) try: while True: line = stdin.readline() if not line: break key, val = line.split(' ') val = float(val) opcd.set('pilot.cal.' + key, val) opcd.persist() except: print 'calibration invalid; please collect better data'
line = stdin.readline() if not line: return arr = line.split(' ') arr[3] = arr[3][0:-1] return arr header = read_array() if not header: raise ValueError("could not read txt file header") indices = [] for name in ['current', 'mag_cal_x', 'mag_cal_y', 'mag_cal_z']: indices.append(header.index(name)) opcd = OPCD_Interface(generate_map('opcd_shell')['opcd_ctrl']) data = np.loadtxt(stdin, usecols = indices) current = np.asarray(data[:, 0]) mag_x = np.asarray(data[:, 1]) mag_y = np.asarray(data[:, 2]) mag_z = np.asarray(data[:, 3]) A = np.vstack([current, np.ones(len(current))]).T a1, b1 = np.linalg.lstsq(A, mag_x)[0] a2, b2 = np.linalg.lstsq(A, mag_y)[0] a3, b3 = np.linalg.lstsq(A, mag_z)[0] opcd.set('autopilot.cmc.scale_x', float(a1))
class PowerMan: def __init__(self, name): # set-up logger: logfile = user_data_dir() + sep + 'PowerMan.log' log_config(filename = logfile, level = DEBUG, format = '%(asctime)s - %(levelname)s: %(message)s') # initialized and load config: log_info('powerman starting up') map = generate_map(name) self.ctrl_socket = map['ctrl'] self.monitor_socket = map['mon'] self.opcd = OPCD_Interface(map['opcd_ctrl'], 'powerman') bus = SMBus(self.opcd.get('gpio_i2c_bus')) self.gpio_mosfet = GPIO_Bank(bus, self.opcd.get('gpio_i2c_address')) self.power_pin = self.opcd.get('gpio_power_pin') self.cells = self.opcd.get('battery_cells') self.low_cell_voltage = self.opcd.get('battery_low_cell_voltage') self.capacity = self.opcd.get('battery_capacity') self.low_battery_voltage = self.cells * self.low_cell_voltage self.critical = False self.gpio_mosfet.write() self.warning_started = False # start threads: self.standing = True self.adc_thread = start_daemon_thread(self.adc_reader) self.emitter_thread = start_daemon_thread(self.power_state_emitter) self.request_thread = start_daemon_thread(self.request_handler) log_info('powerman running') def battery_warning(self): # do something in order to indicate a low battery: msg = 'CRITICAL WARNING: SYSTEM BATTERY VOLTAGE IS LOW; IMMEDIATE SHUTDOWN REQUIRED OR SYSTEM WILL BE DAMAGED' log_warn(msg) system('echo "%s" | wall' % msg) while True: self.gpio_mosfet.set_gpio(5, False) sleep(0.1) self.gpio_mosfet.set_gpio(5, True) sleep(0.1) def adc_reader(self): voltage_adc = ADC(self.opcd.get('voltage_adc')) current_adc = ADC(self.opcd.get('current_adc')) voltage_lambda = eval(self.opcd.get('adc_2_voltage')) current_lambda = eval(self.opcd.get('adc_2_current')) self.current_integral = 0.0 hysteresis = Hysteresis(self.opcd.get('low_voltage_hysteresis')) while True: sleep(1) try: self.voltage = voltage_lambda(voltage_adc.read()) self.current = current_lambda(current_adc.read()) if self.current < 4.0: self.current = 0.5 self.current_integral += self.current / 3600 print self.voltage, self.low_battery_voltage if self.voltage < self.low_battery_voltage: self.critical = hysteresis.set() else: hysteresis.reset() if self.critical: if not self.warning_started: self.warning_started = True start_daemon_thread(self.battery_warning) except Exception, e: log_err(str(e))
line = stdin.readline() if not line: return arr = line.split(' ') arr[3] = arr[3][0:-1] return arr header = read_array() if not header: raise ValueError("could not read txt file header") indices = [] for name in ['current', 'mag_cal_x', 'mag_cal_y', 'mag_cal_z']: indices.append(header.index(name)) opcd = OPCD_Interface(generate_map('opcd_shell')['opcd_ctrl']) data = np.loadtxt(stdin, usecols=indices) current = np.asarray(data[:, 0]) mag_x = np.asarray(data[:, 1]) mag_y = np.asarray(data[:, 2]) mag_z = np.asarray(data[:, 3]) A = np.vstack([current, np.ones(len(current))]).T a1, b1 = np.linalg.lstsq(A, mag_x)[0] a2, b2 = np.linalg.lstsq(A, mag_y)[0] a3, b3 = np.linalg.lstsq(A, mag_z)[0] opcd.set('autopilot.cmc.scale_x', float(a1)) opcd.set('autopilot.cmc.scale_y', float(a2))
if not channels_valid: if rt.expired(): print 'please enable your remote control' else: result = detectors[state].run(channels, channels_prev) if result is not None: if result in channel_map: print 'channel already used, please try again' detectors[state].reset() else: channel_map[state] = result state += 1 if state == len(specs): print channel_map break channels_prev = copy(channels) sleep(0.3) if killed: raise Exception print 'writing to opcd' opcd = OPCD_Interface(socket_map['opcd_ctrl']) for name, index, max, min in channel_map: prefix = 'rc_cal.' + name + '.' opcd.set(prefix + 'index', index) opcd.set(prefix + 'max', max) opcd.set(prefix + 'min', min) opcd.persist() print 'done' except Exception: print 'canceled by user'
from random import uniform from scl import generate_map from opcd_interface import OPCD_Interface from msgpack import loads import sys MUTATION_RATE = 0.1 NUM_SAMPLES = 300 gates = generate_map('optimizer') opcd = OPCD_Interface(gates['opcd_ctrl']) debug = gates['blackbox'] param_names = ['kp', 'ki', 'kii', 'kd'] prefix = 'pilot.controllers.stabilizing.att_' vec = [ opcd.get(prefix + n) for n in param_names] fit_best = sys.float_info.max vec_best = vec while True: # apply mutation: vec = map(lambda x: x * uniform(1.0 - MUTATION_RATE, 1.0 + MUTATION_RATE), vec) # send new gains to opcd: for i, n in zip(range(4), param_names): opcd.set(prefix + n, vec[i]) # read data from autopilot and compute fitness: fit = 0.0
class PowerMan: def __init__(self, name): map = generate_map(name) self.ctrl_socket = map['ctrl'] self.monitor_socket = map['mon'] self.opcd = OPCD_Interface(map['opcd_ctrl'], 'powerman') bus = SMBus(self.opcd.get('gpio_i2c_bus')) self.gpio_mosfet = GPIO_Bank(bus, self.opcd.get('gpio_i2c_address')) self.power_pin = self.opcd.get('gpio_power_pin') self.cells = self.opcd.get('battery_cells') self.low_cell_voltage_idle = self.opcd.get( 'battery_low_cell_voltage_idle') self.low_cell_voltage_load = self.opcd.get( 'battery_low_cell_voltage_load') self.battery_current_treshold = self.opcd.get( 'battery_current_treshold') self.capacity = self.opcd.get('battery_capacity') self.low_battery_voltage_idle = self.cells * self.low_cell_voltage_idle self.low_battery_voltage_load = self.cells * self.low_cell_voltage_load self.critical = False #self.gpio_mosfet.write() self.warning_started = False # start threads: self.standing = True self.adc_thread = start_daemon_thread(self.adc_reader) self.request_thread = start_daemon_thread(self.request_handler) def battery_warning(self): # do something in order to indicate a low battery: msg = 'CRITICAL WARNING: SYSTEM BATTERY VOLTAGE IS LOW; IMMEDIATE SHUTDOWN REQUIRED OR SYSTEM WILL BE DAMAGED' system('echo "%s" | wall' % msg) beeper_enabled = self.opcd.get('beeper_enabled') while True: if beeper_enabled: self.gpio_mosfet.set_gpio(5, False) sleep(0.1) self.gpio_mosfet.set_gpio(5, True) sleep(0.1) else: sleep(1.0) def adc_reader(self): voltage_adc = ADC(self.opcd.get('voltage_adc')) current_adc = ADC(self.opcd.get('current_adc')) voltage_lambda = eval(self.opcd.get('adc_2_voltage')) current_lambda = eval(self.opcd.get('adc_2_current')) self.consumed = 0.0 vmin = 13.2 vmax = 16.4 self.voltage = voltage_lambda(voltage_adc.read()) batt = min(1.0, max(0.0, (self.voltage - vmin) / (vmax - vmin))) self.consumed = (1.0 - batt) * self.capacity hysteresis = Hysteresis(self.opcd.get('low_voltage_hysteresis')) time_prev = time() while True: sleep(0.2) try: self.voltage = voltage_lambda(voltage_adc.read()) self.current = current_lambda(current_adc.read()) self.consumed += self.current / (3600 * (time() - time_prev)) time_prev = time() if self.voltage < self.low_battery_voltage_idle and self.current < self.battery_current_treshold or self.voltage < self.low_battery_voltage_load and self.current >= self.battery_current_treshold: self.critical = hysteresis.set() else: hysteresis.reset() if self.critical: if not self.warning_started: self.warning_started = True start_daemon_thread(self.battery_warning) voltage = self.voltage current = self.current capacity = self.capacity consumed = self.consumed remaining = self.capacity - self.consumed if remaining < 0: remaining = 0 critical = self.critical state = [ self.voltage, # 0 [V] self.current, # 1 [A] remaining, # 2 [Ah] self.critical ] # 3 [Bool] except Exception, e: continue self.monitor_socket.send(dumps(state))
from random import choice, uniform from scl import generate_map from opcd_interface import OPCD_Interface from msgpack import loads import sys MUTATION_RATE = 0.1 NUM_SAMPLES = 200 gates = generate_map('optimizer') opcd = OPCD_Interface(gates['opcd_ctrl']) debug = gates['blackbox'] from copy import copy param_names = ['kp', 'ki', 'kii', 'kd'] prefix = 'pilot.controllers.stabilizing.yaw_' vec = [opcd.get(prefix + n) for n in param_names] fit_best = sys.float_info.max vec_best = vec while True: # apply mutation: new = map(lambda x: x * uniform(1.0 - MUTATION_RATE, 1.0 + MUTATION_RATE), vec) i = choice(range(4)) vec[i] = new[ i] #map(lambda x: x * uniform(1.0 - MUTATION_RATE, 1.0 + MUTATION_RATE), vec) # send new gains to opcd: for i, n in zip(range(4), param_names): opcd.set(prefix + n, vec[i])
# set-up command history: _path = user_data_dir + os.sep + 'OPCD_shell.history' _history = os.path.expanduser(_path) def _save_history(historyPath=_history): readline.write_history_file(_history) if os.path.exists(_history): readline.read_history_file(_history) readline.parse_and_bind("tab: complete") atexit.register(_save_history) #initialize and define interface: _interface = OPCD_Interface(generate_map('opcd_shell')['opcd_ctrl']) def get(key=''): try: return _interface.get(key, True) except KeyError: print('key not found') def set(key, val): try: _interface.set(key, val) except KeyError: print('key not found')