Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
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)
Ejemplo n.º 3
0
    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)
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
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
Ejemplo n.º 6
0
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))
Ejemplo n.º 7
0
   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')
Ejemplo n.º 8
0
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
Ejemplo n.º 9
0
 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)
Ejemplo n.º 10
0
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
Ejemplo n.º 11
0
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)
Ejemplo n.º 12
0
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)
Ejemplo n.º 13
0
 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)
Ejemplo n.º 14
0
   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)
Ejemplo n.º 15
0
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))
Ejemplo n.º 16
0
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))
Ejemplo n.º 17
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))
Ejemplo n.º 18
0
 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'
Ejemplo n.º 19
0
   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))
Ejemplo n.º 20
0
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))
Ejemplo n.º 21
0
    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))
Ejemplo n.º 22
0
      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'
Ejemplo n.º 23
0
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
Ejemplo n.º 24
0
 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'
Ejemplo n.º 25
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))
Ejemplo n.º 26
0
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])
Ejemplo n.º 27
0
# 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')