diff --git a/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml b/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml index 3086b7767d..abf35096de 100644 --- a/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml +++ b/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml @@ -16,29 +16,52 @@ + + + + + + + + + + - + + + + + + + + + + + - - - + + + + + + - + @@ -90,7 +113,24 @@ +
+ + + + + + + + + + + + + + + +
@@ -154,11 +194,22 @@
- + - + + + + + + + + + + + +
@@ -168,10 +219,19 @@
+ + + + + + + +
diff --git a/conf/airframes/AGGIEAIR/aggieair_conf.xml b/conf/airframes/AGGIEAIR/aggieair_conf.xml index 2ca9d177ef..baf1e91ab5 100644 --- a/conf/airframes/AGGIEAIR/aggieair_conf.xml +++ b/conf/airframes/AGGIEAIR/aggieair_conf.xml @@ -3,22 +3,22 @@ name="Ark_Quad" ac_id="2" airframe="airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml" - radio="radios/AGGIEAIR/aggieair_taranis.xml" - telemetry="telemetry/default_rotorcraft.xml" + radio="radios/AGGIEAIR/aggieair_sbus_fakerator.xml" + telemetry="telemetry/AGGIEAIR/aggieair_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic_geofence.xml" - settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml settings/control/stabilization_att_float_euler.xml" - settings_modules="" + settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml" + settings_modules="modules/gps.xml" gui_color="#ffff954c0000" /> diff --git a/conf/airframes/AGGIEAIR/aggieair_control_panel.xml b/conf/airframes/AGGIEAIR/aggieair_control_panel.xml new file mode 100644 index 0000000000..f953b70f64 --- /dev/null +++ b/conf/airframes/AGGIEAIR/aggieair_control_panel.xml @@ -0,0 +1,216 @@ + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + diff --git a/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml b/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml index e86ace1859..a934a51135 100644 --- a/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml +++ b/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml @@ -7,17 +7,26 @@ RP3 Lisa MX - - + + + + + - + + + + + + @@ -37,7 +46,7 @@ RP3 Lisa MX - + @@ -45,17 +54,23 @@ RP3 Lisa MX + + + + + + - - + + - + @@ -107,7 +122,6 @@ RP3 Lisa MX -
@@ -130,10 +144,20 @@ RP3 Lisa MX
- + + + + + + + + +
@@ -144,7 +168,8 @@ RP3 Lisa MX
- + + @@ -247,6 +272,10 @@ RP3 Lisa MX + + + +
diff --git a/conf/airframes/AGGIEAIR/ark_hexa_1-8.xml b/conf/airframes/AGGIEAIR/ark_hexa_1-8.xml new file mode 100644 index 0000000000..5c821a98f9 --- /dev/null +++ b/conf/airframes/AGGIEAIR/ark_hexa_1-8.xml @@ -0,0 +1,283 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + +
+ + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + +
+ +
+ + + + + + + + +
+ + diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index 2203723c98..8560a0da8c 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -2,10 +2,10 @@ # +USE_HITL ?= 0 + nps.ARCHDIR = sim # include Makefile.nps instead of Makefile.sim nps.MAKEFILE = nps nps.CFLAGS += -DSITL -DUSE_NPS -nps.CFLAGS += $(shell pkg-config glib-2.0 --cflags) -nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy $(shell pcre-config --libs) -lgsl -lgslcblas +nps.LDFLAGS += -lm -livy $(shell pcre-config --libs) -lgsl -lgslcblas -lrt nps.CFLAGS += -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I$(PAPARAZZI_SRC)/sw/simulator -I$(PAPARAZZI_SRC)/sw/simulator/nps -I$(PAPARAZZI_HOME)/conf/simulator/nps + +# sdl needed for joystick input nps.LDFLAGS += $(shell sdl-config --libs) @@ -25,7 +30,7 @@ nps.LDFLAGS += $(shell sdl-config --libs) VPATH += $(PAPARAZZI_SRC)/sw/simulator NPSDIR = nps -nps.srcs += $(NPSDIR)/nps_main.c \ +nps.srcs += \ $(NPSDIR)/nps_random.c \ $(NPSDIR)/nps_sensors.c \ $(NPSDIR)/nps_sensors_utils.c \ @@ -39,11 +44,20 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_sensor_temperature.c \ $(NPSDIR)/nps_electrical.c \ $(NPSDIR)/nps_atmosphere.c \ + $(NPSDIR)/nps_ivy.c \ + $(NPSDIR)/nps_flightgear.c \ $(NPSDIR)/nps_radio_control.c \ $(NPSDIR)/nps_radio_control_joystick.c \ $(NPSDIR)/nps_radio_control_spektrum.c \ - $(NPSDIR)/nps_ivy.c \ - $(NPSDIR)/nps_flightgear.c + $(NPSDIR)/nps_main_common.c + +ifeq ($(USE_HITL),1) +$(info USE_HITL defined) +include $(CFG_SHARED)/nps_hitl.makefile +else +$(info USE_HITL undefined) +include $(CFG_SHARED)/nps_sitl.makefile +endif # for geo mag calculation nps.srcs += math/pprz_geodetic_wmm2015.c diff --git a/conf/firmwares/subsystems/shared/nps_hitl.makefile b/conf/firmwares/subsystems/shared/nps_hitl.makefile new file mode 100644 index 0000000000..4f8166186e --- /dev/null +++ b/conf/firmwares/subsystems/shared/nps_hitl.makefile @@ -0,0 +1,28 @@ +# Hey Emacs, this is a -*- makefile -*- + +# +# NPS HITL Simulator +# +# HITL specific makefile +# + +nps.srcs += $(NPSDIR)/nps_main_hitl.c + +# TODO: have this in ins_vectornav.xml +# will hopefully work better once nps and HITL are separate targets +nps.srcs += $(NPSDIR)/nps_ins_vectornav.c + +# glib is still needed for some components (such as radio input) +nps.CFLAGS += $(shell pkg-config glib-2.0 --cflags) +nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) + +INS_DEV ?= \"/dev/ttyUSB1\" +INS_BAUD ?= B921600 + +AP_DEV ?= \"/dev/ttyUSB2\" +AP_BAUD ?= B921600 + +nps.CFLAGS += -DAP_DEV=\"$(AP_DEV)\" +nps.CFLAGS += -DAP_BAUD=$(AP_BAUD) +nps.CFLAGS += -DINS_DEV=\"$(INS_DEV)\" +nps.CFLAGS += -DINS_BAUD=$(INS_BAUD) diff --git a/conf/firmwares/subsystems/shared/nps_sitl.makefile b/conf/firmwares/subsystems/shared/nps_sitl.makefile new file mode 100644 index 0000000000..d851b72176 --- /dev/null +++ b/conf/firmwares/subsystems/shared/nps_sitl.makefile @@ -0,0 +1,13 @@ +# Hey Emacs, this is a -*- makefile -*- + +# +# NPS SITL Simulator +# +# SITL specific makefile +# + +# glib is still needed for some components (such as radio input) +nps.CFLAGS += $(shell pkg-config glib-2.0 --cflags) +nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) + +nps.srcs += $(NPSDIR)/nps_main_sitl.c diff --git a/conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml b/conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml index 1ceffb9254..88089a0056 100644 --- a/conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml +++ b/conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml @@ -56,6 +56,11 @@ + + + + + diff --git a/conf/flight_plans/basic_sim.xml b/conf/flight_plans/basic_sim.xml new file mode 100644 index 0000000000..337e8732ef --- /dev/null +++ b/conf/flight_plans/basic_sim.xml @@ -0,0 +1,92 @@ + + + +
+#include "subsystems/datalink/datalink.h" +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/extra_dl.xml b/conf/modules/extra_dl.xml index 0c0c1da894..966860b8b7 100644 --- a/conf/modules/extra_dl.xml +++ b/conf/modules/extra_dl.xml @@ -3,25 +3,43 @@ Extra datalink (PPRZ transport) - +
+ + # Check for UDP port ifneq (,$(findstring udp,$(EXTRA_DL_PORT_LOWER))) include $(CFG_SHARED)/udp.makefile else + ifneq (,$(findstring usb_serial,$(EXTRA_DL_PORT_LOWER))) + # usb_serial telemetry chosen, add files based on architecture + ifeq ($(ARCH), lpc21) + $(TARGET).srcs += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c + $(TARGET).srcs += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c + else + ifeq ($(ARCH), stm32) + $(TARGET).srcs += $(SRC_ARCH)/usb_ser_hw.c + else + ifneq ($(ARCH), sim) + $(error telemetry_transparent_usb currently only implemented for the lpc21 and stm32) + endif + endif + endif + else EXTRA_DL_BAUD ?= B57600 $(TARGET).CFLAGS += -D$(EXTRA_DL_PORT_UPPER)_BAUD=$(EXTRA_DL_BAUD) - endif + endif # USB serial + endif # UDP diff --git a/conf/modules/ins_vectornav.xml b/conf/modules/ins_vectornav.xml index 01562e646a..87ca37e68f 100644 --- a/conf/modules/ins_vectornav.xml +++ b/conf/modules/ins_vectornav.xml @@ -48,4 +48,11 @@ endif + + diff --git a/conf/radios/AGGIEAIR/aggieair_sbus_fakerator.xml b/conf/radios/AGGIEAIR/aggieair_sbus_fakerator.xml new file mode 100644 index 0000000000..b8c71b5497 --- /dev/null +++ b/conf/radios/AGGIEAIR/aggieair_sbus_fakerator.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/simulator/jsbsim/aircraft/AGGIEAIR/aggieair_ark_hexa.xml b/conf/simulator/jsbsim/aircraft/AGGIEAIR/aggieair_ark_hexa.xml new file mode 100644 index 0000000000..4085974cdb --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/AGGIEAIR/aggieair_ark_hexa.xml @@ -0,0 +1,502 @@ + + + + + + AggieAir + 27-01-2015 + Version 0.1 - beta + AggieAir ARK hexarotor + + + + 78.53 + 10 + 6.79 + 0 + 0 + 0 + 0 + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + + + 0.005 + 0.005 + 0.010 + 0. + 0. + 0. + 0.84 + + 0 + 0 + 0 + + + + + + + -0.15 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0.15 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + 0.15 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + -0.15 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + + + + + fcs/f_motor + fcs/fr_motor + fcs/br_motor + fcs/b_motor + fcs/bl_motor + fcs/fl_motor + + + + + + + fcs/f_motor + 0.84 + + + + -21 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/fr_motor + 0.84 + + + + -10.5 + 18.1865 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/br_motor + 0.84 + + + + 10.5 + 18.1865 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/b_motor + 0.84 + + + + 21 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/bl_motor + 0.84 + + + + 10.5 + -18.1865 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/fl_motor + 0.84 + + + + -10.5 + -18.1865 + 0 + + + 0 + 0 + -1 + + + + + + + + + fcs/f_motor + 0.84 + + + + -27.375 + 0 + 0 + + + 0 + -1 + 0 + + + + + + + fcs/f_motor + 0.84 + + + + -14.625 + 0 + 0 + + + 0 + 1 + 0 + + + + + + + fcs/fr_motor + 0.84 + + + + + -10.5 + 11.8115 + 0 + + + -1 + 0 + 0 + + + + + + + fcs/fr_motor + 0.84 + + + + -10.5 + 24.5615 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/br_motor + 0.84 + + + + + 10.5 + 11.8115 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/br_motor + 0.84 + + + + 10.5 + 24.5615 + 0 + + + -1 + 0 + 0 + + + + + + + + fcs/b_motor + 0.84 + + + + 27.375 + 0 + 0 + + + 0 + -1 + 0 + + + + + + + fcs/b_motor + 0.84 + + + + 14.625 + 0 + 0 + + + 0 + 1 + 0 + + + + + + + fcs/bl_motor + 0.84 + + + + 10.5 + -11.8115 + 0 + + + -1 + 0 + 0 + + + + + + + fcs/bl_motor + 0.84 + + + + 10.5 + -24.5615 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/fl_motor + 0.84 + + + + + -10.5 + -11.8115 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/fl_motor + 0.84 + + + + -10.5 + -24.5615 + 0 + + + -1 + 0 + 0 + + + + + + + + + + + + Drag + + aero/qbar-psf + 47.9 + 0.0151 + 0.224808943 + + + + + + diff --git a/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml b/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml new file mode 100644 index 0000000000..372204c6be --- /dev/null +++ b/conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml @@ -0,0 +1,125 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/AGGIEAIR/aggieair_rotorcraft.xml b/conf/telemetry/AGGIEAIR/aggieair_rotorcraft.xml new file mode 100644 index 0000000000..a4fa0bcdf3 --- /dev/null +++ b/conf/telemetry/AGGIEAIR/aggieair_rotorcraft.xml @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/datalink/extra_pprz_dl.c b/sw/airborne/modules/datalink/extra_pprz_dl.c index 20693a2d9d..65a16a73d9 100644 --- a/sw/airborne/modules/datalink/extra_pprz_dl.c +++ b/sw/airborne/modules/datalink/extra_pprz_dl.c @@ -1,6 +1,7 @@ /* * Copyright (C) 2006 Pascal Brisset, Antoine Drouin * Copyright (C) 2010 ENAC + * Copyright (C) 2016 2016 Michal Podhradsky * * This file is part of paparazzi. * @@ -20,8 +21,31 @@ * Boston, MA 02111-1307, USA. * */ +/** + * @file "modules/datalink/extra_pprz_dl.c" + * Extra datalink and telemetry using PPRZ protocol + * + * NOTES (for future reference): + * This note is not needed unless we want to define our own messages - in such case you would define the two + * structs below and then define individual messages and registered them in the same way as + * periodic telemetry. + * struct telemetry_cb_slots telemetry_cbs_logger[TELEMETRY_PPRZ_NB_MSG] = TELEMETRY_PPRZ_CBS; + * struct periodic_telemetry logger_telemetry = { TELEMETRY_PPRZ_NB_MSG, telemetry_cbs_logger }; + * + * The registration should be done in the init function: + * register_periodic_telemetry(&extra_telemetry, PPRZ_MSG_ID_xxx, send_xxx_message); + * + * Two extra notes for the periodic function: + * 1) this sends registered messages from ExtraTelemetry process (as mentioned above) over dedicated port do: + * periodic_telemetry_send_ExtraTelemetryr(&extra_telemetry, &pprz_tp_extra.trans_tx, &(EXTRA_TELEMETRY_PORT).device); + * 2) to send ExtraTelemetry messages over default channel just change to: + * periodic_telemetry_send_ExtraTelemetry(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); + */ +#define PERIODIC_C_EXTRA #include "modules/datalink/extra_pprz_dl.h" +#include "subsystems/datalink/telemetry.h" + struct pprz_transport extra_pprz_tp; @@ -30,3 +54,11 @@ void extra_pprz_dl_init(void) pprz_transport_init(&extra_pprz_tp); } +void extra_pprz_dl_periodic(void) +{ +#if PERIODIC_TELEMETRY + // send periodic messages as defined in the Extra process, we are using DefaultPeriodic so we can send standard messages + periodic_telemetry_send_Extra(DefaultPeriodic, &extra_pprz_tp.trans_tx, &(EXTRA_DOWNLINK_DEVICE).device); +#endif +} + diff --git a/sw/airborne/modules/datalink/extra_pprz_dl.h b/sw/airborne/modules/datalink/extra_pprz_dl.h index 3752d81f8f..faa940f136 100644 --- a/sw/airborne/modules/datalink/extra_pprz_dl.h +++ b/sw/airborne/modules/datalink/extra_pprz_dl.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2010 ENAC + * Copyright (C) 2016 2016 Michal Podhradsky * * This file is part of paparazzi. * @@ -19,12 +20,10 @@ * Boston, MA 02111-1307, USA. * */ - -/** \file extra_pprz_dl.h - * \brief Extra datalink using PPRZ protocol - * +/** + * @file "modules/datalink/extra_pprz_dl.h" + * Extra datalink and telemetry using PPRZ protocol */ - #ifndef EXTRA_PPRZ_DL_H #define EXTRA_PPRZ_DL_H @@ -35,12 +34,15 @@ #include "mcu_periph/udp.h" #endif +#if USE_USB_SERIAL +#include "mcu_periph/usb_serial.h" +#endif + /* PPRZ transport structure */ extern struct pprz_transport extra_pprz_tp; /* Datalink Event */ - -#define ExtraDatalinkEvent() { \ +#define ExtraDatalinkEvent() { \ pprz_check_and_parse(&EXTRA_DOWNLINK_DEVICE.device, &extra_pprz_tp, dl_buffer, &dl_msg_available); \ DlCheckAndParse(&EXTRA_DOWNLINK_DEVICE.device, &extra_pprz_tp.trans_tx, dl_buffer); \ } @@ -48,5 +50,11 @@ extern struct pprz_transport extra_pprz_tp; /** Init function */ extern void extra_pprz_dl_init(void); +/** Periodic function + * + * should be called at TELEMETRY_FREQUENCY + */ +extern void extra_pprz_dl_periodic(void); + #endif /* EXTRA_PPRZ_DL_H */ diff --git a/sw/airborne/subsystems/datalink/bluegiga.c b/sw/airborne/subsystems/datalink/bluegiga.c index c15373f230..9f5bd2e759 100644 --- a/sw/airborne/subsystems/datalink/bluegiga.c +++ b/sw/airborne/subsystems/datalink/bluegiga.c @@ -63,7 +63,7 @@ struct spi_transaction bluegiga_spi; uint8_t broadcast_msg[20]; -void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans); +void bluegiga_load_tx(struct bluegiga_periph *p); void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data); void bluegiga_receive(struct spi_transaction *trans); @@ -214,7 +214,7 @@ void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data) } /* Load waiting data into tx peripheral buffer */ -void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans) +void bluegiga_load_tx(struct bluegiga_periph *p) { uint8_t packet_len; // check data available in buffer to send @@ -330,7 +330,7 @@ void bluegiga_receive(struct spi_transaction *trans) } // load next message to be sent into work buffer, needs to be loaded before calling spi_slave_register - bluegiga_load_tx(&bluegiga_p, trans); + bluegiga_load_tx(&bluegiga_p); // register spi slave read for next transaction spi_slave_register(&(BLUEGIGA_SPI_DEV), trans); diff --git a/sw/airborne/test/test_can.c b/sw/airborne/test/test_can.c index 9515cf79a1..277131ff6b 100644 --- a/sw/airborne/test/test_can.c +++ b/sw/airborne/test/test_can.c @@ -70,7 +70,7 @@ static inline void main_init(void) mcu_init(); sys_time_register_timer((0.5 / PERIODIC_FREQUENCY), NULL); downlink_init(); - ppz_can_init(main_on_can_msg); + ppz_can_init((can_rx_callback_t)main_on_can_msg); } static inline void main_periodic_task(void) diff --git a/sw/simulator/nps/nps_atmosphere.c b/sw/simulator/nps/nps_atmosphere.c index 6b8355eee1..2a1a9ccd5f 100644 --- a/sw/simulator/nps/nps_atmosphere.c +++ b/sw/simulator/nps/nps_atmosphere.c @@ -101,7 +101,7 @@ void nps_atmosphere_update(double dt) req_time += dt; if (req_time - nps_atmosphere.last_world_env_req >= NPS_WORLD_ENV_UPDATE) { nps_atmosphere.last_world_env_req = req_time; - nps_ivy_send_WORLD_ENV_REQ(); + nps_ivy_send_world_env = true; } nps_fdm_set_wind_ned(nps_atmosphere.wind.x, nps_atmosphere.wind.y, nps_atmosphere.wind.z); diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h index 9957524e42..b1cba54254 100644 --- a/sw/simulator/nps/nps_autopilot.h +++ b/sw/simulator/nps/nps_autopilot.h @@ -15,14 +15,9 @@ #if defined MOTOR_MIXING_NB_MOTOR #define NPS_COMMANDS_NB MOTOR_MIXING_NB_MOTOR #else -#ifdef NPS_ACTUATOR_NAMES #define NPS_COMMANDS_NB COMMANDS_NB -#else -/* not using explicitly set NPS_ACTUATOR_NAMES -> throttle,roll,pitch,yaw commands */ -#define NPS_COMMANDS_NB 4 -#endif -#endif -#endif +#endif /* #if defined MOTOR_MIXING_NB_MOTOR */ +#endif /* #ifndef NPS_COMMANDS_NB */ struct NpsAutopilot { double commands[NPS_COMMANDS_NB]; diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index c1c0269713..3745882467 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -77,7 +77,9 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha autopilot.launch = FALSE; - nps_radio_control_init(type_rc, num_rc_script, rc_dev); + if (rc_dev != NULL) { + nps_radio_control_init(type_rc, num_rc_script, rc_dev); + } nps_electrical_init(); nps_bypass_ahrs = NPS_BYPASS_AHRS; @@ -180,10 +182,8 @@ void nps_autopilot_run_step(double time) #ifdef COMMAND_YAW PRINT_CONFIG_VAR(COMMAND_YAW) autopilot.commands[COMMAND_YAW] = (double)commands[COMMAND_YAW] / MAX_PPRZ; -#else - autopilot.commands[3] = 0.; -#endif -#endif +#endif /* COMMAND_YAW */ +#endif /* NPS_ACTUATOR_NAMES */ // do the launch when clicking launch in GCS autopilot.launch = launch && !kill_throttle; diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp index 4d89830d60..5de4c08905 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.cpp +++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp @@ -52,7 +52,7 @@ // end ignore unused param warnings in JSBSim #pragma GCC diagnostic pop - +#include "nps_autopilot.h" #include "nps_fdm.h" #include "math/pprz_geodetic.h" #include "math/pprz_geodetic_double.h" @@ -124,12 +124,13 @@ * Around 1/10000 seems to be good for ground impacts */ #define MIN_DT (1.0/10240.0) +// TODO: maybe lower for slower CPUs & HITL? +//#define MIN_DT (1.0/1000.0) using namespace JSBSim; using namespace std; static void feed_jsbsim(double *commands, int commands_nb); -static void feed_jsbsim(double throttle, double aileron, double elevator, double rudder); static void fetch_state(void); static int check_for_nan(void); @@ -289,7 +290,7 @@ void nps_fdm_set_temperature(double temp, double h) * @param commands Pointer to array of doubles holding actuator commands * @param commands_nb Number of commands (length of array) */ -static void feed_jsbsim(double *commands, int commands_nb) +static void feed_jsbsim(double *commands, int commands_nb __attribute__((unused))) { #ifdef NPS_ACTUATOR_NAMES char buf[64]; @@ -302,42 +303,48 @@ static void feed_jsbsim(double *commands, int commands_nb) property = string(buf); FDMExec->GetPropertyManager()->GetNode(property)->SetDouble("", commands[i]); } -#else - if (commands_nb != 4) { - cerr << "commands_nb must be 4!" << endl; - exit(-1); - } - /* call version that directly feeds throttle, aileron, elevator, rudder */ - feed_jsbsim(commands[COMMAND_THROTTLE], commands[COMMAND_ROLL], commands[COMMAND_PITCH], commands[3]); -#endif -} +#else /* use COMMAND names */ -static void feed_jsbsim(double throttle, double aileron, double elevator, double rudder) -{ + // get FGFCS instance FGFCS *FCS = FDMExec->GetFCS(); - FGPropulsion *FProp = FDMExec->GetPropulsion(); // Set trims FCS->SetPitchTrimCmd(NPS_JSBSIM_PITCH_TRIM); FCS->SetRollTrimCmd(NPS_JSBSIM_ROLL_TRIM); FCS->SetYawTrimCmd(NPS_JSBSIM_YAW_TRIM); - // Set commands - FCS->SetDaCmd(aileron); - FCS->SetDeCmd(elevator); - FCS->SetDrCmd(rudder); - +#ifdef COMMAND_THROTTLE + FGPropulsion *FProp = FDMExec->GetPropulsion(); for (unsigned int i = 0; i < FDMExec->GetPropulsion()->GetNumEngines(); i++) { - FCS->SetThrottleCmd(i, throttle); + FCS->SetThrottleCmd(i, commands[COMMAND_THROTTLE]); - if (throttle > 0.01) { + // Hack to show spinning propellers in flight gear models + if (commands[COMMAND_THROTTLE] > 0.01) { FProp->SetStarter(1); } else { FProp->SetStarter(0); } } -} +#endif /* COMMAND_THROTTLE */ +#ifdef COMMAND_ROLL + FCS->SetDaCmd(commands[COMMAND_ROLL]); +#endif /* COMMAND_ROLL */ + +#ifdef COMMAND_PITCH + FCS->SetDeCmd(commands[COMMAND_PITCH]); +#endif /* COMMAND_PITCH */ + +#ifdef COMMAND_YAW + FCS->SetDrCmd(commands[COMMAND_YAW]); +#endif /* COMMAND_YAW */ + +#ifdef COMMAND_FLAP + FCS->SetDfCmd(commands[COMMAND_FLAP]); +#endif /* COMMAND_FLAP */ + +#endif /* NPS_ACTUATOR_NAMES */ +} /** * Populates the NPS fdm struct after a simulation step. @@ -597,7 +604,8 @@ static void init_jsbsim(double dt) } // initial commands to zero - feed_jsbsim(0.0, 0.0, 0.0, 0.0); + double init_commands[NPS_COMMANDS_NB] = {0.0}; + feed_jsbsim(init_commands, NPS_COMMANDS_NB); //loop JSBSim once w/o integrating if (!FDMExec->RunIC()) { diff --git a/sw/simulator/nps/nps_flightgear.c b/sw/simulator/nps/nps_flightgear.c index ae105d2a98..ac994e46e0 100644 --- a/sw/simulator/nps/nps_flightgear.c +++ b/sw/simulator/nps/nps_flightgear.c @@ -10,10 +10,11 @@ #include #include #include - +#include #include "std.h" #include "../flight_gear.h" +#include "nps_main.h" #include "nps_fdm.h" #include "nps_atmosphere.h" @@ -25,6 +26,9 @@ static struct { unsigned int time_offset; } flightgear; +pthread_t th_fg_rx; // fligh gear receive thread + +void* nps_flightgear_receive(void* data __attribute__((unused))); double htond(double x) { @@ -98,6 +102,9 @@ void nps_flightgear_init(const char *host, unsigned int port, unsigned int port time_t t = time(NULL); flightgear.initial_time = t; flightgear.time_offset = time_offset; + + // launch rx thread + pthread_create(&th_fg_rx, NULL, nps_flightgear_receive, NULL); } /** @@ -221,7 +228,8 @@ void nps_flightgear_send() /** * Receive Flight Gear environment messages */ -void nps_flightgear_receive() { +void* nps_flightgear_receive(void* data __attribute__((unused))) +{ if (flightgear.socket_in != -1) { // socket is correctly opened @@ -230,32 +238,37 @@ void nps_flightgear_receive() { size_t s_env = sizeof(env); int bytes_read; - //read first message - memset(&env, 0, s_env); - bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_DONTWAIT, NULL, NULL); - while (bytes_read != -1) { // while we read a message (empty buffer) - if (bytes_read == (int)s_env){ - // Update wind info - nps_atmosphere_set_wind_ned( - (double)env.wind_from_north, - (double)env.wind_from_east, - (double)env.wind_from_down); - } - else { - //error - printf("WARNING : ignoring packet with size %d (%d expected)", bytes_read, (int)s_env); - } - - //read next message + while(TRUE) + { + //read first message memset(&env, 0, s_env); - bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_DONTWAIT, NULL, NULL); - } + bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_WAITALL, NULL, NULL); + while (bytes_read != -1) { // while we read a message (empty buffer) + if (bytes_read == (int)s_env){ + // Update wind info + pthread_mutex_lock(&fdm_mutex); + nps_atmosphere_set_wind_ned( + (double)env.wind_from_north, + (double)env.wind_from_east, + (double)env.wind_from_down); + pthread_mutex_unlock(&fdm_mutex); + } + else { + //error + printf("WARNING : ignoring packet with size %d (%d expected)", bytes_read, (int)s_env); + } - if ((errno & (EAGAIN | EWOULDBLOCK)) == 0) { - perror("nps_flightgear_receive recvfrom()"); + //read next message + memset(&env, 0, s_env); + bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_WAITALL, NULL, NULL); + } + + if ((errno & (EAGAIN | EWOULDBLOCK)) == 0) { + perror("nps_flightgear_receive recvfrom()"); + } } } - + return NULL; } diff --git a/sw/simulator/nps/nps_flightgear.h b/sw/simulator/nps/nps_flightgear.h index d463d49cc4..183a07b73c 100644 --- a/sw/simulator/nps/nps_flightgear.h +++ b/sw/simulator/nps/nps_flightgear.h @@ -5,7 +5,6 @@ extern void nps_flightgear_init(const char* host, unsigned int port, unsigned int port_in, unsigned int time_offset); extern void nps_flightgear_send(); extern void nps_flightgear_send_fdm(); -extern void nps_flightgear_receive(); #endif /* NPS_FLIGHTGEAR_H */ diff --git a/sw/simulator/nps/nps_ins.h b/sw/simulator/nps/nps_ins.h new file mode 100644 index 0000000000..af18bfaea4 --- /dev/null +++ b/sw/simulator/nps/nps_ins.h @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * Copyright (C) 2012 The Paparazzi Team + * Copyright (C) 2016 Michal Podhradsky + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ +#ifndef NPS_INS_H +#define NPS_INS_H + +#include "std.h" +#include "nps_fdm.h" + +// if undefined, match with control frequency because that is how it should be used +#ifndef INS_FREQUENCY +#ifdef CONTROL_FREQUENCY +#define INS_FREQUENCY CONTROL_FREQUENCY +#else +#define INS_FREQUENCY PERIODIC_FREQUENCY +#endif +#endif + +extern uint8_t *ins_buffer; + +extern void nps_ins_init(void); +void nps_ins_fetch_data(struct NpsFdm *fdm_ins); +uint16_t nps_ins_fill_buffer(void); + +#endif /* NPS_INS_H */ diff --git a/sw/simulator/nps/nps_ins_vectornav.c b/sw/simulator/nps/nps_ins_vectornav.c new file mode 100644 index 0000000000..eff50175db --- /dev/null +++ b/sw/simulator/nps/nps_ins_vectornav.c @@ -0,0 +1,273 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * Copyright (C) 2012 The Paparazzi Team + * Copyright (C) 2016 Michal Podhradsky + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#include "nps_ins.h" +#include +#include "nps_fdm.h" +#include +#include + +/* + * Vectornav info + */ +#define VN_DATA_START 10 +#define VN_BUFFER_SIZE 512 +#define GPS_SEC_IN_DAY 86400 + +static uint8_t VN_SYNC = 0xFA; +static uint8_t VN_OUTPUT_GROUP = 0x39; +static uint16_t VN_GROUP_FIELD_1 = 0x01E9; +static uint16_t VN_GROUP_FIELD_2 = 0x061A; +static uint16_t VN_GROUP_FIELD_3 = 0x0140; +static uint16_t VN_GROUP_FIELD_4 = 0x0009; + +uint8_t vn_buffer[VN_BUFFER_SIZE]; + +uint8_t *ins_buffer; + +struct VectornavData { + uint64_t TimeStartup; + float YawPitchRoll[3]; + float AngularRate[3]; + double Position[3]; + float Velocity[3]; + float Accel[3]; + uint64_t Tow; + uint8_t NumSats; + uint8_t Fix; + float PosU[3]; + float VelU; + float LinearAccelBody[3]; + float YprU[3]; + uint16_t InsStatus; + float VelBody[3]; +}; + +struct VectornavData vn_data; + +void ins_vectornav_init(void) {} +void ins_vectornav_event(void) {} + +/** + * Calculates the 16-bit CRC for the given ASCII or binary message. + * The CRC is calculated over the packet starting just after the sync byte (not including the sync byte) + * and ending at the end of payload. + */ +unsigned short vn_calculate_crc(unsigned char data[], unsigned int length) +{ + unsigned int i; + unsigned short crc = 0; + for (i = 0; i < length; i++) { + crc = (unsigned char)(crc >> 8) | (crc << 8); + crc ^= data[i]; + crc ^= (unsigned char)(crc & 0xff) >> 4; + crc ^= crc << 12; + crc ^= (crc & 0x00ff) << 5; + } + return crc; +} + +void nps_ins_init(void) +{ + ins_buffer = &vn_buffer[0]; +} + + +/** + * @return GPS TOW + */ +static uint64_t vn_get_time_of_week(void) +{ + struct timeval curTime; + gettimeofday(&curTime, NULL); + int milli = curTime.tv_usec / 1000; + struct tm t_res; + localtime_r(&curTime.tv_sec, &t_res); + struct tm *tt = &t_res; + + uint64_t tow = GPS_SEC_IN_DAY * tt->tm_wday + 3600 * tt->tm_hour + 60 * tt->tm_min + tt->tm_sec; // sec + tow = tow * 1000; // tow to ms + tow = tow + milli; // tow with added ms + tow = tow * 1e6; // tow in nanoseconds + + return tow; +} + +void nps_ins_fetch_data(struct NpsFdm *fdm_ins) +{ + struct NpsFdm fdm_data; + memcpy(&fdm_data, fdm_ins, sizeof(struct NpsFdm)); + + // Timestamp + vn_data.TimeStartup = (uint64_t)(fdm_data.time * 1000000000.0); + + //Attitude, float, [degrees], yaw, pitch, roll, NED frame + vn_data.YawPitchRoll[0] = DegOfRad((float)fdm_data.ltp_to_body_eulers.psi); // yaw + vn_data.YawPitchRoll[1] = DegOfRad((float)fdm_data.ltp_to_body_eulers.theta); // pitch + vn_data.YawPitchRoll[2] = DegOfRad((float)fdm_data.ltp_to_body_eulers.phi); // roll + + // Rates (imu frame), float, [rad/s] + vn_data.AngularRate[0] = (float)fdm_data.body_ecef_rotvel.p; + vn_data.AngularRate[1] = (float)fdm_data.body_ecef_rotvel.q; + vn_data.AngularRate[2] = (float)fdm_data.body_ecef_rotvel.r; + + //Pos LLA, double,[beg, deg, m] + //The estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectfully. + vn_data.Position[0] = DegOfRad(fdm_data.lla_pos.lat); + vn_data.Position[1] = DegOfRad(fdm_data.lla_pos.lon); + vn_data.Position[2] = fdm_data.lla_pos.alt; // TODO: make sure it shows the correct starting point + + //VelNed, float [m/s] + //The estimated velocity in the North East Down (NED) frame, given in m/s. + vn_data.Velocity[0] = (float)fdm_data.ltp_ecef_vel.x; + vn_data.Velocity[1] = (float)fdm_data.ltp_ecef_vel.y; + vn_data.Velocity[2] = (float)fdm_data.ltp_ecef_vel.z; + + // Accel (imu-frame), float, [m/s^-2] + vn_data.Accel[0] = (float)fdm_data.body_ecef_accel.x; + vn_data.Accel[1] = (float)fdm_data.body_ecef_accel.y; + vn_data.Accel[2] = (float)fdm_data.body_ecef_accel.z; + + // tow (in nanoseconds), uint64 + vn_data.Tow = vn_get_time_of_week(); + + //num sats, uint8 + vn_data.NumSats = 8; // random number + + //gps fix, uint8 + vn_data.Fix = 3; // 3D fix + + //posU, float[3] + // TODO + + //velU, float + // TODO + + //linear acceleration imu-body frame, float [m/s^2] + vn_data.LinearAccelBody[0] = (float)fdm_data.ltp_ecef_vel.x; + vn_data.LinearAccelBody[1] = (float)fdm_data.ltp_ecef_vel.y; + vn_data.LinearAccelBody[2] = (float)fdm_data.ltp_ecef_vel.z; + + //YprU, float[3] + // TODO + + //instatus, uint16 + vn_data.InsStatus = 0x02; + + //Vel body, float [m/s] + // The estimated velocity in the body (i.e. imu) frame, given in m/s. + vn_data.VelBody[0] = (float)fdm_data.body_accel.x; + vn_data.VelBody[1] = (float)fdm_data.body_accel.y; + vn_data.VelBody[2] = (float)fdm_data.body_accel.z; +} + + +uint16_t nps_ins_fill_buffer(void) +{ + static uint16_t idx; + + vn_buffer[0] = VN_SYNC; + vn_buffer[1] = VN_OUTPUT_GROUP; + vn_buffer[2] = (uint8_t)(VN_GROUP_FIELD_1 >> 8); + vn_buffer[3] = (uint8_t)(VN_GROUP_FIELD_1); + vn_buffer[4] = (uint8_t)(VN_GROUP_FIELD_2 >> 8); + vn_buffer[5] = (uint8_t)(VN_GROUP_FIELD_2); + vn_buffer[6] = (uint8_t)(VN_GROUP_FIELD_3 >> 8); + vn_buffer[7] = (uint8_t)(VN_GROUP_FIELD_3); + vn_buffer[8] = (uint8_t)(VN_GROUP_FIELD_4 >> 8); + vn_buffer[9] = (uint8_t)(VN_GROUP_FIELD_4); + + idx = VN_DATA_START; + + // Timestamp + memcpy(&vn_buffer[idx], &vn_data.TimeStartup, sizeof(uint64_t)); + idx += sizeof(uint64_t); + + //Attitude, float, [degrees], yaw, pitch, roll, NED frame + memcpy(&vn_buffer[idx], &vn_data.YawPitchRoll, 3 * sizeof(float)); + idx += 3 * sizeof(float); + + // Rates (imu frame), float, [rad/s] + memcpy(&vn_buffer[idx], &vn_data.AngularRate, 3 * sizeof(float)); + idx += 3 * sizeof(float); + + //Pos LLA, double,[beg, deg, m] + //The estimated position given as latitude, longitude, and altitude given in [deg, deg, m] respectfully. + memcpy(&vn_buffer[idx], &vn_data.Position, 3 * sizeof(double)); + idx += 3 * sizeof(double); + + //VelNed, float [m/s] + //The estimated velocity in the North East Down (NED) frame, given in m/s. + memcpy(&vn_buffer[idx], &vn_data.Velocity, 3 * sizeof(float)); + idx += 3 * sizeof(float); + + // Accel (imu-frame), float, [m/s^-2] + memcpy(&vn_buffer[idx], &vn_data.Accel, 3 * sizeof(float)); + idx += 3 * sizeof(float); + + // tow (in nanoseconds), uint64 + memcpy(&vn_buffer[idx], &vn_data.Tow, sizeof(uint64_t)); + idx += sizeof(uint64_t); + + //num sats, uint8 + vn_buffer[idx] = vn_data.NumSats; + idx++; + + //gps fix, uint8 + vn_buffer[idx] = vn_data.Fix; + idx++; + + //posU, float[3] + memcpy(&vn_buffer[idx], &vn_data.PosU, 3 * sizeof(float)); + idx += 3 * sizeof(float); + + //velU, float + memcpy(&vn_buffer[idx], &vn_data.VelU, sizeof(float)); + idx += sizeof(float); + + //linear acceleration imu-body frame, float [m/s^2] + memcpy(&vn_buffer[idx], &vn_data.LinearAccelBody, 3 * sizeof(float)); + idx += 3 * sizeof(float); + + //YprU, float[3] + memcpy(&vn_buffer[idx], &vn_data.YprU, 3 * sizeof(float)); + idx += 3 * sizeof(float); + + //instatus, uint16 + memcpy(&vn_buffer[idx], &vn_data.InsStatus, sizeof(uint16_t)); + idx += sizeof(uint16_t); + + //Vel body, float [m/s] + // The estimated velocity in the body (i.e. imu) frame, given in m/s. + memcpy(&vn_buffer[idx], &vn_data.VelBody, 3 * sizeof(float)); + idx += 3 * sizeof(float); + + // calculate checksum & send + uint16_t chk = vn_calculate_crc(&vn_buffer[1], idx - 1); + vn_buffer[idx] = (uint8_t)(chk >> 8); + idx++; + vn_buffer[idx] = (uint8_t)(chk & 0xFF); + idx++; + + return idx; +} diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy.c index 4a62d901d5..92b915f34e 100644 --- a/sw/simulator/nps/nps_ivy.c +++ b/sw/simulator/nps/nps_ivy.c @@ -5,7 +5,9 @@ #include #include #include -#include + +#include +#include #include "generated/airframe.h" #include "math/pprz_algebra_float.h" @@ -16,7 +18,9 @@ #include "nps_sensors.h" #include "nps_atmosphere.h" -//#include "subsystems/navigation/common_flight_plan.h" +#include "generated/settings.h" +#include "pprzlink/dl_protocol.h" +#include "subsystems/datalink/downlink.h" #if USE_GPS #include "subsystems/gps.h" @@ -24,6 +28,12 @@ #include NPS_SENSORS_PARAMS +pthread_t th_ivy_main; // runs main Ivy loop +static MsgRcvPtr ivyPtr = NULL; +static int seq = 1; +static int ap_launch_index; + + /* Gaia Ivy functions */ static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)), void *user_data __attribute__((unused)), @@ -34,6 +44,18 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)), void *user_data __attribute__((unused)), int argc __attribute__((unused)), char *argv[]); +void* ivy_main_loop(void* data __attribute__((unused))); + +int find_launch_index(void); + + +void* ivy_main_loop(void* data __attribute__((unused))) +{ + IvyMainLoop(); + + return NULL; +} + void nps_ivy_init(char *ivy_bus) { const char *agent_name = AIRFRAME_NAME"_NPS"; @@ -56,6 +78,14 @@ void nps_ivy_init(char *ivy_bus) } else { IvyStart(ivy_bus); } + + nps_ivy_send_world_env = false; + + ap_launch_index = find_launch_index(); + + // Launch separate thread with IvyMainLoop() + pthread_create(&th_ivy_main, NULL, ivy_main_loop, NULL); + } /* @@ -90,8 +120,7 @@ static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)), /* * Send a WORLD_ENV_REQ message */ -static MsgRcvPtr ivyPtr = NULL; -static int seq = 1; + void nps_ivy_send_WORLD_ENV_REQ(void) { @@ -102,24 +131,41 @@ void nps_ivy_send_WORLD_ENV_REQ(void) } int pid = (int)getpid(); + // Bind to the reply ivyPtr = IvyBindMsg(on_WORLD_ENV, NULL, "^%d_%d (\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", pid, seq); + // Send actual request + struct NpsFdm fdm_ivy; + memcpy(&fdm_ivy, &fdm, sizeof(struct NpsFdm)); + IvySendMsg("nps %d_%d WORLD_ENV_REQ %f %f %f %f %f %f", pid, seq, - DegOfRad(fdm.lla_pos_pprz.lat), - DegOfRad(fdm.lla_pos_pprz.lon), - (fdm.hmsl), - (fdm.ltpprz_pos.x), - (fdm.ltpprz_pos.y), - (fdm.ltpprz_pos.z)); + DegOfRad(fdm_ivy.lla_pos_pprz.lat), + DegOfRad(fdm_ivy.lla_pos_pprz.lon), + (fdm_ivy.hmsl), + (fdm_ivy.ltpprz_pos.x), + (fdm_ivy.ltpprz_pos.y), + (fdm_ivy.ltpprz_pos.z)); seq++; + + nps_ivy_send_world_env = false; } +int find_launch_index(void) +{ + static const char ap_launch[] = "lau"; // short name has only 3 first letters + char *ap_settings[NB_SETTING] = SETTINGS_NAMES_SHORT; + + // list through the settinsg + for (uint8_t idx=0;idx= 0) || (ap_launch_index < NB_SETTING)) { + if (index==ap_launch_index){ + nps_update_launch_from_dl(value); + } + } } -void nps_ivy_display(void) + +void nps_ivy_display(struct NpsFdm* fdm_data, struct NpsSensors* sensors_data) { + struct NpsFdm fdm_ivy; + memcpy (&fdm_ivy, fdm_data, sizeof(struct NpsFdm)); + + struct NpsSensors sensors_ivy; + memcpy (&sensors_ivy, sensors_data, sizeof(struct NpsSensors)); + IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f", AC_ID, - DegOfRad(fdm.body_ecef_rotvel.p), - DegOfRad(fdm.body_ecef_rotvel.q), - DegOfRad(fdm.body_ecef_rotvel.r), - DegOfRad(fdm.ltp_to_body_eulers.phi), - DegOfRad(fdm.ltp_to_body_eulers.theta), - DegOfRad(fdm.ltp_to_body_eulers.psi)); + DegOfRad(fdm_ivy.body_ecef_rotvel.p), + DegOfRad(fdm_ivy.body_ecef_rotvel.q), + DegOfRad(fdm_ivy.body_ecef_rotvel.r), + DegOfRad(fdm_ivy.ltp_to_body_eulers.phi), + DegOfRad(fdm_ivy.ltp_to_body_eulers.theta), + DegOfRad(fdm_ivy.ltp_to_body_eulers.psi)); IvySendMsg("%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f", AC_ID, - (fdm.lla_pos_pprz.lat), - (fdm.lla_pos_geod.lat), - (fdm.lla_pos_geoc.lat), - (fdm.lla_pos_pprz.lon), - (fdm.lla_pos_geod.lon), - (fdm.lla_pos_pprz.alt), - (fdm.lla_pos_geod.alt), - (fdm.agl), - (fdm.hmsl)); + (fdm_ivy.lla_pos_pprz.lat), + (fdm_ivy.lla_pos_geod.lat), + (fdm_ivy.lla_pos_geoc.lat), + (fdm_ivy.lla_pos_pprz.lon), + (fdm_ivy.lla_pos_geod.lon), + (fdm_ivy.lla_pos_pprz.alt), + (fdm_ivy.lla_pos_geod.alt), + (fdm_ivy.agl), + (fdm_ivy.hmsl)); IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f", AC_ID, - (fdm.ltpprz_ecef_accel.x), - (fdm.ltpprz_ecef_accel.y), - (fdm.ltpprz_ecef_accel.z), - (fdm.ltpprz_ecef_vel.x), - (fdm.ltpprz_ecef_vel.y), - (fdm.ltpprz_ecef_vel.z), - (fdm.ltpprz_pos.x), - (fdm.ltpprz_pos.y), - (fdm.ltpprz_pos.z)); + (fdm_ivy.ltpprz_ecef_accel.x), + (fdm_ivy.ltpprz_ecef_accel.y), + (fdm_ivy.ltpprz_ecef_accel.z), + (fdm_ivy.ltpprz_ecef_vel.x), + (fdm_ivy.ltpprz_ecef_vel.y), + (fdm_ivy.ltpprz_ecef_vel.z), + (fdm_ivy.ltpprz_pos.x), + (fdm_ivy.ltpprz_pos.y), + (fdm_ivy.ltpprz_pos.z)); IvySendMsg("%d NPS_GYRO_BIAS %f %f %f", AC_ID, - DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.x) + sensors.gyro.bias_initial.x), - DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.y) + sensors.gyro.bias_initial.y), - DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.z) + sensors.gyro.bias_initial.z)); + DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.x) + sensors_ivy.gyro.bias_initial.x), + DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.y) + sensors_ivy.gyro.bias_initial.y), + DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.z) + sensors_ivy.gyro.bias_initial.z)); /* transform magnetic field to body frame */ struct DoubleVect3 h_body; - double_quat_vmult(&h_body, &fdm.ltp_to_body_quat, &fdm.ltp_h); + double_quat_vmult(&h_body, &fdm_ivy.ltp_to_body_quat, &fdm_ivy.ltp_h); IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f", AC_ID, - ((sensors.accel.value.x - sensors.accel.neutral.x) / NPS_ACCEL_SENSITIVITY_XX), - ((sensors.accel.value.y - sensors.accel.neutral.y) / NPS_ACCEL_SENSITIVITY_YY), - ((sensors.accel.value.z - sensors.accel.neutral.z) / NPS_ACCEL_SENSITIVITY_ZZ), + ((sensors_ivy.accel.value.x - sensors_ivy.accel.neutral.x) / NPS_ACCEL_SENSITIVITY_XX), + ((sensors_ivy.accel.value.y - sensors_ivy.accel.neutral.y) / NPS_ACCEL_SENSITIVITY_YY), + ((sensors_ivy.accel.value.z - sensors_ivy.accel.neutral.z) / NPS_ACCEL_SENSITIVITY_ZZ), h_body.x, h_body.y, h_body.z); IvySendMsg("%d NPS_WIND %f %f %f", AC_ID, - fdm.wind.x, - fdm.wind.y, - fdm.wind.z); + fdm_ivy.wind.x, + fdm_ivy.wind.y, + fdm_ivy.wind.z); + + if(nps_ivy_send_world_env){ + nps_ivy_send_WORLD_ENV_REQ(); + } } diff --git a/sw/simulator/nps/nps_ivy.h b/sw/simulator/nps/nps_ivy.h index 22b922dd30..6ada725a70 100644 --- a/sw/simulator/nps/nps_ivy.h +++ b/sw/simulator/nps/nps_ivy.h @@ -1,8 +1,13 @@ #ifndef NPS_IVY #define NPS_IVY +#include "nps_fdm.h" +#include "nps_sensors.h" + +bool nps_ivy_send_world_env; + extern void nps_ivy_init(char *ivy_bus); -extern void nps_ivy_display(void); +extern void nps_ivy_display(struct NpsFdm* fdm_ivy, struct NpsSensors* sensors_ivy); extern void nps_ivy_send_WORLD_ENV_REQ(void); #endif /* NPS_IVY */ diff --git a/sw/simulator/nps/nps_main.h b/sw/simulator/nps/nps_main.h index 61dcc5abc7..c6c9c2f4d4 100644 --- a/sw/simulator/nps/nps_main.h +++ b/sw/simulator/nps/nps_main.h @@ -1,6 +1,62 @@ #ifndef NPS_MAIN_H #define NPS_MAIN_H -extern void nps_set_time_factor(float time_factor); +#include +#include +#include "nps_fdm.h" +#include "mcu_periph/sys_time.h" +#include "nps_atmosphere.h" +#include "nps_sensors.h" +#include "nps_autopilot.h" + +#define SIM_DT (1./SYS_TIME_FREQUENCY) +#define DISPLAY_DT (1./30.) +#define HOST_TIMEOUT_MS 40 + +pthread_t th_flight_gear; // sends/receives flight gear packets +pthread_t th_display_ivy; // sends Ivy messages +pthread_t th_main_loop; // handles simulation + +pthread_mutex_t fdm_mutex; // mutex for fdm data + +int pauseSignal; // for catching SIGTSTP + +bool nps_main_parse_options(int argc, char **argv); + +int nps_main_init(int argc, char **argv); +void nps_radio_and_autopilot_init(void); +void nps_main_run_sim_step(void); +void nps_set_time_factor(float time_factor); + +void* nps_main_loop(void* data __attribute__((unused))); +void* nps_flight_gear_loop(void* data __attribute__((unused))); +void* nps_main_display(void* data __attribute__((unused))); + +void tstp_hdl(int n __attribute__((unused))); +void cont_hdl(int n __attribute__((unused))); + +double time_to_double(struct timeval *t); +double ntime_to_double(struct timespec *t); + +void nps_update_launch_from_dl(uint8_t value); + +struct NpsMain { + double real_initial_time; + double scaled_initial_time; + double host_time_factor; + double sim_time; + double display_time; + char *fg_host; + unsigned int fg_port; + unsigned int fg_port_in; + unsigned int fg_time_offset; + int fg_fdm; + char *js_dev; + char *spektrum_dev; + int rc_script; + char *ivy_bus; +}; + +struct NpsMain nps_main; #endif /* NPS_MAIN_H */ diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main_common.c similarity index 57% rename from sw/simulator/nps/nps_main.c rename to sw/simulator/nps/nps_main_common.c index e716bc14df..bf470eac52 100644 --- a/sw/simulator/nps/nps_main.c +++ b/sw/simulator/nps/nps_main_common.c @@ -1,6 +1,7 @@ /* * Copyright (C) 2009 Antoine Drouin * Copyright (C) 2012 The Paparazzi Team + * Copyright (C) 2016 Michal Podhradsky * * This file is part of paparazzi. * @@ -20,51 +21,14 @@ * Boston, MA 02111-1307, USA. */ -#include -#include -#include +#include "nps_main.h" #include -#include -#include +#include #include -#include "nps_main.h" -#include "nps_fdm.h" -#include "nps_sensors.h" -#include "nps_atmosphere.h" -#include "nps_autopilot.h" -#include "nps_ivy.h" #include "nps_flightgear.h" -#include "mcu_periph/sys_time.h" -#define SIM_DT (1./SYS_TIME_FREQUENCY) -#define DISPLAY_DT (1./30.) -#define HOST_TIMEOUT_MS 40 - -static struct { - double real_initial_time; - double scaled_initial_time; - double host_time_factor; - double sim_time; - double display_time; - char *fg_host; - unsigned int fg_port; - unsigned int fg_port_in; - unsigned int fg_time_offset; - int fg_fdm; - char *js_dev; - char *spektrum_dev; - int rc_script; - char *ivy_bus; -} nps_main; - -static bool nps_main_parse_options(int argc, char **argv); -static void nps_main_init(void); -static void nps_main_display(void); -static void nps_main_run_sim_step(void); -static gboolean nps_main_periodic(gpointer data __attribute__((unused))); - -int pauseSignal = 0; +#include "nps_ivy.h" void tstp_hdl(int n __attribute__((unused))) { @@ -77,6 +41,7 @@ void tstp_hdl(int n __attribute__((unused))) } } + void cont_hdl(int n __attribute__((unused))) { signal(SIGCONT, cont_hdl); @@ -84,13 +49,20 @@ void cont_hdl(int n __attribute__((unused))) printf("Press to continue.\n"); } + double time_to_double(struct timeval *t) { return ((double)t->tv_sec + (double)(t->tv_usec * 1e-6)); } -int main(int argc, char **argv) +double ntime_to_double(struct timespec *t) { + return ((double)t->tv_sec + (double)(t->tv_nsec * 1e-9)); +} + +int nps_main_init(int argc, char **argv) +{ + pauseSignal = 0; if (!nps_main_parse_options(argc, argv)) { return 1; } @@ -101,22 +73,6 @@ int main(int argc, char **argv) */ setbuf(stdout, NULL); - nps_main_init(); - - signal(SIGCONT, cont_hdl); - signal(SIGTSTP, tstp_hdl); - printf("Time factor is %f. (Press Ctrl-Z to change)\n", nps_main.host_time_factor); - - GMainLoop *ml = g_main_loop_new(NULL, FALSE); - g_timeout_add(HOST_TIMEOUT_MS, nps_main_periodic, NULL); - g_main_loop_run(ml); - - return 0; -} - - -static void nps_main_init(void) -{ nps_main.sim_time = 0.; nps_main.display_time = 0.; @@ -125,67 +81,22 @@ static void nps_main_init(void) nps_main.real_initial_time = time_to_double(&t); nps_main.scaled_initial_time = time_to_double(&t); - nps_ivy_init(nps_main.ivy_bus); nps_fdm_init(SIM_DT); nps_atmosphere_init(); nps_sensors_init(nps_main.sim_time); printf("Simulating with dt of %f\n", SIM_DT); - enum NpsRadioControlType rc_type; - char *rc_dev = NULL; - if (nps_main.js_dev) { - rc_type = JOYSTICK; - rc_dev = nps_main.js_dev; - } else if (nps_main.spektrum_dev) { - rc_type = SPEKTRUM; - rc_dev = nps_main.spektrum_dev; - } else { - rc_type = SCRIPT; - } - nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev); - - if (nps_main.fg_host) - nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_port_in, nps_main.fg_time_offset); - + nps_radio_and_autopilot_init(); #if DEBUG_NPS_TIME printf("host_time_factor,host_time_elapsed,host_time_now,scaled_initial_time,sim_time_before,display_time_before,sim_time_after,display_time_after\n"); #endif -} + signal(SIGCONT, cont_hdl); + signal(SIGTSTP, tstp_hdl); + printf("Time factor is %f. (Press Ctrl-Z to change)\n", nps_main.host_time_factor); - - -static void nps_main_run_sim_step(void) -{ - // printf("sim at %f\n", nps_main.sim_time); - - nps_atmosphere_update(SIM_DT); - - nps_autopilot_run_systime_step(); - - nps_fdm_run_step(autopilot.launch, autopilot.commands, NPS_COMMANDS_NB); - - nps_sensors_run_step(nps_main.sim_time); - - nps_autopilot_run_step(nps_main.sim_time); - -} - - -static void nps_main_display(void) -{ - // printf("display at %f\n", nps_main.display_time); - nps_ivy_display(); - - if (nps_main.fg_host) { - if (nps_main.fg_fdm) { - nps_flightgear_send_fdm(); - } else { - nps_flightgear_send(); - } - } - nps_flightgear_receive(); + return 0; } @@ -217,84 +128,7 @@ void nps_set_time_factor(float time_factor) } -static gboolean nps_main_periodic(gpointer data __attribute__((unused))) -{ - struct timeval tv_now; - double host_time_now; - - if (pauseSignal) { - char line[128]; - double tf = 1.0; - double t1, t2, irt; - - gettimeofday(&tv_now, NULL); - t1 = time_to_double(&tv_now); - /* unscale to initial real time*/ - irt = t1 - (t1 - nps_main.scaled_initial_time) * nps_main.host_time_factor; - - printf("Press to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ", - nps_main.host_time_factor); - fflush(stdout); - if (fgets(line, 127, stdin)) { - if ((sscanf(line, " %le ", &tf) == 1)) { - if (tf > 0 && tf < 1000) { - nps_main.host_time_factor = tf; - } - } - printf("Time factor is %f\n", nps_main.host_time_factor); - } - gettimeofday(&tv_now, NULL); - t2 = time_to_double(&tv_now); - /* add the pause to initial real time */ - irt += t2 - t1; - nps_main.real_initial_time += t2 - t1; - /* convert to scaled initial real time */ - nps_main.scaled_initial_time = t2 - (t2 - irt) / nps_main.host_time_factor; - pauseSignal = 0; - } - - gettimeofday(&tv_now, NULL); - host_time_now = time_to_double(&tv_now); - double host_time_elapsed = nps_main.host_time_factor * (host_time_now - nps_main.scaled_initial_time); - -#if DEBUG_NPS_TIME - printf("%f,%f,%f,%f,%f,%f,", nps_main.host_time_factor, host_time_elapsed, host_time_now, nps_main.scaled_initial_time, - nps_main.sim_time, nps_main.display_time); -#endif - - int cnt = 0; - static int prev_cnt = 0; - static int grow_cnt = 0; - while (nps_main.sim_time <= host_time_elapsed) { - nps_main_run_sim_step(); - nps_main.sim_time += SIM_DT; - if (nps_main.display_time < (host_time_now - nps_main.real_initial_time)) { - nps_main_display(); - nps_main.display_time += DISPLAY_DT; - } - cnt++; - } - - /* Check to make sure the simulation doesn't get too far behind real time looping */ - if (cnt > (prev_cnt)) {grow_cnt++;} - else { grow_cnt--;} - if (grow_cnt < 0) {grow_cnt = 0;} - prev_cnt = cnt; - - if (grow_cnt > 10) { - printf("Warning: The time factor is too large for efficient operation! Please reduce the time factor.\n"); - } - -#if DEBUG_NPS_TIME - printf("%f,%f\n", nps_main.sim_time, nps_main.display_time); -#endif - - return TRUE; - -} - - -static bool nps_main_parse_options(int argc, char **argv) +bool nps_main_parse_options(int argc, char **argv) { nps_main.fg_host = NULL; @@ -391,3 +225,93 @@ static bool nps_main_parse_options(int argc, char **argv) } return TRUE; } + + +void *nps_flight_gear_loop(void *data __attribute__((unused))) +{ + struct timespec requestStart; + struct timespec requestEnd; + struct timespec waitFor; + long int period_ns = DISPLAY_DT * 1000000000L; // thread period in nanoseconds + long int task_ns = 0; // time it took to finish the task in nanoseconds + + + nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_port_in, nps_main.fg_time_offset); + + while (TRUE) { + clock_gettime(CLOCK_REALTIME, &requestStart); + + pthread_mutex_lock(&fdm_mutex); + if (nps_main.fg_host) { + if (nps_main.fg_fdm) { + nps_flightgear_send_fdm(); + } else { + nps_flightgear_send(); + } + } + pthread_mutex_unlock(&fdm_mutex); + + clock_gettime(CLOCK_REALTIME, &requestEnd); + + // Calculate time it took + task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec); + + // task took less than one period, sleep for the rest of time + if (task_ns < period_ns) { + waitFor.tv_sec = 0; + waitFor.tv_nsec = period_ns - task_ns; + nanosleep(&waitFor, NULL); + } else { + // task took longer than the period + printf("FG THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n", + (double)task_ns / 1E6, (double)period_ns / 1E6); + } + } + + return(NULL); +} + + + +void *nps_main_display(void *data __attribute__((unused))) +{ + struct timespec requestStart; + struct timespec requestEnd; + struct timespec waitFor; + long int period_ns = 3 * DISPLAY_DT * 1000000000L; // thread period in nanoseconds + long int task_ns = 0; // time it took to finish the task in nanoseconds + + struct NpsFdm fdm_ivy; + struct NpsSensors sensors_ivy; + + nps_ivy_init(nps_main.ivy_bus); + + while (TRUE) { + clock_gettime(CLOCK_REALTIME, &requestStart); + + pthread_mutex_lock(&fdm_mutex); + memcpy(&fdm_ivy, &fdm, sizeof(fdm)); + memcpy(&sensors_ivy, &sensors, sizeof(sensors)); + pthread_mutex_unlock(&fdm_mutex); + + nps_ivy_display(&fdm_ivy, &sensors_ivy); + + clock_gettime(CLOCK_REALTIME, &requestEnd); + + // Calculate time it took + task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec); + + // task took less than one period, sleep for the rest of time + if (task_ns < period_ns) { + waitFor.tv_sec = 0; + waitFor.tv_nsec = period_ns - task_ns; + nanosleep(&waitFor, NULL); + } else { + // task took longer than the period + printf("IVY DISPLAY THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n", + (double)task_ns / 1E6, (double)period_ns / 1E6); + } + + } + return(NULL); +} diff --git a/sw/simulator/nps/nps_main_hitl.c b/sw/simulator/nps/nps_main_hitl.c new file mode 100644 index 0000000000..6177629f68 --- /dev/null +++ b/sw/simulator/nps/nps_main_hitl.c @@ -0,0 +1,334 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * Copyright (C) 2012 The Paparazzi Team + * Copyright (C) 2016 Michal Podhradsky + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#include +#include +#include +#include + +#include "termios.h" +#include +#include + + +#include "paparazzi.h" +#include "pprzlink/messages.h" +#include "pprzlink/dl_protocol.h" +#include "pprzlink/pprz_transport.h" +#include "generated/airframe.h" + +/* Message id helpers */ +#define SenderIdOfPprzMsg(x) (x[0]) +#define IdOfPprzMsg(x) (x[1]) + +#include "nps_main.h" +#include "nps_sensors.h" +#include "nps_atmosphere.h" +#include "nps_autopilot.h" +#include "nps_ivy.h" +#include "nps_flightgear.h" +#include "mcu_periph/sys_time.h" + +#include "nps_ins.h" + +void *nps_ins_data_loop(void *data __attribute__((unused))); +void *nps_ap_data_loop(void *data __attribute__((unused))); + +pthread_t th_ins_data; // sends INS packets to the autopilot +pthread_t th_ap_data; // receives commands from the autopilot + +#define NPS_MAX_MSG_SIZE 512 + +int main(int argc, char **argv) +{ + nps_main_init(argc, argv); + + if (nps_main.fg_host) { + pthread_create(&th_flight_gear, NULL, nps_flight_gear_loop, NULL); + } + pthread_create(&th_display_ivy, NULL, nps_main_display, NULL); + pthread_create(&th_main_loop, NULL, nps_main_loop, NULL); + pthread_create(&th_ins_data, NULL, nps_ins_data_loop, NULL); + pthread_create(&th_ap_data, NULL, nps_ap_data_loop, NULL); + pthread_join(th_main_loop, NULL); + + return 0; +} + +void nps_radio_and_autopilot_init(void) +{ + enum NpsRadioControlType rc_type; + char *rc_dev = NULL; + if (nps_main.js_dev) { + rc_type = JOYSTICK; + rc_dev = nps_main.js_dev; + } else if (nps_main.spektrum_dev) { + rc_type = SPEKTRUM; + rc_dev = nps_main.spektrum_dev; + } else { + rc_type = SCRIPT; + } + nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev); +} + +void nps_update_launch_from_dl(uint8_t value) +{ + autopilot.launch = value; +} + +void nps_main_run_sim_step(void) +{ + nps_atmosphere_update(SIM_DT); + nps_fdm_run_step(autopilot.launch, autopilot.commands, NPS_COMMANDS_NB); +} + +void *nps_ins_data_loop(void *data __attribute__((unused))) +{ + struct timespec requestStart; + struct timespec requestEnd; + struct timespec waitFor; + long int period_ns = (1. / INS_FREQUENCY) * 1000000000L; // thread period in nanoseconds + long int task_ns = 0; // time it took to finish the task in nanoseconds + + nps_ins_init(); // initialize ins variables and pointers + + // configure port + int fd = open(INS_DEV, O_WRONLY | O_NOCTTY | O_SYNC);//open(INS_DEV, O_RDWR | O_NOCTTY); + if (fd < 0) { + printf("INS THREAD: data loop error opening port %i\n", fd); + return(NULL); + } + + struct termios new_settings; + tcgetattr(fd, &new_settings); + memset(&new_settings, 0, sizeof(new_settings)); + new_settings.c_iflag = 0; + new_settings.c_cflag = 0; + new_settings.c_lflag = 0; + new_settings.c_cc[VMIN] = 0; + new_settings.c_cc[VTIME] = 0; + cfsetispeed(&new_settings, (speed_t)INS_BAUD); + cfsetospeed(&new_settings, (speed_t)INS_BAUD); + tcsetattr(fd, TCSANOW, &new_settings); + + struct NpsFdm fdm_ins; + + while (TRUE) { + // lock mutex + pthread_mutex_lock(&fdm_mutex); + + // start timing + clock_gettime(CLOCK_REALTIME, &requestStart); + + // make a copy of fdm struct to speed things up + memcpy(&fdm_ins, &fdm, sizeof(fdm)); + + // unlock mutex + pthread_mutex_unlock(&fdm_mutex); + + // fetch data + nps_ins_fetch_data(&fdm_ins); + + // send ins data here + static uint16_t idx; + idx = nps_ins_fill_buffer(); + + static int wlen; + wlen = write(fd, ins_buffer, idx); + if (wlen != idx) { + printf("INS THREAD: Warning - sent only %u bytes to the autopilot, instead of expected %u\n", wlen, idx); + } + + clock_gettime(CLOCK_REALTIME, &requestEnd); + + // Calculate time it took + task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec); + + // task took less than one period, sleep for the rest of time + if (task_ns < period_ns) { + waitFor.tv_sec = 0; + waitFor.tv_nsec = period_ns - task_ns; + nanosleep(&waitFor, NULL); + } else { + // task took longer than the period + printf("INS THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n", + (double)task_ns / 1E6, (double)period_ns / 1E6); + } + } + return(NULL); +} + + + +void *nps_ap_data_loop(void *data __attribute__((unused))) +{ + // configure port + int fd = open(AP_DEV, O_RDONLY | O_NOCTTY); + if (fd < 0) { + printf("AP data loop error opening port %i\n", fd); + return(NULL); + } + + struct termios new_settings; + tcgetattr(fd, &new_settings); + memset(&new_settings, 0, sizeof(new_settings)); + new_settings.c_iflag = 0; + new_settings.c_cflag = 0; + new_settings.c_lflag = 0; + new_settings.c_cc[VMIN] = 1; + new_settings.c_cc[VTIME] = 5; + cfsetispeed(&new_settings, (speed_t)AP_BAUD); + cfsetospeed(&new_settings, (speed_t)AP_BAUD); + tcsetattr(fd, TCSANOW, &new_settings); + + int rdlen; + uint8_t buf[NPS_MAX_MSG_SIZE]; + uint8_t cmd_len; + pprz_t cmd_buf[NPS_COMMANDS_NB]; + + struct pprz_transport pprz_tp_logger; + + while (TRUE) { + // receive messages from the autopilot + rdlen = read(fd, buf, sizeof(buf) - 1); + + for (int i = 0; i < rdlen; i++) { + // parse data + parse_pprz(&pprz_tp_logger, buf[i]); + + // if msg_available then read + if (pprz_tp_logger.trans_rx.msg_received) { + for (int k = 0; k < pprz_tp_logger.trans_rx.payload_len; k++) { + buf[k] = pprz_tp_logger.trans_rx.payload[k]; + } + //Parse message + uint8_t sender_id = SenderIdOfPprzMsg(buf); + uint8_t msg_id = IdOfPprzMsg(buf); + + // parse telemetry messages coming from other AC + if (sender_id != AC_ID) { + switch (msg_id) { + default: { + break; + } + } + } else { + /* parse telemetry messages coming from the correct AC_ID */ + switch (msg_id) { + case DL_COMMANDS: + // parse commands message + cmd_len = DL_COMMANDS_values_length(buf); + memcpy(&cmd_buf, DL_COMMANDS_values(buf), cmd_len * sizeof(int16_t)); + pthread_mutex_lock(&fdm_mutex); + // update commands + for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) { + autopilot.commands[i] = (double)cmd_buf[i] / MAX_PPRZ; + } + // hack: invert pitch to fit most JSBSim models + autopilot.commands[COMMAND_PITCH] = -(double)cmd_buf[COMMAND_PITCH] / MAX_PPRZ; + pthread_mutex_unlock(&fdm_mutex); + break; + case DL_MOTOR_MIXING: + // parse actuarors message + cmd_len = DL_MOTOR_MIXING_values_length(buf); + memcpy(&cmd_buf, DL_MOTOR_MIXING_values(buf), cmd_len * sizeof(int16_t)); + pthread_mutex_lock(&fdm_mutex); + // update commands + for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) { + autopilot.commands[i] = (double)cmd_buf[i] / MAX_PPRZ; + } + pthread_mutex_unlock(&fdm_mutex); + break; + default: + break; + } + } + pprz_tp_logger.trans_rx.msg_received = false; + } + } + } + return(NULL); +} + + + + +void *nps_main_loop(void *data __attribute__((unused))) +{ + struct timespec requestStart; + struct timespec requestEnd; + struct timespec waitFor; + long int period_ns = SIM_DT * 1000000000L; // thread period in nanoseconds + long int task_ns = 0; // time it took to finish the task in nanoseconds + + // check the sim time difference from the realtime + // fdm.time - simulation time + struct timespec startTime; + struct timespec realTime; + clock_gettime(CLOCK_REALTIME, &startTime); + double start_secs = ntime_to_double(&startTime); + double real_secs = 0; + double real_time = 0; + static int guard; + + while (TRUE) { + clock_gettime(CLOCK_REALTIME, &requestStart); + + pthread_mutex_lock(&fdm_mutex); + + // check the current simulation time + clock_gettime(CLOCK_REALTIME, &realTime); + real_secs = ntime_to_double(&realTime); + real_time = real_secs - start_secs; // real time elapsed + + guard = 0; + while ((real_time - fdm.time) > SIM_DT) { + nps_main_run_sim_step(); + guard++; + if (guard > 2) { + //If we are too much behind, catch up incrementaly + break; + } + } + pthread_mutex_unlock(&fdm_mutex); + + clock_gettime(CLOCK_REALTIME, &requestEnd); + + // Calculate time it took + task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec); + + // task took less than one period, sleep for the rest of time + if (task_ns < period_ns) { + waitFor.tv_sec = 0; + waitFor.tv_nsec = period_ns - task_ns; + nanosleep(&waitFor, NULL); + } else { + // task took longer than the period + printf("MAIN THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n", + (double)task_ns / 1E6, (double)period_ns / 1E6); + } + } + return(NULL); +} + + diff --git a/sw/simulator/nps/nps_main_sitl.c b/sw/simulator/nps/nps_main_sitl.c new file mode 100644 index 0000000000..bf2b03fb84 --- /dev/null +++ b/sw/simulator/nps/nps_main_sitl.c @@ -0,0 +1,182 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * Copyright (C) 2012 The Paparazzi Team + * Copyright (C) 2016 Michal Podhradsky + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#include +#include +#include +#include + +#include "nps_main.h" +#include "nps_fdm.h" + + + + +int main(int argc, char **argv) +{ + if (nps_main_init(argc, argv)) { + return 1; + } + + if (nps_main.fg_host) { + pthread_create(&th_flight_gear, NULL, nps_flight_gear_loop, NULL); + } + pthread_create(&th_display_ivy, NULL, nps_main_display, NULL); + pthread_create(&th_main_loop, NULL, nps_main_loop, NULL); + pthread_join(th_main_loop, NULL); + + return 0; +} + + +void nps_update_launch_from_dl(uint8_t value __attribute__((unused))) {} + + +void nps_radio_and_autopilot_init(void) +{ + enum NpsRadioControlType rc_type; + char *rc_dev = NULL; + if (nps_main.js_dev) { + rc_type = JOYSTICK; + rc_dev = nps_main.js_dev; + } else if (nps_main.spektrum_dev) { + rc_type = SPEKTRUM; + rc_dev = nps_main.spektrum_dev; + } else { + rc_type = SCRIPT; + } + nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev); +} + + +void nps_main_run_sim_step(void) +{ + nps_atmosphere_update(SIM_DT); + + nps_autopilot_run_systime_step(); + + nps_fdm_run_step(autopilot.launch, autopilot.commands, NPS_COMMANDS_NB); + + nps_sensors_run_step(nps_main.sim_time); + + nps_autopilot_run_step(nps_main.sim_time); + +} + + +void *nps_main_loop(void *data __attribute__((unused))) +{ + struct timespec requestStart; + struct timespec requestEnd; + struct timespec waitFor; + long int period_ns = HOST_TIMEOUT_MS * 1000000LL; // thread period in nanoseconds + long int task_ns = 0; // time it took to finish the task in nanoseconds + + struct timeval tv_now; + double host_time_now; + + while (TRUE) { + if (pauseSignal) { + char line[128]; + double tf = 1.0; + double t1, t2, irt; + + gettimeofday(&tv_now, NULL); + t1 = time_to_double(&tv_now); + // unscale to initial real time + irt = t1 - (t1 - nps_main.scaled_initial_time) * nps_main.host_time_factor; + + printf("Press to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ", + nps_main.host_time_factor); + fflush(stdout); + if (fgets(line, 127, stdin)) { + if ((sscanf(line, " %le ", &tf) == 1)) { + if (tf > 0 && tf < 1000) { + nps_main.host_time_factor = tf; + } + } + printf("Time factor is %f\n", nps_main.host_time_factor); + } + gettimeofday(&tv_now, NULL); + t2 = time_to_double(&tv_now); + // add the pause to initial real time + irt += t2 - t1; + nps_main.real_initial_time += t2 - t1; + // convert to scaled initial real time + nps_main.scaled_initial_time = t2 - (t2 - irt) / nps_main.host_time_factor; + pauseSignal = 0; + } + + clock_gettime(CLOCK_REALTIME, &requestStart); // init measurement (after the pause signal) + + gettimeofday(&tv_now, NULL); + host_time_now = time_to_double(&tv_now); + double host_time_elapsed = nps_main.host_time_factor * (host_time_now - nps_main.scaled_initial_time); + +#if DEBUG_NPS_TIME + printf("%f,%f,%f,%f,%f,%f,", nps_main.host_time_factor, host_time_elapsed, host_time_now, nps_main.scaled_initial_time, + nps_main.sim_time, nps_main.display_time); +#endif + + int cnt = 0; + static int prev_cnt = 0; + static int grow_cnt = 0; + while (nps_main.sim_time <= host_time_elapsed) { + pthread_mutex_lock(&fdm_mutex); + nps_main_run_sim_step(); + nps_main.sim_time += SIM_DT; + pthread_mutex_unlock(&fdm_mutex); + cnt++; + } + + // Check to make sure the simulation doesn't get too far behind real time looping + if (cnt > (prev_cnt)) {grow_cnt++;} + else { grow_cnt--;} + if (grow_cnt < 0) {grow_cnt = 0;} + prev_cnt = cnt; + + if (grow_cnt > 10) { + printf("Warning: The time factor is too large for efficient operation! Please reduce the time factor.\n"); + } + +#if DEBUG_NPS_TIME + printf("%f,%f\n", nps_main.sim_time, nps_main.display_time); +#endif + + clock_gettime(CLOCK_REALTIME, &requestEnd); // end measurement + + // Calculate time it took + task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec); + + if (task_ns > 0) { + waitFor.tv_sec = 0; + waitFor.tv_nsec = period_ns - task_ns; + nanosleep(&waitFor, NULL); + } else { + // task took longer than the period + printf("MAIN THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n", + (double)task_ns / 1E6, (double)period_ns / 1E6); + } + } + return(NULL); +} diff --git a/sw/simulator/nps/nps_radio_control.c b/sw/simulator/nps/nps_radio_control.c index cfe1398739..2cfc4224a4 100644 --- a/sw/simulator/nps/nps_radio_control.c +++ b/sw/simulator/nps/nps_radio_control.c @@ -48,6 +48,8 @@ void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char break; case SCRIPT: break; + default: + break; } } diff --git a/sw/simulator/nps/nps_radio_control_spektrum.c b/sw/simulator/nps/nps_radio_control_spektrum.c index a84db7319b..c9faf6849f 100644 --- a/sw/simulator/nps/nps_radio_control_spektrum.c +++ b/sw/simulator/nps/nps_radio_control_spektrum.c @@ -16,6 +16,8 @@ #define IUCLC 0 #endif +#define CHANNEL_OF_FRAME(i) ((((frame_buf[2*i]<<8) + frame_buf[2*i+1])&0x03FF)-512) + static int sp_fd; static gboolean on_serial_data_received(GIOChannel *source, @@ -132,19 +134,12 @@ static void parse_data(char *buf, int len) } } - -#define CHANNEL_OF_FRAME(i) ((((frame_buf[2*i]<<8) + frame_buf[2*i+1])&0x03FF)-512) static void handle_frame(void) { - - nps_radio_control.roll = (float)CHANNEL_OF_FRAME(0) / -340.; nps_radio_control.throttle = (float)(CHANNEL_OF_FRAME(1) + 340) / 680.; nps_radio_control.pitch = (float)CHANNEL_OF_FRAME(2) / -340.; nps_radio_control.yaw = (float)CHANNEL_OF_FRAME(3) / -340.; nps_radio_control.mode = (float)CHANNEL_OF_FRAME(5) / 340.; - - // printf("%f %f %f %f %f \n", nps_radio_control.roll, nps_radio_control.throttle, nps_radio_control.pitch, nps_radio_control.yaw, nps_radio_control.mode); - }