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))
