Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
mesonh.py
Go to the documentation of this file.
1 from __future__ import absolute_import, print_function, division
2 from mesonh_atmosphere import MesoNHAtmosphere
3 import os
4 import sys
5 import signal
6 import time
7 import socket
8 import struct
9 import cmath
10 import numpy as np
11 from os import getenv
12 # if PAPARAZZI_SRC not set, then assume the tree containing this
13 # file is a reasonable substitute
14 PPRZ_SRC = getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../../')))
15 sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python")
16 
17 from pprzlink.ivy import IvyMessagesInterface
18 from pprzlink.message import PprzMessage
19 
20 M_IN_KM = 1000.
21 
22 atm = None
23 origin = np.array([0, 0, 0, 0])
24 scale = np.array([1., 1/M_IN_KM, 1/M_IN_KM, 1/M_IN_KM])
25 
26 start_time = time.time()
27 
28 
29 def get_wind(east, north, up):
30  t = time.time() - start_time
31  print("east :",east)
32  print("north :",north)
33  print("up :",up)
34  loc = np.array([t, up, east, north])
35  loc = loc*scale + origin
36  print("loc:",loc)
37  weast, wnorth, wup = atm.get_wind(loc)
38  return weast, wnorth, wup
39 
40 
41 def ivy_request_callback(sender, msg, resp, *args, **kwargs):
42  """
43  Ivy Callback for Paparazzi Requests
44  """
45 
46  if msg.msg_class == "ground" and msg.name == "WORLD_ENV_REQ":
47  return worldenv_cb(msg, resp)
48  else:
49  return None
50 
51 
52 #def worldenv_cb(m, r):
53 def worldenv_cb(ac_id, msg):
54  """
55  Callback for paparazzi WORLD_ENV requests
56  """
57  # request location (in meters)
58  east, north, up = float(msg.get_field(3)),\
59  float(msg.get_field(4)),\
60  float(msg.get_field(5))
61  up *= -1
62  # convert in km + translation with mesoNH origin
63  weast, wnorth, wup = get_wind(east, north, up)
64  print("wind_est:")
65  print(weast)
66  print(wnorth)
67  print(wup)
68  msg_back=PprzMessage("ground", "WORLD_ENV")
69  msg_back.set_value_by_name("wind_east",weast)
70  msg_back.set_value_by_name("wind_north",wnorth)
71  msg_back.set_value_by_name("wind_up",wup)
72  msg_back.set_value_by_name("ir_contrast",400)
73  msg_back.set_value_by_name("time_scale",1)
74  msg_back.set_value_by_name("gps_availability",1)
75  ivy.send(msg_back,None)
76 
77 
78 def signal_handler(signal, frame):
79  print('\nShutting down IVY...')
80  ivy.shutdown()
81  print("Done.")
82 
83 
84 def main():
85  # parse arguments
86  import argparse as ap
87 
88  argp = ap.ArgumentParser(description="Environment variables provider "
89  "for Paparazzi simulation from MesoNH data")
90 
91  argp.add_argument("-t", "--time-step", required=True, type=int,
92  help="Duration of a time step between MesoNH Files.")
93  argp.add_argument("-f", "--files", required=True, nargs='+',
94  help="MesoNH netCDF files, in temporal ordering")
95  argp.add_argument("-x", "--origin-x", required=False, type=float,
96  default=0.,
97  help="Origin translation x.")
98  argp.add_argument("-y", "--origin-y", required=False, type=float,
99  default=0.,
100  help="Origin translation y.")
101  argp.add_argument("-z", "--origin-z", required=False, type=float,
102  default=0.,
103  help="Origin translation z.")
104  arguments = argp.parse_args()
105 
106  print(arguments)
107 
108  # register signal handler for ctrl+c to stop the program
109  signal.signal(signal.SIGINT, signal_handler)
110 
111  # origin for translation from paparazzi to mesoNH frame
112  global origin
113  origin = np.array([0, arguments.origin_z, arguments.origin_x, arguments.origin_y])
114 
115  # build atmosphere simulation source
116  global atm
117  atm = MesoNHAtmosphere(arguments.files, arguments.time_step, tinit=0)
118 
119  # init ivy and register callback for WORLD_ENV_REQ and NPS_SPEED_POS
120  global ivy
121  ivy = IvyMessagesInterface("MesoNH");
122  ivy.subscribe(worldenv_cb,'(.* WORLD_ENV_REQ .*)')
123 
124  # wait for ivy to stop
125  from ivy.std_api import IvyMainLoop
126 
127  signal.pause()
128 
129 
130 if __name__ == '__main__':
131  main()
def signal_handler
Definition: mesonh.py:78
def worldenv_cb
Definition: mesonh.py:53
def get_wind
Definition: mesonh.py:29
def ivy_request_callback
Definition: mesonh.py:41
def main
Definition: mesonh.py:84