Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
84struct NpsFdm fdm;
85
86#define INPUT_BUFFER_SIZE (3*FULL_PACKET_SIZE)
87// Input buffer
88struct inputbuf {
90 byte start;
91 int length;
92};
93
94// Socket structure
101
102static struct _crrcsim crrcsim;
103
104// Reference point
105static struct LtpDef_d ltpdef;
106
107// static functions declaration
108static void open_udp(char *host, int port, int blocking);
109static void inputbuf_init(struct inputbuf *c);
110static void read_into_buffer(struct _crrcsim *io);
111static void init_ltp(void);
112static int get_msg(struct _crrcsim *io, byte *data_buffer);
113static void decode_imupacket(struct NpsFdm *fdm, byte *buffer);
114static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer);
115static void decode_ahrspacket(struct NpsFdm *fdm, byte *buffer);
116static void send_servo_cmd(struct _crrcsim *io, double *commands);
117
118// NPS FDM interface
119void 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;
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
150void 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
200void nps_fdm_set_wind(double speed __attribute__((unused)),
201 double dir __attribute__((unused)))
202{
203}
204
206 double wind_east __attribute__((unused)),
207 double wind_down __attribute__((unused)))
208{
209}
210
211void 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
220static 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);
233
234 if (connect(crrcsim.socket, (struct sockaddr *)&crrcsim.addr, sizeof(crrcsim.addr)) < 0) {
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
245static void inputbuf_init(struct inputbuf *c)
246{
247 c->start = 0;
248 c->length = 0;
249}
250
251static 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
265static 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;
276
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."
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
296static 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
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':
324 break;
325
326 case 'N':
328 break;
329
330 case 'I':
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 ***************************************************************************************/
384static 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 */
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,
433 fdm->time);
434#endif
435}
436
437/***************************************************************************************
438 *decode the ahrs data packet
439 ***************************************************************************************/
440static void decode_ahrspacket(struct NpsFdm *fdm, byte *buffer)
441{
442 /* euler angles (0.9387340515702713e04 rad to rad) */
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,
452 fdm->ltp_to_body_eulers.psi * 57.3);
453#endif
454}
455
456/***************************************************************************************
457 *decode the imu data packet
458 ***************************************************************************************/
459void 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",
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 ***************************************************************************************/
510static 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;
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 const bool blocking[]
double psi
in radians
double p
in rad/s^2
double theta
in radians
double q
in rad/s^2
double phi
in radians
double r
in rad/s^2
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
static double double_vect3_norm(struct DoubleVect3 *v)
#define VECT3_COPY(_a, _b)
double y
in meters
struct LlaCoor_d lla
origin of local frame in LLA
double z
in meters
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
double lon
in radians
double x
in meters
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
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:117
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition pprz_isa.h:50
uint16_t foo
Definition main_demo5.c:58
double time
Definition nps_fdm.h:46
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition nps_fdm.h:79
struct DoubleVect3 ltp_g
Definition nps_fdm.h:104
double pressure_sl
pressure at sea level in Pascal
Definition nps_fdm.h:114
double init_dt
Definition nps_fdm.h:47
double total_pressure
total atmospheric pressure in Pascal
Definition nps_fdm.h:111
double hmsl
Definition nps_fdm.h:56
struct EcefCoor_d ecef_pos
Definition nps_fdm.h:53
struct NedCoor_d ltpprz_pos
Definition nps_fdm.h:54
struct LlaCoor_d lla_pos_pprz
Definition nps_fdm.h:58
struct DoubleRates body_inertial_rotvel
Definition nps_fdm.h:101
double airspeed
equivalent airspeed in m/s
Definition nps_fdm.h:109
struct NedCoor_d ltp_ecef_vel
velocity in LTP frame, wrt ECEF frame
Definition nps_fdm.h:74
struct DoubleVect3 ltp_h
Definition nps_fdm.h:105
double dynamic_pressure
dynamic pressure in Pascal
Definition nps_fdm.h:112
struct DoubleEulers ltp_to_body_eulers
Definition nps_fdm.h:92
double pressure
current (static) atmospheric pressure in Pascal
Definition nps_fdm.h:110
double curr_dt
Definition nps_fdm.h:48
struct DoubleQuat ltp_to_body_quat
Definition nps_fdm.h:91
struct DoubleRates body_ecef_rotvel
Definition nps_fdm.h:97
struct EcefCoor_d ecef_ecef_vel
velocity in ECEF frame, wrt ECEF frame
Definition nps_fdm.h:64
struct DoubleVect3 body_ecef_accel
acceleration in body frame, wrt ECEF frame
Definition nps_fdm.h:71
double temperature
current temperature in degrees Celcius
Definition nps_fdm.h:113
struct LlaCoor_d lla_pos
Definition nps_fdm.h:55
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
Definition nps_fdm.h:87
int nan_count
Definition nps_fdm.h:50
static void open_udp(char *host, int port, int blocking)
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
byte data_buffer[FULL_PACKET_SIZE]
#define GPS_PACKET_LENGTH
void nps_fdm_init(double dt)
#define ShortOfBuf(_buf, _idx)
static void init_ltp(void)
static struct _crrcsim crrcsim
static void read_into_buffer(struct _crrcsim *io)
#define UShortOfBuf(_buf, _idx)
struct sockaddr_in addr
#define AHRS_PACKET_LENGTH
#define NPS_CRRCSIM_HOST_PORT
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
#define NPS_CRRCSIM_ROLL_NEUTRAL
struct inputbuf buf
static int get_msg(struct _crrcsim *io, byte *data_buffer)
#define NPS_CRRCSIM_PITCH_NEUTRAL
static void inputbuf_init(struct inputbuf *c)
#define byte
static void send_servo_cmd(struct _crrcsim *io, double *commands)
#define UDP_BLOCKING
static void decode_ahrspacket(struct NpsFdm *fdm, byte *buffer)
#define word
#define UDP_NONBLOCKING
#define IMU_PACKET_LENGTH
#define INPUT_BUFFER_SIZE
#define NPS_CRRCSIM_HOST_IP
#define FULL_PACKET_SIZE
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
static void decode_imupacket(struct NpsFdm *fdm, byte *buffer)
#define LongOfBuf(_buf, _idx)
void nps_fdm_set_wind(double speed, double dir)
struct NpsFdm fdm
static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer)
static struct LtpDef_d ltpdef
byte buf[INPUT_BUFFER_SIZE]
Paparazzi generic algebra macros.
Paparazzi floating point algebra.
Paparazzi generic macros for geodetic calculations.
Paparazzi double-precision floating point math for geodetic calculations.
Paparazzi floating point math for geodetic calculations.
Paparazzi atmospheric pressure conversion utilities.
static const ShellCommand commands[]
Definition shell_arch.c:78
static const float dir[]