Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
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 #include <Ivy/ivyglibloop.h>
9 
10 #include "generated/airframe.h"
13 #include "nps_main.h"
14 #include "nps_autopilot.h"
15 #include "nps_fdm.h"
16 #include "nps_sensors.h"
17 #include "nps_atmosphere.h"
18 
19 //#include "subsystems/navigation/common_flight_plan.h"
20 
21 #if USE_GPS
22 #include "subsystems/gps.h"
23 #endif
24 
25 #include NPS_SENSORS_PARAMS
26 
27 /* Gaia Ivy functions */
28 static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
29  void *user_data __attribute__((unused)),
30  int argc __attribute__((unused)), char *argv[]);
31 
32 /* Datalink Ivy functions */
33 static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
34  void *user_data __attribute__((unused)),
35  int argc __attribute__((unused)), char *argv[]);
36 
37 void nps_ivy_init(char *ivy_bus)
38 {
39  const char *agent_name = AIRFRAME_NAME"_NPS";
40  const char *ready_msg = AIRFRAME_NAME"_NPS Ready";
41  IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL);
42 
43  // bind on a general WORLD_ENV (not a reply to request)
44  IvyBindMsg(on_WORLD_ENV, NULL, "^(\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
45 
46  // to be able to change datalink_enabled setting back on
47  IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
48 
49 #ifdef __APPLE__
50  const char *default_ivy_bus = "224.255.255.255";
51 #else
52  const char *default_ivy_bus = "127.255.255.255";
53 #endif
54  if (ivy_bus == NULL) {
55  IvyStart(default_ivy_bus);
56  } else {
57  IvyStart(ivy_bus);
58  }
59 }
60 
61 /*
62  * Parse WORLD_ENV message from gaia.
63  *
64  */
65 static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
66  void *user_data __attribute__((unused)),
67  int argc __attribute__((unused)), char *argv[])
68 {
69  // wind speed in m/s
70  struct FloatVect3 wind;
71  wind.x = atof(argv[1]); //east
72  wind.y = atof(argv[2]); //north
73  wind.z = atof(argv[3]); //up
74 
75  /* set wind speed in NED */
76  nps_atmosphere_set_wind_ned(wind.y, wind.x, -wind.z);
77 
78  /* not used so far */
79  //float ir_contrast = atof(argv[4]);
80 
81  /* set new time factor */
82  nps_set_time_factor(atof(argv[5]));
83 
84 #if USE_GPS
85  // directly set gps fix in subsystems/gps/gps_sim_nps.h
86  gps_has_fix = atoi(argv[6]); // gps_availability
87 #endif
88 }
89 
90 /*
91  * Send a WORLD_ENV_REQ message
92  */
93 static MsgRcvPtr ivyPtr = NULL;
94 static int seq = 1;
95 
97 {
98  // First unbind from previous request if needed
99  if (ivyPtr != NULL) {
100  IvyUnbindMsg(ivyPtr);
101  ivyPtr = NULL;
102  }
103 
104  int pid = (int)getpid();
105  // Bind to the reply
106  ivyPtr = IvyBindMsg(on_WORLD_ENV, NULL, "^%d_%d (\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", pid, seq);
107  // Send actual request
108  IvySendMsg("nps %d_%d WORLD_ENV_REQ %f %f %f %f %f %f",
109  pid, seq,
110  DegOfRad(fdm.lla_pos_pprz.lat),
111  DegOfRad(fdm.lla_pos_pprz.lon),
112  (fdm.hmsl),
113  (fdm.ltpprz_pos.x),
114  (fdm.ltpprz_pos.y),
115  (fdm.ltpprz_pos.z));
116  seq++;
117 }
118 
119 
120 #include "generated/settings.h"
121 #include "pprzlink/dl_protocol.h"
123 static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
124  void *user_data __attribute__((unused)),
125  int argc __attribute__((unused)), char *argv[])
126 {
127  if (atoi(argv[1]) != AC_ID) {
128  return;
129  }
130 
131  /* HACK:
132  * we actually don't want to allow changing settings if datalink is disabled,
133  * but since we currently change this variable via settings we have to allow it
134  * TODO: only allow changing the datalink_enabled setting
135  */
136 
137  uint8_t index = atoi(argv[2]);
138  float value = atof(argv[3]);
139  DlSetting(index, value);
140  DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value);
141  printf("setting %d %f\n", index, value);
142 }
143 
144 void nps_ivy_display(void)
145 {
146  IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
147  AC_ID,
148  DegOfRad(fdm.body_ecef_rotvel.p),
149  DegOfRad(fdm.body_ecef_rotvel.q),
150  DegOfRad(fdm.body_ecef_rotvel.r),
151  DegOfRad(fdm.ltp_to_body_eulers.phi),
152  DegOfRad(fdm.ltp_to_body_eulers.theta),
153  DegOfRad(fdm.ltp_to_body_eulers.psi));
154  IvySendMsg("%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f",
155  AC_ID,
156  (fdm.lla_pos_pprz.lat),
157  (fdm.lla_pos_geod.lat),
158  (fdm.lla_pos_geoc.lat),
159  (fdm.lla_pos_pprz.lon),
160  (fdm.lla_pos_geod.lon),
161  (fdm.lla_pos_pprz.alt),
162  (fdm.lla_pos_geod.alt),
163  (fdm.agl),
164  (fdm.hmsl));
165  IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f",
166  AC_ID,
173  (fdm.ltpprz_pos.x),
174  (fdm.ltpprz_pos.y),
175  (fdm.ltpprz_pos.z));
176  IvySendMsg("%d NPS_GYRO_BIAS %f %f %f",
177  AC_ID,
181 
182  /* transform magnetic field to body frame */
183  struct DoubleVect3 h_body;
185 
186  IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
187  AC_ID,
188  ((sensors.accel.value.x - sensors.accel.neutral.x) / NPS_ACCEL_SENSITIVITY_XX),
189  ((sensors.accel.value.y - sensors.accel.neutral.y) / NPS_ACCEL_SENSITIVITY_YY),
190  ((sensors.accel.value.z - sensors.accel.neutral.z) / NPS_ACCEL_SENSITIVITY_ZZ),
191  h_body.x,
192  h_body.y,
193  h_body.z);
194 
195  IvySendMsg("%d NPS_WIND %f %f %f",
196  AC_ID,
197  fdm.wind.x,
198  fdm.wind.y,
199  fdm.wind.z);
200 }
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:17
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:81
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:37
double phi
in radians
static int seq
Definition: nps_ivy.c:94
double psi
in radians
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
double q
in rad/s^2
void nps_ivy_display(void)
Definition: nps_ivy.c:144
struct NpsSensorGyro gyro
Definition: nps_sensors.h:16
double theta
in radians
struct LlaCoor_d lla_pos_pprz
Definition: nps_fdm.h:58
static MsgRcvPtr ivyPtr
Definition: nps_ivy.c:93
double r
in rad/s^2
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
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.
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:96
struct LlaCoor_d lla_pos_geod
Definition: nps_fdm.h:59
struct NpsSensors sensors
Definition: nps_sensors.c:6
#define RATE_FLOAT_OF_BFP(_ai)
void nps_set_time_factor(float time_factor)
Definition: nps_main.c:192
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
static void on_WORLD_ENV(IvyClientPtr app, void *user_data, int argc, char *argv[])
Definition: nps_ivy.c:65
Paparazzi double precision floating point algebra.
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
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:123
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105