Paparazzi UAS  v5.15_devel-113-g1b57ff1
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 File Reference

Flight Dynamics Model (FDM) for NPS using CRRCSIM. More...

#include "nps_fdm.h"
#include <stdlib.h>
#include <stdio.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#include "math/pprz_geodetic.h"
#include "math/pprz_geodetic_double.h"
#include "math/pprz_geodetic_float.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_isa.h"
#include "generated/airframe.h"
#include "generated/flight_plan.h"
+ Include dependency graph for nps_fdm_crrcsim.c:

Go to the source code of this file.

Data Structures

struct  inputbuf
 
struct  _crrcsim
 

Macros

#define NPS_CRRCSIM_HOST_IP   "127.0.0.1"
 
#define NPS_CRRCSIM_HOST_PORT   9002
 
#define NPS_CRRCSIM_ROLL_NEUTRAL   0.
 
#define NPS_CRRCSIM_PITCH_NEUTRAL   0.
 
#define UDP_BLOCKING   0
 
#define UDP_NONBLOCKING   1
 
#define word   unsigned short
 
#define byte   unsigned char
 
#define IMU_PACKET_LENGTH   51
 
#define GPS_PACKET_LENGTH   86
 
#define AHRS_PACKET_LENGTH   93
 
#define FULL_PACKET_SIZE   93
 
#define INPUT_BUFFER_SIZE   (3*FULL_PACKET_SIZE)
 
#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]))
 
#define UShortOfBuf(_buf, _idx)   (uint16_t)(((uint16_t)_buf[_idx+1]<<8)|((uint16_t)_buf[_idx]))
 
#define ShortOfBuf(_buf, _idx)   (int16_t)(((uint16_t)_buf[_idx]<<8)|((uint16_t)_buf[_idx+1]))
 

Functions

static void open_udp (char *host, int port, int blocking)
 
static void inputbuf_init (struct inputbuf *c)
 
static void read_into_buffer (struct _crrcsim *io)
 
static void init_ltp (void)
 
static int get_msg (struct _crrcsim *io, byte *data_buffer)
 
static void decode_imupacket (struct NpsFdm *fdm, byte *buffer)
 
static void decode_gpspacket (struct NpsFdm *fdm, byte *buffer)
 
static void decode_ahrspacket (struct NpsFdm *fdm, byte *buffer)
 
static void send_servo_cmd (struct _crrcsim *io, double *commands)
 
void nps_fdm_init (double dt)
 
void nps_fdm_run_step (bool launch, double *commands, int commands_nb)
 
void nps_fdm_set_wind (double speed, double dir)
 
void nps_fdm_set_wind_ned (double wind_north, double wind_east, double wind_down)
 
void nps_fdm_set_turbulence (double wind_speed, int turbulence_severity)
 

Variables

struct NpsFdm fdm
 
static struct _crrcsim crrcsim
 
static struct LtpDef_d ltpdef
 

Detailed Description

Flight Dynamics Model (FDM) for NPS using CRRCSIM.

Definition in file nps_fdm_crrcsim.c.


Data Structure Documentation

struct inputbuf

Definition at line 88 of file nps_fdm_crrcsim.c.

Data Fields
byte buf[INPUT_BUFFER_SIZE]
int length
byte start
struct _crrcsim

Definition at line 95 of file nps_fdm_crrcsim.c.

+ Collaboration diagram for _crrcsim:
Data Fields
struct sockaddr_in addr
struct inputbuf buf
byte data_buffer[FULL_PACKET_SIZE]
int socket

Macro Definition Documentation

#define AHRS_PACKET_LENGTH   93

Definition at line 79 of file nps_fdm_crrcsim.c.

Referenced by get_msg().

#define FULL_PACKET_SIZE   93

Definition at line 80 of file nps_fdm_crrcsim.c.

#define GPS_PACKET_LENGTH   86

Definition at line 78 of file nps_fdm_crrcsim.c.

Referenced by get_msg().

#define IMU_PACKET_LENGTH   51

Definition at line 77 of file nps_fdm_crrcsim.c.

Referenced by get_msg().

#define INPUT_BUFFER_SIZE   (3*FULL_PACKET_SIZE)

Definition at line 86 of file nps_fdm_crrcsim.c.

Referenced by read_into_buffer().

#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]))

Definition at line 375 of file nps_fdm_crrcsim.c.

Referenced by decode_gpspacket().

#define NPS_CRRCSIM_HOST_IP   "127.0.0.1"

Definition at line 53 of file nps_fdm_crrcsim.c.

Referenced by nps_fdm_init().

#define NPS_CRRCSIM_HOST_PORT   9002

Definition at line 57 of file nps_fdm_crrcsim.c.

Referenced by nps_fdm_init().

#define NPS_CRRCSIM_PITCH_NEUTRAL   0.

Definition at line 65 of file nps_fdm_crrcsim.c.

Referenced by decode_ahrspacket().

#define NPS_CRRCSIM_ROLL_NEUTRAL   0.

Definition at line 61 of file nps_fdm_crrcsim.c.

Referenced by decode_ahrspacket().

#define ShortOfBuf (   _buf,
  _idx 
)    (int16_t)(((uint16_t)_buf[_idx]<<8)|((uint16_t)_buf[_idx+1]))

Definition at line 379 of file nps_fdm_crrcsim.c.

Referenced by decode_ahrspacket(), and decode_imupacket().

#define UDP_BLOCKING   0

Definition at line 69 of file nps_fdm_crrcsim.c.

Referenced by open_udp().

#define UDP_NONBLOCKING   1

Definition at line 70 of file nps_fdm_crrcsim.c.

Referenced by nps_fdm_init().

#define UShortOfBuf (   _buf,
  _idx 
)    (uint16_t)(((uint16_t)_buf[_idx+1]<<8)|((uint16_t)_buf[_idx]))

Definition at line 377 of file nps_fdm_crrcsim.c.

Referenced by decode_gpspacket().

#define word   unsigned short

Definition at line 73 of file nps_fdm_crrcsim.c.

Referenced by get_msg(), and send_servo_cmd().

Function Documentation

static void decode_ahrspacket ( struct NpsFdm fdm,
byte buffer 
)
static

Definition at line 440 of file nps_fdm_crrcsim.c.

References double_quat_of_eulers(), NpsFdm::ltp_to_body_eulers, NpsFdm::ltp_to_body_quat, NPS_CRRCSIM_PITCH_NEUTRAL, NPS_CRRCSIM_ROLL_NEUTRAL, DoubleEulers::phi, DoubleEulers::psi, ShortOfBuf, and DoubleEulers::theta.

Referenced by nps_fdm_run_step().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void decode_imupacket ( struct NpsFdm fdm,
byte buffer 
)
static
static int get_msg ( struct _crrcsim io,
byte data_buffer 
)
static

Definition at line 296 of file nps_fdm_crrcsim.c.

References AHRS_PACKET_LENGTH, inputbuf::buf, _crrcsim::buf, byte, GPS_PACKET_LENGTH, IMU_PACKET_LENGTH, inputbuf::length, read_into_buffer(), inputbuf::start, and word.

Referenced by nps_fdm_run_step().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void init_ltp ( void  )
static

Definition at line 265 of file nps_fdm_crrcsim.c.

References LlaCoor_d::alt, ecef_of_lla_d(), fdm, LlaCoor_d::lat, LlaCoor_d::lon, ltp_def_from_ecef_d(), NpsFdm::ltp_g, NpsFdm::ltp_h, ltpdef, DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

Referenced by nps_fdm_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void inputbuf_init ( struct inputbuf c)
static

Definition at line 245 of file nps_fdm_crrcsim.c.

References inputbuf::length, and inputbuf::start.

Referenced by nps_fdm_init().

+ Here is the caller graph for this function:

void nps_fdm_run_step ( bool  launch,
double *  commands,
int  commands_nb 
)

Definition at line 150 of file nps_fdm_crrcsim.c.

References crrcsim, _crrcsim::data_buffer, decode_ahrspacket(), decode_gpspacket(), decode_imupacket(), fdm, get_msg(), and send_servo_cmd().

+ Here is the call graph for this function:

void nps_fdm_set_turbulence ( double  wind_speed,
int  turbulence_severity 
)

Definition at line 211 of file nps_fdm_crrcsim.c.

void nps_fdm_set_wind ( double  speed,
double  dir 
)

Definition at line 200 of file nps_fdm_crrcsim.c.

void nps_fdm_set_wind_ned ( double  wind_north,
double  wind_east,
double  wind_down 
)

Definition at line 205 of file nps_fdm_crrcsim.c.

static void open_udp ( char *  host,
int  port,
int  blocking 
)
static

Definition at line 220 of file nps_fdm_crrcsim.c.

References _crrcsim::addr, crrcsim, _crrcsim::socket, and UDP_BLOCKING.

Referenced by nps_fdm_init().

+ Here is the caller graph for this function:

static void read_into_buffer ( struct _crrcsim io)
static

Definition at line 251 of file nps_fdm_crrcsim.c.

References inputbuf::buf, _crrcsim::buf, INPUT_BUFFER_SIZE, inputbuf::length, _crrcsim::socket, and inputbuf::start.

Referenced by get_msg().

+ Here is the caller graph for this function:

static void send_servo_cmd ( struct _crrcsim io,
double *  commands 
)
static

Definition at line 510 of file nps_fdm_crrcsim.c.

References byte, _crrcsim::socket, and word.

Referenced by nps_fdm_init(), and nps_fdm_run_step().

+ Here is the caller graph for this function:

Variable Documentation

struct _crrcsim crrcsim
static

Definition at line 102 of file nps_fdm_crrcsim.c.

Referenced by nps_fdm_init(), nps_fdm_run_step(), and open_udp().

struct NpsFdm fdm

Definition at line 84 of file nps_fdm_crrcsim.c.

Referenced by init_ltp(), nps_fdm_init(), and nps_fdm_run_step().

struct LtpDef_d ltpdef
static

Definition at line 105 of file nps_fdm_crrcsim.c.

Referenced by decode_gpspacket(), and init_ltp().