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