Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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

◆ inputbuf

struct inputbuf

Definition at line 88 of file nps_fdm_crrcsim.c.

Data Fields
byte buf[INPUT_BUFFER_SIZE]
int length
byte start

◆ _crrcsim

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

◆ AHRS_PACKET_LENGTH

#define AHRS_PACKET_LENGTH   93

Definition at line 79 of file nps_fdm_crrcsim.c.

◆ byte

#define byte   unsigned char

Definition at line 74 of file nps_fdm_crrcsim.c.

◆ FULL_PACKET_SIZE

#define FULL_PACKET_SIZE   93

Definition at line 80 of file nps_fdm_crrcsim.c.

◆ GPS_PACKET_LENGTH

#define GPS_PACKET_LENGTH   86

Definition at line 78 of file nps_fdm_crrcsim.c.

◆ IMU_PACKET_LENGTH

#define IMU_PACKET_LENGTH   51

Definition at line 77 of file nps_fdm_crrcsim.c.

◆ INPUT_BUFFER_SIZE

#define INPUT_BUFFER_SIZE   (3*FULL_PACKET_SIZE)

Definition at line 86 of file nps_fdm_crrcsim.c.

◆ LongOfBuf

#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.

◆ NPS_CRRCSIM_HOST_IP

#define NPS_CRRCSIM_HOST_IP   "127.0.0.1"

Definition at line 53 of file nps_fdm_crrcsim.c.

◆ NPS_CRRCSIM_HOST_PORT

#define NPS_CRRCSIM_HOST_PORT   9002

Definition at line 57 of file nps_fdm_crrcsim.c.

◆ NPS_CRRCSIM_PITCH_NEUTRAL

#define NPS_CRRCSIM_PITCH_NEUTRAL   0.

Definition at line 65 of file nps_fdm_crrcsim.c.

◆ NPS_CRRCSIM_ROLL_NEUTRAL

#define NPS_CRRCSIM_ROLL_NEUTRAL   0.

Definition at line 61 of file nps_fdm_crrcsim.c.

◆ ShortOfBuf

#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.

◆ UDP_BLOCKING

#define UDP_BLOCKING   0

Definition at line 69 of file nps_fdm_crrcsim.c.

◆ UDP_NONBLOCKING

#define UDP_NONBLOCKING   1

Definition at line 70 of file nps_fdm_crrcsim.c.

◆ UShortOfBuf

#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.

◆ word

#define word   unsigned short

Definition at line 73 of file nps_fdm_crrcsim.c.

Function Documentation

◆ decode_ahrspacket()

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(), fdm, 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:

◆ decode_gpspacket()

◆ decode_imupacket()

void decode_imupacket ( struct NpsFdm fdm,
byte buffer 
)
static

◆ get_msg()

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, 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:

◆ init_ltp()

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:

◆ inputbuf_init()

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:

◆ nps_fdm_init()

void nps_fdm_init ( double  dt)

Definition at line 119 of file nps_fdm_crrcsim.c.

◆ nps_fdm_run_step()

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

Definition at line 150 of file nps_fdm_crrcsim.c.

◆ nps_fdm_set_turbulence()

void nps_fdm_set_turbulence ( double  wind_speed,
int  turbulence_severity 
)

Definition at line 211 of file nps_fdm_crrcsim.c.

◆ nps_fdm_set_wind()

void nps_fdm_set_wind ( double  speed,
double  dir 
)

Definition at line 200 of file nps_fdm_crrcsim.c.

◆ nps_fdm_set_wind_ned()

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.

◆ open_udp()

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

Definition at line 220 of file nps_fdm_crrcsim.c.

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

Referenced by nps_fdm_init().

+ Here is the caller graph for this function:

◆ read_into_buffer()

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:

◆ send_servo_cmd()

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

Definition at line 510 of file nps_fdm_crrcsim.c.

References byte, commands, _crrcsim::socket, and word.

Referenced by nps_fdm_init(), and nps_fdm_run_step().

+ Here is the caller graph for this function:

Variable Documentation

◆ crrcsim

struct _crrcsim crrcsim
static

Definition at line 1 of file nps_fdm_crrcsim.c.

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

◆ fdm

◆ ltpdef

struct LtpDef_d ltpdef
static

Definition at line 1 of file nps_fdm_crrcsim.c.

Referenced by decode_gpspacket(), and init_ltp().