9#include <Ivy/ivyloop.h>
12#include "generated/airframe.h"
21#include "generated/settings.h"
22#include "pprzlink/dl_protocol.h"
35static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
36 void *user_data __attribute__((unused)),
37 int argc,
char *argv[]);
40static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
41 void *user_data __attribute__((unused)),
42 int argc,
char *argv[]);
45#define NPS_IVY_DISPLAY_PERIOD_MS ((long)(3 * DISPLAY_DT * 1000))
61 void *user_data __attribute__((unused)),
62 unsigned long delta __attribute__((unused)))
66 printf(
"IVY DISPLAY: timer late, %lu [ms] elapsed for a %ld [ms] period\n",
75 const char *agent_name = AIRFRAME_NAME
"_NPS";
76 const char *ready_msg = AIRFRAME_NAME
"_NPS Ready";
77 IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL);
80 IvyBindMsg(
on_WORLD_ENV, NULL,
"^(\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
83 IvyBindMsg(
on_DL_SETTING, NULL,
"^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
95 const char *default_ivy_bus =
"224.255.255.255";
97 const char *default_ivy_bus =
"127.255.255.255";
99 if (ivy_bus == NULL) {
100 IvyStart(default_ivy_bus);
115 void *user_data __attribute__((unused)),
116 int argc,
char *argv[])
118 if (argc < 6) {
return; }
122 wind.
x = atof(argv[1]);
123 wind.
y = atof(argv[2]);
124 wind.
z = atof(argv[3]);
155 int pid = (int)getpid();
158 ivyPtr = IvyBindMsg(
on_WORLD_ENV, NULL,
"^%d_%d (\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", pid,
seq);
163 memcpy(&fdm_ivy, &
fdm,
sizeof(
struct NpsFdm));
166 IvySendMsg(
"nps %d_%d WORLD_ENV_REQ %f %f %f %f %f %f",
180 return AP_LAUNCH - 1;
183 static const char ap_launch[] =
"aut_lau";
184 char *ap_settings[NB_SETTING] = SETTINGS_NAMES_SHORT;
189 if (strcmp(ap_settings[
idx],ap_launch) == 0) {
199 void *user_data __attribute__((unused)),
200 int argc,
char *argv[])
202 if (argc < 3) {
return; }
204 if (atoi(argv[1]) != AC_ID) {
214 float value = atof(argv[3]);
216 if (!datalink_enabled) {
217 DlSetting(index, value);
221 printf(
"setting %d %f\n", index, value);
243 bool do_world_env_req;
245 memcpy(&fdm_ivy, fdm_data,
sizeof(
fdm));
246 memcpy(&sensors_ivy, sensors_data,
sizeof(
sensors));
251 IvySendMsg(
"%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
259 IvySendMsg(
"%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f",
270 IvySendMsg(
"%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f",
281 IvySendMsg(
"%d NPS_GYRO_BIAS %f %f %f",
291 IvySendMsg(
"%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
300 IvySendMsg(
"%d NPS_WIND %f %f %f",
306 if (do_world_env_req) {
Common code for AP and FBW telemetry.
Device independent GPS code (interface).
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
#define RATE_FLOAT_OF_BFP(_ai)
double alt
in meters above WGS84 reference ellipsoid
void nps_atmosphere_set_wind_ned(double wind_north, double wind_east, double wind_down)
Atmosphere model (pressure, wind) for NPS.
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
struct NedCoor_d ltpprz_pos
struct LlaCoor_d lla_pos_pprz
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
struct DoubleEulers ltp_to_body_eulers
struct DoubleQuat ltp_to_body_quat
struct DoubleRates body_ecef_rotvel
struct LlaCoor_d lla_pos_geoc
struct DoubleVect3 wind
velocity in m/s in NED
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
struct LlaCoor_d lla_pos_geod
void nps_ivy_init(char *ivy_bus, bool nodisplay)
static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *argv[])
#define NPS_IVY_DISPLAY_PERIOD_MS
static TimerId nps_ivy_display_timer
void nps_ivy_send_WORLD_ENV_REQ(void)
static void nps_ivy_display_cb(TimerId id, void *user_data, unsigned long delta)
static void on_WORLD_ENV(IvyClientPtr app, void *user_data, int argc, char *argv[])
bool nps_ivy_send_world_env
static int ap_launch_index
void nps_ivy_display(struct NpsFdm *fdm_data, struct NpsSensors *sensors_data)
int find_launch_index(void)
void nps_update_launch_from_dl(uint8_t value)
void nps_set_time_factor(float time_factor)
pthread_mutex_t fdm_mutex
struct DoubleVect3 neutral
struct DoubleVect3 bias_random_walk_value
struct DoubleVect3 bias_initial
struct NpsSensorGyro gyro
struct NpsSensorAccel accel
Paparazzi double precision floating point algebra.
Paparazzi floating point algebra.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.