<----SKIPPED LINES---->




  azimuth = angles['azimuth_degrees']
  altitude = angles['altitude_degrees']
  return (azimuth, altitude)

def DrainQueue(q):
  """Empties a queue, returning the last-retrieved value."""
  value = None
  while not q.empty():
    value = q.get(block=False)
  return value


def InitialMessageValues(q):
  """Initializes the arduino main processes with values from messageboard."""
  v = DrainQueue(q)
  if v:
    return v
  return {}, {}, {}, {}


def ServoTestOrdinal(configuration, link):
  """Point laser at each of 0, 90, 180, 270 and hold for a second with different colors."""
  messageboard.RemoveSetting(configuration, 'test_servos_ordinal')
  link.Write((0, 0, *LASER_ALL))
  time.sleep(1)
  link.Write((90, 0, *LASER_RED))
  time.sleep(1)
  link.Write((180, 0, *LASER_GREEN))
  time.sleep(1)
  link.Write((270, 0, *LASER_BLUE))
  time.sleep(1)


def ServoTestSweep(configuration, link, altitude=45):
  """Sweep red laser around 360 degrees."""
  messageboard.RemoveSetting(configuration, 'test_servos_sweep')
  for azimuth in range(0, 360, 10):
    link.Write((azimuth, altitude, *LASER_RED))
    time.sleep(0.15)


def ServoMain(to_arduino_q, to_parent_q, shutdown):
  """Main servo controller for projecting the plane position on a hemisphere.

  Takes the latest flight from the to_arduino_q and converts that to the current
  azimuth and altitude of the plane on a hemisphere.
  """
  sys.stderr = open(messageboard.STDERR_FILE, 'a')

  Log('Process started with process id %d' % os.getpid())

  # Ensures that the child can exit if the parent exits unexpectedly
  # docs.python.org/2/library/multiprocessing.html#multiprocessing.Queue.cancel_join_thread
  to_arduino_q.cancel_join_thread()
  to_parent_q.cancel_join_thread()

  # write_format: azimuth, altitude, R, G, & B intensity
  # read heartbeat: millis
  link = Serial(
      #TODO: maybe without this read timeout, we won't get the correlated BT failures
      *SERVO_CONNECTION, read_timeout=60,
      error_pin=messageboard.GPIO_ERROR_ARDUINO_SERVO_CONNECTION, to_parent_q=to_parent_q,
      read_format='l', write_format='ff???', name='Servo')
  link.Open()

  last_flight = {}
  last_angles = (0, 0)
  flight, json_desc_dict, configuration, additional_attr = InitialMessageValues(
      to_arduino_q)
  next_read = 0
  next_write = 0
  now = GetNow(json_desc_dict, additional_attr)

  while not shutdown.value:
    if not to_arduino_q.empty():
      flight, json_desc_dict, configuration, additional_attr = to_arduino_q.get(
          block=False)

      if 'test_servos_ordinal' in configuration:

        ServoTestOrdinal(configuration, link)
      elif 'test_servos_sweep' in configuration:

        ServoTestSweep(configuration, link)

      new_flight = DifferentFlights(flight, last_flight)
      if new_flight:
        Log('Flight changed from %s to %s' % (
            messageboard.DisplayFlightNumber(last_flight),
            messageboard.DisplayFlightNumber(flight)
        ), ser=link)

        # Turn off laser so that line isn't traced while it moves to new position
        link.Write((*last_angles, *LASER_OFF))

      last_flight = flight

    if time.time() >= next_read:
      heartbeat = link.Read()  # simple ack message sent by servos
      next_read = time.time() + READ_DELAY_TIME
      if heartbeat and VERBOSE:
        Log(heartbeat)

    now = GetNow(json_desc_dict, additional_attr)




                            <----SKIPPED LINES---->

                            <----SKIPPED LINES---->




  azimuth = angles['azimuth_degrees']
  altitude = angles['altitude_degrees']
  return (azimuth, altitude)

def DrainQueue(q):
  """Empties a queue, returning the last-retrieved value."""
  value = None
  while not q.empty():
    value = q.get(block=False)
  return value


def InitialMessageValues(q):
  """Initializes the arduino main processes with values from messageboard."""
  v = DrainQueue(q)
  if v:
    return v
  return {}, {}, {}, {}


def ServoTestOrdinal(link):
  """Point laser at each of 0, 90, 180, 270 and hold for a second with different colors."""

  link.Write((0, 0, *LASER_ALL))
  time.sleep(1)
  link.Write((90, 0, *LASER_RED))
  time.sleep(1)
  link.Write((180, 0, *LASER_GREEN))
  time.sleep(1)
  link.Write((270, 0, *LASER_BLUE))
  time.sleep(1)


def ServoTestSweep(link, altitude=45):
  """Sweep red laser around 360 degrees."""

  for azimuth in range(360, 10):
    link.Write((azimuth, altitude, *LASER_RED))
    time.sleep(WRITE_DELAY_TIME)


def ServoMain(to_arduino_q, to_parent_q, shutdown):
  """Main servo controller for projecting the plane position on a hemisphere.

  Takes the latest flight from the to_arduino_q and converts that to the current
  azimuth and altitude of the plane on a hemisphere.
  """
  sys.stderr = open(messageboard.STDERR_FILE, 'a')

  Log('Process started with process id %d' % os.getpid())

  # Ensures that the child can exit if the parent exits unexpectedly
  # docs.python.org/2/library/multiprocessing.html#multiprocessing.Queue.cancel_join_thread
  to_arduino_q.cancel_join_thread()
  to_parent_q.cancel_join_thread()

  # write_format: azimuth, altitude, R, G, & B intensity
  # read heartbeat: millis
  link = Serial(
      #TODO: maybe without this read timeout, we won't get the correlated BT failures
      *SERVO_CONNECTION, read_timeout=60,
      error_pin=messageboard.GPIO_ERROR_ARDUINO_SERVO_CONNECTION, to_parent_q=to_parent_q,
      read_format='l', write_format='ff???', name='Servo')
  link.Open()

  last_flight = {}
  last_angles = (0, 0)
  flight, json_desc_dict, configuration, additional_attr = InitialMessageValues(
      to_arduino_q)
  next_read = 0
  next_write = 0
  now = GetNow(json_desc_dict, additional_attr)

  while not shutdown.value:
    if not to_arduino_q.empty():
      flight, json_desc_dict, configuration, additional_attr = to_arduino_q.get(
          block=False)

      if 'test_servos_ordinal' in configuration:
        messageboard.RemoveSetting(configuration, 'test_servos_ordinal')
        ServoTestOrdinal(link)
      elif 'test_servos_sweep' in configuration:
        messageboard.RemoveSetting(configuration, 'test_servos_sweep')
        ServoTestSweep(link)

      new_flight = DifferentFlights(flight, last_flight)
      if new_flight:
        Log('Flight changed from %s to %s' % (
            messageboard.DisplayFlightNumber(last_flight),
            messageboard.DisplayFlightNumber(flight)
        ), ser=link)

        # Turn off laser so that line isn't traced while it moves to new position
        link.Write((*last_angles, *LASER_OFF))

      last_flight = flight

    if time.time() >= next_read:
      heartbeat = link.Read()  # simple ack message sent by servos
      next_read = time.time() + READ_DELAY_TIME
      if heartbeat and VERBOSE:
        Log(heartbeat)

    now = GetNow(json_desc_dict, additional_attr)




                            <----SKIPPED LINES---->