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