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

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

struct NpsFdm fdm

Definition at line 1 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 1 of file nps_fdm_crrcsim.c.

Referenced by decode_gpspacket(), and init_ltp().

Data Structures

Macros

Functions

Variables

Detailed Description

Data Structure Documentation

◆ inputbuf

◆ _crrcsim

Macro Definition Documentation

◆ AHRS_PACKET_LENGTH

◆ byte

◆ FULL_PACKET_SIZE

◆ GPS_PACKET_LENGTH

◆ IMU_PACKET_LENGTH

◆ INPUT_BUFFER_SIZE

◆ LongOfBuf

◆ NPS_CRRCSIM_HOST_IP

◆ NPS_CRRCSIM_HOST_PORT

◆ NPS_CRRCSIM_PITCH_NEUTRAL

◆ NPS_CRRCSIM_ROLL_NEUTRAL

◆ ShortOfBuf

◆ UDP_BLOCKING

◆ UDP_NONBLOCKING

◆ UShortOfBuf

◆ word

Function Documentation

◆ decode_ahrspacket()

◆ decode_gpspacket()

◆ decode_imupacket()

◆ get_msg()

◆ init_ltp()

◆ inputbuf_init()

◆ nps_fdm_init()

◆ nps_fdm_run_step()

◆ nps_fdm_set_turbulence()

◆ nps_fdm_set_wind()

◆ nps_fdm_set_wind_ned()

◆ open_udp()

◆ read_into_buffer()

◆ send_servo_cmd()

Variable Documentation

◆ crrcsim

◆ fdm

◆ ltpdef