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)