def __get_declared_xy(self, ls_adsb_msg):
"""
get declared xy
"""
# get aircraft (ICAO24 address)
ls_icao24 = str(dcdr.get_icao_addr(ls_adsb_msg)).upper()
# airborne position msg type (9-18) ok ?
if 8 < dcdr.get_tc(ls_adsb_msg) < 19:
# get airborne position
lf_lat, lf_lon, lf_alt = self.__decode_position(
ls_icao24, ls_adsb_msg)
#M_LOG.debug("declared_ll: {} / {} / {}".format(lf_lat, lf_lon, lf_alt))
# valid position ?
if (lf_lat is not None) and (lf_lon is not None) and (lf_alt
is not None):
# convert lat/lng/alt to x, y, z
return gutl.geog2ecef(lf_lat, lf_lon, lf_alt)
# senão, already have a position ?
elif ls_icao24 in self.__dct_lst_pos:
# use last reported position as reference
lf_lat, lf_lon, lf_alt = self.__dct_lst_pos[ls_icao24]
#M_LOG.debug("declared_ll: {} / {} / {}".format(lf_lat, lf_lon, lf_alt))
# convert lat/lng/alt to x, y, z
return gutl.geog2ecef(lf_lat, lf_lon, lf_alt)
# return
return None, None, None
def __estimate_toa(self, fs_message):
"""
make a estimate of time of arrival
"""
# clear to go
assert fs_message
# split message
llst_msg = fs_message.split()
#M_LOG.debug("llst_msg: {}".format(llst_msg))
# ads-b message
ls_msg_adsb = llst_msg[0]
# received position
lf_rcv_lat = float(llst_msg[1])
lf_rcv_lon = float(llst_msg[2])
lf_rcv_alt = float(llst_msg[3])
# get aircraft (ICAO24 address)
ls_icao24 = str(dcdr.get_icao_addr(ls_msg_adsb)).upper()
#M_LOG.debug(">>>>>>>>>>>>>>>>>>>> Aeronave: {} <<<<<<<<<<<<<<<<<<<<<<<<".format(ls_icao24))
#M_LOG.debug("position lat: {}, lng: {}, alt: {}".format(lf_rcv_lat, lf_rcv_lon, lf_rcv_alt))
# aircraft position (ECEF)
lf_anv_x, lf_anv_y, lf_anv_z = gutl.geog2ecef(lf_rcv_lat, lf_rcv_lon,
lf_rcv_alt)
#M_LOG.debug("lf_anv_x: {}, lf_anv_y: {}, lf_anv_z: {}".format(lf_anv_x, lf_anv_y, lf_anv_z))
# convert lat/lng to enu
#lf_flt_x, lf_flt_y, lf_flt_z = gutl.geog2enu(lf_rcv_lat, lf_rcv_lon, lf_rcv_alt,
# self.__f_lat, self.__f_lng, self.__f_alt)
#M_LOG.debug("lf_flt_x: {}, lf_flt_y: {}, lf_flt_z: {}".format(lf_flt_x, lf_flt_y, lf_flt_z))
# euclidean distance
#lf_dist = math.sqrt(lf_flt_x * lf_flt_x + lf_flt_y * lf_flt_y + lf_flt_z * lf_flt_z)
#M_LOG.debug("lf_dist: {}".format(lf_dist))
# 2D distance between aircraft and sensor positions
#lf_dist_2d = math.sqrt(pow(lf_anv_x - self.__f_x, 2) + pow(lf_anv_y - self.__f_y, 2))
#M_LOG.debug("lf_dist_2d: {}".format(lf_dist_2d))
# 3D distance between aircraft and sensor positions
lf_dist_3d = math.sqrt(
pow(lf_anv_x - self.__f_x, 2) + pow(lf_anv_y - self.__f_y, 2) +
pow(lf_anv_z - self.__f_z, 2))
#M_LOG.debug("lf_dist_3d: {}".format(lf_dist_3d))
# return ads-b message, estimated time (distance / speed of light)
return ls_msg_adsb, lf_dist_3d / M_LIGHT_SPEED
def __init__(self,
fi_id,
ff_lat,
ff_lng,
ff_alt,
fs_config="adsb_in.cfg",
fv_store_msgs=False):
"""
constructor
@param fi_id: sensor id
@param ff_(lat, lng, alt): station position
@param fs_config: arquivo de configuração
@param fv_store_msgs: store messages flag
"""
# destinations to which messages will be forwarded to
self.__lst_forwarders = []
# station location
self.__f_lat = ff_lat
self.__f_lng = ff_lng
self.__f_alt = ff_alt
# id
self.__i_id = fi_id
# lista de mensagens
self.__q_rec_msgs = Queue.Queue(M_MAX_REC_MSG)
self.__v_store_rec_msgs = fv_store_msgs
# asterix forwarder/server
self.__v_asterix_server = False
self.__i_asterix_rx_port = None
self.__i_asterix_tx_port = None
self.__i_asterix_sic = None
self.__s_asterix_dest = None
#M_LOG.debug(">>>>>>>>>>>>>>>>>>>> Sensor: {} <<<<<<<<<<<<<<<<<<<<<<<<".format(fi_id))
#M_LOG.debug("position lat: {}, lng: {}, alt: {}".format(self.__f_lat, self.__f_lng, self.__f_alt))
# station location (ECEF)
self.__f_x, self.__f_y, self.__f_z = gutl.geog2ecef(
ff_lat, ff_lng, ff_alt)
#M_LOG.debug("self.__f_x: {}, self.__f_y: {}, self.__f_z: {}".format(self.__f_x, self.__f_y, self.__f_z))
# load configuration file
self.__load_config(fs_config)
l_real = (-15.5, -47.5)
# time of arrival
llst_toa = [
vdst.vincenty((l_sns[0], l_sns[1]), l_real)
for l_sns in ldct_sns.values()
]
# init positions matrix
lmat_sns_ecef = [[0., 0., 0.] for _ in xrange(len(ldct_sns))]
# for all stations...
for li_ndx, llst_sns in enumerate(ldct_sns.values()):
# convert stations positions to ECEF
lmat_sns_ecef[li_ndx][0], lmat_sns_ecef[li_ndx][1], lmat_sns_ecef[
li_ndx][2] = gutl.geog2ecef(llst_sns[0], llst_sns[1], llst_sns[2])
# calc estimated (method 2)
l_xe, l_ye, l_ze = mlat_1(lmat_sns_ecef, llst_toa)
# estimated position
lat = math.degrees(math.asin(l_ze / gutl.a))
lng = math.degrees(math.atan2(l_ye, l_xe))
print "lat / lng estimated:", lat, lng
print "lat / lng real.....:", l_real[0], l_real[1]
# converte real to ecef
l_xr, l_yr, l_zr = gutl.geog2ecef(l_real[0], l_real[1], 1500.)
print "distance:", math.sqrt(pow((l_xr - l_xe), 2) + pow((l_yr - l_ye), 2))