nps_fdm_crrcsim.c File Reference

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

static void decode_gpspacket ( struct NpsFdm *  fdm, byte *  buffer  )

static

Definition at line 384 of file nps_fdm_crrcsim.c.

References NpsFdm::airspeed, LlaCoor_d::alt, double_vect3_norm(), NpsFdm::ecef_ecef_vel, ecef_of_lla_d(), ecef_of_ned_vect_d(), NpsFdm::ecef_pos, fdm, NpsFdm::hmsl, LlaCoor_d::lat, LtpDef_d::lla, NpsFdm::lla_pos, NpsFdm::lla_pos_pprz, LlaCoor_d::lon, LongOfBuf, NpsFdm::ltp_ecef_vel, ltpdef, NpsFdm::ltpprz_ecef_vel, NpsFdm::ltpprz_pos, ned_of_ecef_point_d(), ned_of_ecef_vect_d(), pprz_isa_pressure_of_altitude(), NpsFdm::pressure, NpsFdm::time, UShortOfBuf, VECT3_COPY, NedCoor_d::x, NedCoor_d::y, and NedCoor_d::z.

Referenced by nps_fdm_run_step().

Here is the call graph for this function:

Here is the caller graph for this function:

decode_imupacket()

void decode_imupacket ( struct NpsFdm *  fdm, byte *  buffer  )

static

Definition at line 459 of file nps_fdm_crrcsim.c.

References NpsFdm::body_accel, NpsFdm::body_ecef_accel, NpsFdm::body_ecef_rotvel, NpsFdm::body_inertial_rotvel, fdm, DoubleRates::p, DoubleRates::q, DoubleRates::r, ShortOfBuf, DoubleVect3::x, DoubleVect3::y, and DoubleVect3::z.

Referenced by nps_fdm_run_step().

Here is the caller graph for this function:

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, _crrcsim::buf, c(), GPS_PACKET_LENGTH, IMU_PACKET_LENGTH, read_into_buffer(), 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 c().

Referenced by nps_fdm_init().

Here is the call graph for this function:

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.

References _crrcsim::buf, crrcsim, NpsFdm::curr_dt, NpsFdm::dynamic_pressure, fdm, NpsFdm::init_dt, init_ltp(), inputbuf_init(), NpsFdm::nan_count, NPS_CRRCSIM_HOST_IP, NPS_CRRCSIM_HOST_PORT, open_udp(), PPRZ_ISA_SEA_LEVEL_PRESSURE, NpsFdm::pressure, NpsFdm::pressure_sl, send_servo_cmd(), _crrcsim::socket, NpsFdm::temperature, NpsFdm::total_pressure, and UDP_NONBLOCKING.

Here is the call graph for this function:

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.

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

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 _crrcsim::buf, c(), INPUT_BUFFER_SIZE, and _crrcsim::socket.

Referenced by get_msg().

Here is the call graph for this function:

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 102 of file nps_fdm_crrcsim.c.

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

fdm

struct NpsFdm fdm

Definition at line 84 of file nps_fdm_crrcsim.c.

Referenced by decode_ahrspacket(), decode_gpspacket(), decode_imupacket(), init_ltp(), nps_fdm_init(), and nps_fdm_run_step().

ltpdef

struct LtpDef_d ltpdef

static

Definition at line 105 of file nps_fdm_crrcsim.c.

Referenced by decode_gpspacket(), and init_ltp().