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 = 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 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 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 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'], '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): 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): 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 ParamHandler(Thread): 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): 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)
class ParamHandler(Thread): 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)
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]) # read data from autopilot and compute fitness:
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 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 for _ in range(NUM_SAMPLES): array = loads(debug.recv()) gyro = array[1:3]
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))
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))