Paparazzi UAS  v6.2_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 
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 static pthread_mutex_t ivy_mutex; // mutex for ivy send
36 
37 /* Gaia Ivy functions */
38 static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
39  void *user_data __attribute__((unused)),
40  int argc, 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, 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, char *argv[])
98 {
99  if (argc < 6) { return; }
100 
101  // wind speed in m/s
102  struct FloatVect3 wind;
103  wind.x = atof(argv[1]); //east
104  wind.y = atof(argv[2]); //north
105  wind.z = atof(argv[3]); //up
106 
107  /* set wind speed in NED */
108  nps_atmosphere_set_wind_ned(wind.y, wind.x, -wind.z);
109 
110  /* not used so far */
111  //float ir_contrast = atof(argv[4]);
112 
113  /* set new time factor */
114  nps_set_time_factor(atof(argv[5]));
115 
116 #if USE_GPS
117  // directly set gps fix in modules/gps/gps_sim_nps.h
118  gps_has_fix = atoi(argv[6]); // gps_availability
119 #endif
120 
121 }
122 
123 /*
124  * Send a WORLD_ENV_REQ message
125  */
126 
127 
129 {
130  pthread_mutex_lock(&ivy_mutex);
131 
132  // First unbind from previous request if needed
133  if (ivyPtr != NULL) {
134  IvyUnbindMsg(ivyPtr);
135  ivyPtr = NULL;
136  }
137 
138  int pid = (int)getpid();
139 
140  // Bind to the reply
141  ivyPtr = IvyBindMsg(on_WORLD_ENV, NULL, "^%d_%d (\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", pid, seq);
142 
143  // Send actual request
144  struct NpsFdm fdm_ivy;
145  memcpy(&fdm_ivy, &fdm, sizeof(struct NpsFdm));
146 
147  IvySendMsg("nps %d_%d WORLD_ENV_REQ %f %f %f %f %f %f",
148  pid, seq,
149  DegOfRad(fdm_ivy.lla_pos_pprz.lat),
150  DegOfRad(fdm_ivy.lla_pos_pprz.lon),
151  (fdm_ivy.hmsl),
152  (fdm_ivy.ltpprz_pos.x),
153  (fdm_ivy.ltpprz_pos.y),
154  (fdm_ivy.ltpprz_pos.z));
155  seq++;
156 
157  nps_ivy_send_world_env = false;
158 
159  pthread_mutex_unlock(&ivy_mutex);
160 }
161 
163 {
164  static const char ap_launch[] = "aut_lau"; // short name
165  char *ap_settings[NB_SETTING] = SETTINGS_NAMES_SHORT;
166 
167  // list through the settings
168  // TODO: maybe search for a substring with if(strstr(sent, word) != NULL)
169  for (uint8_t idx=0;idx<NB_SETTING;idx++) {
170  if (strcmp(ap_settings[idx],ap_launch) == 0) {
171  return (int)idx;
172  }
173  }
174  return -1;
175 }
176 
177 static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
178  void *user_data __attribute__((unused)),
179  int argc, char *argv[])
180 {
181  if (argc < 3) { return; }
182 
183  if (atoi(argv[1]) != AC_ID) {
184  return;
185  }
186 
187  /* HACK:
188  * we actually don't want to allow changing settings if datalink is disabled,
189  * but since we currently change this variable via settings we have to allow it
190  * TODO: only allow changing the datalink_enabled setting
191  */
192  uint8_t index = atoi(argv[2]);
193  float value = atof(argv[3]);
194  if (!datalink_enabled) {
195  DlSetting(index, value);
196  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value);
197  }
198  printf("setting %d %f\n", index, value);
199 
200  /*
201  * In case of HITL, update nps_autopilot.launch from DL_SETTINGS
202  * so the plane can be properly launched.
203  *
204  * In case of STIL nps_update_launch_from_dl() is an empty function
205  */
206  if ((ap_launch_index >= 0) || (ap_launch_index < NB_SETTING)) {
207  if (index==ap_launch_index){
209  }
210  }
211 }
212 
213 
214 void nps_ivy_display(struct NpsFdm* fdm_data, struct NpsSensors* sensors_data)
215 {
216  struct NpsFdm fdm_ivy;
217  struct NpsSensors sensors_ivy;
218 
219  // make a local copy with mutex
220  pthread_mutex_lock(&fdm_mutex);
221  memcpy(&fdm_ivy, fdm_data, sizeof(fdm));
222  memcpy(&sensors_ivy, sensors_data, sizeof(sensors));
223  pthread_mutex_unlock(&fdm_mutex);
224 
225  // protect Ivy thread
226  pthread_mutex_lock(&ivy_mutex);
227 
228  IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
229  AC_ID,
230  DegOfRad(fdm_ivy.body_ecef_rotvel.p),
231  DegOfRad(fdm_ivy.body_ecef_rotvel.q),
232  DegOfRad(fdm_ivy.body_ecef_rotvel.r),
233  DegOfRad(fdm_ivy.ltp_to_body_eulers.phi),
234  DegOfRad(fdm_ivy.ltp_to_body_eulers.theta),
235  DegOfRad(fdm_ivy.ltp_to_body_eulers.psi));
236  IvySendMsg("%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f",
237  AC_ID,
238  (fdm_ivy.lla_pos_pprz.lat),
239  (fdm_ivy.lla_pos_geod.lat),
240  (fdm_ivy.lla_pos_geoc.lat),
241  (fdm_ivy.lla_pos_pprz.lon),
242  (fdm_ivy.lla_pos_geod.lon),
243  (fdm_ivy.lla_pos_pprz.alt),
244  (fdm_ivy.lla_pos_geod.alt),
245  (fdm_ivy.agl),
246  (fdm_ivy.hmsl));
247  IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f",
248  AC_ID,
249  (fdm_ivy.ltpprz_ecef_accel.x),
250  (fdm_ivy.ltpprz_ecef_accel.y),
251  (fdm_ivy.ltpprz_ecef_accel.z),
252  (fdm_ivy.ltpprz_ecef_vel.x),
253  (fdm_ivy.ltpprz_ecef_vel.y),
254  (fdm_ivy.ltpprz_ecef_vel.z),
255  (fdm_ivy.ltpprz_pos.x),
256  (fdm_ivy.ltpprz_pos.y),
257  (fdm_ivy.ltpprz_pos.z));
258  IvySendMsg("%d NPS_GYRO_BIAS %f %f %f",
259  AC_ID,
260  DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.x) + sensors_ivy.gyro.bias_initial.x),
261  DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.y) + sensors_ivy.gyro.bias_initial.y),
262  DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.z) + sensors_ivy.gyro.bias_initial.z));
263 
264  /* transform magnetic field to body frame */
265  struct DoubleVect3 h_body;
266  double_quat_vmult(&h_body, &fdm_ivy.ltp_to_body_quat, &fdm_ivy.ltp_h);
267 
268  IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
269  AC_ID,
270  ((sensors_ivy.accel.value.x - sensors_ivy.accel.neutral.x) / NPS_ACCEL_SENSITIVITY_XX),
271  ((sensors_ivy.accel.value.y - sensors_ivy.accel.neutral.y) / NPS_ACCEL_SENSITIVITY_YY),
272  ((sensors_ivy.accel.value.z - sensors_ivy.accel.neutral.z) / NPS_ACCEL_SENSITIVITY_ZZ),
273  h_body.x,
274  h_body.y,
275  h_body.z);
276 
277  IvySendMsg("%d NPS_WIND %f %f %f",
278  AC_ID,
279  fdm_ivy.wind.x,
280  fdm_ivy.wind.y,
281  fdm_ivy.wind.z);
282 
283  pthread_mutex_unlock(&ivy_mutex);
284 
287  }
288 }
on_DL_SETTING
static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *argv[])
Definition: nps_ivy.c:177
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
fdm_mutex
pthread_mutex_t fdm_mutex
Definition: nps_main.h:28
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
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
ivy_mutex
static pthread_mutex_t ivy_mutex
Definition: nps_ivy.c:35
nps_ivy_display
void nps_ivy_display(struct NpsFdm *fdm_data, struct NpsSensors *sensors_data)
Definition: nps_ivy.c:214
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
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:162
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:128
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:210
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
sensors
#define sensors(...)
Definition: cc2500_compat.h:68