Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 {
90  byte start;
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
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;
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 */
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,
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 
NedCoor_d::y
double y
in meters
Definition: pprz_geodetic_double.h:69
AHRS_PACKET_LENGTH
#define AHRS_PACKET_LENGTH
Definition: nps_fdm_crrcsim.c:79
NpsFdm::ltp_h
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105
ltp_def_from_ecef_d
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:34
UDP_NONBLOCKING
#define UDP_NONBLOCKING
Definition: nps_fdm_crrcsim.c:70
_crrcsim::data_buffer
byte data_buffer[FULL_PACKET_SIZE]
Definition: nps_fdm_crrcsim.c:99
NPS_CRRCSIM_PITCH_NEUTRAL
#define NPS_CRRCSIM_PITCH_NEUTRAL
Definition: nps_fdm_crrcsim.c:65
NpsFdm::airspeed
double airspeed
equivalent airspeed in m/s
Definition: nps_fdm.h:109
pprz_geodetic.h
Paparazzi generic macros for geodetic calculations.
NpsFdm::body_ecef_accel
struct DoubleVect3 body_ecef_accel
acceleration in body frame, wrt ECEF frame
Definition: nps_fdm.h:71
DoubleVect3::z
double z
Definition: pprz_algebra_double.h:49
LlaCoor_d
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_double.h:58
NpsFdm
Definition: nps_fdm.h:44
double_quat_of_eulers
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
Definition: pprz_algebra_double.c:49
nps_fdm_set_wind
void nps_fdm_set_wind(double speed, double dir)
Definition: nps_fdm_crrcsim.c:200
_crrcsim::socket
int socket
Definition: nps_fdm_crrcsim.c:96
ecef_of_lla_d
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
Definition: pprz_geodetic_double.c:120
LlaCoor_d::alt
double alt
in meters above WGS84 reference ellipsoid
Definition: pprz_geodetic_double.h:61
FULL_PACKET_SIZE
#define FULL_PACKET_SIZE
Definition: nps_fdm_crrcsim.c:80
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
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
double_vect3_norm
static double double_vect3_norm(struct DoubleVect3 *v)
Definition: pprz_algebra_double.h:100
inputbuf_init
static void inputbuf_init(struct inputbuf *c)
Definition: nps_fdm_crrcsim.c:245
inputbuf::start
byte start
Definition: nps_fdm_crrcsim.c:90
DoubleRates::r
double r
in rad/s^2
Definition: pprz_algebra_double.h:88
NpsFdm::body_accel
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
Definition: nps_fdm.h:87
inputbuf::buf
byte buf[INPUT_BUFFER_SIZE]
Definition: nps_fdm_crrcsim.c:89
DoubleEulers::psi
double psi
in radians
Definition: pprz_algebra_double.h:79
ltpdef
static struct LtpDef_d ltpdef
Definition: nps_fdm_crrcsim.c:105
NpsFdm::lla_pos_pprz
struct LlaCoor_d lla_pos_pprz
Definition: nps_fdm.h:58
commands
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
nps_fdm.h
pprz_algebra_float.h
Paparazzi floating point algebra.
open_udp
static void open_udp(char *host, int port, int blocking)
Definition: nps_fdm_crrcsim.c:220
NpsFdm::ltp_to_body_quat
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
NpsFdm::ltp_ecef_vel
struct NedCoor_d ltp_ecef_vel
velocity in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:74
nps_fdm_set_wind_ned
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
Definition: nps_fdm_crrcsim.c:205
ecef_of_ned_vect_d
void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
Definition: pprz_geodetic_double.c:186
NPS_CRRCSIM_ROLL_NEUTRAL
#define NPS_CRRCSIM_ROLL_NEUTRAL
Definition: nps_fdm_crrcsim.c:61
NpsFdm::body_ecef_rotvel
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
NpsFdm::ecef_ecef_vel
struct EcefCoor_d ecef_ecef_vel
velocity in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:64
pprz_geodetic_float.h
Paparazzi floating point math for geodetic calculations.
get_msg
static int get_msg(struct _crrcsim *io, byte *data_buffer)
Definition: nps_fdm_crrcsim.c:296
NpsFdm::time
double time
Definition: nps_fdm.h:46
INPUT_BUFFER_SIZE
#define INPUT_BUFFER_SIZE
Definition: nps_fdm_crrcsim.c:86
DoubleVect3::x
double x
Definition: pprz_algebra_double.h:47
NpsFdm::ltp_g
struct DoubleVect3 ltp_g
Definition: nps_fdm.h:104
LongOfBuf
#define LongOfBuf(_buf, _idx)
Definition: nps_fdm_crrcsim.c:375
decode_ahrspacket
static void decode_ahrspacket(struct NpsFdm *fdm, byte *buffer)
Definition: nps_fdm_crrcsim.c:440
dir
static const float dir[]
Definition: shift_tracking.c:91
NpsFdm::total_pressure
double total_pressure
total atmospheric pressure in Pascal
Definition: nps_fdm.h:111
ShortOfBuf
#define ShortOfBuf(_buf, _idx)
Definition: nps_fdm_crrcsim.c:379
crrcsim
static struct _crrcsim crrcsim
Definition: nps_fdm_crrcsim.c:102
_crrcsim::buf
struct inputbuf buf
Definition: nps_fdm_crrcsim.c:98
NedCoor_d::z
double z
in meters
Definition: pprz_geodetic_double.h:70
NedCoor_d
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_double.h:67
byte
#define byte
Definition: nps_fdm_crrcsim.c:74
nps_fdm_set_turbulence
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
Definition: nps_fdm_crrcsim.c:211
blocking
static const bool blocking[]
Definition: gpio_ext_pca95xx.c:94
LlaCoor_d::lat
double lat
in radians
Definition: pprz_geodetic_double.h:59
decode_gpspacket
static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer)
Definition: nps_fdm_crrcsim.c:384
pprz_geodetic_double.h
Paparazzi double-precision floating point math for geodetic calculations.
LtpDef_d
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_double.h:97
pprz_isa_pressure_of_altitude
static float pprz_isa_pressure_of_altitude(float altitude)
Get pressure in Pa from absolute altitude (using simplified equation).
Definition: pprz_isa.h:117
NpsFdm::lla_pos
struct LlaCoor_d lla_pos
Definition: nps_fdm.h:55
send_servo_cmd
static void send_servo_cmd(struct _crrcsim *io, double *commands)
Definition: nps_fdm_crrcsim.c:510
NpsFdm::hmsl
double hmsl
Definition: nps_fdm.h:56
decode_imupacket
static void decode_imupacket(struct NpsFdm *fdm, byte *buffer)
Definition: nps_fdm_crrcsim.c:459
_crrcsim::addr
struct sockaddr_in addr
Definition: nps_fdm_crrcsim.c:97
fdm
struct NpsFdm fdm
Definition: nps_fdm_crrcsim.c:84
NpsFdm::ltp_to_body_eulers
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
NpsFdm::init_dt
double init_dt
Definition: nps_fdm.h:47
NpsFdm::ltpprz_pos
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
UShortOfBuf
#define UShortOfBuf(_buf, _idx)
Definition: nps_fdm_crrcsim.c:377
ned_of_ecef_point_d
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:147
EcefCoor_d
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_double.h:49
NpsFdm::ecef_pos
struct EcefCoor_d ecef_pos
Definition: nps_fdm.h:53
NpsFdm::temperature
double temperature
current temperature in degrees Celcius
Definition: nps_fdm.h:113
word
#define word
Definition: nps_fdm_crrcsim.c:73
nps_fdm_init
void nps_fdm_init(double dt)
Definition: nps_fdm_crrcsim.c:119
ned_of_ecef_vect_d
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:159
NpsFdm::pressure
double pressure
current (static) atmospheric pressure in Pascal
Definition: nps_fdm.h:110
init_ltp
static void init_ltp(void)
Definition: nps_fdm_crrcsim.c:265
NpsFdm::dynamic_pressure
double dynamic_pressure
dynamic pressure in Pascal
Definition: nps_fdm.h:112
inputbuf
Definition: nps_fdm_crrcsim.c:88
DoubleVect3
Definition: pprz_algebra_double.h:46
pprz_algebra.h
Paparazzi generic algebra macros.
GPS_PACKET_LENGTH
#define GPS_PACKET_LENGTH
Definition: nps_fdm_crrcsim.c:78
read_into_buffer
static void read_into_buffer(struct _crrcsim *io)
Definition: nps_fdm_crrcsim.c:251
DoubleVect3::y
double y
Definition: pprz_algebra_double.h:48
NPS_CRRCSIM_HOST_IP
#define NPS_CRRCSIM_HOST_IP
Definition: nps_fdm_crrcsim.c:53
_crrcsim
Definition: nps_fdm_crrcsim.c:95
LlaCoor_d::lon
double lon
in radians
Definition: pprz_geodetic_double.h:60
IMU_PACKET_LENGTH
#define IMU_PACKET_LENGTH
Definition: nps_fdm_crrcsim.c:77
NpsFdm::curr_dt
double curr_dt
Definition: nps_fdm.h:48
nps_fdm_run_step
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Definition: nps_fdm_crrcsim.c:150
inputbuf::length
int length
Definition: nps_fdm_crrcsim.c:91
UDP_BLOCKING
#define UDP_BLOCKING
Definition: nps_fdm_crrcsim.c:69
NpsFdm::nan_count
int nan_count
Definition: nps_fdm.h:50
LtpDef_d::lla
struct LlaCoor_d lla
origin of local frame in LLA
Definition: pprz_geodetic_double.h:99
DoubleRates::p
double p
in rad/s^2
Definition: pprz_algebra_double.h:86
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
NpsFdm::body_inertial_rotvel
struct DoubleRates body_inertial_rotvel
Definition: nps_fdm.h:101
PPRZ_ISA_SEA_LEVEL_PRESSURE
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition: pprz_isa.h:50
NpsFdm::pressure_sl
double pressure_sl
pressure at sea level in Pascal
Definition: nps_fdm.h:114
pprz_isa.h
Paparazzi atmospheric pressure conversion utilities.
NPS_CRRCSIM_HOST_PORT
#define NPS_CRRCSIM_HOST_PORT
Definition: nps_fdm_crrcsim.c:57