diff --git a/Makefile.ac b/Makefile.ac index 2ce27693ba..1b9c25a1c7 100644 --- a/Makefile.ac +++ b/Makefile.ac @@ -195,6 +195,16 @@ endif @echo "#######################################" $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) PAPARAZZI_QT_GEN=$(PAPARAZZI_QT_GEN) TARGET=$* Q=$(Q) $(GENERATORS)/gen_aircraft.out -all -name $(AIRCRAFT) -target $* -conf $(CONF_XML) +# generate headers for HITL as is it was AP target +hitl.ac_h : $(GENERATORS)/gen_aircraft.out + $(Q)if (expr "$(AIRCRAFT)") > /dev/null; then : ; else echo "AIRCRAFT undefined: type 'make AIRCRAFT=AircraftName ...'"; exit 1; fi + @echo "#######################################" + @echo "# BUILD AIRCRAFT=$(AIRCRAFT), TARGET hitl" + @echo "#######################################" + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) PAPARAZZI_QT_GEN=$(PAPARAZZI_QT_GEN) TARGET=hitl Q=$(Q) $(GENERATORS)/gen_aircraft.out -airframe -flight_plan -settings -name $(AIRCRAFT) -target hitl -conf $(CONF_XML) + $(Q)rm -f $(AIRCRAFT_BUILD_DIR)/Makefile.ac + $(Q)PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME) PAPARAZZI_QT_GEN=$(PAPARAZZI_QT_GEN) TARGET=ap Q=$(Q) $(GENERATORS)/gen_aircraft.out -all -name $(AIRCRAFT) -target ap -conf $(CONF_XML) + %.qt: %.ac_h @echo "GENERATED Qt project" @@ -202,9 +212,17 @@ endif $(MAKE) TARGET=$* -f Makefile.ac all_ac_h cd $(AIRBORNE); $(MAKE) -j$(NPROCS) TARGET=$* all +hitl.compile: hitl.ac_h | print_version + $(MAKE) TARGET=ap -f Makefile.ac all_ac_h + cd $(AIRBORNE); $(MAKE) -j$(NPROCS) TARGET=ap all + cd $(AIRBORNE); $(MAKE) -j$(NPROCS) TARGET=hitl AP_LAUNCH=$(shell grep ' /dev/null; then : ; else echo "AIRCRAFT undefined: type 'make AIRCRAFT=AircraftName ...'"; exit 1; fi @echo "CLEANING $(AIRCRAFT)" diff --git a/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml b/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml index ff2c7a4a85..18e2c1f3d6 100644 --- a/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml +++ b/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml @@ -31,17 +31,6 @@ Aggie Air ARK - - - - - - - - - - - diff --git a/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml b/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml index 8e6ec4f38b..2d291c40bd 100644 --- a/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml +++ b/conf/airframes/AGGIEAIR/aggieair_atomic_lia.xml @@ -32,16 +32,6 @@ AggieAir Atomic Tangerine - - - - - - - - - - diff --git a/conf/airframes/AGGIEAIR/aggieair_blujay_goose.xml b/conf/airframes/AGGIEAIR/aggieair_blujay_goose.xml index fcc85f6a82..6bdf1e15a4 100644 --- a/conf/airframes/AGGIEAIR/aggieair_blujay_goose.xml +++ b/conf/airframes/AGGIEAIR/aggieair_blujay_goose.xml @@ -27,16 +27,6 @@ AggieAir Blujayujay - - - - - - - - - - diff --git a/conf/airframes/AGGIEAIR/aggieair_control_panel.xml b/conf/airframes/AGGIEAIR/aggieair_control_panel.xml index 56dbd4325d..f559aaad2f 100644 --- a/conf/airframes/AGGIEAIR/aggieair_control_panel.xml +++ b/conf/airframes/AGGIEAIR/aggieair_control_panel.xml @@ -195,99 +195,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -310,50 +217,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/airframes/AGGIEAIR/aggieair_el_captitan_lia.xml b/conf/airframes/AGGIEAIR/aggieair_el_captitan_lia.xml index 03c91fb8d6..39aee2aff8 100644 --- a/conf/airframes/AGGIEAIR/aggieair_el_captitan_lia.xml +++ b/conf/airframes/AGGIEAIR/aggieair_el_captitan_lia.xml @@ -32,16 +32,6 @@ AggieAir El Capitan - - - - - - - - - - diff --git a/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml b/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml index b714e497bd..db30b783e2 100644 --- a/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml +++ b/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml @@ -27,15 +27,6 @@ AggieAir RP3 Minion - - - - - - - - - diff --git a/conf/airframes/AGGIEAIR/aggieair_minionsim_lia.xml b/conf/airframes/AGGIEAIR/aggieair_minionsim_lia.xml index 89a4e5566a..e46d7a2c18 100644 --- a/conf/airframes/AGGIEAIR/aggieair_minionsim_lia.xml +++ b/conf/airframes/AGGIEAIR/aggieair_minionsim_lia.xml @@ -33,16 +33,6 @@ AggieAir Minion Sim - - - - - - - - - - diff --git a/conf/airframes/AGGIEAIR/aggieair_minty_lia.xml b/conf/airframes/AGGIEAIR/aggieair_minty_lia.xml index 0633ce6fc1..a328e003c6 100644 --- a/conf/airframes/AGGIEAIR/aggieair_minty_lia.xml +++ b/conf/airframes/AGGIEAIR/aggieair_minty_lia.xml @@ -32,16 +32,6 @@ AggieAir Minty Fresh - - - - - - - - - - diff --git a/conf/airframes/examples/matek_h7_fixedwing_hitl.xml b/conf/airframes/examples/matek_h7_fixedwing_hitl.xml new file mode 100644 index 0000000000..59190e363c --- /dev/null +++ b/conf/airframes/examples/matek_h7_fixedwing_hitl.xml @@ -0,0 +1,238 @@ + + + + + + Mateksys FC H743 SLIM Fixedwing HITL + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + +
+ + +
+ +
+ + + + + + + + +
+ + + +
+ + +
+ + + +
+ + + + + +
+ +
+ + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + +
+ +
+ + + +
+ + diff --git a/conf/airframes/examples/matek_h7_rotorcraft_hitl.xml b/conf/airframes/examples/matek_h7_rotorcraft_hitl.xml new file mode 100644 index 0000000000..f63110b2f2 --- /dev/null +++ b/conf/airframes/examples/matek_h7_rotorcraft_hitl.xml @@ -0,0 +1,279 @@ + + + + + + + Mateksys FC H743 SLIM Rotorcraft HITL + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + +
+ +
+ + + + +
+ +
+ + + + + +
+ +
+ + + + + +
+ +
+ + + + +
+ + diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index d14e03ef43..0aabaac962 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -87,4 +87,26 @@ settings_modules="modules/ahrs_float_mlkf.xml modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/stabilization_int_quat.xml" gui_color="blue" /> + + diff --git a/conf/control_panel_example.xml b/conf/control_panel_example.xml index aa01236bb9..f8e485840c 100644 --- a/conf/control_panel_example.xml +++ b/conf/control_panel_example.xml @@ -192,6 +192,19 @@ + + + + + + + + + + + + + diff --git a/conf/firmwares/baro_board.makefile b/conf/firmwares/baro_board.makefile index d033533ab2..71363e279d 100644 --- a/conf/firmwares/baro_board.makefile +++ b/conf/firmwares/baro_board.makefile @@ -258,15 +258,11 @@ BARO_BOARD_CFLAGS += -DBARO_PERIODIC_FREQUENCY=$(BARO_PERIODIC_FREQUENCY) ap.CFLAGS += $(BARO_BOARD_CFLAGS) ap.srcs += $(BARO_BOARD_SRCS) -# don't use for NPS or HITL +# don't use for NPS ifeq ($(TARGET),nps) $(TARGET).CFLAGS += -DUSE_BARO_BOARD=FALSE endif -ifeq ($(TARGET),hitl) -$(TARGET).CFLAGS += -DUSE_BARO_BOARD=FALSE -endif - else # USE_BARO_BOARD is not TRUE, was explicitly disabled ap.CFLAGS += -DUSE_BARO_BOARD=FALSE diff --git a/conf/firmwares/generic_hitl.makefile b/conf/firmwares/generic_hitl.makefile new file mode 100644 index 0000000000..02a2ca6be5 --- /dev/null +++ b/conf/firmwares/generic_hitl.makefile @@ -0,0 +1,6 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# generic_hitl.makefile +# +# + diff --git a/conf/firmwares/uart.makefile b/conf/firmwares/uart.makefile index 44ad819437..ae19239380 100644 --- a/conf/firmwares/uart.makefile +++ b/conf/firmwares/uart.makefile @@ -12,10 +12,6 @@ ifeq ($(TARGET), nps) UART_CFLAGS += -Iarch/linux UART_SRCS += arch/linux/serial_port.c endif -ifeq ($(TARGET), hitl) -UART_CFLAGS += -Iarch/linux -UART_SRCS += arch/linux/serial_port.c -endif ifeq ($(TARGET), sim) UART_CFLAGS += -Iarch/linux UART_SRCS += arch/linux/serial_port.c diff --git a/conf/firmwares/udp.makefile b/conf/firmwares/udp.makefile index 51837930ec..f8980d7431 100644 --- a/conf/firmwares/udp.makefile +++ b/conf/firmwares/udp.makefile @@ -13,10 +13,6 @@ ifeq ($(TARGET), nps) UDP_CFLAGS += -Iarch/linux UDP_SRCS += arch/linux/udp_socket.c endif -ifeq ($(TARGET), hitl) -UDP_CFLAGS += -Iarch/linux -UDP_SRCS += arch/linux/udp_socket.c -endif $(TARGET).CFLAGS += $(UDP_CFLAGS) $(TARGET).srcs += $(UDP_SRCS) diff --git a/conf/modules/actuators_hitl.xml b/conf/modules/actuators_hitl.xml new file mode 100644 index 0000000000..e57721635f --- /dev/null +++ b/conf/modules/actuators_hitl.xml @@ -0,0 +1,31 @@ + + + + + + Sends commands or actuators for Hardware In The Loop simulation + + + + + + actuators + +
+ +
+ + + + + + + + + + + + + + + diff --git a/conf/modules/control.xml b/conf/modules/control.xml index f32d888c49..210c330128 100644 --- a/conf/modules/control.xml +++ b/conf/modules/control.xml @@ -12,5 +12,5 @@ stabilization_attitude_fw,guidance_basic_fw - + diff --git a/conf/modules/dfu_command.xml b/conf/modules/dfu_command.xml index 42bc23b04d..d483751e4a 100644 --- a/conf/modules/dfu_command.xml +++ b/conf/modules/dfu_command.xml @@ -17,7 +17,7 @@ - + diff --git a/conf/modules/electrical.xml b/conf/modules/electrical.xml index 93c7b57b45..b79004be83 100644 --- a/conf/modules/electrical.xml +++ b/conf/modules/electrical.xml @@ -22,7 +22,7 @@ - + diff --git a/conf/modules/extra_dl.xml b/conf/modules/extra_dl.xml index 21be70f68b..c050e877b0 100644 --- a/conf/modules/extra_dl.xml +++ b/conf/modules/extra_dl.xml @@ -6,6 +6,9 @@ + + uart|udp +
@@ -25,28 +28,9 @@ + - - # 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), stm32) - $(TARGET).srcs += $(SRC_ARCH)/usb_ser_hw.c - else - ifneq ($(ARCH), sim) - $(error telemetry_transparent_usb currently only implemented for the stm32) - endif - endif - else - EXTRA_DL_BAUD ?= B57600 - $(TARGET).CFLAGS += -D$(EXTRA_DL_PORT_UPPER)_BAUD=$(EXTRA_DL_BAUD) - endif # USB serial - endif # UDP - diff --git a/conf/modules/firmwares/fixedwing.xml b/conf/modules/firmwares/fixedwing.xml index 562b291770..54194df182 100644 --- a/conf/modules/firmwares/fixedwing.xml +++ b/conf/modules/firmwares/fixedwing.xml @@ -11,7 +11,7 @@ system_core,autopilot_gnc_fw,actuators_pwm nav_basic_fw - + @@ -28,7 +28,7 @@ - + diff --git a/conf/modules/firmwares/generic_hitl.xml b/conf/modules/firmwares/generic_hitl.xml new file mode 100644 index 0000000000..4663f6afcd --- /dev/null +++ b/conf/modules/firmwares/generic_hitl.xml @@ -0,0 +1,15 @@ + + + + + + Generic firmware config for Hardware In The Loop simualtion + + + + + + + + + diff --git a/conf/modules/firmwares/rotorcraft.xml b/conf/modules/firmwares/rotorcraft.xml index 7e3404b7af..06d087b971 100644 --- a/conf/modules/firmwares/rotorcraft.xml +++ b/conf/modules/firmwares/rotorcraft.xml @@ -25,7 +25,7 @@ - + diff --git a/conf/modules/firmwares/rover.xml b/conf/modules/firmwares/rover.xml index 1b612dd14a..38279bbf58 100644 --- a/conf/modules/firmwares/rover.xml +++ b/conf/modules/firmwares/rover.xml @@ -28,7 +28,7 @@ $(error "Rover firmware should use generated autopilot") endif - + diff --git a/conf/modules/gps.xml b/conf/modules/gps.xml index ce443c47e0..c062bee331 100644 --- a/conf/modules/gps.xml +++ b/conf/modules/gps.xml @@ -31,7 +31,7 @@ - + diff --git a/conf/modules/gps_nps.xml b/conf/modules/gps_nps.xml index d98e630b5a..68f6e15860 100644 --- a/conf/modules/gps_nps.xml +++ b/conf/modules/gps_nps.xml @@ -17,7 +17,7 @@ - + diff --git a/conf/modules/gps_sim_hitl.xml b/conf/modules/gps_sim_hitl.xml deleted file mode 100644 index 1db6e11fbb..0000000000 --- a/conf/modules/gps_sim_hitl.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - Sim HITL GPS - Simulate GPS for HITL (HardwareInTheLoop) from rotorcrafts horizontal/vertical reference system. - - - - gps - gps - -
- -
- - - - - - - - - diff --git a/conf/modules/gps_ublox.xml b/conf/modules/gps_ublox.xml index d95dcf20b5..7d34bd7374 100644 --- a/conf/modules/gps_ublox.xml +++ b/conf/modules/gps_ublox.xml @@ -35,7 +35,7 @@ - + diff --git a/conf/modules/guidance_basic_fw.xml b/conf/modules/guidance_basic_fw.xml index c2772b0f67..5c1280c56d 100644 --- a/conf/modules/guidance_basic_fw.xml +++ b/conf/modules/guidance_basic_fw.xml @@ -54,7 +54,7 @@ - + diff --git a/conf/modules/guidance_pid_rotorcraft.xml b/conf/modules/guidance_pid_rotorcraft.xml index c8578ae333..5b1c1eddb7 100644 --- a/conf/modules/guidance_pid_rotorcraft.xml +++ b/conf/modules/guidance_pid_rotorcraft.xml @@ -45,7 +45,7 @@ - + diff --git a/conf/modules/guidance_rotorcraft.xml b/conf/modules/guidance_rotorcraft.xml index 8c65276007..647d852bb1 100644 --- a/conf/modules/guidance_rotorcraft.xml +++ b/conf/modules/guidance_rotorcraft.xml @@ -37,7 +37,7 @@ - + diff --git a/conf/modules/hard_fault_recovery.xml b/conf/modules/hard_fault_recovery.xml index 54a34ef012..5a9cc841e6 100644 --- a/conf/modules/hard_fault_recovery.xml +++ b/conf/modules/hard_fault_recovery.xml @@ -17,7 +17,8 @@ recovery - + + diff --git a/conf/modules/imu_aspirin_v2_common.xml b/conf/modules/imu_aspirin_v2_common.xml index 69f066d883..e77d206640 100644 --- a/conf/modules/imu_aspirin_v2_common.xml +++ b/conf/modules/imu_aspirin_v2_common.xml @@ -30,7 +30,7 @@ - + diff --git a/conf/modules/imu_nps.xml b/conf/modules/imu_nps.xml index 280f15bffc..1137664499 100644 --- a/conf/modules/imu_nps.xml +++ b/conf/modules/imu_nps.xml @@ -16,7 +16,7 @@ - + diff --git a/conf/modules/ins.xml b/conf/modules/ins.xml index 86fb7a8e6d..8be3999142 100644 --- a/conf/modules/ins.xml +++ b/conf/modules/ins.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/modules/ins_nps.xml b/conf/modules/ins_nps.xml index e77d409559..e29123cd56 100644 --- a/conf/modules/ins_nps.xml +++ b/conf/modules/ins_nps.xml @@ -14,7 +14,7 @@ - + @@ -29,12 +29,12 @@ - + - + diff --git a/conf/modules/nav_basic_fw.xml b/conf/modules/nav_basic_fw.xml index c3e6f7e64c..bb23d78941 100644 --- a/conf/modules/nav_basic_fw.xml +++ b/conf/modules/nav_basic_fw.xml @@ -34,7 +34,7 @@ - + diff --git a/conf/modules/nav_basic_rotorcraft.xml b/conf/modules/nav_basic_rotorcraft.xml index add7cd5d8a..9343f332aa 100644 --- a/conf/modules/nav_basic_rotorcraft.xml +++ b/conf/modules/nav_basic_rotorcraft.xml @@ -14,7 +14,7 @@ - + diff --git a/conf/modules/nav_flower.xml b/conf/modules/nav_flower.xml index 59cf92bbd0..ef9f174c84 100644 --- a/conf/modules/nav_flower.xml +++ b/conf/modules/nav_flower.xml @@ -14,7 +14,7 @@ - + diff --git a/conf/modules/nav_hybrid.xml b/conf/modules/nav_hybrid.xml index 59b6454a7e..c40d3c384d 100644 --- a/conf/modules/nav_hybrid.xml +++ b/conf/modules/nav_hybrid.xml @@ -24,7 +24,7 @@ - + diff --git a/conf/modules/nav_lace.xml b/conf/modules/nav_lace.xml index e18f4cb7bc..34fe8f549a 100644 --- a/conf/modules/nav_lace.xml +++ b/conf/modules/nav_lace.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/modules/nav_launcher.xml b/conf/modules/nav_launcher.xml index 87980cb43c..8db5455e64 100644 --- a/conf/modules/nav_launcher.xml +++ b/conf/modules/nav_launcher.xml @@ -18,7 +18,7 @@
- + diff --git a/conf/modules/nav_line.xml b/conf/modules/nav_line.xml index c45187a7bf..a8843f46e3 100644 --- a/conf/modules/nav_line.xml +++ b/conf/modules/nav_line.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/modules/nav_rosette.xml b/conf/modules/nav_rosette.xml index 0360e0e93b..9cbc688e83 100644 --- a/conf/modules/nav_rosette.xml +++ b/conf/modules/nav_rosette.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/modules/nav_rotorcraft.xml b/conf/modules/nav_rotorcraft.xml index e8962ef6b0..e7547780a0 100644 --- a/conf/modules/nav_rotorcraft.xml +++ b/conf/modules/nav_rotorcraft.xml @@ -26,7 +26,7 @@ - + diff --git a/conf/modules/nav_skid_landing.xml b/conf/modules/nav_skid_landing.xml index f5b0b16772..f07e153462 100644 --- a/conf/modules/nav_skid_landing.xml +++ b/conf/modules/nav_skid_landing.xml @@ -55,7 +55,7 @@
- + diff --git a/conf/modules/nav_spiral_3D.xml b/conf/modules/nav_spiral_3D.xml index cf9ba270ca..1b7431af82 100644 --- a/conf/modules/nav_spiral_3D.xml +++ b/conf/modules/nav_spiral_3D.xml @@ -30,7 +30,7 @@ - + diff --git a/conf/modules/nav_survey_poly_osam.xml b/conf/modules/nav_survey_poly_osam.xml index 7a44f5c8b1..c1ead8879f 100644 --- a/conf/modules/nav_survey_poly_osam.xml +++ b/conf/modules/nav_survey_poly_osam.xml @@ -34,7 +34,7 @@ You can use:
- + diff --git a/conf/modules/navigation.xml b/conf/modules/navigation.xml index 96bbb84ef2..ff06bd4e90 100644 --- a/conf/modules/navigation.xml +++ b/conf/modules/navigation.xml @@ -9,5 +9,5 @@ nav_basic_fw - + diff --git a/conf/modules/nps_hitl_sensors.xml b/conf/modules/nps_hitl_sensors.xml new file mode 100644 index 0000000000..0e3698bac8 --- /dev/null +++ b/conf/modules/nps_hitl_sensors.xml @@ -0,0 +1,19 @@ + + + + + + Send simulated sensors for HITL (HardwareInTheLoop). + + + + imu,mag,gps,baro,airspeed + + + + + + + + + diff --git a/conf/modules/radio_control_ppm.xml b/conf/modules/radio_control_ppm.xml index 9a6a1adf60..91097eebdf 100644 --- a/conf/modules/radio_control_ppm.xml +++ b/conf/modules/radio_control_ppm.xml @@ -21,7 +21,7 @@ - + diff --git a/conf/modules/radio_control_spektrum.xml b/conf/modules/radio_control_spektrum.xml index cd97626f83..b17c099833 100644 --- a/conf/modules/radio_control_spektrum.xml +++ b/conf/modules/radio_control_spektrum.xml @@ -53,7 +53,7 @@ - + diff --git a/conf/modules/rust_demo_module.xml b/conf/modules/rust_demo_module.xml index a7d3f7c248..d9df118170 100644 --- a/conf/modules/rust_demo_module.xml +++ b/conf/modules/rust_demo_module.xml @@ -26,7 +26,7 @@ - + - diff --git a/conf/modules/targets/nps_common.xml b/conf/modules/targets/nps_common.xml new file mode 100644 index 0000000000..c04ef309a4 --- /dev/null +++ b/conf/modules/targets/nps_common.xml @@ -0,0 +1,69 @@ + + + + + + New Paparazzi Simulator (NPS) common files + + Bindings between embedded autopilot code and a flight dynamic model (FDM). + Can run Software In The Loop (SITL) or Hardware In The Loop (HITL) simulations. + + + + math + + + + + +$(TARGET).MAKEFILE = $(TARGET) +$(TARGET).ARCHDIR = sim + +# detect system arch and include rt and pthread library only on linux +UNAME_S := $(shell uname -s) +ifeq ($(UNAME_S),Linux) + $(TARGET).LDFLAGS += -lrt -pthread +endif + +# sdl needed for joystick input +$(TARGET).LDFLAGS += $(shell sdl-config --libs) + +# glib is still needed for some components (such as radio input) +$(TARGET).CFLAGS += $(shell pkg-config glib-2.0 --cflags) +$(TARGET).LDFLAGS += $(shell pkg-config glib-2.0 --libs) + +# +# add the simulator and var directory to the make searchpath +# +VPATH += $(PAPARAZZI_SRC)/sw/simulator +VPATH += $(PAPARAZZI_HOME)/var/share + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/telemetry_nps.xml b/conf/modules/telemetry_nps.xml index c66f64d15d..9d29933ed2 100644 --- a/conf/modules/telemetry_nps.xml +++ b/conf/modules/telemetry_nps.xml @@ -22,7 +22,7 @@ - + diff --git a/conf/modules/telemetry_nps_secure.xml b/conf/modules/telemetry_nps_secure.xml index 752683435d..779b79d788 100644 --- a/conf/modules/telemetry_nps_secure.xml +++ b/conf/modules/telemetry_nps_secure.xml @@ -17,7 +17,7 @@ udp,datalink_common datalink,telemetry - + diff --git a/conf/modules/telemetry_secure_common.xml b/conf/modules/telemetry_secure_common.xml index c32c7a4a1b..ad3cf96abb 100644 --- a/conf/modules/telemetry_secure_common.xml +++ b/conf/modules/telemetry_secure_common.xml @@ -14,7 +14,7 @@ telemetry_nps_secure - + diff --git a/conf/modules/telemetry_transparent.xml b/conf/modules/telemetry_transparent.xml index f2100153a2..9437337e35 100644 --- a/conf/modules/telemetry_transparent.xml +++ b/conf/modules/telemetry_transparent.xml @@ -22,7 +22,7 @@ - + diff --git a/conf/modules/telemetry_transparent_frsky_x.xml b/conf/modules/telemetry_transparent_frsky_x.xml index 58cc4b6260..ad33cefb32 100644 --- a/conf/modules/telemetry_transparent_frsky_x.xml +++ b/conf/modules/telemetry_transparent_frsky_x.xml @@ -34,7 +34,7 @@ - + diff --git a/conf/modules/telemetry_transparent_usb.xml b/conf/modules/telemetry_transparent_usb.xml index b0999833a1..d6723ac9ff 100644 --- a/conf/modules/telemetry_transparent_usb.xml +++ b/conf/modules/telemetry_transparent_usb.xml @@ -10,7 +10,7 @@ telemetry_transparent - + $(warning Warning: telemetry_transparent_usb is deprecated. Use telemetry_transparent with configure MODEM_PORT=usb_serial instead.) diff --git a/conf/modules/telemetry_xbee_api.xml b/conf/modules/telemetry_xbee_api.xml index 8e147692cb..597cea84ae 100644 --- a/conf/modules/telemetry_xbee_api.xml +++ b/conf/modules/telemetry_xbee_api.xml @@ -18,7 +18,7 @@ - + diff --git a/conf/modules/uart.xml b/conf/modules/uart.xml index eb97bdfd36..4489d03520 100644 --- a/conf/modules/uart.xml +++ b/conf/modules/uart.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/modules/udp.xml b/conf/modules/udp.xml index 2730c50648..638593d53b 100644 --- a/conf/modules/udp.xml +++ b/conf/modules/udp.xml @@ -19,7 +19,7 @@ - + diff --git a/conf/userconf/OPENUAS/openuas_control_panel.xml b/conf/userconf/OPENUAS/openuas_control_panel.xml index 122f00528a..4ca7637291 100644 --- a/conf/userconf/OPENUAS/openuas_control_panel.xml +++ b/conf/userconf/OPENUAS/openuas_control_panel.xml @@ -224,30 +224,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/conf/userconf/OPENUAS/openuas_obc2014_control_panel.xml b/conf/userconf/OPENUAS/openuas_obc2014_control_panel.xml index 1700ef5a8f..8ccc41791c 100644 --- a/conf/userconf/OPENUAS/openuas_obc2014_control_panel.xml +++ b/conf/userconf/OPENUAS/openuas_obc2014_control_panel.xml @@ -114,23 +114,6 @@ - - - - - - - - - - - - - - - - - diff --git a/doc/sphinx/source/user_guide/PPRZ_Simulation.png b/doc/sphinx/source/user_guide/PPRZ_Simulation.png new file mode 100644 index 0000000000..03efd3e625 Binary files /dev/null and b/doc/sphinx/source/user_guide/PPRZ_Simulation.png differ diff --git a/doc/sphinx/source/user_guide/simulation/hitl.rst b/doc/sphinx/source/user_guide/simulation/hitl.rst index 5497611665..9eefe11fbc 100644 --- a/doc/sphinx/source/user_guide/simulation/hitl.rst +++ b/doc/sphinx/source/user_guide/simulation/hitl.rst @@ -8,9 +8,7 @@ Hardware in the Loop Hardware In The Loop (HITL) simulation is a way to test an embedded system (the real hardware and software) by simulating its environment, ie. sensor inputs, and comparing its output, ie. actuator outputs, to expected output values. -It is the closest to an actual flight without actually flying. Using Paparazzi's Software In The Loop (SITL) and HITL for validation of a -flight dynamics of a fixed wing UAV is in detail described in a paper "Software-and hardware-in-the-loop verification of flight dynamics -model and flight control simulationof a fixed-wing unmanned aerial vehicle" by Cal Coopmans and Michal Podhradsky. Refer to the paper for more details. +It is the closest to an actual flight without actually flying. Principle ----------- @@ -38,17 +36,19 @@ Commands computed by the autopilot are sent to the flight model which sends back Limitations ----------------- -.. warning:: - - HITL currently works only with Pprzlink 1.0, which is not supported anymore. The code might not work as desired yet. - For practical reasons (it is very difficult to simulate SPI/I2C devices such as accelerometer, gyroscope etc.), -Paparazzi HITL simulates only sensors that connect to the autopilot via serial port (for example GPS unit, or an external AHRS/INS). -Currently implemented is Vectornav VN-200 in INS mode, but other sensors and modes can be added (i.e. VN-200 as IMU, Xsens INS etc.). -Because the benefit of HITL is to test the autopilot code that is identical to the actual flight code, no other means of transporting -sensor data to the autopilot are currently supported (such as sending them through uplink). +Paparazzi HITL simulates sensors and sends them through a dedicated link (independent of the telemetry link). It is thus recommanded to use +the *usb_serial* driver or if not available a serial link with high baudrate. -Another consideration is the bandwidth of the system - the sensor data and the actuator values have to be send/received at ``PERIODIC_FREQUENCY`` (between 40-512 Hz) for HITL to work correctly. +Another consideration is the bandwidth of the system - the sensor data and the actuator values have to be send/received at ``PERIODIC_FREQUENCY`` (between 40-512 Hz) for HITL to work correctly. Not all communication links and frequencies are feasible without introducing latency or bandwidth saturation. + +As an example, for rotorcraft with a periodic frequency of 500 Hz with default settings: + +- ``HITL_IMU`` message (45 bytes) comes at 500 Hz +- ``HITL_GPS`` message (42 bytes) comes at 10 Hz +- ``HITL_AIR_DATA`` message (26 bytes) comes at 50 Hz + +Total bandwidth is 24220 bytes/s. A standard UART has 10 bits per bytes, so a baudrate of at least 242200 is required. To give enough margins and reduce latency, the default baudrate is set to 921600. A USB serial, even at low speed, has a baudrate of more than 1 Mb/s. When to use SITL and when HITL? ------------------------------------ @@ -63,6 +63,10 @@ has been configured. Usually HITL is the last thing to run before going flying. Prerequisites -------------------- +.. warning:: + some recommendations may not apply, in particular if real USB is used instead of UART or UART-USB bridge + + HITL currently (Ubuntu 16.04) needs the following two steps to run correctly: - set rtpriority for the uart threads detailes `here `_ @@ -80,156 +84,66 @@ HITL currently (Ubuntu 16.04) needs the following two steps to run correctly: Configuration ------------------------ +.. warning:: + HITL support as changed in version 6.3, older procedure (especially described in the `wiki `_, no longer applies + HITL can currently run on any that has: -- Serial port for Vectornav INS input (provides position and orientation data, including GPS coordinates) -- Serial port for additional high-speed telemetry output (so not your regular 57600 telemetry) +- Serial port (at high speed) or USB for sending commands and receiving sensor data - Other serial/io for regular telemetry, RC input etc. -If you have high-speed telemetry (like over WiFi) it should be possible to use only one telemetry link and demux the messages on GCS, -but it is not currently supported. Note that HITL is timing sensitive (at 512Hz you need to receive, process, and send data every ~2ms). +Note that HITL is timing sensitive (at 512Hz you need to receive, process, and send data every ~2ms). -HITL has been tested on: +Examples are provided based on the Matek H7 Slim: ``conf/airframes/examples/matek_h7_fixedwing_hitl.xml`` and ``conf/airframes/examples/matek_h7_rotorcraft_hitl.xml`` -- Lisa M/MX (exampes for `fixedwing `_ and - `rotorcraft `_) - -- Umarim v 2.0 (example for `fixedwing with Unarim `_) +.. figure:: images/hitl_gcs.jpg -We recommend a dedicated computer for HITL, with enough CPU power and memory, and a nice graphics card for :ref:`flightgear` visualisation -(see the test station in the picture). HITL can run on a regular laptop too (tested on both Lenovo Thinkpad and Toughbooks). +The airframe needs to contain: -.. figure:: images/600px-HITL_station.jpg +* ``sensors_hitl`` module in main firmware section +* ``actuators_hitl`` module in main firmware section +* a specific HITL firmware section ``...`` that contains an ``hitl`` target with the selected FDM and the module ``nps_hitl_sensors`` - Hardware-in-the-loop (HITL) test station in simulated flight +.. warning:: + an ariframe file configured for HITL simulation can usually also be used with normal SITL simulation, but is not compatible with normal ``ap`` target as the sensors and actuators drivers are replaced by simulation modules -There are a few example airframes to choose from. Let's start with a fixed wing airplane and walk you through step by step. Get a fresh copy of the latest paparazzi and do: +Modules in main firmware section +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -.. code-block:: php +Here is an example of configuration for fixedwing or rotorcraft using an USB link (alternate debug link, primary `usb_serial` is used for telemetry): - # in prrz root dir - ./start.py +.. code-block:: xml -and choose AggieAir's ``conf`` and ``control panel``: + + + + + + -.. figure:: images/Aggieair_conf.png +It is preferable to disable the hard fault recovery mode for HITL. - Select AggieAir's conf and control panel and then Launch - -Choose **Minion_RP3** airframe: - -.. figure:: images/900px-Minion_rp3_airfame.png - - Minion RP3 airfame - -and click on **Edit**. The airframe file is on github: https://github.com/paparazzi/paparazzi/blob/master/conf/airframes/AGGIEAIR/aggieair_minion_rp3_lia.xml For HITL to work, there have to be 4 things: - -- `extra_dl `_ telemetry module -- specified ``COMMANDS`` (Fixedwing) or ``ACTUATORS`` (rotorcrafts) Extra telemetry message in the telemetry config file (an example `with AggieAir here `_) -- HITL target -- Airframe configured to use external INS - -Extra_DL Module -^^^^^^^^^^^^^^^^^^^^^^ - -This is the additiona high speed telemetry link that sends the actuators data back to the FDM. - -.. code-block:: php - - # in your airframe config file - - - - - - - -If you have `Umarim `_ board or similar, you can also use a usb serial port: - -.. code-block:: php - - # in your airframe config file - - - - - -Telemetry config file -^^^^^^^^^^^^^^^^^^^^^^^^ - -Just add this section to your telemetry config file: - -.. code-block:: php - - # in your telemetry config file - - - - - - -The period has to be matching your ``PERIODIC_FREQUENCY`` - best if you explicitly define all the frequencies to avoid ambiguity: - -.. code-block:: - - # in your airfame config file - - - - - - -NOTE: the ``TELEMETRY_FREQUENCY`` has to match your ``PERIODIC_FREQUENCY`` +Since the sensors data received are IMU, GPS and barometer data, any internal INS/AHRS filter compatible with your airframe should work. HITL Target ^^^^^^^^^^^^^^^ -Add the target in your airfame config file: +Add an extra firmware and an ``hitl`` target in your airfame config file: -.. code-block:: php +.. code-block:: xml - # in your airfame config file - - - - - - - - -What does it mean? First, we have to specify the FDM for the HITL simulation. We recommend :ref:`jsbsim`, but any FDM that :ref:`nps` supports should work (because NPS is the backend for HITL). - -Then we have to specify the serial ports to talk to the autopilot. ``INS_DEV`` is the port your external INS (such as Vectornav) is using. -AP_DEV is the port for the extra telemetry. Make sure your baud rates are matching too. - -Note that you can either specify the devices in ``/dev/ttyUSB*`` format, which makes it universal across different USB-to-serial converters, -but you have to remember to plug in the ports in the right order (since they enumerate sequentially). - -The other option is to specify the ``/dev/serial/by-id/usb-FTDI_*****`` format, in which case it doesn't matter in which order you plug the -devices in, but you can use it only for a particular FTDI converter. - -It might be handy to use a simple Lia breakout board for connecting all the serial -ports - `the breakout board files are available here `_. - -.. figure:: images/500px-Liabreakoutboard.jpeg - - Lia breakout board - -Airframe Configuration for External INS -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Indeed, HITL will work only if your aiframe is configured to use external INS of some sort. In our example, we specify using Vectornav: - -.. code-block:: - - # in your airfame config file - - - + + + + + + + -See the `Minion_RP3 airframe config `_ for more details. +We have to specify the FDM for the HITL simulation. We recommend :ref:`jsbsim`, but any FDM that :ref:`nps` supports should work (because NPS is the backend for HITL). + +``AP_DEV`` correcponds to the serial link which is used to have a direct communication with the autopilot and exchange commands and data. It as to match the one selected on the airborne side (``usb_serial_debug`` in example above). Running @@ -237,42 +151,17 @@ Running Once you have your setup completed: -- Clean, compile and upload the AP target (HINT: use keyboard shortcuts **Alt+C** to **Clean**, **Alt+B** to **Build** and **Alt+U** to **Upload**) -- Clean and build HITL target -- Choose ``HITL USB-serial@57600`` session and Execute +- Clean, compile and upload the `hitl` target. This target will build at the same time the `ap` target and the ground part for simulation. It is also possible to build separately the AP and then HITL but it is not recommended. +- Choose ``HITL demo`` session and Execute .. note:: If you want to use your own session, you have to pass ``-t hitl`` flag into ``sw/simulator/pprzsim-launch`` to start in HITL mode. Have a look at the ``HITL USB-serual@57600`` session for example, or add this to your own: -Messages will pop up and you can verify that you are getting data by looking at the ``VECTORNAV_INFO`` message: - -.. figure:: images/300px-Hitl_messages.png - - VECTORNAV_INFO message - -And once you take-off you will see something like this: - -.. figure:: images/1000px-Hitl_flight.png - - HITL Flight with fixedwing airplane Similar steps work for rotorcraft. -SBUS Fakerator -^^^^^^^^^^^^^^^^^^^^^ - -A simple tool simulating SBUS radio inputs is available. It is useful if you don't have a radio around, and want to test flight in manual mode. -It has to be used with a `Sbus_fakerator radio config file `_ and it requires an additional serial port (for example ``/dev/ttyUSB3``). -It can be launched as a tool from the Paparazzi center. - -Source code is available at: https://github.com/paparazzi/paparazzi/tree/master/sw/tools/sbus_fakerator - -.. figure:: images/300px-Sbus_fakerator.png - - SBUS fakerator tool - FlightGear ^^^^^^^^^^^^^ @@ -302,5 +191,4 @@ Then I believe you have to restart your computer in order for limits to refresh. Happy flying! -.. figure:: images/600px-Minion_HITL.png diff --git a/doc/sphinx/source/user_guide/simulation/images/1000px-Hitl_flight.png b/doc/sphinx/source/user_guide/simulation/images/1000px-Hitl_flight.png deleted file mode 100644 index ff1b111f76..0000000000 Binary files a/doc/sphinx/source/user_guide/simulation/images/1000px-Hitl_flight.png and /dev/null differ diff --git a/doc/sphinx/source/user_guide/simulation/images/300px-Hitl_messages.png b/doc/sphinx/source/user_guide/simulation/images/300px-Hitl_messages.png deleted file mode 100644 index d0f5146b8a..0000000000 Binary files a/doc/sphinx/source/user_guide/simulation/images/300px-Hitl_messages.png and /dev/null differ diff --git a/doc/sphinx/source/user_guide/simulation/images/300px-Sbus_fakerator.png b/doc/sphinx/source/user_guide/simulation/images/300px-Sbus_fakerator.png deleted file mode 100644 index fe2a6f635f..0000000000 Binary files a/doc/sphinx/source/user_guide/simulation/images/300px-Sbus_fakerator.png and /dev/null differ diff --git a/doc/sphinx/source/user_guide/simulation/images/500px-Liabreakoutboard.jpeg b/doc/sphinx/source/user_guide/simulation/images/500px-Liabreakoutboard.jpeg deleted file mode 100644 index a8406e5190..0000000000 Binary files a/doc/sphinx/source/user_guide/simulation/images/500px-Liabreakoutboard.jpeg and /dev/null differ diff --git a/doc/sphinx/source/user_guide/simulation/images/600px-HITL_station.jpg b/doc/sphinx/source/user_guide/simulation/images/600px-HITL_station.jpg deleted file mode 100644 index a2e6e0faa7..0000000000 Binary files a/doc/sphinx/source/user_guide/simulation/images/600px-HITL_station.jpg and /dev/null differ diff --git a/doc/sphinx/source/user_guide/simulation/images/600px-Minion_HITL.png b/doc/sphinx/source/user_guide/simulation/images/600px-Minion_HITL.png deleted file mode 100644 index 07a3e991f2..0000000000 Binary files a/doc/sphinx/source/user_guide/simulation/images/600px-Minion_HITL.png and /dev/null differ diff --git a/doc/sphinx/source/user_guide/simulation/images/900px-Minion_rp3_airfame.png b/doc/sphinx/source/user_guide/simulation/images/900px-Minion_rp3_airfame.png deleted file mode 100644 index 386a88dc14..0000000000 Binary files a/doc/sphinx/source/user_guide/simulation/images/900px-Minion_rp3_airfame.png and /dev/null differ diff --git a/doc/sphinx/source/user_guide/simulation/images/Aggieair_conf.png b/doc/sphinx/source/user_guide/simulation/images/Aggieair_conf.png deleted file mode 100644 index 172bc8e502..0000000000 Binary files a/doc/sphinx/source/user_guide/simulation/images/Aggieair_conf.png and /dev/null differ diff --git a/doc/sphinx/source/user_guide/simulation/images/hitl_gcs.jpg b/doc/sphinx/source/user_guide/simulation/images/hitl_gcs.jpg new file mode 100644 index 0000000000..f70e6993bc Binary files /dev/null and b/doc/sphinx/source/user_guide/simulation/images/hitl_gcs.jpg differ diff --git a/doc/sphinx/source/user_guide/simulation_main.rst b/doc/sphinx/source/user_guide/simulation_main.rst index b0ace39127..af22ac553c 100644 --- a/doc/sphinx/source/user_guide/simulation_main.rst +++ b/doc/sphinx/source/user_guide/simulation_main.rst @@ -4,15 +4,20 @@ Simulation =========================== -Paparazzi currently has two different simulator targets with different degrees of realism and intended purpose: +.. figure:: PPRZ_Simulation.png -- **sim**: The basic fixedwing simulator written in OCaml without IMU simulation or any sensor models (noise, bias, etc) and mainly intended to validate your :ref:`flightplans` logic. -- **nps**: NPS is a more advanced rotorcraft and fixedwing simulator with sensor models and commonly uses JSBSim as FDM (Flight Dynamic Model). - Other FDM's can be integrated easily. At the moment CRRCSIM, YASIM and JSBSIM are tried as FDM backend. -- **gazebo**: There is someting brand new developed going, using laters Master you can start using the Gazebo engine in Paparazzi. - Take a look on that page to see if it offers what you are looking for. + Architecture for software or hardware in the loop simulation A FDM is a set of mathematical equations used to calculate the physical forces acting on a simulated aircraft, such as thrust, lift, and drag. +Paparazzi currently has two different simulator targets with different degrees of realism and intended purpose: + +* **sim**: The basic fixedwing simulator written in OCaml without IMU simulation or any sensor models (noise, bias, etc) and mainly intended to validate your :ref:`flightplans` logic. +* **nps**: NPS is a more advanced rotorcraft and fixedwing simulator with sensor models and commonly uses JSBSim as FDM (Flight Dynamic Model). Other FDM's can be integrated easily. At the moment JSBSIM, CRRCSIM, GAZEBO and PyBullet are supported as FDM backend. It is also possible to implement ad-hoc simulation code, based on Matlab/Simulink generated code for instance. + * **JSBSim**: a flight dynamic library used by default by various simulation sofware including Flight Gear. + * **gazebo**: Gazebo can be used as flight dynamic engine, but its main interest is to generate virtual sensor readings (cameras, lidars, ...) from a 3D realistic environment. + Take a look on that page to see if it offers what you are looking for. + * **PyBullet**: Bullet is a physics simulation engine (simular to gazebo) that allows to easily express the different forces applied to the drones in a high level language thanks to the bindings with Python. +* **hitl**: Hardware in the Loop simulation can be used to run the autopilot on a real flight controller while the flight dynamic model is runing on the host computer. .. toctree:: :maxdepth: 1 @@ -20,4 +25,4 @@ A FDM is a set of mathematical equations used to calculate the physical forces a simulation/sim simulation/nps simulation/gazebo - simulation/hitl \ No newline at end of file + simulation/hitl diff --git a/sw/airborne/Makefile b/sw/airborne/Makefile index 1e16e1ac6d..0aeed6d152 100644 --- a/sw/airborne/Makefile +++ b/sw/airborne/Makefile @@ -65,6 +65,9 @@ ifneq ($(MAKECMDGOALS),clean) ifdef PRINT_CONFIG $(TARGET).CFLAGS += -DPRINT_CONFIG endif + ifdef AP_LAUNCH + $(TARGET).CFLAGS += -DAP_LAUNCH=$(AP_LAUNCH) + endif # sort cflags and sources to throw out duplicates # diff --git a/sw/airborne/arch/chibios/mcu_periph/uart_arch.c b/sw/airborne/arch/chibios/mcu_periph/uart_arch.c index d3431fd296..48bf2354dc 100644 --- a/sw/airborne/arch/chibios/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/chibios/mcu_periph/uart_arch.c @@ -40,6 +40,8 @@ #include "mcu_periph/gpio.h" #include BOARD_CONFIG +#if HAL_USE_SERIAL + // Default stack size #ifndef UART_THREAD_STACK_SIZE #define UART_THREAD_STACK_SIZE 512 @@ -1160,3 +1162,6 @@ void uart_send_message(struct uart_periph *p, long fd) // send signal to start transmission chSemSignal(init_struct->tx_sem); } + +#endif // HAL_USE_SERIAL + diff --git a/sw/airborne/modules/actuators/actuators_hitl.c b/sw/airborne/modules/actuators/actuators_hitl.c new file mode 100644 index 0000000000..22afec18ff --- /dev/null +++ b/sw/airborne/modules/actuators/actuators_hitl.c @@ -0,0 +1,70 @@ +/* + * Copyright (C) 2023 Gautier Hattenberger + * + * 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, see + * . + */ + +/** @file "modules/actuators/actuators_hitl.c" + * @author Gautier Hattenberger + * Sends commands or actuators for Hardware In The Loop simulation + */ + +#include "modules/actuators/actuators_hitl.h" +#include "modules/datalink/telemetry.h" +#include "modules/core/commands.h" +#include "modules/actuators/actuators.h" +#include "generated/airframe.h" +#if ROTORCRAFT_FIRMWARE && NPS_NO_MOTOR_MIXING +#include "modules/actuators/motor_mixing.h" +#endif + + +#ifndef HITL_DEVICE +#error "HITL_DEVICE must be defined" +#endif + +// for sending only +static struct pprz_transport actuators_hitl_tp; + +void actuators_hitl_init(void) +{ + pprz_transport_init(&actuators_hitl_tp); +} + +void actuators_hitl_periodic(void) +{ +#if FIXEDWING_FIRMWARE + pprz_msg_send_COMMANDS(&actuators_hitl_tp.trans_tx, &(HITL_DEVICE).device, AC_ID, + COMMANDS_NB, commands); +#endif +#if ROTORCRAFT_FIRMWARE +#if NPS_NO_MOTOR_MIXING + pprz_msg_send_ACTUATORS(&actuators_hitl_tp.trans_tx, &(HITL_DEVICE).device, AC_ID, + ACTUATORS_NB, actuators_pprz); +#else // use motor mixing + int16_t motors[MOTOR_MIXING_NB_MOTOR]; + for (uint8_t i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) + { + motors[i] = (int16_t)motor_mixing.commands[i]; + } + pprz_msg_send_MOTOR_MIXING(&actuators_hitl_tp.trans_tx, &(HITL_DEVICE).device, AC_ID, + MOTOR_MIXING_NB_MOTOR, motors); +#endif +#endif +} + + diff --git a/sw/airborne/modules/actuators/actuators_hitl.h b/sw/airborne/modules/actuators/actuators_hitl.h new file mode 100644 index 0000000000..c0e588e57b --- /dev/null +++ b/sw/airborne/modules/actuators/actuators_hitl.h @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2023 Gautier Hattenberger + * + * 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, see + * . + */ + +/** @file "modules/actuators/actuators_hitl.h" + * @author Gautier Hattenberger + * Sends commands or actuators for Hardware In The Loop simulation + */ + +#ifndef ACTUATORS_HITL_H +#define ACTUATORS_HITL_H + +extern void actuators_hitl_init(void); +extern void actuators_hitl_periodic(void); + +#endif // ACTUATORS_HITL_H diff --git a/sw/airborne/modules/gps/gps_sim_hitl.c b/sw/airborne/modules/gps/gps_sim_hitl.c deleted file mode 100644 index e2648536cc..0000000000 --- a/sw/airborne/modules/gps/gps_sim_hitl.c +++ /dev/null @@ -1,101 +0,0 @@ -/* - * Copyright (C) 2014 Sergey Krukowski - * - * 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. - */ - -/** - * @file modules/gps/gps_sim_hitl.c - * GPS subsystem simulation from rotorcrafts horizontal/vertical reference system - */ - -#include "modules/gps/gps_sim_hitl.h" -#include "modules/gps/gps.h" -#include "modules/core/abi.h" - -#include "state.h" -#include "guidance/guidance_h.h" -#include "guidance/guidance_v.h" -#include "autopilot.h" - -bool gps_available; -uint32_t gps_sim_hitl_timer; - -void gps_sim_hitl_init(void) -{ - gps.fix = GPS_FIX_NONE; -} - -void gps_sim_hitl_event(void) -{ - if (SysTimeTimer(gps_sim_hitl_timer) > 100000) { - SysTimeTimerStart(gps_sim_hitl_timer); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; - if (state.ned_initialized_i) { - if (!autopilot_in_flight()) { - struct Int32Vect2 zero_vector_i; - struct FloatVect2 zero_vector_f; - INT_VECT2_ZERO(zero_vector_i); - FLOAT_VECT2_ZERO(zero_vector_f); - gh_set_ref(zero_vector_i, zero_vector_f, zero_vector_f); - INT_VECT2_ZERO(guidance_h.ref.pos); - INT_VECT2_ZERO(guidance_h.ref.speed); - INT_VECT2_ZERO(guidance_h.ref.accel); - gv_set_ref(0, 0, 0); - guidance_v.z_ref = 0; - guidance_v.zd_ref = 0; - guidance_v.zdd_ref = 0; - } - struct NedCoor_i ned_c; - ned_c.x = guidance_h.ref.pos.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM; - ned_c.y = guidance_h.ref.pos.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM; - ned_c.z = guidance_v.z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM; - ecef_of_ned_point_i(&gps.ecef_pos, &state.ned_origin_i, &ned_c); - gps.lla_pos.alt = state.ned_origin_i.lla.alt - ned_c.z; - gps.hmsl = state.ned_origin_i.hmsl - ned_c.z; - ned_c.x = guidance_h.ref.speed.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM; - ned_c.y = guidance_h.ref.speed.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM; - ned_c.z = guidance_v.zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM; - ecef_of_ned_vect_i(&gps.ecef_vel, &state.ned_origin_i, &ned_c); - gps.fix = GPS_FIX_3D; - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; - gps_available = true; - } - else { - struct Int32Vect2 zero_vector_i; - struct FloatVect2 zero_vector_f; - INT_VECT2_ZERO(zero_vector_i); - FLOAT_VECT2_ZERO(zero_vector_f); - gh_set_ref(zero_vector_i, zero_vector_f, zero_vector_f); - gv_set_ref(0, 0, 0); - } - - // publish gps data - uint32_t now_ts = get_sys_time_usec(); - gps.last_msg_ticks = sys_time.nb_sec_rem; - gps.last_msg_time = sys_time.nb_sec; - if (gps.fix == GPS_FIX_3D) { - gps.last_3dfix_ticks = sys_time.nb_sec_rem; - gps.last_3dfix_time = sys_time.nb_sec; - } - AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps); - } -} - diff --git a/sw/airborne/modules/gps/gps_sim_hitl.h b/sw/airborne/modules/gps/gps_sim_hitl.h deleted file mode 100644 index e5006811ef..0000000000 --- a/sw/airborne/modules/gps/gps_sim_hitl.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Copyright (C) 2014 Sergey Krukowski - * - * 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. - */ - -/** - * @file modules/gps/gps_sim_hitl.h - * GPS subsystem simulation from rotorcrafts horizontal/vertical reference system - */ - -#ifndef GPS_SIM_HITL_H -#define GPS_SIM_HITL_H - -#ifndef PRIMARY_GPS -#define PRIMARY_GPS GPS_SIM -#endif - -extern void gps_sim_hitl_event(void); -extern void gps_sim_hitl_init(void); - -#endif /* GPS_SIM_HITL_H */ diff --git a/sw/airborne/modules/gps/gps_sim_nps.c b/sw/airborne/modules/gps/gps_sim_nps.c index 5dd6b8fd08..db303a1178 100644 --- a/sw/airborne/modules/gps/gps_sim_nps.c +++ b/sw/airborne/modules/gps/gps_sim_nps.c @@ -24,6 +24,7 @@ #include "modules/core/abi.h" #include "nps_sensors.h" #include "nps_fdm.h" +#include "modules/datalink/datalink.h" struct GpsState gps_nps; bool gps_has_fix; @@ -104,3 +105,4 @@ void gps_nps_init(void) { gps_has_fix = true; } + diff --git a/sw/airborne/modules/gps/gps_sim_nps.h b/sw/airborne/modules/gps/gps_sim_nps.h index 14a9841afa..ee73eed6ac 100644 --- a/sw/airborne/modules/gps/gps_sim_nps.h +++ b/sw/airborne/modules/gps/gps_sim_nps.h @@ -10,9 +10,9 @@ extern struct GpsState gps_nps; extern bool gps_has_fix; -extern void gps_feed_value(); +extern void gps_feed_value(void); -extern void gps_nps_init(); +extern void gps_nps_init(void); #define gps_nps_periodic_check() gps_periodic_check(&gps_nps) diff --git a/sw/airborne/modules/imu/imu_nps.c b/sw/airborne/modules/imu/imu_nps.c index b323b0c3d5..596eac617f 100644 --- a/sw/airborne/modules/imu/imu_nps.c +++ b/sw/airborne/modules/imu/imu_nps.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Felix Ruess * * This file is part of paparazzi. * @@ -14,9 +14,8 @@ * 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. + * along with paparazzi; see the file COPYING. If not, see + * . */ #include "modules/imu/imu_nps.h" @@ -84,6 +83,7 @@ void imu_feed_mag(void) } + void imu_nps_event(void) { uint32_t now_ts = get_sys_time_usec(); diff --git a/sw/airborne/modules/imu/imu_nps.h b/sw/airborne/modules/imu/imu_nps.h index 72edec50fa..df2cba1921 100644 --- a/sw/airborne/modules/imu/imu_nps.h +++ b/sw/airborne/modules/imu/imu_nps.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012 Felix Ruess * * This file is part of paparazzi. * @@ -14,9 +14,8 @@ * 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. + * along with paparazzi; see the file COPYING. If not, see + * . */ #ifndef IMU_NPS_H diff --git a/sw/airborne/modules/sensors/sensors_hitl.c b/sw/airborne/modules/sensors/sensors_hitl.c new file mode 100644 index 0000000000..427559f5e7 --- /dev/null +++ b/sw/airborne/modules/sensors/sensors_hitl.c @@ -0,0 +1,238 @@ +/* + * Copyright (C) 2023 Gautier Hattenberger + * + * 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, see + * . + */ + +#include "modules/sensors/sensors_hitl.h" +#include "modules/imu/imu.h" +#include "modules/gps/gps.h" +#include "modules/core/abi.h" +#include "modules/energy/electrical.h" +#include "generated/airframe.h" +#include "modules/datalink/datalink.h" +#include "modules/datalink/telemetry.h" +#include "nps_sensors_params_common.h" +#if USE_BATTERY_MONITOR +#include "modules/energy/electrical.h" +#endif + +struct ImuHitl { + uint8_t mag_available; + uint8_t accel_available; + uint8_t gyro_available; + + struct Int32Rates gyro; + struct Int32Vect3 accel; + struct Int32Vect3 mag; +}; + +struct ImuHitl imu_hitl; +struct GpsState gps_hitl; +bool gps_has_fix; + +static bool sensors_hitl_msg_available = false; +static uint8_t sensors_hitl_dl_buffer[MSG_SIZE] __attribute__((aligned)); +static struct pprz_transport sensors_hitl_tp; + +void sensors_hitl_init(void) +{ + pprz_transport_init(&sensors_hitl_tp); // for receiving only + + gps_has_fix = true; + + imu_hitl.gyro_available = false; + imu_hitl.mag_available = false; + imu_hitl.accel_available = false; + + // Set the default scaling + const struct Int32Rates gyro_scale[2] = { + {NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM, NPS_GYRO_SENSITIVITY_NUM}, + {NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN, NPS_GYRO_SENSITIVITY_DEN} + }; + const struct Int32Rates gyro_neutral = { + NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R + }; + const struct Int32Vect3 accel_scale[2] = { + {NPS_ACCEL_SENSITIVITY_NUM, NPS_ACCEL_SENSITIVITY_NUM, NPS_ACCEL_SENSITIVITY_NUM}, + {NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN, NPS_ACCEL_SENSITIVITY_DEN} + }; + const struct Int32Vect3 accel_neutral = { + NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z + }; + const struct Int32Vect3 mag_scale[2] = { + {NPS_MAG_SENSITIVITY_NUM, NPS_MAG_SENSITIVITY_NUM, NPS_MAG_SENSITIVITY_NUM}, + {NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN, NPS_MAG_SENSITIVITY_DEN} + }; + const struct Int32Vect3 mag_neutral = { + NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z + }; + imu_set_defaults_gyro(IMU_NPS_ID, NULL, &gyro_neutral, gyro_scale); + imu_set_defaults_accel(IMU_NPS_ID, NULL, &accel_neutral, accel_scale); + imu_set_defaults_mag(IMU_NPS_ID, NULL, &mag_neutral, mag_scale); +} + +void sensors_hitl_periodic(void) { +#if USE_BATTERY_MONITOR +#ifdef MAX_BAT_LEVEL + // init vsupply to MAX_BAT in case battery voltage is not available + electrical.vsupply = MAX_BAT_LEVEL; +#else + electrical.vsupply = 12.f; // 3S battery +#endif +#endif +} + +void sensors_hitl_parse_HITL_IMU(uint8_t *buf) +{ + if (DL_HITL_IMU_ac_id(buf) != AC_ID) { + return; + } + + RATES_ASSIGN(imu_hitl.gyro, + NPS_GYRO_SIGN_P * DL_HITL_IMU_gp(buf), + NPS_GYRO_SIGN_Q * DL_HITL_IMU_gq(buf), + NPS_GYRO_SIGN_R * DL_HITL_IMU_gr(buf)); + VECT3_ASSIGN(imu_hitl.accel, + NPS_ACCEL_SIGN_X * DL_HITL_IMU_ax(buf), + NPS_ACCEL_SIGN_Y * DL_HITL_IMU_ay(buf), + NPS_ACCEL_SIGN_Z * DL_HITL_IMU_az(buf)); + VECT3_ASSIGN(imu_hitl.mag, + NPS_MAG_SIGN_X * DL_HITL_IMU_mx(buf), + NPS_MAG_SIGN_Y * DL_HITL_IMU_my(buf), + NPS_MAG_SIGN_Z * DL_HITL_IMU_mz(buf)); + + imu_hitl.accel_available = true; + imu_hitl.gyro_available = true; + imu_hitl.mag_available = true; +} + +void sensors_hitl_parse_HITL_GPS(uint8_t *buf) +{ + if (DL_HITL_GPS_ac_id(buf) != AC_ID) { + return; + } + + gps_hitl.week = 1794; + gps_hitl.tow = DL_HITL_GPS_time(buf) * 1000; + + gps_hitl.ecef_vel.x = DL_HITL_GPS_ecef_vel_x(buf) * 100.; + gps_hitl.ecef_vel.y = DL_HITL_GPS_ecef_vel_y(buf) * 100.; + gps_hitl.ecef_vel.z = DL_HITL_GPS_ecef_vel_z(buf) * 100.; + SetBit(gps_hitl.valid_fields, GPS_VALID_VEL_ECEF_BIT); + + gps_hitl.lla_pos.lat = DL_HITL_GPS_lat(buf) * 1e7; + gps_hitl.lla_pos.lon = DL_HITL_GPS_lon(buf) * 1e7; + gps_hitl.lla_pos.alt = DL_HITL_GPS_alt(buf) * 1000.; + SetBit(gps_hitl.valid_fields, GPS_VALID_POS_LLA_BIT); + + gps_hitl.hmsl = DL_HITL_GPS_hmsl(buf) * 1000.; + SetBit(gps_hitl.valid_fields, GPS_VALID_HMSL_BIT); + + /* calc NED speed from ECEF */ + struct LtpDef_i ref_ltp; + ltp_def_from_lla_i(&ref_ltp, &gps_hitl.lla_pos); + struct NedCoor_i ned_vel_i; + ned_of_ecef_vect_i(&ned_vel_i, &ref_ltp, &gps_hitl.ecef_vel); + gps_hitl.ned_vel.x = ned_vel_i.x; + gps_hitl.ned_vel.y = ned_vel_i.y; + gps_hitl.ned_vel.z = ned_vel_i.z; + SetBit(gps_hitl.valid_fields, GPS_VALID_VEL_NED_BIT); + struct NedCoor_f ned_vel_f; + VECT3_FLOAT_OF_CM(ned_vel_f, gps_hitl.ned_vel); + + /* horizontal and 3d ground speed in cm/s */ + gps_hitl.gspeed = sqrtf(ned_vel_f.x * ned_vel_f.x + ned_vel_f.y * ned_vel_f.y) * 100; + gps_hitl.speed_3d = sqrtf(ned_vel_f.x * ned_vel_f.x + ned_vel_f.y * ned_vel_f.y + ned_vel_f.z * ned_vel_f.z) * 100; + + /* ground course in radians * 1e7 */ + gps_hitl.course = atan2f(ned_vel_f.y, ned_vel_f.x) * 1e7; + SetBit(gps_hitl.valid_fields, GPS_VALID_COURSE_BIT); + + gps_hitl.pacc = 650; + gps_hitl.hacc = 450; + gps_hitl.vacc = 200; + gps_hitl.sacc = 100; + gps_hitl.pdop = 650; + + if (gps_has_fix) { + gps_hitl.num_sv = 11; + gps_hitl.fix = GPS_FIX_3D; + } else { + gps_hitl.num_sv = 1; + gps_hitl.fix = GPS_FIX_NONE; + } + + // publish gps data + uint32_t now_ts = get_sys_time_usec(); + gps_hitl.last_msg_ticks = sys_time.nb_sec_rem; + gps_hitl.last_msg_time = sys_time.nb_sec; + if (gps_hitl.fix == GPS_FIX_3D) { + gps_hitl.last_3dfix_ticks = sys_time.nb_sec_rem; + gps_hitl.last_3dfix_time = sys_time.nb_sec; + } + AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps_hitl); +} + +void sensors_hitl_parse_HITL_AIR_DATA(uint8_t *buf) { + if (DL_HITL_AIR_DATA_ac_id(buf) != AC_ID) { + return; + } + + uint8_t flag = DL_HITL_AIR_DATA_update_flag(buf); + if (bit_is_set(flag, 0)) { + uint32_t now_ts = get_sys_time_usec(); + float pressure = DL_HITL_AIR_DATA_baro(buf); + AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, now_ts, pressure); + } + if (bit_is_set(flag, 1)) { + AbiSendMsgAIRSPEED(AIRSPEED_NPS_ID, DL_HITL_AIR_DATA_airspeed(buf)); + } + if (bit_is_set(flag, 2) || bit_is_set(flag, 3)) { + uint8_t incidence_flag = (flag >> 2) & (0x3); + float aoa = DL_HITL_AIR_DATA_aoa(buf); + float sideslip = DL_HITL_AIR_DATA_sideslip(buf); + AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, incidence_flag, aoa, sideslip); + } +} + + +void sensors_hitl_event(void) +{ + uint32_t now_ts = get_sys_time_usec(); + if (imu_hitl.gyro_available) { + AbiSendMsgIMU_GYRO_RAW(IMU_NPS_ID, now_ts, &imu_hitl.gyro, 1, NPS_PROPAGATE, NAN); + imu_hitl.gyro_available = false; + } + if (imu_hitl.accel_available) { + AbiSendMsgIMU_ACCEL_RAW(IMU_NPS_ID, now_ts, &imu_hitl.accel, 1, NPS_PROPAGATE, NAN); + imu_hitl.accel_available = false; + } + if (imu_hitl.mag_available) { + AbiSendMsgIMU_MAG_RAW(IMU_NPS_ID, now_ts, &imu_hitl.mag); + imu_hitl.mag_available = false; + } + + // parse incoming messages + pprz_check_and_parse(&HITL_DEVICE.device, &sensors_hitl_tp, sensors_hitl_dl_buffer, &sensors_hitl_msg_available); + DlCheckAndParse(&HITL_DEVICE.device, &sensors_hitl_tp.trans_tx, sensors_hitl_dl_buffer, &sensors_hitl_msg_available, false); +} + +void imu_feed_gyro_accel(void) {} +void imu_feed_mag(void) {} +void gps_feed_value(void) {} + diff --git a/sw/airborne/modules/sensors/sensors_hitl.h b/sw/airborne/modules/sensors/sensors_hitl.h new file mode 100644 index 0000000000..1bdcf943ec --- /dev/null +++ b/sw/airborne/modules/sensors/sensors_hitl.h @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2023 Gautier Hattenberger + * + * 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, see + * . + */ + +#ifndef SENSORS_HITL_H +#define SENSORS_HITL_H + +#include "std.h" + +#ifndef PRIMARY_GPS +#define PRIMARY_GPS GPS_SIM +#endif + +extern bool gps_has_fix; + +extern void sensors_hitl_init(void); +extern void sensors_hitl_periodic(void); +extern void sensors_hitl_event(void); +extern void sensors_hitl_parse_HITL_IMU(uint8_t *buf); +extern void sensors_hitl_parse_HITL_GPS(uint8_t *buf); +extern void sensors_hitl_parse_HITL_AIR_DATA(uint8_t *buf); + +// dummy definition for compilation +extern void imu_feed_gyro_accel(void); +extern void imu_feed_mag(void); +extern void gps_feed_value(void); + +#endif /* SENSORS_HITL_H */ + diff --git a/sw/airborne/peripherals/vn200_serial.c b/sw/airborne/peripherals/vn200_serial.c index dd6374fae2..a4f00cc5a9 100644 --- a/sw/airborne/peripherals/vn200_serial.c +++ b/sw/airborne/peripherals/vn200_serial.c @@ -27,6 +27,8 @@ * @author Michal Podhradsky */ #include "peripherals/vn200_serial.h" +#include "mcu_periph/uart.h" +#include "mcu_periph/usb_serial.h" void vn200_check_status(struct VNData *vn_data); @@ -101,15 +103,17 @@ static inline bool verify_chk(unsigned char data[], unsigned int length, uint16_ static inline void vn200_read_buffer(struct VNPacket *vnp) { - while (uart_char_available(&VN_PORT) && !(vnp->msg_available)) { - vn200_parse(vnp, uart_getch(&VN_PORT)); + struct link_device *dev = &(VN_PORT.device); + while (dev->char_available(dev->periph) && !(vnp->msg_available)) { + vn200_parse(vnp, dev->get_byte(dev->periph)); } } void vn200_event(struct VNPacket *vnp) { - if (uart_char_available(&VN_PORT)) { + struct link_device *dev = &(VN_PORT.device); + if (dev->char_available(dev->periph)) { vn200_read_buffer(vnp); } } diff --git a/sw/airborne/peripherals/vn200_serial.h b/sw/airborne/peripherals/vn200_serial.h index 47619b8942..692fbc8672 100644 --- a/sw/airborne/peripherals/vn200_serial.h +++ b/sw/airborne/peripherals/vn200_serial.h @@ -30,7 +30,6 @@ #define VN200_SERIAl_H #include "std.h" -#include "mcu_periph/uart.h" // Geodetic / Math #include "math/pprz_algebra.h" diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 87797bda6a..d4bfd949d7 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 87797bda6a7a55602fb2b809918d95b10f43d9d7 +Subproject commit d4bfd949d7c001803a9b26df3065556bafeb4c2d diff --git a/sw/simulator/nps/nps_hitl_sensors.c b/sw/simulator/nps/nps_hitl_sensors.c new file mode 100644 index 0000000000..01db660555 --- /dev/null +++ b/sw/simulator/nps/nps_hitl_sensors.c @@ -0,0 +1,364 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * Copyright (C) 2012 The Paparazzi Team + * Copyright (C) 2016 Michal Podhradsky + * Copyright (C) 2023 Gautier Hattenberger + * + * 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 +#include +#include +#include +#include +#include +#include +#include "nps_sensors.h" +#include "nps_main.h" +#include "paparazzi.h" +#include "pprzlink/messages.h" +#include "pprzlink/dl_protocol.h" +#include "pprzlink/pprzlink_device.h" +#include "pprzlink/pprz_transport.h" +#include "arch/linux/serial_port.h" + +#ifndef AP_DEV +#warning "[hitl] Please define AP_DEV in your airframe file" +#define AP_DEV "/dev/ttyUSB0" +#endif +#ifndef AP_BAUD +#define AP_BAUD B921600 +PRINT_CONFIG_MSG_VALUE("[hitl] Using default baudrate for AP_DEV (B921600)", AP_BAUD) +#endif + +#define NPS_HITL_DEBUG 0 +#if NPS_HITL_DEBUG +#define DEBUG_PRINT printf +#else +#define DEBUG_PRINT(...) {} +#endif + +void *nps_sensors_loop(void *data __attribute__((unused))); +void *nps_ap_data_loop(void *data __attribute__((unused))); +pthread_t th_sensors; // send sensors to AP +pthread_t th_ap_data; // receive commands from AP + +/* implement pprzlink_device interface */ +#define PPRZLINK_BUFFER_SIZE 256 +struct linkdev { + /** Receive buffer */ + uint8_t rx_buf[PPRZLINK_BUFFER_SIZE]; + uint16_t rx_idx; + /** Transmit buffer */ + uint8_t tx_buf[PPRZLINK_BUFFER_SIZE]; + uint16_t tx_idx; + //volatile uint8_t tx_running; + /** Generic device interface */ + struct link_device device; + /** transport */ + struct pprz_transport pprz_tp; + /** Serial port */ + struct SerialPort *port; + struct pollfd fds[1]; +}; + +static struct linkdev dev; + +static int check_free_space(struct linkdev *d, long *fd __attribute__((unused)), uint16_t len) +{ + int space = PPRZLINK_BUFFER_SIZE - d->tx_idx; + return space >= len ? space : 0; +} + +static void put_byte(struct linkdev *d, long fd __attribute__((unused)), uint8_t data) +{ + d->tx_buf[d->tx_idx++] = data; +} + +static void put_buffer(struct linkdev *d, long fd __attribute__((unused)), const uint8_t *data, uint16_t len) +{ + memcpy(&(d->tx_buf[d->tx_idx]), data, len); + d->tx_idx += len; +} + +static void send_message(struct linkdev *d, long fd __attribute__((unused))) +{ + int ret = 0; + do { + ret = write((int)(d->port->fd), d->tx_buf, d->tx_idx); + } while (ret < 1 && errno == EAGAIN); //FIXME: max retry + if (ret < 1) { + DEBUG_PRINT("[hitl] put_byte: write failed [%d: %s]\n", ret, strerror(errno)); + } + d->tx_idx = 0; +} + +static uint8_t getch(struct linkdev *d) +{ + // this function should only be called when bytes are available + unsigned char c = 'B'; + ssize_t ret = 0; + ret = read(d->port->fd, &c, 1); + if (ret > 0) { + d->rx_buf[d->rx_idx] = c; + if (d->rx_idx < PPRZLINK_BUFFER_SIZE) { + d->rx_idx++; + } else { + DEBUG_PRINT("[hitl] rx buffer overflow\n"); + } + } else { + DEBUG_PRINT("[hitl] rx read error\n"); + } + return c; +} + +static int char_available(struct linkdev *d) +{ + int ret = poll(d->fds, 1, 1000); + if (ret > 0) { + if (d->fds[0].revents & POLLHUP) { + printf("[hitl] lost connection. Exiting\n"); + exit(1); + } + if (d->fds[0].revents & POLLIN) { + return true; + } + } else if (ret == -1) { + DEBUG_PRINT("[hitl] poll failed\n"); + } + return false; +} + +/// END pprzlink_dev + +void nps_hitl_impl_init(void) +{ + pthread_create(&th_sensors, NULL, nps_sensors_loop, NULL); + pthread_create(&th_ap_data, NULL, nps_ap_data_loop, NULL); + + dev.device.periph = (void *) (&dev); + dev.device.check_free_space = (check_free_space_t) check_free_space; + dev.device.put_byte = (put_byte_t) put_byte; + dev.device.put_buffer = (put_buffer_t) put_buffer; + dev.device.send_message = (send_message_t) send_message; + dev.device.char_available = (char_available_t) char_available; + dev.device.get_byte = (get_byte_t) getch; + pprz_transport_init(&dev.pprz_tp); + + // open serial port + dev.port = serial_port_new(); + int ret = serial_port_open_raw(dev.port, AP_DEV, AP_BAUD); + if (ret != 0) { + printf("[hitl] Error opening %s code %d\n", AP_DEV, ret); + serial_port_free(dev.port); + } + + // poll + if (dev.port != NULL) { + dev.fds[0].fd = dev.port->fd; + dev.fds[0].events = POLLIN; + } +} + +void *nps_sensors_loop(void *data __attribute__((unused))) +{ + struct timespec requestStart; + struct timespec requestEnd; + struct timespec waitFor; + long int period_ns = (1. / PERIODIC_FREQUENCY) * 1000000000L; // thread period in nanoseconds + long int task_ns = 0; // time it took to finish the task in nanoseconds + + while (TRUE) { + // lock mutex + pthread_mutex_lock(&fdm_mutex); + + // start timing + clock_get_current_time(&requestStart); + + uint8_t id = AC_ID; + + if (nps_sensors_gyro_available()) { + float gx = (float)sensors.gyro.value.x; + float gy = (float)sensors.gyro.value.y; + float gz = (float)sensors.gyro.value.z; + float ax = (float)sensors.accel.value.x; + float ay = (float)sensors.accel.value.y; + float az = (float)sensors.accel.value.z; + float mx = (float)sensors.mag.value.x; + float my = (float)sensors.mag.value.y; + float mz = (float)sensors.mag.value.z; + uint8_t id = AC_ID; + pprz_msg_send_HITL_IMU(&dev.pprz_tp.trans_tx, &dev.device, 0, + &gx, &gy, &gz, + &ax, &ay, &az, + &mx, &my, &mz, + &id); + } + + if (nps_sensors_gps_available()) { + float gps_lat = (float)DegOfRad(sensors.gps.lla_pos.lat); + float gps_lon = (float)DegOfRad(sensors.gps.lla_pos.lon); + float gps_alt = (float)sensors.gps.lla_pos.alt; + float gps_hmsl = (float)sensors.gps.hmsl; + float gps_vx = (float)sensors.gps.ecef_vel.x; + float gps_vy = (float)sensors.gps.ecef_vel.y; + float gps_vz = (float)sensors.gps.ecef_vel.z; + float gps_time = (float)nps_main.sim_time; + uint8_t gps_fix = 3; // GPS fix + pprz_msg_send_HITL_GPS(&dev.pprz_tp.trans_tx, &dev.device, 0, + &gps_lat, &gps_lon, &gps_alt, &gps_hmsl, + &gps_vx, &gps_vy, &gps_vz, + &gps_time, &gps_fix, &id); + } + + uint8_t air_data_flag = 0; + float baro = -1.f; + float airspeed = -1.f; + float aoa = 0.f; + float sideslip = 0.f; + if (nps_sensors_baro_available()) { + SetBit(air_data_flag, 0); + baro = (float) sensors.baro.value; + } + if (nps_sensors_airspeed_available()) { + SetBit(air_data_flag, 1); + airspeed = (float) sensors.airspeed.value; + } + if (nps_sensors_aoa_available()) { + SetBit(air_data_flag, 2); + aoa = (float) sensors.aoa.value; + } + if (nps_sensors_sideslip_available()) { + SetBit(air_data_flag, 3); + sideslip = (float) sensors.sideslip.value; + } + if (air_data_flag != 0) { + pprz_msg_send_HITL_AIR_DATA(&dev.pprz_tp.trans_tx, &dev.device, 0, + &baro, &airspeed, &aoa, &sideslip, &air_data_flag, &id); + } + + // unlock mutex + pthread_mutex_unlock(&fdm_mutex); + + clock_get_current_time(&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 +#ifdef PRINT_TIME + printf("SENSORS: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n", + (double)task_ns / 1E6, (double)period_ns / 1E6); +#endif + } + } + return(NULL); +} + +void *nps_ap_data_loop(void *data __attribute__((unused))) +{ + struct timespec waitFor; + + uint8_t msg_buffer[PPRZLINK_BUFFER_SIZE]; + bool msg_available = false; + + bool first_command = true; + + uint8_t cmd_len = 0; + pprz_t cmd_buf[NPS_COMMANDS_NB]; + + while (TRUE) { + + pprz_check_and_parse(&dev.device, &dev.pprz_tp, msg_buffer, &msg_available); + if (msg_available) { + // reset rx index to zero for next message + dev.rx_idx = 0; + //Parse message + uint8_t sender_id = SenderIdOfPprzMsg(msg_buffer); + uint8_t msg_id = IdOfPprzMsg(msg_buffer); + + if (sender_id != AC_ID) { + printf("[hitl] receiving message from wrong id (%d)\n", sender_id); + return(NULL); // wrong A/C ? + } + /* parse telemetry messages coming from the correct AC_ID */ + switch (msg_id) { + case DL_COMMANDS: + // parse commands message + cmd_len = DL_COMMANDS_values_length(msg_buffer); + memcpy(&cmd_buf, DL_COMMANDS_values(msg_buffer), cmd_len * sizeof(int16_t)); + pthread_mutex_lock(&fdm_mutex); + // update commands + for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) { + nps_autopilot.commands[i] = (double)cmd_buf[i] / MAX_PPRZ; + } + // hack: invert pitch to fit most JSBSim models + nps_autopilot.commands[COMMAND_PITCH] = -(double)cmd_buf[COMMAND_PITCH] / MAX_PPRZ; + if (first_command) { + printf("[hitl] receiving COMMANDS message\n"); + first_command = false; + } + pthread_mutex_unlock(&fdm_mutex); + break; + case DL_MOTOR_MIXING: + // parse actuarors message + cmd_len = DL_MOTOR_MIXING_values_length(msg_buffer); + // check for out-of-bounds access + if (cmd_len > NPS_COMMANDS_NB) { + cmd_len = NPS_COMMANDS_NB; + } + memcpy(&cmd_buf, DL_MOTOR_MIXING_values(msg_buffer), cmd_len * sizeof(int16_t)); + pthread_mutex_lock(&fdm_mutex); + // update commands + for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) { + nps_autopilot.commands[i] = (double)cmd_buf[i] / MAX_PPRZ; + } + if (first_command) { + printf("[hitl] receiving MOTOR_MIXING message\n"); + first_command = false; + } + pthread_mutex_unlock(&fdm_mutex); + break; + default: + break; + } + + msg_available = false; + } + + // wait before next loop + waitFor.tv_sec = 0; + waitFor.tv_nsec = 1000; + nanosleep(&waitFor, NULL); + + } + return(NULL); +} + + diff --git a/sw/simulator/nps/nps_ins_vectornav.c b/sw/simulator/nps/nps_ins_vectornav.c deleted file mode 100644 index ba1b85bebb..0000000000 --- a/sw/simulator/nps/nps_ins_vectornav.c +++ /dev/null @@ -1,290 +0,0 @@ -/* - * 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 -#include "nps_sensors.h" -#include /* srand, rand */ - -/* - * 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_init(void) {} -void ins_vectornav_event(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. - */ -static 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 = (uint64_t)GPS_SEC_IN_DAY * tt->tm_wday + (uint64_t)3600 * tt->tm_hour + (uint64_t)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; -} - -/** - * Fetch data from FDM and store them into vectornav packet - * NOTE: some noise is being added, see Vectornav specifications - * for details about the precision: http://www.vectornav.com/products/vn-200/specifications - */ -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(sensors.gps.lla_pos.lat); - vn_data.Position[1] = DegOfRad(sensors.gps.lla_pos.lon); - vn_data.Position[2] = sensors.gps.lla_pos.alt; - - //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 - // TODO: add warm-up time - vn_data.Fix = 3; // 3D fix - - //posU, float[3] - // TODO: use proper sensor simulation - vn_data.PosU[0] = 2.5+(((float)rand())/RAND_MAX)*0.1; - vn_data.PosU[1] = 2.5+(((float)rand())/RAND_MAX)*0.1; - vn_data.PosU[2] = 2.5+(((float)rand())/RAND_MAX)*0.1; - - //velU, float - // TODO: use proper sensor simulation - vn_data.VelU = 5.0+(((float)rand())/RAND_MAX)*0.1; - - //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: use proper sensor simulation - vn_data.YprU[0] = 2.5+(((float)rand())/RAND_MAX)*0.1; - vn_data.YprU[1] = 0.5+(((float)rand())/RAND_MAX)*0.1; - vn_data.YprU[2] = 0.5+(((float)rand())/RAND_MAX)*0.1; - - //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 9edfd12551..aa6fca60f7 100644 --- a/sw/simulator/nps/nps_ivy.c +++ b/sw/simulator/nps/nps_ivy.c @@ -160,6 +160,10 @@ void nps_ivy_send_WORLD_ENV_REQ(void) int find_launch_index(void) { +#ifdef AP_LAUNCH + return AP_LAUNCH - 1; // index of AP_LAUNCH starts at 1, but it should be 0 here +#else +#if NB_SETTING > 0 static const char ap_launch[] = "aut_lau"; // short name char *ap_settings[NB_SETTING] = SETTINGS_NAMES_SHORT; @@ -170,7 +174,9 @@ int find_launch_index(void) return (int)idx; } } +#endif return -1; +#endif } static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)), @@ -190,10 +196,12 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)), */ uint8_t index = atoi(argv[2]); float value = atof(argv[3]); +#ifndef HITL if (!datalink_enabled) { DlSetting(index, value); DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value); } +#endif printf("setting %d %f\n", index, value); /* diff --git a/sw/simulator/nps/nps_ivy.h b/sw/simulator/nps/nps_ivy.h index 6074927fea..6d4e973b22 100644 --- a/sw/simulator/nps/nps_ivy.h +++ b/sw/simulator/nps/nps_ivy.h @@ -7,6 +7,7 @@ extern bool nps_ivy_send_world_env; extern void nps_ivy_init(char *ivy_bus); +extern void nps_ivy_hitl(struct NpsSensors* sensors_data); extern void nps_ivy_display(struct NpsFdm* fdm_ivy, struct NpsSensors* sensors_ivy); extern void nps_ivy_send_WORLD_ENV_REQ(void); diff --git a/sw/simulator/nps/nps_main.h b/sw/simulator/nps/nps_main.h index 4b2016d824..6c4df16112 100644 --- a/sw/simulator/nps/nps_main.h +++ b/sw/simulator/nps/nps_main.h @@ -48,6 +48,8 @@ extern double ntime_to_double(struct timespec *t); void nps_update_launch_from_dl(uint8_t value); +extern void nps_hitl_impl_init(void); // implement for HITL specific implementation + struct NpsMain { double real_initial_time; double scaled_initial_time; diff --git a/sw/simulator/nps/nps_main_hitl.c b/sw/simulator/nps/nps_main_hitl.c index 2f3215fe7d..d6e0a50d71 100644 --- a/sw/simulator/nps/nps_main_hitl.c +++ b/sw/simulator/nps/nps_main_hitl.c @@ -26,47 +26,30 @@ #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" #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 +// nps_autopilot.c is not compiled in HITL +struct NpsAutopilot nps_autopilot; int main(int argc, char **argv) { nps_main_init(argc, argv); + nps_hitl_impl_init(); + 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; @@ -74,20 +57,6 @@ int main(int argc, char **argv) void nps_radio_and_autopilot_init(void) { - enum NpsRadioControlType rc_type; - char *rc_dev = NULL; - if (nps_main.norc) { - rc_type = NORC; - } else 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) @@ -105,202 +74,6 @@ void nps_main_run_sim_step(void) nps_sensors_run_step(nps_main.sim_time); } -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(STRINGIFY(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_get_current_time(&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_get_current_time(&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 -#ifdef PRINT_TIME - 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); -#endif - } - } - return(NULL); -} - - - -void *nps_ap_data_loop(void *data __attribute__((unused))) -{ - // configure port - int fd = open(STRINGIFY(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; - - pprz_transport_init(&pprz_tp_logger); - uint32_t rx_msgs = 0; - uint32_t rx_commands = 0; - uint32_t rx_motor_mixing = 0; - uint8_t show_stats = 1; - - while (TRUE) { - if ((rx_msgs % 100 == 0) && show_stats && (rx_msgs > 0) ) { - printf("AP data loop received %i messages.\n", rx_msgs); - printf("From those, %i were COMMANDS messages, and %i were MOTOR_MIXING messages.\n", - rx_commands, rx_motor_mixing); - show_stats = 0; - } - - // receive messages from the autopilot - // Note: read function might wait until the buffer is full, in which case - // it could contain several messages. That might lead to delay in processing, - // for example if we send COMMANDS at 100Hz, and each read() will hold 10 COMMANDS - // it means a delay of 100ms before the message is processed. - 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) { - rx_msgs++; - 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 - rx_commands++; - 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++) { - nps_autopilot.commands[i] = (double)cmd_buf[i] / MAX_PPRZ; - } - // hack: invert pitch to fit most JSBSim models - nps_autopilot.commands[COMMAND_PITCH] = -(double)cmd_buf[COMMAND_PITCH] / MAX_PPRZ; - pthread_mutex_unlock(&fdm_mutex); - break; - case DL_MOTOR_MIXING: - // parse actuarors message - rx_motor_mixing++; - cmd_len = DL_MOTOR_MIXING_values_length(buf); - // check for out-of-bounds access - if (cmd_len > NPS_COMMANDS_NB) { - cmd_len = NPS_COMMANDS_NB; - } - 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++) { - nps_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))) { diff --git a/sw/tools/generators/gen_aircraft.ml b/sw/tools/generators/gen_aircraft.ml index 482cee11ca..dd0aca82a6 100644 --- a/sw/tools/generators/gen_aircraft.ml +++ b/sw/tools/generators/gen_aircraft.ml @@ -342,15 +342,22 @@ let () = | _ -> Printf.eprintf "Missing airframe or flight_plan" end; (* Create Makefile.ac only if needed *) + Printf.printf "Dumping makefile...%!"; let makefile_ac = aircraft_dir // "Makefile.ac" in match ac.Aircraft.airframe with - | None -> () + | None -> Printf.printf "(skip)" | Some airframe -> let module_files = List.map (fun m -> m.Module.xml_filename) loaded_modules in if is_older makefile_ac (airframe.Airframe.filename :: module_files) then - assert(Sys.command (sprintf "mv %s %s" temp_makefile_ac makefile_ac) = 0) + begin + Printf.printf("(copying)"); + assert(Sys.command (sprintf "mv %s %s" temp_makefile_ac makefile_ac) = 0); + end + else + Printf.printf("(skip copying)"); ; + Printf.printf " done\n%!"; with Failure f -> prerr_endline f;