def _state_flySpline(drone, elapsed, dt):
    if elapsed > drone.lastTime:
        return _stateTransition(drone, 'waiting')

    t_lookAt = drone.time_to_lookAt(elapsed)
    t_lookFrom = drone.time_to_lookFrom(elapsed)

    lookFromPointNED, TF, dTF = trajectoryAPI._evaluate_spatial_spline(
        drone.C_lookFrom_spline,
        drone.T_lookFrom_spline,
        drone.sd_lookFrom_spline,
        T_eval=np.array([[t_lookFrom, t_lookFrom, t_lookFrom]]))
    lookAtPointNED, TA, dTA = trajectoryAPI._evaluate_spatial_spline(
        drone.C_lookAt_spline,
        drone.T_lookAt_spline,
        drone.sd_lookAt_spline,
        T_eval=np.array([[t_lookAt, t_lookAt, t_lookAt]]))

    lookFromPoint = coord_system.ned2llh(lookFromPointNED[0], drone.refLLH)
    lookAtPoint = coord_system.ned2llh(lookAtPointNED[0], drone.refLLH)
    lookFromPoint[2] = lookFromPoint[2] - drone.altitudeOffset
    lookAtPoint[2] = lookAtPoint[2] - drone.altitudeOffset

    sendLookFrom(drone, lookFromPoint)

    if drone.lastLookAtPoint == None or coord_system.get_distance_llh(
            drone.lastLookAtPoint,
            lookAtPoint) > DISTANCE_LIMIT_LOOK_AT_METERS:
        drone.lastLookAtPoint = lookAtPoint
        sendLookAt(drone, lookAtPoint)
def _state_flyToStart(drone, elapsed, dt):
  l = drone.vehicle.location
  if l is None:
    return
  lookFromStartLLH = coord_system.ned2llh(drone.P_lookFromNED_spline[0], drone.refLLH)
  lookAtStartLLH = coord_system.ned2llh(drone.P_lookAtNED_spline[0], drone.refLLH)

  lookFromStartLLH[2] = lookFromStartLLH[2] - drone.altitudeOffset
  lookAtStartLLH[2] = lookFromStartLLH[2] - drone.altitudeOffset
  
  distanceToStart = coord_system.get_distance_llh(lookFromStartLLH, numpy.array([l.lat,l.lon,l.alt]))
  print "Distance to start point: %.2fm" % distanceToStart
  if distanceToStart > 2.0 or np.linalg.norm(drone.vehicle.velocity) > 0.5:
    sendLookFrom(drone, lookFromStartLLH)
    sendLookAt(drone, lookAtStartLLH)
  else:
    return _stateTransition(drone, 'flySpline')
def _state_flyToStart(drone, elapsed, dt):
    l = drone.vehicle.location
    if l is None:
        return
    lookFromStartLLH = coord_system.ned2llh(drone.P_lookFromNED_spline[0],
                                            drone.refLLH)
    lookAtStartLLH = coord_system.ned2llh(drone.P_lookAtNED_spline[0],
                                          drone.refLLH)

    lookFromStartLLH[2] = lookFromStartLLH[2] - drone.altitudeOffset
    lookAtStartLLH[2] = lookFromStartLLH[2] - drone.altitudeOffset

    distanceToStart = coord_system.get_distance_llh(
        lookFromStartLLH, numpy.array([l.lat, l.lon, l.alt]))
    print "Distance to start point: %.2fm" % distanceToStart
    if distanceToStart > 2.0 or np.linalg.norm(drone.vehicle.velocity) > 0.5:
        sendLookFrom(drone, lookFromStartLLH)
        sendLookAt(drone, lookAtStartLLH)
    else:
        return _stateTransition(drone, 'flySpline')
def _state_flySpline(drone, elapsed, dt): 
  if elapsed > drone.lastTime:
    return _stateTransition(drone, 'waiting') 

  t_lookAt   = drone.time_to_lookAt(elapsed)
  t_lookFrom = drone.time_to_lookFrom(elapsed)

  lookFromPointNED, TF, dTF = trajectoryAPI._evaluate_spatial_spline(drone.C_lookFrom_spline,drone.T_lookFrom_spline,drone.sd_lookFrom_spline,T_eval=np.array([[t_lookFrom,t_lookFrom,t_lookFrom]]))
  lookAtPointNED, TA, dTA = trajectoryAPI._evaluate_spatial_spline(drone.C_lookAt_spline,drone.T_lookAt_spline,drone.sd_lookAt_spline,T_eval=np.array([[t_lookAt,t_lookAt,t_lookAt]]))

  lookFromPoint = coord_system.ned2llh(lookFromPointNED[0], drone.refLLH)
  lookAtPoint   = coord_system.ned2llh(lookAtPointNED[0], drone.refLLH)
  lookFromPoint[2] = lookFromPoint[2] - drone.altitudeOffset
  lookAtPoint[2] = lookAtPoint[2] - drone.altitudeOffset

  sendLookFrom(drone, lookFromPoint)
  
  if drone.lastLookAtPoint == None or coord_system.get_distance_llh(drone.lastLookAtPoint, lookAtPoint) > DISTANCE_LIMIT_LOOK_AT_METERS: 
    drone.lastLookAtPoint = lookAtPoint
    sendLookAt(drone, lookAtPoint)