Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
nps_ivy.c
Go to the documentation of this file.
1 #include "nps_ivy.h"
2 
3 #include <stdlib.h>
4 #include <stdio.h>
5 #include <sys/types.h>
6 #include <unistd.h>
7 #include <Ivy/ivy.h>
8 
9 #include <Ivy/ivyloop.h>
10 #include <pthread.h>
11 
12 #include "generated/airframe.h"
15 #include "nps_main.h"
16 #include "nps_autopilot.h"
17 #include "nps_fdm.h"
18 #include "nps_sensors.h"
19 #include "nps_atmosphere.h"
20 
21 #include "generated/settings.h"
22 #include "pprzlink/dl_protocol.h"
24 
25 #if USE_GPS
26 #include "modules/gps/gps.h"
27 #endif
28 
30 pthread_t th_ivy_main; // runs main Ivy loop
31 static MsgRcvPtr ivyPtr = NULL;
32 static int seq = 1;
33 static int ap_launch_index;
34 static pthread_mutex_t ivy_mutex; // mutex for ivy send
35 
36 /* Gaia Ivy functions */
37 static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
38  void *user_data __attribute__((unused)),
39  int argc, char *argv[]);
40 
41 /* Datalink Ivy functions */
42 static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
43  void *user_data __attribute__((unused)),
44  int argc, char *argv[]);
45 
46 void* ivy_main_loop(void* data __attribute__((unused)));
47 
48 int find_launch_index(void);
49 
50 
51 void* ivy_main_loop(void* data __attribute__((unused)))
52 {
53  IvyMainLoop();
54 
55  return NULL;
56 }
57 
58 void nps_ivy_init(char *ivy_bus)
59 {
60  const char *agent_name = AIRFRAME_NAME"_NPS";
61  const char *ready_msg = AIRFRAME_NAME"_NPS Ready";
62  IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL);
63 
64  // bind on a general WORLD_ENV (not a reply to request)
65  IvyBindMsg(on_WORLD_ENV, NULL, "^(\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
66 
67  // to be able to change datalink_enabled setting back on
68  IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
69 
70 #ifdef __APPLE__
71  const char *default_ivy_bus = "224.255.255.255";
72 #else
73  const char *default_ivy_bus = "127.255.255.255";
74 #endif
75  if (ivy_bus == NULL) {
76  IvyStart(default_ivy_bus);
77  } else {
78  IvyStart(ivy_bus);
79  }
80 
81  nps_ivy_send_world_env = false;
82 
84 
85  // Launch separate thread with IvyMainLoop()
86  pthread_create(&th_ivy_main, NULL, ivy_main_loop, NULL);
87 
88 }
89 
90 /*
91  * Parse WORLD_ENV message from gaia.
92  *
93  */
94 static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
95  void *user_data __attribute__((unused)),
96  int argc, char *argv[])
97 {
98  if (argc < 6) { return; }
99 
100  // wind speed in m/s
101  struct FloatVect3 wind;
102  wind.x = atof(argv[1]); //east
103  wind.y = atof(argv[2]); //north
104  wind.z = atof(argv[3]); //up
105 
106  /* set wind speed in NED */
107  nps_atmosphere_set_wind_ned(wind.y, wind.x, -wind.z);
108 
109  /* not used so far */
110  //float ir_contrast = atof(argv[4]);
111 
112  /* set new time factor */
113  nps_set_time_factor(atof(argv[5]));
114 
115 #if USE_GPS
116  // directly set gps fix in modules/gps/gps_sim_nps.h
117  gps_has_fix = atoi(argv[6]); // gps_availability
118 #endif
119 
120 }
121 
122 /*
123  * Send a WORLD_ENV_REQ message
124  */
125 
126 
128 {
129  pthread_mutex_lock(&ivy_mutex);
130 
131  // First unbind from previous request if needed
132  if (ivyPtr != NULL) {
133  IvyUnbindMsg(ivyPtr);
134  ivyPtr = NULL;
135  }
136 
137  int pid = (int)getpid();
138 
139  // Bind to the reply
140  ivyPtr = IvyBindMsg(on_WORLD_ENV, NULL, "^%d_%d (\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", pid, seq);
141 
142  // Send actual request
143  struct NpsFdm fdm_ivy;
144  memcpy(&fdm_ivy, &fdm, sizeof(struct NpsFdm));
145 
146  IvySendMsg("nps %d_%d WORLD_ENV_REQ %f %f %f %f %f %f",
147  pid, seq,
148  DegOfRad(fdm_ivy.lla_pos_pprz.lat),
149  DegOfRad(fdm_ivy.lla_pos_pprz.lon),
150  (fdm_ivy.hmsl),
151  (fdm_ivy.ltpprz_pos.x),
152  (fdm_ivy.ltpprz_pos.y),
153  (fdm_ivy.ltpprz_pos.z));
154  seq++;
155 
156  nps_ivy_send_world_env = false;
157 
158  pthread_mutex_unlock(&ivy_mutex);
159 }
160 
162 {
163  static const char ap_launch[] = "aut_lau"; // short name
164  char *ap_settings[NB_SETTING] = SETTINGS_NAMES_SHORT;
165 
166  // list through the settings
167  // TODO: maybe search for a substring with if(strstr(sent, word) != NULL)
168  for (uint8_t idx=0;idx<NB_SETTING;idx++) {
169  if (strcmp(ap_settings[idx],ap_launch) == 0) {
170  return (int)idx;
171  }
172  }
173  return -1;
174 }
175 
176 static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
177  void *user_data __attribute__((unused)),
178  int argc, char *argv[])
179 {
180  if (argc < 3) { return; }
181 
182  if (atoi(argv[1]) != AC_ID) {
183  return;
184  }
185 
186  /* HACK:
187  * we actually don't want to allow changing settings if datalink is disabled,
188  * but since we currently change this variable via settings we have to allow it
189  * TODO: only allow changing the datalink_enabled setting
190  */
191  uint8_t index = atoi(argv[2]);
192  float value = atof(argv[3]);
193  if (!datalink_enabled) {
194  DlSetting(index, value);
195  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value);
196  }
197  printf("setting %d %f\n", index, value);
198 
199  /*
200  * In case of HITL, update nps_autopilot.launch from DL_SETTINGS
201  * so the plane can be properly launched.
202  *
203  * In case of STIL nps_update_launch_from_dl() is an empty function
204  */
205  if ((ap_launch_index >= 0) || (ap_launch_index < NB_SETTING)) {
206  if (index==ap_launch_index){
208  }
209  }
210 }
211 
212 
213 void nps_ivy_display(struct NpsFdm* fdm_data, struct NpsSensors* sensors_data)
214 {
215  struct NpsFdm fdm_ivy;
216  struct NpsSensors sensors_ivy;
217 
218  // make a local copy with mutex
219  pthread_mutex_lock(&fdm_mutex);
220  memcpy(&fdm_ivy, fdm_data, sizeof(fdm));
221  memcpy(&sensors_ivy, sensors_data, sizeof(sensors));
222  pthread_mutex_unlock(&fdm_mutex);
223 
224  // protect Ivy thread
225  pthread_mutex_lock(&ivy_mutex);
226 
227  IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
228  AC_ID,
229  DegOfRad(fdm_ivy.body_ecef_rotvel.p),
230  DegOfRad(fdm_ivy.body_ecef_rotvel.q),
231  DegOfRad(fdm_ivy.body_ecef_rotvel.r),
232  DegOfRad(fdm_ivy.ltp_to_body_eulers.phi),
233  DegOfRad(fdm_ivy.ltp_to_body_eulers.theta),
234  DegOfRad(fdm_ivy.ltp_to_body_eulers.psi));
235  IvySendMsg("%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f",
236  AC_ID,
237  (fdm_ivy.lla_pos_pprz.lat),
238  (fdm_ivy.lla_pos_geod.lat),
239  (fdm_ivy.lla_pos_geoc.lat),
240  (fdm_ivy.lla_pos_pprz.lon),
241  (fdm_ivy.lla_pos_geod.lon),
242  (fdm_ivy.lla_pos_pprz.alt),
243  (fdm_ivy.lla_pos_geod.alt),
244  (fdm_ivy.agl),
245  (fdm_ivy.hmsl));
246  IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f",
247  AC_ID,
248  (fdm_ivy.ltpprz_ecef_accel.x),
249  (fdm_ivy.ltpprz_ecef_accel.y),
250  (fdm_ivy.ltpprz_ecef_accel.z),
251  (fdm_ivy.ltpprz_ecef_vel.x),
252  (fdm_ivy.ltpprz_ecef_vel.y),
253  (fdm_ivy.ltpprz_ecef_vel.z),
254  (fdm_ivy.ltpprz_pos.x),
255  (fdm_ivy.ltpprz_pos.y),
256  (fdm_ivy.ltpprz_pos.z));
257  IvySendMsg("%d NPS_GYRO_BIAS %f %f %f",
258  AC_ID,
259  DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.x) + sensors_ivy.gyro.bias_initial.x),
260  DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.y) + sensors_ivy.gyro.bias_initial.y),
261  DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.z) + sensors_ivy.gyro.bias_initial.z));
262 
263  /* transform magnetic field to body frame */
264  struct DoubleVect3 h_body;
265  double_quat_vmult(&h_body, &fdm_ivy.ltp_to_body_quat, &fdm_ivy.ltp_h);
266 
267  IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
268  AC_ID,
269  ((sensors_ivy.accel.value.x - sensors_ivy.accel.neutral.x) / NPS_ACCEL_SENSITIVITY_XX),
270  ((sensors_ivy.accel.value.y - sensors_ivy.accel.neutral.y) / NPS_ACCEL_SENSITIVITY_YY),
271  ((sensors_ivy.accel.value.z - sensors_ivy.accel.neutral.z) / NPS_ACCEL_SENSITIVITY_ZZ),
272  h_body.x,
273  h_body.y,
274  h_body.z);
275 
276  IvySendMsg("%d NPS_WIND %f %f %f",
277  AC_ID,
278  fdm_ivy.wind.x,
279  fdm_ivy.wind.y,
280  fdm_ivy.wind.z);
281 
282  pthread_mutex_unlock(&ivy_mutex);
283 
286  }
287 }
#define sensors(...)
Definition: cc2500_compat.h:68
Device independent GPS code (interface)
bool gps_has_fix
Definition: gps_sim_nps.c:29
double psi
in radians
double p
in rad/s^2
double theta
in radians
double q
in rad/s^2
double phi
in radians
double r
in rad/s^2
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
#define RATE_FLOAT_OF_BFP(_ai)
double y
in meters
double z
in meters
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
double lon
in radians
double x
in meters
void nps_atmosphere_set_wind_ned(double wind_north, double wind_east, double wind_down)
Atmosphere model (pressure, wind) for NPS.
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
double hmsl
Definition: nps_fdm.h:56
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
struct LlaCoor_d lla_pos_pprz
Definition: nps_fdm.h:58
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:81
double agl
Definition: nps_fdm.h:61
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
struct LlaCoor_d lla_pos_geoc
Definition: nps_fdm.h:60
struct DoubleVect3 wind
velocity in m/s in NED
Definition: nps_fdm.h:107
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
struct LlaCoor_d lla_pos_geod
Definition: nps_fdm.h:59
Definition: nps_fdm.h:44
void * ivy_main_loop(void *data)
Definition: nps_ivy.c:51
static pthread_mutex_t ivy_mutex
Definition: nps_ivy.c:34
pthread_t th_ivy_main
Definition: nps_ivy.c:30
static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *argv[])
Definition: nps_ivy.c:176
void nps_ivy_send_WORLD_ENV_REQ(void)
Definition: nps_ivy.c:127
static void on_WORLD_ENV(IvyClientPtr app, void *user_data, int argc, char *argv[])
Definition: nps_ivy.c:94
bool nps_ivy_send_world_env
Definition: nps_ivy.c:29
static MsgRcvPtr ivyPtr
Definition: nps_ivy.c:31
static int seq
Definition: nps_ivy.c:32
static int ap_launch_index
Definition: nps_ivy.c:33
void nps_ivy_display(struct NpsFdm *fdm_data, struct NpsSensors *sensors_data)
Definition: nps_ivy.c:213
void nps_ivy_init(char *ivy_bus)
Definition: nps_ivy.c:58
int find_launch_index(void)
Definition: nps_ivy.c:161
void nps_update_launch_from_dl(uint8_t value)
Definition: nps_main_hitl.c:93
void nps_set_time_factor(float time_factor)
pthread_mutex_t fdm_mutex
static uint32_t idx
struct DoubleVect3 neutral
struct DoubleVect3 value
struct DoubleVect3 bias_random_walk_value
struct DoubleVect3 bias_initial
struct NpsSensorGyro gyro
Definition: nps_sensors.h:24
struct NpsSensorAccel accel
Definition: nps_sensors.h:25
Paparazzi double precision floating point algebra.
Paparazzi floating point algebra.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98