![]() |
Paparazzi UAS
v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
|
#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"
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 |
Flight Dynamics Model (FDM) for NPS using CRRCSIM.
Definition in file nps_fdm_crrcsim.c.
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.
Data Fields | ||
---|---|---|
struct sockaddr_in | addr | |
struct inputbuf | buf | |
byte | data_buffer[FULL_PACKET_SIZE] | |
int | socket |
#define AHRS_PACKET_LENGTH 93 |
Definition at line 79 of file nps_fdm_crrcsim.c.
#define byte unsigned char |
Definition at line 74 of file nps_fdm_crrcsim.c.
#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.
#define IMU_PACKET_LENGTH 51 |
Definition at line 77 of file nps_fdm_crrcsim.c.
#define INPUT_BUFFER_SIZE (3*FULL_PACKET_SIZE) |
Definition at line 86 of file nps_fdm_crrcsim.c.
#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.
#define NPS_CRRCSIM_HOST_IP "127.0.0.1" |
Definition at line 53 of file nps_fdm_crrcsim.c.
#define NPS_CRRCSIM_HOST_PORT 9002 |
Definition at line 57 of file nps_fdm_crrcsim.c.
#define NPS_CRRCSIM_PITCH_NEUTRAL 0. |
Definition at line 65 of file nps_fdm_crrcsim.c.
#define NPS_CRRCSIM_ROLL_NEUTRAL 0. |
Definition at line 61 of file nps_fdm_crrcsim.c.
Definition at line 379 of file nps_fdm_crrcsim.c.
#define UDP_BLOCKING 0 |
Definition at line 69 of file nps_fdm_crrcsim.c.
#define UDP_NONBLOCKING 1 |
Definition at line 70 of file nps_fdm_crrcsim.c.
Definition at line 377 of file nps_fdm_crrcsim.c.
#define word unsigned short |
Definition at line 73 of file nps_fdm_crrcsim.c.
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().
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().
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().
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().
|
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().
|
static |
Definition at line 245 of file nps_fdm_crrcsim.c.
References c().
Referenced by 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.
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().
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 |
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().
|
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().
|
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().
|
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 decode_ahrspacket(), decode_gpspacket(), decode_imupacket(), init_ltp(), nps_fdm_init(), and nps_fdm_run_step().
|
static |
Definition at line 105 of file nps_fdm_crrcsim.c.
Referenced by decode_gpspacket(), and init_ltp().