35#include <netinet/in.h>
38#include <sys/socket.h>
49#include "generated/airframe.h"
50#include "generated/flight_plan.h"
52#ifndef NPS_CRRCSIM_HOST_IP
53#define NPS_CRRCSIM_HOST_IP "127.0.0.1"
56#ifndef NPS_CRRCSIM_HOST_PORT
57#define NPS_CRRCSIM_HOST_PORT 9002
60#ifndef NPS_CRRCSIM_ROLL_NEUTRAL
61#define NPS_CRRCSIM_ROLL_NEUTRAL 0.
64#ifndef NPS_CRRCSIM_PITCH_NEUTRAL
65#define NPS_CRRCSIM_PITCH_NEUTRAL 0.
70#define UDP_NONBLOCKING 1
73#define word unsigned short
74#define byte unsigned char
77#define IMU_PACKET_LENGTH 51
78#define GPS_PACKET_LENGTH 86
79#define AHRS_PACKET_LENGTH 93
80#define FULL_PACKET_SIZE 93
86#define INPUT_BUFFER_SIZE (3*FULL_PACKET_SIZE)
135 printf(
"Starting to connect to CRRCsim server.\n");
139 printf(
"Connection to CRRCsim failed\n");
142 printf(
"Connection to CRRCsim succed\n");
146 double zero[] = { 0., 0., 0. };
195 printf(
"invalid data packet...!\n");
256 if (
io->socket >= 0) {
284#pragma message "Using magnetic field as defined in airframe file."
309 while (c->
length >= 4 && (c->
buf[c->
start] != (
byte)0x55 || c->
buf[(
byte)(c->
start + 1)] != (
byte)0x55)) {
321 switch (c->
buf[(
byte)(c->
start + 2)]) {
354 printf(
"checksum error\n");
375#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]))
377#define UShortOfBuf(_buf,_idx) (uint16_t)(((uint16_t)_buf[_idx+1]<<8)|((uint16_t)_buf[_idx]))
379#define ShortOfBuf(_buf,_idx) (int16_t)(((uint16_t)_buf[_idx]<<8)|((uint16_t)_buf[_idx+1]))
426 printf(
"decode gps | pos %f %f %f | vel %f %f %f | time %f\n",
449 printf(
"decode ahrs %f %f %f\n",
492 printf(
"decode imu | accel %f %f %f | gyro %f %f %f\n",
504#define MSG_NOSIGNAL SO_NOSIGPIPE
514 byte data[24] = {0,};
526 printf(
"send servo %f %f %f | %d %d %d | %d %d\n",
554 for (i = 4; i < 22; i++) { sum += data[i]; }
556 data[22] = (
byte)(sum >> 8);
557 data[23] = (
byte)sum;
560 if (
io->socket >= 0) {
static const bool blocking[]
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
static double double_vect3_norm(struct DoubleVect3 *v)
#define VECT3_COPY(_a, _b)
struct LlaCoor_d lla
origin of local frame in LLA
double alt
in meters above WGS84 reference ellipsoid
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates Units: meters
static float pprz_isa_pressure_of_altitude(float altitude)
Get pressure in Pa from absolute altitude (using simplified equation).
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
double pressure_sl
pressure at sea level in Pascal
double total_pressure
total atmospheric pressure in Pascal
struct EcefCoor_d ecef_pos
struct NedCoor_d ltpprz_pos
struct LlaCoor_d lla_pos_pprz
struct DoubleRates body_inertial_rotvel
double airspeed
equivalent airspeed in m/s
struct NedCoor_d ltp_ecef_vel
velocity in LTP frame, wrt ECEF frame
double dynamic_pressure
dynamic pressure in Pascal
struct DoubleEulers ltp_to_body_eulers
double pressure
current (static) atmospheric pressure in Pascal
struct DoubleQuat ltp_to_body_quat
struct DoubleRates body_ecef_rotvel
struct EcefCoor_d ecef_ecef_vel
velocity in ECEF frame, wrt ECEF frame
struct DoubleVect3 body_ecef_accel
acceleration in body frame, wrt ECEF frame
double temperature
current temperature in degrees Celcius
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
static void open_udp(char *host, int port, int blocking)
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
byte data_buffer[FULL_PACKET_SIZE]
#define GPS_PACKET_LENGTH
void nps_fdm_init(double dt)
#define ShortOfBuf(_buf, _idx)
static void init_ltp(void)
static struct _crrcsim crrcsim
static void read_into_buffer(struct _crrcsim *io)
#define UShortOfBuf(_buf, _idx)
#define AHRS_PACKET_LENGTH
#define NPS_CRRCSIM_HOST_PORT
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
#define NPS_CRRCSIM_ROLL_NEUTRAL
static int get_msg(struct _crrcsim *io, byte *data_buffer)
#define NPS_CRRCSIM_PITCH_NEUTRAL
static void inputbuf_init(struct inputbuf *c)
static void send_servo_cmd(struct _crrcsim *io, double *commands)
static void decode_ahrspacket(struct NpsFdm *fdm, byte *buffer)
#define IMU_PACKET_LENGTH
#define INPUT_BUFFER_SIZE
#define NPS_CRRCSIM_HOST_IP
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
static void decode_imupacket(struct NpsFdm *fdm, byte *buffer)
#define LongOfBuf(_buf, _idx)
void nps_fdm_set_wind(double speed, double dir)
static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer)
static struct LtpDef_d ltpdef
byte buf[INPUT_BUFFER_SIZE]
Paparazzi generic algebra macros.
Paparazzi floating point algebra.
Paparazzi generic macros for geodetic calculations.
Paparazzi double-precision floating point math for geodetic calculations.
Paparazzi floating point math for geodetic calculations.
Paparazzi atmospheric pressure conversion utilities.
static const ShellCommand commands[]