message: =More Python work
diff: @@ -16,10 +16,8 @@ VEL_I = 1.0
  VEL_D = 0.05
  def makesock():
  -
  sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
  sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, True)
  -
  return sock
  if __name__ == '__main__':
  @@ -40,11 +38,31 @@ if __name__ == '__main__':
  telemSocket.bind(('127.0.0.1', 5001))
  + u = 0
  +
  while True:
  telem = np.frombuffer(telemSocket.recvfrom(80)[0])
  - print(telem)
  + # Extract time, altitude from state. Altitude is in NED coordinates, so we negate it to use as input
  + # to PID controller.
  + t = telem[0]
  + z = -telem[9]
  +
  + print('%+5.5f,%+3.3f,%+3.3f,%+3.3f' % (t, u, z, dzdt))
  +
  + # Compute vertical climb rate as first difference of altitude over time
  + if t > tprev:
  +
  + # Use temporal first difference to compute vertical velocity
  + dt = t - tprev
  + dzdt = (z-zprev) / dt
  +
  + # Get correction from PID controller
  + u = pid.u(z, dzdt, dt)
  +
  + # Constrain correction to [0,1] to represent motor value
  + u = max(0, min(1, u))
  - motorSocket.sendto(np.ndarray.tobytes(0.55*np.ones(4)), ('127.0.0.1', 5000))
  + motorSocket.sendto(np.ndarray.tobytes(u*np.ones(4)), ('127.0.0.1', 5000))
  