Paparazzi UAS  v5.18.0_stable
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 "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  if (!datalink_enabled) {
186  DlSetting(index, value);
187  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value);
188  }
189  printf("setting %d %f\n", index, value);
190 
191  /*
192  * In case of HITL, update nps_autopilot.launch from DL_SETTINGS
193  * so the plane can be properly launched.
194  *
195  * In case of STIL nps_update_launch_from_dl() is an empty function
196  */
197  if ((ap_launch_index >= 0) || (ap_launch_index < NB_SETTING)) {
198  if (index==ap_launch_index){
200  }
201  }
202 }
203 
204 
205 void nps_ivy_display(struct NpsFdm* fdm_data, struct NpsSensors* sensors_data)
206 {
207  struct NpsFdm fdm_ivy;
208  memcpy (&fdm_ivy, fdm_data, sizeof(struct NpsFdm));
209 
210  struct NpsSensors sensors_ivy;
211  memcpy (&sensors_ivy, sensors_data, sizeof(struct NpsSensors));
212 
213  IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
214  AC_ID,
215  DegOfRad(fdm_ivy.body_ecef_rotvel.p),
216  DegOfRad(fdm_ivy.body_ecef_rotvel.q),
217  DegOfRad(fdm_ivy.body_ecef_rotvel.r),
218  DegOfRad(fdm_ivy.ltp_to_body_eulers.phi),
219  DegOfRad(fdm_ivy.ltp_to_body_eulers.theta),
220  DegOfRad(fdm_ivy.ltp_to_body_eulers.psi));
221  IvySendMsg("%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f",
222  AC_ID,
223  (fdm_ivy.lla_pos_pprz.lat),
224  (fdm_ivy.lla_pos_geod.lat),
225  (fdm_ivy.lla_pos_geoc.lat),
226  (fdm_ivy.lla_pos_pprz.lon),
227  (fdm_ivy.lla_pos_geod.lon),
228  (fdm_ivy.lla_pos_pprz.alt),
229  (fdm_ivy.lla_pos_geod.alt),
230  (fdm_ivy.agl),
231  (fdm_ivy.hmsl));
232  IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f",
233  AC_ID,
234  (fdm_ivy.ltpprz_ecef_accel.x),
235  (fdm_ivy.ltpprz_ecef_accel.y),
236  (fdm_ivy.ltpprz_ecef_accel.z),
237  (fdm_ivy.ltpprz_ecef_vel.x),
238  (fdm_ivy.ltpprz_ecef_vel.y),
239  (fdm_ivy.ltpprz_ecef_vel.z),
240  (fdm_ivy.ltpprz_pos.x),
241  (fdm_ivy.ltpprz_pos.y),
242  (fdm_ivy.ltpprz_pos.z));
243  IvySendMsg("%d NPS_GYRO_BIAS %f %f %f",
244  AC_ID,
245  DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.x) + sensors_ivy.gyro.bias_initial.x),
246  DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.y) + sensors_ivy.gyro.bias_initial.y),
247  DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.z) + sensors_ivy.gyro.bias_initial.z));
248 
249  /* transform magnetic field to body frame */
250  struct DoubleVect3 h_body;
251  double_quat_vmult(&h_body, &fdm_ivy.ltp_to_body_quat, &fdm_ivy.ltp_h);
252 
253  IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
254  AC_ID,
255  ((sensors_ivy.accel.value.x - sensors_ivy.accel.neutral.x) / NPS_ACCEL_SENSITIVITY_XX),
256  ((sensors_ivy.accel.value.y - sensors_ivy.accel.neutral.y) / NPS_ACCEL_SENSITIVITY_YY),
257  ((sensors_ivy.accel.value.z - sensors_ivy.accel.neutral.z) / NPS_ACCEL_SENSITIVITY_ZZ),
258  h_body.x,
259  h_body.y,
260  h_body.z);
261 
262  IvySendMsg("%d NPS_WIND %f %f %f",
263  AC_ID,
264  fdm_ivy.wind.x,
265  fdm_ivy.wind.y,
266  fdm_ivy.wind.z);
267 
270  }
271 }
on_DL_SETTING
static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *argv[])
Definition: nps_ivy.c:170
seq
static int seq
Definition: nps_ivy.c:33
NedCoor_d::y
double y
in meters
Definition: pprz_geodetic_double.h:69
NpsFdm::ltp_h
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105
nps_ivy_send_world_env
bool nps_ivy_send_world_env
Definition: nps_ivy.h:7
NpsSensors::gyro
struct NpsSensorGyro gyro
Definition: nps_sensors.h:18
nps_ivy_display
void nps_ivy_display(struct NpsFdm *fdm_data, struct NpsSensors *sensors_data)
Definition: nps_ivy.c:205
DoubleVect3::z
double z
Definition: pprz_algebra_double.h:49
NpsFdm
Definition: nps_fdm.h:44
nps_ivy_init
void nps_ivy_init(char *ivy_bus)
Definition: nps_ivy.c:59
NpsSensors::accel
struct NpsSensorAccel accel
Definition: nps_sensors.h:19
LlaCoor_d::alt
double alt
in meters above WGS84 reference ellipsoid
Definition: pprz_geodetic_double.h:61
NpsFdm::ltpprz_ecef_vel
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
NedCoor_d::x
double x
in meters
Definition: pprz_geodetic_double.h:68
NpsFdm::wind
struct DoubleVect3 wind
velocity in m/s in NED
Definition: nps_fdm.h:107
DoubleEulers::phi
double phi
in radians
Definition: pprz_algebra_double.h:77
DoubleRates::q
double q
in rad/s^2
Definition: pprz_algebra_double.h:87
DoubleEulers::theta
double theta
in radians
Definition: pprz_algebra_double.h:78
NpsSensorAccel::neutral
struct DoubleVect3 neutral
Definition: nps_sensor_accel.h:14
DoubleRates::r
double r
in rad/s^2
Definition: pprz_algebra_double.h:88
DoubleEulers::psi
double psi
in radians
Definition: pprz_algebra_double.h:79
nps_atmosphere_set_wind_ned
void nps_atmosphere_set_wind_ned(double wind_north, double wind_east, double wind_down)
Definition: nps_atmosphere.c:83
gps_has_fix
bool gps_has_fix
Definition: gps_sim_nps.c:29
NpsFdm::lla_pos_pprz
struct LlaCoor_d lla_pos_pprz
Definition: nps_fdm.h:58
NpsFdm::ltpprz_ecef_accel
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:81
nps_fdm.h
pprz_algebra_float.h
Paparazzi floating point algebra.
NpsFdm::ltp_to_body_quat
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
nps_autopilot.h
nps_main.h
idx
static uint32_t idx
Definition: nps_radio_control_spektrum.c:105
NpsSensorGyro::bias_random_walk_value
struct DoubleVect3 bias_random_walk_value
Definition: nps_sensor_gyro.h:18
fdm
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
Definition: nps_fdm_crrcsim.c:84
NpsFdm::agl
double agl
Definition: nps_fdm.h:61
FloatVect3
Definition: pprz_algebra_float.h:54
NpsFdm::body_ecef_rotvel
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
gps.h
Device independent GPS code (interface)
DoubleVect3::x
double x
Definition: pprz_algebra_double.h:47
double_quat_vmult
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
Definition: pprz_algebra_double.c:90
NpsFdm::lla_pos_geoc
struct LlaCoor_d lla_pos_geoc
Definition: nps_fdm.h:60
nps_atmosphere.h
uint8_t
unsigned char uint8_t
Definition: types.h:14
NedCoor_d::z
double z
in meters
Definition: pprz_geodetic_double.h:70
find_launch_index
int find_launch_index(void)
Definition: nps_ivy.c:155
nps_sensors.h
LlaCoor_d::lat
double lat
in radians
Definition: pprz_geodetic_double.h:59
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
ivyPtr
static MsgRcvPtr ivyPtr
Definition: nps_ivy.c:32
nps_set_time_factor
void nps_set_time_factor(float time_factor)
Definition: nps_main_common.c:119
pprz_algebra_double.h
Paparazzi double precision floating point algebra.
NpsFdm::hmsl
double hmsl
Definition: nps_fdm.h:56
NpsFdm::ltp_to_body_eulers
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
NpsFdm::ltpprz_pos
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
nps_ivy_send_WORLD_ENV_REQ
void nps_ivy_send_WORLD_ENV_REQ(void)
Definition: nps_ivy.c:125
nps_update_launch_from_dl
void nps_update_launch_from_dl(uint8_t value)
Definition: nps_main_hitl.c:93
on_WORLD_ENV
static void on_WORLD_ENV(IvyClientPtr app, void *user_data, int argc, char *argv[])
Definition: nps_ivy.c:95
ap_launch_index
static int ap_launch_index
Definition: nps_ivy.c:34
ivy_main_loop
void * ivy_main_loop(void *data)
Definition: nps_ivy.c:52
NpsSensorAccel::value
struct DoubleVect3 value
Definition: nps_sensor_accel.h:10
NpsSensorGyro::bias_initial
struct DoubleVect3 bias_initial
Definition: nps_sensor_gyro.h:16
DoubleVect3
Definition: pprz_algebra_double.h:46
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
DoubleVect3::y
double y
Definition: pprz_algebra_double.h:48
NpsFdm::lla_pos_geod
struct LlaCoor_d lla_pos_geod
Definition: nps_fdm.h:59
LlaCoor_d::lon
double lon
in radians
Definition: pprz_geodetic_double.h:60
RATE_FLOAT_OF_BFP
#define RATE_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:209
nps_ivy.h
th_ivy_main
pthread_t th_ivy_main
Definition: nps_ivy.c:31
DoubleRates::p
double p
in rad/s^2
Definition: pprz_algebra_double.h:86
NpsSensors
Definition: nps_sensors.h:16