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_fdm_crrcsim.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Gautier Hattenberger
3  *
4  * Mixed with modified version of MNAV
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 
31 #include "nps_fdm.h"
32 
33 #include <stdlib.h>
34 #include <stdio.h>
35 #include <netinet/in.h>
36 #include <arpa/inet.h>
37 #include <fcntl.h>
38 #include <sys/socket.h>
39 #include <sys/types.h>
40 #include <unistd.h>
41 
42 #include "math/pprz_geodetic.h"
45 #include "math/pprz_algebra.h"
47 #include "math/pprz_isa.h"
48 
49 #include "generated/airframe.h"
50 #include "generated/flight_plan.h"
51 
52 #ifndef NPS_CRRCSIM_HOST_IP
53 #define NPS_CRRCSIM_HOST_IP "127.0.0.1"
54 #endif
55 
56 #ifndef NPS_CRRCSIM_HOST_PORT
57 #define NPS_CRRCSIM_HOST_PORT 9002
58 #endif
59 
60 #ifndef NPS_CRRCSIM_ROLL_NEUTRAL
61 #define NPS_CRRCSIM_ROLL_NEUTRAL 0.
62 #endif
63 
64 #ifndef NPS_CRRCSIM_PITCH_NEUTRAL
65 #define NPS_CRRCSIM_PITCH_NEUTRAL 0.
66 #endif
67 
68 /* blocking */
69 #define UDP_BLOCKING 0
70 #define UDP_NONBLOCKING 1
71 
72 // type
73 #define word unsigned short
74 #define byte unsigned char
75 
76 // uNAV packet length definition
77 #define IMU_PACKET_LENGTH 51
78 #define GPS_PACKET_LENGTH 86
79 #define AHRS_PACKET_LENGTH 93
80 #define FULL_PACKET_SIZE 93
81 
82 
83 // NpsFdm structure
84 struct NpsFdm fdm;
85 
86 #define INPUT_BUFFER_SIZE (3*FULL_PACKET_SIZE)
87 // Input buffer
88 struct inputbuf {
91  int length;
92 };
93 
94 // Socket structure
95 struct _crrcsim {
96  int socket;
97  struct sockaddr_in addr;
98  struct inputbuf buf;
100 };
101 
102 static struct _crrcsim crrcsim;
103 
104 // Reference point
105 static struct LtpDef_d ltpdef;
106 
107 // static functions declaration
108 static void open_udp(char *host, int port, int blocking);
109 static void inputbuf_init(struct inputbuf *c);
110 static void read_into_buffer(struct _crrcsim *io);
111 static void init_ltp(void);
112 static int get_msg(struct _crrcsim *io, byte *data_buffer);
113 static void decode_imupacket(struct NpsFdm *fdm, byte *buffer);
114 static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer);
115 static void decode_ahrspacket(struct NpsFdm *fdm, byte *buffer);
116 static void send_servo_cmd(struct _crrcsim *io, double *commands);
117 
118 // NPS FDM interface
119 void nps_fdm_init(double dt)
120 {
121  fdm.init_dt = dt;
122  fdm.curr_dt = dt;
123  fdm.nan_count = 0;
124  fdm.pressure = -1;
126  fdm.total_pressure = -1;
127  fdm.dynamic_pressure = -1;
128  fdm.temperature = -1;
129 
130  init_ltp();
131 
132  // init circfuf
134 
135  printf("Starting to connect to CRRCsim server.\n");
137 
138  if (crrcsim.socket < 0) {
139  printf("Connection to CRRCsim failed\n");
140  exit(0);
141  } else {
142  printf("Connection to CRRCsim succed\n");
143  }
144 
145  // Send something to let crrcsim that we are here
146  double zero[] = { 0., 0., 0. };
147  send_servo_cmd(&crrcsim, zero);
148 }
149 
150 void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb)
151 {
152  // read state
153  if (get_msg(&crrcsim, crrcsim.data_buffer) <= 0) {
154  return; // nothing on the socket
155  }
156 
157  // send commands
158  send_servo_cmd(&crrcsim, commands);
159 
160  //printf("new data %c %c\n", crrcsim.data_buffer[2], crrcsim.data_buffer[33]);
161  switch (crrcsim.data_buffer[2]) {
162  case 'S': /* IMU packet without GPS */
164  break;
165 
166  case 'N': /* IMU packet with GPS */
167 
169 
170  /******************************************
171  *check GPS data packet
172  ******************************************/
173  //if(data_buffer[31]==(byte)0x55 && data_buffer[32]==(byte)0x55 && data_buffer[33]=='G')
174  if (crrcsim.data_buffer[33] == 'G') {
176  }
177  break;
178 
179  case 'I': /* IMU packet with GPS and AHRS */
180 
182 
183  /******************************************
184  *check GPS data packet
185  ******************************************/
186  if (crrcsim.data_buffer[33] == 'G') {
188  }
189  if (crrcsim.data_buffer[66] == 'A') {
191  }
192  break;
193 
194  default :
195  printf("invalid data packet...!\n");
196  } /* end case */
197 
198 }
199 
200 void nps_fdm_set_wind(double speed __attribute__((unused)),
201  double dir __attribute__((unused)))
202 {
203 }
204 
205 void nps_fdm_set_wind_ned(double wind_north __attribute__((unused)),
206  double wind_east __attribute__((unused)),
207  double wind_down __attribute__((unused)))
208 {
209 }
210 
211 void nps_fdm_set_turbulence(double wind_speed __attribute__((unused)),
212  int turbulence_severity __attribute__((unused)))
213 {
214 }
215 
216 /***************************************************************************
217  ** Open and configure UDP connection
218  ****************************************************************************/
219 
220 static void open_udp(char *host, int port, int blocking)
221 {
222  int flags;
223 
224  bzero((char *) &crrcsim.addr, sizeof(crrcsim.addr));
225  crrcsim.addr.sin_family = AF_INET;
226  crrcsim.addr.sin_addr.s_addr = inet_addr(host);
227  crrcsim.addr.sin_port = htons(port);
228  crrcsim.socket = socket(AF_INET, SOCK_DGRAM, 0);
229 
230  //make a nonblocking connection
231  flags = fcntl(crrcsim.socket, F_GETFL, 0);
232  fcntl(crrcsim.socket, F_SETFL, flags | O_NONBLOCK);
233 
234  if (connect(crrcsim.socket, (struct sockaddr *)&crrcsim.addr, sizeof(crrcsim.addr)) < 0) {
235  close(crrcsim.socket);
236  crrcsim.socket = -1;
237  }
238 
239  if (crrcsim.socket != -1 && blocking == UDP_BLOCKING) {
240  //restore
241  fcntl(crrcsim.socket, F_SETFL, flags);
242  }
243 }
244 
245 static void inputbuf_init(struct inputbuf *c)
246 {
247  c->start = 0;
248  c->length = 0;
249 }
250 
251 static void read_into_buffer(struct _crrcsim *io)
252 {
253  struct inputbuf *c = &io->buf;
254  int res;
255 
256  if (io->socket >= 0) {
257  // get latest data from the buffer (crapy but working)
258  while ((res = recv(io->socket, c->buf, INPUT_BUFFER_SIZE, 0)) > 0) {
259  c->start = 0;
260  c->length = res;
261  }
262  }
263 }
264 
265 static void init_ltp(void)
266 {
267 
268  struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
269  llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
270  llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
271  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
272  llh_nav0.alt = (NAV_ALT0 + NAV_MSL0) / 1000.;
273 
274  struct EcefCoor_d ecef_nav0;
275  ecef_of_lla_d(&ecef_nav0, &llh_nav0);
276 
277  ltp_def_from_ecef_d(&ltpdef, &ecef_nav0);
278 
279  fdm.ltp_g.x = 0.;
280  fdm.ltp_g.y = 0.;
281  fdm.ltp_g.z = 0.; // accel data are already with the correct format
282 
283 #ifdef AHRS_H_X
284 #pragma message "Using magnetic field as defined in airframe file."
285  fdm.ltp_h.x = AHRS_H_X;
286  fdm.ltp_h.y = AHRS_H_Y;
287  fdm.ltp_h.z = AHRS_H_Z;
288 #else
289  fdm.ltp_h.x = 0.4912;
290  fdm.ltp_h.y = 0.1225;
291  fdm.ltp_h.z = 0.8624;
292 #endif
293 
294 }
295 
296 static int get_msg(struct _crrcsim *io, byte *data_buffer)
297 {
298  struct inputbuf *c = &io->buf;
299  int count = 0;
300  int packet_len;
301  int i;
302 
303  read_into_buffer(io);
304 
305  while (1) {
306  /*********************************************************************
307  * Find start of packet: the header (2 bytes) starts with 0x5555
308  *********************************************************************/
309  while (c->length >= 4 && (c->buf[c->start] != (byte)0x55 || c->buf[(byte)(c->start + 1)] != (byte)0x55)) {
310  c->start++;
311  c->length--;
312  }
313  if (c->length < 4) {
314  return count;
315  }
316 
317  /*********************************************************************
318  * Read packet contents
319  *********************************************************************/
320  packet_len = 0;
321  switch (c->buf[(byte)(c->start + 2)]) {
322  case 'S':
323  packet_len = IMU_PACKET_LENGTH;
324  break;
325 
326  case 'N':
327  packet_len = GPS_PACKET_LENGTH;
328  break;
329 
330  case 'I':
331  packet_len = AHRS_PACKET_LENGTH;
332  break;
333 
334  default:
335  break;
336  }
337 
338  if (packet_len > 0 && c->length < packet_len) {
339  return count; // not enough data
340  }
341  if (packet_len > 0) {
342  byte ib;
343  word rcvchecksum = 0;
344  word sum = 0;
345 
346  for (i = 2, ib = c->start + (byte)2; i < packet_len - 2; i++, ib++) {
347  sum += c->buf[ib];
348  }
349  rcvchecksum = c->buf[ib++] << 8;
350  rcvchecksum = rcvchecksum | c->buf[ib++];
351 
352  if (rcvchecksum != sum) {
353  packet_len = 0;
354  printf("checksum error\n");
355  }
356  }
357  // fill data buffer or go to next bytes
358  if (packet_len > 0) {
359  for (i = 0; i < packet_len; i++) {
360  data_buffer[i] = c->buf[c->start];
361  c->start++;
362  c->length--;
363  }
364  count++;
365  } else {
366  c->start += 3;
367  c->length -= 3;
368  }
369  }
370 
371  return count;
372 }
373 
374 // Long (32bits) in buffer are little endian in gps message
375 #define LongOfBuf(_buf,_idx) (int32_t)(((uint32_t)_buf[_idx+3]<<24)|((uint32_t)_buf[_idx+2]<<16)|((uint32_t)_buf[_idx+1]<<8)|((uint32_t)_buf[_idx]))
376 // Unsigned short (16bits) in buffer are little endian in gps message
377 #define UShortOfBuf(_buf,_idx) (uint16_t)(((uint16_t)_buf[_idx+1]<<8)|((uint16_t)_buf[_idx]))
378 // Short (16bits) in buffer are big endian in other messages
379 #define ShortOfBuf(_buf,_idx) (int16_t)(((uint16_t)_buf[_idx]<<8)|((uint16_t)_buf[_idx+1]))
380 
381 /***************************************************************************************
382  *decode the gps data packet
383  ***************************************************************************************/
384 static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer)
385 {
386  /* gps velocity (1e2 m/s to m/s */
387  struct NedCoor_d vel;
388  vel.x = (double)LongOfBuf(buffer, 3) * 1.0e-2;
389  vel.y = (double)LongOfBuf(buffer, 7) * 1.0e-2;
390  vel.z = (double)LongOfBuf(buffer, 11) * 1.0e-2;
391  fdm->ltp_ecef_vel = vel;
392  ecef_of_ned_vect_d(&fdm->ecef_ecef_vel, &ltpdef, &vel);
393 
394  /* No airspeed from CRRCSIM?
395  * use ground speed for now, since we also don't know wind
396  */
397  struct DoubleVect3 ltp_airspeed;
398  VECT3_COPY(ltp_airspeed, vel);
399  fdm.airspeed = double_vect3_norm(&ltp_airspeed);
400 
401  /* gps position (1e7 deg to rad and 1e3 m to m) */
402  struct LlaCoor_d pos;
403  pos.lon = (double)LongOfBuf(buffer, 15) * 1.74533e-9;
404  pos.lat = (double)LongOfBuf(buffer, 19) * 1.74533e-9;
405  pos.alt = (double)LongOfBuf(buffer, 23) * 1.0e-3;
406 
407  pos.lat += ltpdef.lla.lat;
408  pos.lon += ltpdef.lla.lon;
409  pos.alt += ltpdef.lla.alt;
410 
411  fdm->lla_pos = pos;
412  ecef_of_lla_d(&fdm->ecef_pos, &pos);
413  fdm->hmsl = pos.alt - NAV_MSL0 / 1000.;
414 
416 
417  /* gps time */
418  fdm->time = (double)UShortOfBuf(buffer, 27);
419 
420  /* in LTP pprz */
422  fdm->lla_pos_pprz = pos;
424 
425 #if NPS_CRRCSIM_DEBUG
426  printf("decode gps | pos %f %f %f | vel %f %f %f | time %f\n",
427  57.3 * fdm->lla_pos.lat,
428  57.3 * fdm->lla_pos.lon,
429  fdm->lla_pos.alt,
430  fdm->ltp_ecef_vel.x,
431  fdm->ltp_ecef_vel.y,
432  fdm->ltp_ecef_vel.z,
433  fdm->time);
434 #endif
435 }
436 
437 /***************************************************************************************
438  *decode the ahrs data packet
439  ***************************************************************************************/
440 static void decode_ahrspacket(struct NpsFdm *fdm, byte *buffer)
441 {
442  /* euler angles (0.9387340515702713e04 rad to rad) */
443  fdm->ltp_to_body_eulers.phi = (double)ShortOfBuf(buffer, 1) * 0.000106526 - NPS_CRRCSIM_ROLL_NEUTRAL;
444  fdm->ltp_to_body_eulers.theta = (double)ShortOfBuf(buffer, 3) * 0.000106526 - NPS_CRRCSIM_PITCH_NEUTRAL;
445  fdm->ltp_to_body_eulers.psi = (double)ShortOfBuf(buffer, 5) * 0.000106526;
447 
448 #if NPS_CRRCSIM_DEBUG
449  printf("decode ahrs %f %f %f\n",
450  fdm->ltp_to_body_eulers.phi * 57.3,
451  fdm->ltp_to_body_eulers.theta * 57.3,
452  fdm->ltp_to_body_eulers.psi * 57.3);
453 #endif
454 }
455 
456 /***************************************************************************************
457  *decode the imu data packet
458  ***************************************************************************************/
459 void decode_imupacket(struct NpsFdm *fdm, byte *buffer)
460 {
461  /* acceleration (0.1670132517315938e04 m/s^2 to m/s^2) */
462  fdm->body_accel.x = (double)ShortOfBuf(buffer, 3) * 5.98755e-04;
463  fdm->body_accel.y = (double)ShortOfBuf(buffer, 5) * 5.98755e-04;
464  fdm->body_accel.z = (double)ShortOfBuf(buffer, 7) * 5.98755e-04;
465 
466  /* since we don't get acceleration in ecef frame, use ECI for now */
467  fdm->body_ecef_accel.x = fdm->body_accel.x;
468  fdm->body_ecef_accel.y = fdm->body_accel.y;
469  fdm->body_ecef_accel.z = fdm->body_accel.z;
470 
471 
472  /* angular rate (0.9387340515702713e4 rad/s to rad/s) */
473  fdm->body_inertial_rotvel.p = (double)ShortOfBuf(buffer, 9) * 1.06526e-04;
474  fdm->body_inertial_rotvel.q = (double)ShortOfBuf(buffer, 11) * 1.06526e-04;
475  fdm->body_inertial_rotvel.r = (double)ShortOfBuf(buffer, 13) * 1.06526e-04;
476 
477  /* since we don't get angular velocity in ECEF frame, use the rotvel in ECI frame for now */
481 
482  /* magnetic field in Gauss */
483  //fdm->mag.x = (double)ShortOfBuf(buffer,15)*6.10352e-05;
484  //fdm->mag.y = (double)ShortOfBuf(buffer,17)*6.10352e-05;
485  //fdm->mag.z = (double)ShortOfBuf(buffer,19)*6.10352e-05;
486 
487  /* pressure in m and m/s */
488  //data->Ps = (double)ShortOfBuf(buffer,27)*3.05176e-01;
489  //data->Pt = (double)ShortOfBuf(buffer,29)*2.44141e-03;
490 
491 #if NPS_CRRCSIM_DEBUG
492  printf("decode imu | accel %f %f %f | gyro %f %f %f\n",
493  fdm->body_accel.x,
494  fdm->body_accel.y,
495  fdm->body_accel.z,
496  fdm->body_inertial_rotvel.p,
497  fdm->body_inertial_rotvel.q,
498  fdm->body_inertial_rotvel.r);
499 #endif
500 }
501 
502 // compatibility with OSX
503 #ifdef __APPLE__
504 #define MSG_NOSIGNAL SO_NOSIGPIPE
505 #endif
506 
507 /***************************************************************************************
508  * send servo command over udp
509  ***************************************************************************************/
510 static void send_servo_cmd(struct _crrcsim *io, double *commands)
511 {
512  //cnt_cmd[1] = ch1:elevator, cnt_cmd[0] = ch0:aileron, cnt_cmd[2] = ch2:throttle
513  word cnt_cmd[3];
514  byte data[24] = {0,};
515  short i = 0;
516  word sum = 0;
517 
518  word roll = (word)((65535 / 4) * commands[NPS_CRRCSIM_COMMAND_ROLL] + (65536 / 2));
519  word pitch = (word)((65535 / 4) * commands[NPS_CRRCSIM_COMMAND_PITCH] + (65536 / 2));
520 
521  cnt_cmd[0] = roll;
522  cnt_cmd[1] = -pitch;
523  cnt_cmd[2] = (word)(65535 * commands[NPS_CRRCSIM_COMMAND_THROTTLE]);
524 
525 #if NPS_CRRCSIM_DEBUG
526  printf("send servo %f %f %f | %d %d %d | %d %d\n",
527  commands[0],
528  commands[1],
529  commands[2],
530  cnt_cmd[0],
531  cnt_cmd[1],
532  cnt_cmd[2],
533  roll, pitch);
534 #endif
535 
536  data[0] = 0x55;
537  data[1] = 0x55;
538  data[2] = 0x53;
539  data[3] = 0x53;
540 
541  //aileron ch#0,elevator ch#1,throttle ch#2
542  //aileron
543  data[4] = (byte)(cnt_cmd[0] >> 8);
544  data[5] = (byte)cnt_cmd[0];
545  //elevator
546  data[6] = (byte)(cnt_cmd[1] >> 8);
547  data[7] = (byte)cnt_cmd[1];
548  //throttle
549  data[8] = (byte)(cnt_cmd[2] >> 8);
550  data[9] = (byte)cnt_cmd[2];
551 
552  //checksum
553  sum = 0xa6; //0x53+0x53
554  for (i = 4; i < 22; i++) { sum += data[i]; }
555 
556  data[22] = (byte)(sum >> 8);
557  data[23] = (byte)sum;
558 
559  //sendout the command packet
560  if (io->socket >= 0) {
561  send(io->socket, (char *)data, 24, MSG_NOSIGNAL);
562  }
563 }
564 
static int get_msg(struct _crrcsim *io, byte *data_buffer)
double total_pressure
total atmospheric pressure in Pascal
Definition: nps_fdm.h:111
#define word
double time
Definition: nps_fdm.h:46
vector in North East Down coordinates Units: meters
static float pprz_isa_pressure_of_altitude(float altitude)
Get pressure in Pa from absolute altitude (using simplified equation).
Definition: pprz_isa.h:110
static struct LtpDef_d ltpdef
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
static void send_servo_cmd(struct _crrcsim *io, double *commands)
struct sockaddr_in addr
void nps_fdm_set_wind(double speed, double dir)
double phi
in radians
#define INPUT_BUFFER_SIZE
#define UShortOfBuf(_buf, _idx)
static void open_udp(char *host, int port, int blocking)
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
#define NPS_CRRCSIM_HOST_PORT
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
double psi
in radians
void nps_fdm_init(double dt)
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
#define NPS_CRRCSIM_ROLL_NEUTRAL
bool launch
Definition: sim_ap.c:38
double q
in rad/s^2
static void inputbuf_init(struct inputbuf *c)
struct LlaCoor_d lla
origin of local frame in LLA
double theta
in radians
struct LlaCoor_d lla_pos_pprz
Definition: nps_fdm.h:58
vector in Latitude, Longitude and Altitude
float dt
struct DoubleRates body_inertial_rotvel
Definition: nps_fdm.h:101
double r
in rad/s^2
int nan_count
Definition: nps_fdm.h:50
static void init_ltp(void)
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer)
struct NedCoor_d ltp_ecef_vel
velocity in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:74
double pressure_sl
pressure at sea level in Pascal
Definition: nps_fdm.h:114
Definition: nps_fdm.h:44
double x
in meters
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
double hmsl
Definition: nps_fdm.h:56
byte buf[INPUT_BUFFER_SIZE]
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Paparazzi double-precision floating point math for geodetic calculations.
#define NPS_CRRCSIM_HOST_IP
Paparazzi floating point math for geodetic calculations.
struct EcefCoor_d ecef_ecef_vel
velocity in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:64
double airspeed
equivalent airspeed in m/s
Definition: nps_fdm.h:109
static void read_into_buffer(struct _crrcsim *io)
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
Definition: nps_fdm.h:87
double pressure
current (static) atmospheric pressure in Pascal
Definition: nps_fdm.h:110
Paparazzi floating point algebra.
#define NPS_CRRCSIM_PITCH_NEUTRAL
static void decode_ahrspacket(struct NpsFdm *fdm, byte *buffer)
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
double init_dt
Definition: nps_fdm.h:47
Paparazzi generic algebra macros.
Paparazzi atmospheric pressure conversion utilities.
struct inputbuf buf
#define UDP_BLOCKING
definition of the local (flat earth) coordinate system
double dynamic_pressure
dynamic pressure in Pascal
Definition: nps_fdm.h:112
static double double_vect3_norm(struct DoubleVect3 *v)
vector in EarthCenteredEarthFixed coordinates
#define FULL_PACKET_SIZE
double curr_dt
Definition: nps_fdm.h:48
struct LlaCoor_d lla_pos
Definition: nps_fdm.h:55
#define UDP_NONBLOCKING
byte data_buffer[FULL_PACKET_SIZE]
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition: pprz_isa.h:50
Paparazzi generic macros for geodetic calculations.
double temperature
current temperature in degrees Celcius
Definition: nps_fdm.h:113
static void decode_imupacket(struct NpsFdm *fdm, byte *buffer)
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
#define LongOfBuf(_buf, _idx)
#define byte
double lon
in radians
struct NpsFdm fdm
#define AHRS_PACKET_LENGTH
static struct _crrcsim crrcsim
struct EcefCoor_d ecef_pos
Definition: nps_fdm.h:53
struct DoubleVect3 body_ecef_accel
acceleration in body frame, wrt ECEF frame
Definition: nps_fdm.h:71
struct DoubleVect3 ltp_g
Definition: nps_fdm.h:104
double z
in meters
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
double p
in rad/s^2
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
#define IMU_PACKET_LENGTH
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
double y
in meters
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105
#define GPS_PACKET_LENGTH
#define ShortOfBuf(_buf, _idx)