diff --git a/conf/airframes/CDW/asctec_cdw.xml b/conf/airframes/CDW/asctec_cdw.xml index e28e7f538b..830c176d2f 100644 --- a/conf/airframes/CDW/asctec_cdw.xml +++ b/conf/airframes/CDW/asctec_cdw.xml @@ -220,7 +220,7 @@
- +
diff --git a/conf/airframes/CDW/tricopter_cdw.xml b/conf/airframes/CDW/tricopter_cdw.xml index 48e2192956..86946f11f3 100644 --- a/conf/airframes/CDW/tricopter_cdw.xml +++ b/conf/airframes/CDW/tricopter_cdw.xml @@ -209,7 +209,7 @@ LiPo/LiIo-Zellen: 2-3
- +
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index ae5f73a8eb..834132e720 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -260,7 +260,7 @@
- +
diff --git a/conf/airframes/NoVa_L.xml b/conf/airframes/NoVa_L.xml index affb520b20..aaf45e3e27 100644 --- a/conf/airframes/NoVa_L.xml +++ b/conf/airframes/NoVa_L.xml @@ -228,7 +228,7 @@
- +
diff --git a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml index 2ff2bb94a6..ace2179575 100644 --- a/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml @@ -182,7 +182,7 @@
- +
diff --git a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml index b73795e5aa..064c01ae5a 100644 --- a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml @@ -184,7 +184,7 @@
- +
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml index 87f9e38388..ee9e0848ac 100644 --- a/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml +++ b/conf/airframes/TestHardware/LisaL_v1.1_aspirin_v1.5_rc.xml @@ -253,7 +253,7 @@
- +
diff --git a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml index f9d1c22a70..06f2f97307 100644 --- a/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml +++ b/conf/airframes/TestHardware/LisaL_v1.1_b2_v1.2_rc.xml @@ -253,7 +253,7 @@
- +
diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 579ff9a31f..99265a1638 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -203,7 +203,7 @@
- +
diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index d7467c3203..a62ace8aee 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -232,7 +232,7 @@
- +
diff --git a/conf/airframes/esden/cocto_lm2a2.xml b/conf/airframes/esden/cocto_lm2a2.xml index 6d77ad1dcb..609e7295bd 100644 --- a/conf/airframes/esden/cocto_lm2a2.xml +++ b/conf/airframes/esden/cocto_lm2a2.xml @@ -207,7 +207,7 @@ B2L -> CW
- +
diff --git a/conf/airframes/esden/gain_scheduling_example.xml b/conf/airframes/esden/gain_scheduling_example.xml index b38357f11f..02f574bd9f 100644 --- a/conf/airframes/esden/gain_scheduling_example.xml +++ b/conf/airframes/esden/gain_scheduling_example.xml @@ -189,7 +189,7 @@
- +
diff --git a/conf/airframes/esden/hexy_ll11a2pwm.xml b/conf/airframes/esden/hexy_ll11a2pwm.xml index 55412a313b..f796f42c68 100644 --- a/conf/airframes/esden/hexy_ll11a2pwm.xml +++ b/conf/airframes/esden/hexy_ll11a2pwm.xml @@ -205,7 +205,7 @@
- +
diff --git a/conf/airframes/esden/hexy_lm2a2pwm.xml b/conf/airframes/esden/hexy_lm2a2pwm.xml index 53292099a9..73554f3209 100644 --- a/conf/airframes/esden/hexy_lm2a2pwm.xml +++ b/conf/airframes/esden/hexy_lm2a2pwm.xml @@ -166,7 +166,7 @@
- +
diff --git a/conf/airframes/esden/lisa2_hex.xml b/conf/airframes/esden/lisa2_hex.xml index ce6a348d5f..b21dddffcb 100644 --- a/conf/airframes/esden/lisa2_hex.xml +++ b/conf/airframes/esden/lisa2_hex.xml @@ -177,7 +177,7 @@
- +
diff --git a/conf/airframes/esden/qs_asp22.xml b/conf/airframes/esden/qs_asp22.xml index f47a8715d1..c4ef62b938 100644 --- a/conf/airframes/esden/qs_asp22.xml +++ b/conf/airframes/esden/qs_asp22.xml @@ -200,7 +200,7 @@
- +
diff --git a/conf/airframes/esden/quady_ll11a2pwm.xml b/conf/airframes/esden/quady_ll11a2pwm.xml index 876ee4a2c3..ca80e5a5d2 100644 --- a/conf/airframes/esden/quady_ll11a2pwm.xml +++ b/conf/airframes/esden/quady_ll11a2pwm.xml @@ -201,7 +201,7 @@
- +
diff --git a/conf/airframes/esden/quady_lm1a1pwm.xml b/conf/airframes/esden/quady_lm1a1pwm.xml index e94f7008ae..406734e757 100644 --- a/conf/airframes/esden/quady_lm1a1pwm.xml +++ b/conf/airframes/esden/quady_lm1a1pwm.xml @@ -162,7 +162,7 @@
- +
diff --git a/conf/airframes/esden/quady_lm2a2pwm.xml b/conf/airframes/esden/quady_lm2a2pwm.xml index 6b8b5da46d..e87c817b3e 100644 --- a/conf/airframes/esden/quady_lm2a2pwm.xml +++ b/conf/airframes/esden/quady_lm2a2pwm.xml @@ -164,7 +164,7 @@
- +
diff --git a/conf/airframes/esden/quady_lm2a2pwmppm.xml b/conf/airframes/esden/quady_lm2a2pwmppm.xml index f33377fe79..31fd65c39d 100644 --- a/conf/airframes/esden/quady_lm2a2pwmppm.xml +++ b/conf/airframes/esden/quady_lm2a2pwmppm.xml @@ -162,7 +162,7 @@
- +
diff --git a/conf/airframes/esden/quady_ls01pwm.xml b/conf/airframes/esden/quady_ls01pwm.xml index 9dc6032179..4a3a194c39 100644 --- a/conf/airframes/esden/quady_ls01pwm.xml +++ b/conf/airframes/esden/quady_ls01pwm.xml @@ -164,7 +164,7 @@
- +
diff --git a/conf/airframes/examples/MentorEnergy.xml b/conf/airframes/examples/MentorEnergy.xml index 9416ac28b8..f282305020 100644 --- a/conf/airframes/examples/MentorEnergy.xml +++ b/conf/airframes/examples/MentorEnergy.xml @@ -21,6 +21,11 @@ + + + + + @@ -251,4 +256,11 @@ +
+ + + + +
+ diff --git a/conf/airframes/examples/booz2.xml b/conf/airframes/examples/booz2.xml index dca8389581..e93ccf9d2c 100644 --- a/conf/airframes/examples/booz2.xml +++ b/conf/airframes/examples/booz2.xml @@ -194,7 +194,7 @@
- +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml index 428fa2fe44..44f2681826 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_hexa_mkk.xml @@ -226,7 +226,7 @@
- +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml index ff2a2c3510..4bf55eeb9f 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_okto_mkk.xml @@ -229,7 +229,7 @@
- +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml index 9166f503b2..ebdda35837 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_mkk.xml @@ -218,7 +218,7 @@
- +
diff --git a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml index df4334d751..cf85a484c0 100644 --- a/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml +++ b/conf/airframes/examples/krooz_sd/krooz_sd_quad_pwm.xml @@ -214,7 +214,7 @@
- +
diff --git a/conf/airframes/examples/lisa_asctec.xml b/conf/airframes/examples/lisa_asctec.xml index 4deef180d9..5e4869cb87 100644 --- a/conf/airframes/examples/lisa_asctec.xml +++ b/conf/airframes/examples/lisa_asctec.xml @@ -179,7 +179,7 @@
- +
diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml index 57f66a9d03..ea03629dd7 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -202,7 +202,7 @@
- + diff --git a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml index 90932d24e5..06a69c6cf2 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum_geo_mag.xml @@ -204,7 +204,7 @@
- +
diff --git a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml index 4a20c2b893..4746f8f78d 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_mkk.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_mkk.xml @@ -193,7 +193,7 @@
- +
diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml index 5dcc646985..6cee5b9cfa 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -182,7 +182,7 @@
- +
diff --git a/conf/airframes/examples/quadrotor_mlkf.xml b/conf/airframes/examples/quadrotor_mlkf.xml index 151464b016..8778f766e9 100644 --- a/conf/airframes/examples/quadrotor_mlkf.xml +++ b/conf/airframes/examples/quadrotor_mlkf.xml @@ -204,7 +204,7 @@
- + diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index 143f5fb701..98df961abd 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -265,7 +265,7 @@
- +
diff --git a/conf/airframes/examples/quadrotor_px4fmu.xml b/conf/airframes/examples/quadrotor_px4fmu.xml index 7d9b21aaeb..a67d1738fd 100644 --- a/conf/airframes/examples/quadrotor_px4fmu.xml +++ b/conf/airframes/examples/quadrotor_px4fmu.xml @@ -198,7 +198,7 @@
- +
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml index 771f4082c8..d5fc8439fd 100644 --- a/conf/airframes/examples/quadshot_asp21_spektrum.xml +++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml @@ -232,7 +232,7 @@ More information on the Quadshot can be found at transition-robotics.com -->
- +
diff --git a/conf/airframes/fraser_lisa_m_rotorcraft.xml b/conf/airframes/fraser_lisa_m_rotorcraft.xml index d57a70036b..7fec69072c 100644 --- a/conf/airframes/fraser_lisa_m_rotorcraft.xml +++ b/conf/airframes/fraser_lisa_m_rotorcraft.xml @@ -238,7 +238,7 @@
- + diff --git a/conf/airframes/obsolete/ENAC/g1_vision.xml b/conf/airframes/obsolete/ENAC/g1_vision.xml index 71ca37d27a..d24b588c2e 100644 --- a/conf/airframes/obsolete/ENAC/g1_vision.xml +++ b/conf/airframes/obsolete/ENAC/g1_vision.xml @@ -194,7 +194,7 @@
- +
diff --git a/conf/airframes/obsolete/Poine/booz2_a1.xml b/conf/airframes/obsolete/Poine/booz2_a1.xml index 8d66978f1b..5823e29e03 100644 --- a/conf/airframes/obsolete/Poine/booz2_a1.xml +++ b/conf/airframes/obsolete/Poine/booz2_a1.xml @@ -189,7 +189,7 @@
- +
diff --git a/conf/airframes/obsolete/Poine/booz2_a7.xml b/conf/airframes/obsolete/Poine/booz2_a7.xml index 59f3bb2082..d3f23c31fd 100644 --- a/conf/airframes/obsolete/Poine/booz2_a7.xml +++ b/conf/airframes/obsolete/Poine/booz2_a7.xml @@ -201,7 +201,7 @@
- +
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml index 4061167138..10b61e41a0 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_BOOZ.xml @@ -263,7 +263,7 @@ second attempt
- +
diff --git a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml index 564e7dbcc4..406d6de2c0 100644 --- a/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml +++ b/conf/airframes/obsolete/UofAdelaide/A1000_NOVA.xml @@ -216,7 +216,7 @@
- +
diff --git a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml index 06ea8ce8ad..82f499852b 100644 --- a/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml +++ b/conf/airframes/obsolete/UofAdelaide/booz2_a1000.xml @@ -265,7 +265,7 @@ second attempt
- +
diff --git a/conf/airframes/obsolete/booz2_a1p.xml b/conf/airframes/obsolete/booz2_a1p.xml index f5906c89d6..18ff3d98f8 100644 --- a/conf/airframes/obsolete/booz2_a1p.xml +++ b/conf/airframes/obsolete/booz2_a1p.xml @@ -193,7 +193,7 @@
- +
diff --git a/conf/airframes/obsolete/booz2_s1.xml b/conf/airframes/obsolete/booz2_s1.xml index 17b8eaf5b5..4cf6bb4bbe 100644 --- a/conf/airframes/obsolete/booz2_s1.xml +++ b/conf/airframes/obsolete/booz2_s1.xml @@ -180,7 +180,7 @@
- +
diff --git a/conf/airframes/obsolete/example_heli_lisam.xml b/conf/airframes/obsolete/example_heli_lisam.xml index e3dbdfd842..79e8d519ac 100644 --- a/conf/airframes/obsolete/example_heli_lisam.xml +++ b/conf/airframes/obsolete/example_heli_lisam.xml @@ -217,7 +217,7 @@ AP_MODE_NAV
- +
diff --git a/conf/airframes/obsolete/mm/rotor/qmk1.xml b/conf/airframes/obsolete/mm/rotor/qmk1.xml index 8066ee667d..37b3b6c07a 100644 --- a/conf/airframes/obsolete/mm/rotor/qmk1.xml +++ b/conf/airframes/obsolete/mm/rotor/qmk1.xml @@ -174,7 +174,7 @@
- +
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml index 93991b0de9..5d1b3e397d 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_l_mkk.xml @@ -218,7 +218,7 @@
- +
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml index 7fabac6e96..5343031357 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_1_pwm.xml @@ -180,7 +180,7 @@
- +
diff --git a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml index ed99bdefa5..2ba487a784 100644 --- a/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml +++ b/conf/airframes/obsolete/mm/rotor/quadrotor_lisa_m_2_pwm.xml @@ -187,7 +187,7 @@
- +
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile index 095cc6fda2..3bd625f6b5 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile @@ -36,6 +36,11 @@ ap.srcs += $(AHRS_SRCS) #ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) #endif +# +# NPS uses the real algorithm +# +nps.CFLAGS += $(AHRS_CFLAGS) +nps.srcs += $(AHRS_SRCS) # # Simple simulation of the AHRS result diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile index 8a5330bea2..6d82542b6c 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile @@ -27,6 +27,11 @@ AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ap.CFLAGS += $(AHRS_CFLAGS) ap.srcs += $(AHRS_SRCS) +# +# NPS uses the real algorithm +# +nps.CFLAGS += $(AHRS_CFLAGS) +nps.srcs += $(AHRS_SRCS) # # Simple simulation of the AHRS result diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile index 26f09e2518..6427993c02 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile @@ -4,27 +4,32 @@ USE_MAGNETOMETER ?= 0 -ifeq ($(TARGET), ap) - -ap.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" -ap.CFLAGS += -DUSE_AHRS_ALIGNER -ap.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" +AHRS_CFLAGS += -DUSE_AHRS_ALIGNER +AHRS_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR ifneq ($(USE_MAGNETOMETER),0) -ap.CFLAGS += -DUSE_MAGNETOMETER +AHRS_CFLAGS += -DUSE_MAGNETOMETER endif -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c -ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c +AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c +AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c +AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c ifneq ($(AHRS_ALIGNER_LED),none) - ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) + AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) endif -endif +ap.CFLAGS += $(AHRS_CFLAGS) +ap.srcs += $(AHRS_SRCS) + +# +# NPS uses the real algorithm +# +nps.CFLAGS += $(AHRS_CFLAGS) +nps.srcs += $(AHRS_SRCS) # # Simple simulation of the AHRS result diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile index b1f1bb294f..67cb44eb1f 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile @@ -22,6 +22,8 @@ AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c ap.CFLAGS += $(AHRS_CFLAGS) ap.srcs += $(AHRS_SRCS) +nps.CFLAGS += $(AHRS_CFLAGS) +nps.srcs += $(AHRS_SRCS) # # Simple simulation of the AHRS result diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile index ca4c99ecdb..5bea385eaa 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile @@ -36,6 +36,11 @@ ap.srcs += $(AHRS_SRCS) #ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) #endif +# +# NPS uses the real algorithm +# +nps.CFLAGS += $(AHRS_CFLAGS) +nps.srcs += $(AHRS_SRCS) # # Simple simulation of the AHRS result diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 3fef9171db..90b9883deb 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -52,17 +52,7 @@ endif $(TARGET).CFLAGS += -DTRAFFIC_INFO -# -# LEDs -# -ifneq ($(ARCH), jsbsim) - $(TARGET).CFLAGS += -DUSE_LED -endif -ifneq ($(ARCH), lpc21) - ifneq ($(ARCH), jsbsim) - $(TARGET).srcs += $(SRC_ARCH)/led_hw.c - endif -endif + # # Sys-time @@ -123,6 +113,9 @@ ns_CFLAGS += -DUSE_LED ifneq ($(SYS_TIME_LED),none) ns_CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) endif +ifneq ($(ARCH), lpc21) + ns_srcs += $(SRC_ARCH)/led_hw.c +endif # @@ -202,10 +195,9 @@ sim.CFLAGS += -DSITL sim.srcs += $(SRC_ARCH)/sim_ap.c sim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c +sim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c -sim.srcs += subsystems/settings.c -sim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c +sim.srcs += $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_adc_generic.c # hack: always compile some of the sim functions, so ocaml sim does not complain about no-existing functions sim.srcs += $(SRC_ARCH)/sim_ahrs.c $(SRC_ARCH)/sim_ir.c @@ -243,10 +235,9 @@ jsbsim.CFLAGS += -I/usr/include $(shell pkg-config glib-2.0 --cflags) jsbsim.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lglibivy -lm jsbsim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -jsbsim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ahrs.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c +jsbsim.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c -jsbsim.srcs += subsystems/settings.c -jsbsim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c +jsbsim.srcs += $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/jsbsim_ahrs.c $(SRC_ARCH)/jsbsim_transport.c ###################################################################### ## diff --git a/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile new file mode 100644 index 0000000000..688d5b7cb4 --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/fdm_jsbsim.makefile @@ -0,0 +1,66 @@ +# Hey Emacs, this is a -*- makefile -*- + +# +# SITL Simulator +# + +JSBSIM_ROOT ?= /opt/jsbsim +JSBSIM_INC = $(JSBSIM_ROOT)/include/JSBSim +JSBSIM_LIB = $(JSBSIM_ROOT)/lib + +SRC_FIRMWARE=firmwares/fixedwing + +SRC_BOARD=boards/$(BOARD) + +NPSDIR = $(SIMDIR)/nps + + +nps.ARCHDIR = sim + +# include Makefile.nps instead of Makefile.sim +nps.MAKEFILE = nps + +# add normal ap and fbw sources define in autopilot.makefile +nps.CFLAGS += $(fbw_CFLAGS) $(ap_CFLAGS) +nps.srcs += $(fbw_srcs) $(ap_srcs) + +nps.CFLAGS += -DSITL -DUSE_NPS +nps.CFLAGS += $(shell pkg-config glib-2.0 --cflags) +nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy -lpcre -lgsl -lgslcblas +nps.CFLAGS += -I$(NPSDIR) -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I../simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps +nps.LDFLAGS += $(shell sdl-config --libs) + +# use the paparazzi-jsbsim package if it is installed, otherwise look for JSBsim under /opt/jsbsim +JSBSIM_PKG ?= $(shell pkg-config JSBSim --exists && echo 'yes') +ifeq ($(JSBSIM_PKG), yes) + nps.CFLAGS += $(shell pkg-config JSBSim --cflags) + nps.LDFLAGS += $(shell pkg-config JSBSim --libs) +else + JSBSIM_PKG = no + nps.CFLAGS += -I$(JSBSIM_INC) + nps.LDFLAGS += -L$(JSBSIM_LIB) -lJSBSim +endif + + +nps.srcs += $(NPSDIR)/nps_main.c \ + $(NPSDIR)/nps_fdm_jsbsim.c \ + $(NPSDIR)/nps_random.c \ + $(NPSDIR)/nps_sensors.c \ + $(NPSDIR)/nps_sensors_utils.c \ + $(NPSDIR)/nps_sensor_gyro.c \ + $(NPSDIR)/nps_sensor_accel.c \ + $(NPSDIR)/nps_sensor_mag.c \ + $(NPSDIR)/nps_sensor_baro.c \ + $(NPSDIR)/nps_sensor_gps.c \ + $(NPSDIR)/nps_radio_control.c \ + $(NPSDIR)/nps_radio_control_joystick.c \ + $(NPSDIR)/nps_radio_control_spektrum.c \ + $(NPSDIR)/nps_autopilot_fixedwing.c \ + $(NPSDIR)/nps_ivy_common.c \ + $(NPSDIR)/nps_ivy_fixedwing.c \ + $(NPSDIR)/nps_flightgear.c \ + + +nps.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport +nps.srcs += subsystems/datalink/downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/ivy_transport.c + diff --git a/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile b/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile index 7e013732be..931c35fed0 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile @@ -23,3 +23,8 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile b/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile index cd68d5f1ee..cf03afedac 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_nmea.makefile @@ -23,3 +23,8 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile b/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile index f34c47628f..2e04091ffc 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile @@ -20,3 +20,8 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim. + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile b/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile index 3b2f2ca3f3..5860741b4c 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_ublox.makefile @@ -22,3 +22,8 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile index 1680755f90..0865b4f8c7 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile @@ -21,3 +21,8 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile b/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile index 1abd1c7881..fb75201564 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_alt_float.makefile @@ -3,3 +3,7 @@ ap_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_alt_float.h\" ap_srcs += $(SRC_FIXEDWING)/subsystems/ins.c ap_srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_alt_float.c + +nps.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_alt_float.h\" +nps.srcs += $(SRC_FIXEDWING)/subsystems/ins.c +nps.srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_alt_float.c diff --git a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile index 19f3fdf8dc..f6f1c628f9 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_gps_passthrough.makefile @@ -2,3 +2,5 @@ ap.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c sim.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c + +nps.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c diff --git a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile index 6268afc6be..4440f92bfd 100644 --- a/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile +++ b/conf/firmwares/subsystems/fixedwing/ins_xsens.makefile @@ -46,21 +46,22 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps.c ######################################### ## Simulator +SIM_TARGETS = sim jsbsim nps -sim.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" -sim.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR +ifneq (,$(findstring $(TARGET),$(SIM_TARGETS))) -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs.c -sim.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c - -sim.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c - -sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG -sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" -sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c -sim.srcs += $(SRC_SUBSYSTEMS)/gps.c +$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\" +$(TARGET).CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough.c +$(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c +endif diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 6304c5a47e..b2b9ba8090 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -38,7 +38,7 @@ else endif -nps.srcs += $(NPSDIR)/nps_main.c \ +nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_fdm_jsbsim.c \ $(NPSDIR)/nps_random.c \ $(NPSDIR)/nps_sensors.c \ @@ -51,17 +51,18 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_radio_control.c \ $(NPSDIR)/nps_radio_control_joystick.c \ $(NPSDIR)/nps_radio_control_spektrum.c \ - $(NPSDIR)/nps_autopilot_rotorcraft.c \ - $(NPSDIR)/nps_ivy.c \ + $(NPSDIR)/nps_autopilot_rotorcraft.c \ + $(NPSDIR)/nps_ivy_common.c \ + $(NPSDIR)/nps_ivy_rotorcraft.c \ $(NPSDIR)/nps_flightgear.c \ nps.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -DPERIPHERALS_AUTO_INIT -nps.srcs += firmwares/rotorcraft/main.c -nps.srcs += mcu.c -nps.srcs += $(SRC_ARCH)/mcu_arch.c +nps.srcs += firmwares/rotorcraft/main.c +nps.srcs += mcu.c +nps.srcs += $(SRC_ARCH)/mcu_arch.c nps.srcs += mcu_periph/i2c.c nps.srcs += $(SRC_ARCH)/mcu_periph/i2c_arch.c diff --git a/conf/firmwares/subsystems/shared/imu_nps.makefile b/conf/firmwares/subsystems/shared/imu_nps.makefile index e26d035cd4..a84003b6d6 100644 --- a/conf/firmwares/subsystems/shared/imu_nps.makefile +++ b/conf/firmwares/subsystems/shared/imu_nps.makefile @@ -20,5 +20,5 @@ #
# -nps.CFLAGS += -DIMU_TYPE_H=\"imu/imu_nps.h\" +nps.CFLAGS += -DIMU_TYPE_H=\"imu/imu_nps.h\" -DUSE_IMU nps.srcs += $(SRC_SUBSYSTEMS)/imu.c $(SRC_SUBSYSTEMS)/imu/imu_nps.c diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml index dcf3d90931..f08147adaa 100644 --- a/conf/flight_plans/versatile.xml +++ b/conf/flight_plans/versatile.xml @@ -1,6 +1,6 @@ - +
#include "subsystems/datalink/datalink.h"
diff --git a/conf/flight_plans/versatile_airspeed.xml b/conf/flight_plans/versatile_airspeed.xml index 1427adf72c..d98264c6e6 100644 --- a/conf/flight_plans/versatile_airspeed.xml +++ b/conf/flight_plans/versatile_airspeed.xml @@ -1,6 +1,6 @@ - +
#include "firmwares/fixedwing/guidance/energy_ctrl.h" #include "subsystems/datalink/datalink.h" diff --git a/conf/modules/light.xml b/conf/modules/light.xml index 33cfd74154..5761db11f1 100644 --- a/conf/modules/light.xml +++ b/conf/modules/light.xml @@ -15,7 +15,7 @@
- + + + + 0.00085 + 18.0 + 2 + 30 + 30 + + + + 0.0 0.0776 + 0.1 0.0744 + 0.2 0.0712 + 0.3 0.0655 + 0.4 0.0588 + 0.5 0.0518 + 0.6 0.0419 + 0.7 0.0318 + 0.8 0.0172 + 1.0 -0.0058 + 1.4 -0.0549 + +
+ + + + 0.0 0.0902 + 0.1 0.0893 + 0.2 0.0880 + 0.3 0.0860 + 0.4 0.0810 + 0.5 0.0742 + 0.6 0.0681 + 0.7 0.0572 + 0.8 0.0467 + 1.0 0.0167 + 1.4 -0.0803 + +
+ + diff --git a/conf/simulator/jsbsim/aircraft/Engines/Zenoah_G-26A.xml b/conf/simulator/jsbsim/aircraft/Engines/Zenoah_G-26A.xml new file mode 100644 index 0000000000..86d43a827e --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/Engines/Zenoah_G-26A.xml @@ -0,0 +1,8 @@ + + + + + + + 2207.27 + diff --git a/conf/simulator/jsbsim/aircraft/LisaAspirin2.xml b/conf/simulator/jsbsim/aircraft/LisaAspirin2.xml new file mode 100644 index 0000000000..b3dafa86fc --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/LisaAspirin2.xml @@ -0,0 +1,546 @@ + + + + + + Author Name + Creation Date + Version + Models a Malolo + + + + 10.57 + 9.17 + 1.15 + 1.69 + 3.28 + 1.06 + 0 + + 37.4 + 0 + 0 + + + 20 + 0 + 5 + + + 0 + 0 + 0 + + + + + 1 + 1 + 2 + 0 + 0 + 0 + 12 + + 36.4 + 0 + 4 + + + 1 + + 0 + 0 + 0 + + + + + + + + 40.1 + -9.9 + -10.1 + + 0.8 + 0.5 + 0.02 + 120 + 20 + 0.0 + LEFT + 0 + + + + 40.1 + 9.9 + -10.1 + + 0.8 + 0.5 + 0.02 + 120 + 20 + 0.0 + RIGHT + 0 + + + + 68.9 + 0 + -4.3 + + 0.8 + 0.5 + 0.02 + 24 + 20 + 360.0 + NONE + 0 + + + + 10 + 0 + -8.3 + + 0.8 + 0.5 + 0.02 + 24 + 20 + 360.0 + NONE + 0 + + + + + + + + 36 + 0 + 0 + + + 0.0 + 0 + 0 + + 0 + + + 1 + 0 + 0 + + + 0.0 + 0.0 + 0.0 + + 1.0 + + + + + 36.36 + 0 + -1.89375 + + 1.5 + 1.5 + + + + + + + + fcs/elevator-cmd-norm + fcs/pitch-trim-cmd-norm + + -1 + 1 + + + + + fcs/pitch-trim-sum + + -0.35 + 0.3 + + fcs/elevator-pos-rad + + + + fcs/elevator-pos-rad + + -0.3 + 0.3 + + + -1 + 1 + + fcs/elevator-pos-norm + + + + fcs/aileron-cmd-norm + fcs/roll-trim-cmd-norm + + -1 + 1 + + + + + fcs/roll-trim-sum + + -0.35 + 0.35 + + fcs/left-aileron-pos-rad + + + + -fcs/roll-trim-sum + + -0.35 + 0.35 + + fcs/right-aileron-pos-rad + + + + fcs/left-aileron-pos-rad + + -0.35 + 0.35 + + + -1 + 1 + + fcs/left-aileron-pos-norm + + + + fcs/right-aileron-pos-rad + + -0.35 + 0.35 + + + -1 + 1 + + fcs/right-aileron-pos-norm + + + + fcs/rudder-cmd-norm + fcs/yaw-trim-cmd-norm + + -1 + 1 + + + + + fcs/rudder-command-sum + + -0.35 + 0.35 + + fcs/rudder-pos-rad + + + + fcs/rudder-pos-rad + + -0.35 + 0.35 + + + -1 + 1 + + fcs/rudder-pos-norm + + + + + + + + Drag_at_zero_lift + + aero/qbar-psf + metrics/Sw-sqft + + aero/alpha-rad + + -1.5700 1.5000 + -0.2600 0.0560 + 0.0000 0.0280 + 0.2600 0.0560 + 1.5700 1.5000 + +
+ + + + Induced_drag + + aero/qbar-psf + metrics/Sw-sqft + aero/cl-squared + 0.0400 + + + + Drag_due_to_sideslip + + aero/qbar-psf + metrics/Sw-sqft + + aero/beta-rad + + -1.5700 1.2300 + -0.2600 0.0500 + 0.0000 0.0000 + 0.2600 0.0500 + 1.5700 1.2300 + +
+ + + + Drag_due_to_Elevator_Deflection + + aero/qbar-psf + metrics/Sw-sqft + fcs/elevator-pos-norm + 0.0300 + + + + + + + Side_force_due_to_beta + + aero/qbar-psf + metrics/Sw-sqft + aero/beta-rad + -1.0000 + + + + + + + Lift_due_to_alpha + + aero/qbar-psf + metrics/Sw-sqft + + aero/alpha-rad + + -0.2000 -0.7500 + 0.0000 0.2500 + 0.2300 1.4000 + 0.6000 0.7100 + +
+ + + + Lift_due_to_Elevator_Deflection + + aero/qbar-psf + metrics/Sw-sqft + fcs/elevator-pos-rad + 0.2000 + + + + + + + Roll_moment_due_to_beta + + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + aero/beta-rad + -0.1000 + + + + Roll_moment_due_to_roll_rate + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + aero/bi2vel + velocities/p-aero-rad_sec + -0.4000 + + + + Roll_moment_due_to_yaw_rate + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + aero/bi2vel + velocities/r-aero-rad_sec + 0.1500 + + + + Roll_moment_due_to_aileron + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + fcs/left-aileron-pos-rad + + velocities/mach + + 0.0000 0.1300 + 2.0000 0.0570 + +
+ + + + Roll_moment_due_to_rudder + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + fcs/rudder-pos-rad + 0.0100 + + + + + + + Pitch_moment_due_to_alpha + + aero/qbar-psf + metrics/Sw-sqft + metrics/cbarw-ft + aero/alpha-rad + -0.5000 + + + + Pitch_moment_due_to_elevator + + aero/qbar-psf + metrics/Sw-sqft + metrics/cbarw-ft + fcs/elevator-pos-rad + + velocities/mach + + 0.0000 -0.5000 + 2.0000 -0.2750 + +
+ + + + Pitch_moment_due_to_pitch_rate + + aero/qbar-psf + metrics/Sw-sqft + metrics/cbarw-ft + aero/ci2vel + velocities/q-aero-rad_sec + -12.0000 + + + + Pitch_moment_due_to_alpha_rate + + aero/qbar-psf + metrics/Sw-sqft + metrics/cbarw-ft + aero/ci2vel + aero/alphadot-rad_sec + -7.0000 + + + + + + + Yaw_moment_due_to_beta + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + aero/beta-rad + 0.1200 + + + + Yaw_moment_due_to_yaw_rate + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + aero/bi2vel + velocities/r-aero-rad_sec + -0.1500 + + + + Yaw_moment_due_to_rudder + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + fcs/rudder-pos-rad + -0.0500 + + + + Adverse_yaw + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + fcs/left-aileron-pos-rad + -0.0300 + + + + Yaw_moment_due_to_tail_incidence + + aero/qbar-psf + metrics/Sw-sqft + metrics/bw-ft + 0.0007 + + + + + diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index 27095c67eb..6ec32bf2da 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -168,9 +168,9 @@ float ac_char_cruise_throttle = 0.0f; float ac_char_cruise_pitch = 0.0f; int ac_char_cruise_count = 0; -static void ac_char_average(float* last, float new, int count) +static void ac_char_average(float* last_v, float new_v, int count) { - *last = (((*last) * (((float)count) - 1.0f)) + new) / ((float) count); + *last_v = (((*last_v) * (((float)count) - 1.0f)) + new_v) / ((float) count); } static void ac_char_update(float throttle, float pitch, float climb, float accelerate) diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 8c1e8bc7be..ee1a5795a3 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -84,6 +84,10 @@ #include "led.h" +#ifdef USE_NPS +#include "nps_autopilot_fixedwing.h" +#endif + /* Default trim commands for roll, pitch and yaw */ #ifndef COMMAND_ROLL_TRIM #define COMMAND_ROLL_TRIM 0 @@ -575,7 +579,7 @@ void sensors_task( void ) { #endif // USE_IMU //FIXME: this is just a kludge -#if USE_AHRS && defined SITL +#if USE_AHRS && defined SITL && !USE_NPS ahrs_propagate(); #endif @@ -720,10 +724,6 @@ static inline void on_gyro_event( void ) { ahrs_propagate(); ahrs_update_accel(); -#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP - new_ins_attitude = 1; -#endif - #else //PERIODIC_FREQUENCY static uint8_t _reduced_propagation_rate = 0; static uint8_t _reduced_correction_rate = 0; @@ -736,6 +736,7 @@ static inline void on_gyro_event( void ) { _reduced_propagation_rate++; if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY)) { + return; } else { @@ -757,13 +758,17 @@ static inline void on_gyro_event( void ) { ImuScaleAccel(imu); ahrs_update_accel(); } - -#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP - new_ins_attitude = 1; -#endif } #endif //PERIODIC_FREQUENCY +#if defined SITL && USE_NPS + if (nps_bypass_ahrs) sim_overwrite_ahrs(); +#endif + +#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP + new_ins_attitude = 1; +#endif + } static inline void on_mag_event(void) diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index d4c5c2b5b8..701a644b82 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -57,6 +57,9 @@ uint8_t fbw_mode; #include "inter_mcu.h" +#ifdef USE_NPS +#include "nps_autopilot_fixedwing.h" +#endif /** Trim commands for roll, pitch and yaw. * These are updated from the trim commands in ap_state via inter_mcu diff --git a/sw/airborne/modules/sensors/airspeed_adc.c b/sw/airborne/modules/sensors/airspeed_adc.c index f461d310bf..af9955437e 100644 --- a/sw/airborne/modules/sensors/airspeed_adc.c +++ b/sw/airborne/modules/sensors/airspeed_adc.c @@ -60,7 +60,7 @@ void airspeed_adc_update( void ) { float airspeed = AIRSPEED_SCALE * (adc_airspeed_val - AIRSPEED_BIAS); #endif stateSetAirspeed_f(&airspeed); -#else // SITL +#elif !defined USE_NPS extern float sim_air_speed; stateSetAirspeed_f(&sim_air_speed); adc_airspeed_val = 0; diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c index 9d92ebac7f..abf946bd96 100644 --- a/sw/airborne/modules/sensors/airspeed_amsys.c +++ b/sw/airborne/modules/sensors/airspeed_amsys.c @@ -109,9 +109,13 @@ void airspeed_amsys_read_periodic( void ) { #endif } -#elif !USE_NPS - extern float sim_air_speed; - stateSetAirspeed_f(&sim_air_speed); +#if USE_AIRSPEED + stateSetAirspeed_f(&airspeed_amsys); +#endif + +#elif !defined USE_NPS + extern float sim_air_speed; + stateSetAirspeed_f(&sim_air_speed); #endif //SITL diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index a6c78cd08b..6f803c30b3 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -128,7 +128,7 @@ void airspeed_ets_read_periodic( void ) { } if (airspeed_ets_i2c_trans.status == I2CTransDone) i2c_receive(&AIRSPEED_ETS_I2C_DEV, &airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2); -#else // SITL +#elif !defined USE_NPS extern float sim_air_speed; stateSetAirspeed_f(&sim_air_speed); #endif //SITL diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c index 55c935af60..549630a8d5 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.c +++ b/sw/airborne/subsystems/gps/gps_sim_nps.c @@ -22,6 +22,12 @@ #include "subsystems/gps/gps_sim_nps.h" #include "subsystems/gps.h" +#if GPS_USE_LATLONG +/* currently needed to get nav_utm_zone0 */ +#include "subsystems/navigation/common_nav.h" +#include "math/pprz_geodetic_float.h" +#endif + bool_t gps_available; void gps_feed_value() { @@ -36,6 +42,39 @@ void gps_feed_value() { gps.lla_pos.lon = sensors.gps.lla_pos.lon * 1e7; gps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.; gps.hmsl = sensors.gps.hmsl * 1000.; + + /* calc NED speed from ECEF */ + struct LtpDef_d ref_ltp; + ltp_def_from_ecef_d(&ref_ltp, &sensors.gps.ecef_pos); + struct NedCoor_d ned_vel_d; + ned_of_ecef_vect_d(&ned_vel_d, &ref_ltp, &sensors.gps.ecef_vel); + gps.ned_vel.x = ned_vel_d.x * 100; + gps.ned_vel.y = ned_vel_d.y * 100; + gps.ned_vel.z = ned_vel_d.z * 100; + + /* horizontal and 3d ground speed in cm/s */ + gps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100; + gps.speed_3d = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y + ned_vel_d.z * ned_vel_d.z) * 100; + + /* ground course in radians * 1e7 */ + gps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7; + +#if GPS_USE_LATLONG + /* Computes from (lat, long) in the referenced UTM zone */ + struct LlaCoor_f lla_f; + lla_f.lat = ((float) gps.lla_pos.lat) / 1e7; + lla_f.lon = ((float) gps.lla_pos.lon) / 1e7; + struct UtmCoor_f utm_f; + utm_f.zone = nav_utm_zone0; + /* convert to utm */ + utm_of_lla_f(&utm_f, &lla_f); + /* copy results of utm conversion */ + gps.utm_pos.east = utm_f.east*100; + gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.alt = gps.lla_pos.alt; + gps.utm_pos.zone = nav_utm_zone0; +#endif + gps.fix = GPS_FIX_3D; gps_available = TRUE; } diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c new file mode 100644 index 0000000000..5cf695e12c --- /dev/null +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -0,0 +1,146 @@ +/* + * Copyright (C) 2009 Antoine Drouin + * Copyright (C) 2013 The Paparazzi Team + * + * 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_autopilot_fixedwing.h" + +#ifdef FBW +#include "firmwares/fixedwing/main_fbw.h" +#define Fbw(f) f ## _fbw() +#else +#define Fbw(f) +#endif + +#ifdef AP +#include "firmwares/fixedwing/main_ap.h" +#define Ap(f) f ## _ap() +#else +#define Ap(f) +#endif + +#include "nps_sensors.h" +#include "nps_radio_control.h" +#include "nps_fdm.h" + +#include "subsystems/radio_control.h" +#include "subsystems/imu.h" +#include "subsystems/sensors/baro.h" +#include "baro_board.h" +#include "subsystems/electrical.h" +#include "mcu_periph/sys_time.h" +#include "state.h" +#include "subsystems/commands.h" + + + +struct NpsAutopilot autopilot; +bool_t nps_bypass_ahrs; + +#ifndef NPS_BYPASS_AHRS +#define NPS_BYPASS_AHRS FALSE +#endif + +#if !defined (FBW) || !defined (AP) +#error NPS does not currently support dual processor simulation for FBW and AP on fixedwing! +#endif + +void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) { + + nps_radio_control_init(type_rc, num_rc_script, rc_dev); + nps_bypass_ahrs = NPS_BYPASS_AHRS; + + Fbw(init); + Ap(init); + + +#ifdef MAX_BAT_LEVEL + electrical.vsupply = MAX_BAT_LEVEL * 10; +#else + electrical.vsupply = 111; +#endif + +} + +void nps_autopilot_run_systime_step( void ) { + sys_tick_handler(); +} + +#include +#include "subsystems/gps.h" + +void nps_autopilot_run_step(double time __attribute__ ((unused))) { + +#ifdef RADIO_CONTROL_TYPE_PPM + if (nps_radio_control_available(time)) { + radio_control_feed(); + Fbw(event_task); + } +#endif + + if (nps_sensors_gyro_available()) { + imu_feed_gyro_accel(); + Fbw(event_task); + Ap(event_task); + } + + if (nps_sensors_mag_available()) { + imu_feed_mag(); + Fbw(event_task); + Ap(event_task); + } + + if (nps_sensors_baro_available()) { + /** @todo feed baro values */ + //baro_feed_value(sensors.baro.value); + Fbw(event_task); + Ap(event_task); + } + + if (nps_sensors_gps_available()) { + gps_feed_value(); + Fbw(event_task); + Ap(event_task); + } + + if (nps_bypass_ahrs) { + sim_overwrite_ahrs(); + } + + Fbw(handle_periodic_tasks); + Ap(handle_periodic_tasks); + + /* scale final motor commands to 0-1 for feeding the fdm */ + for (uint8_t i=0; i < COMMANDS_NB; i++) + autopilot.commands[i] = (double)commands[i]/MAX_PPRZ; + +} + +void sim_overwrite_ahrs(void) { + + struct FloatQuat quat_f; + QUAT_COPY(quat_f, fdm.ltp_to_body_quat); + stateSetNedToBodyQuat_f(&quat_f); + + struct FloatRates rates_f; + RATES_COPY(rates_f, fdm.body_ecef_rotvel); + stateSetBodyRates_f(&rates_f); + +} diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.h b/sw/simulator/nps/nps_autopilot_fixedwing.h new file mode 100644 index 0000000000..a4d05a199b --- /dev/null +++ b/sw/simulator/nps/nps_autopilot_fixedwing.h @@ -0,0 +1,10 @@ +#ifndef NPS_AUTOPILOT_FIXEDWING_H +#define NPS_AUTOPILOT_FIXEDWING_H + +#include "nps_autopilot.h" + + +extern bool_t nps_bypass_ahrs; +extern void sim_overwrite_ahrs(void); + +#endif /* NPS_AUTOPILOT_FIXEDWING_H */ diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 0a8cfa11c4..0064c2f897 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -24,6 +24,8 @@ #include "firmwares/rotorcraft/main.h" #include "nps_sensors.h" #include "nps_radio_control.h" +#include "nps_fdm.h" + #include "subsystems/radio_control.h" #include "subsystems/imu.h" #include "subsystems/sensors/baro.h" @@ -31,7 +33,6 @@ #include "subsystems/electrical.h" #include "mcu_periph/sys_time.h" #include "state.h" -#include "nps_fdm.h" #include "subsystems/ahrs.h" #include "subsystems/ins.h" #include "math/pprz_algebra.h" @@ -124,28 +125,28 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { void sim_overwrite_ahrs(void) { - struct Int32Quat quat; - QUAT_BFP_OF_REAL(quat, fdm.ltp_to_body_quat); - stateSetNedToBodyQuat_i(&quat); + struct FloatQuat quat_f; + QUAT_COPY(quat_f, fdm.ltp_to_body_quat); + stateSetNedToBodyQuat_f(&quat_f); - struct Int32Rates rates; - RATES_BFP_OF_REAL(rates, fdm.body_ecef_rotvel); - stateSetBodyRates_i(&rates); + struct FloatRates rates_f; + RATES_COPY(rates_f, fdm.body_ecef_rotvel); + stateSetBodyRates_f(&rates_f); } void sim_overwrite_ins(void) { - struct NedCoor_i ltp_pos; - POSITIONS_BFP_OF_REAL(ltp_pos, fdm.ltpprz_pos); - stateSetPositionNed_i(<p_pos); + struct NedCoor_f ltp_pos; + VECT3_COPY(ltp_pos, fdm.ltpprz_pos); + stateSetPositionNed_f(<p_pos); - struct NedCoor_i ltp_speed; - SPEEDS_BFP_OF_REAL(ltp_speed, fdm.ltpprz_ecef_vel); - stateSetSpeedNed_i(<p_speed); + struct NedCoor_f ltp_speed; + VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel); + stateSetSpeedNed_f(<p_speed); - struct NedCoor_i ltp_accel; - ACCELS_BFP_OF_REAL(ltp_accel, fdm.ltpprz_ecef_accel); - stateSetAccelNed_i(<p_accel); + struct NedCoor_f ltp_accel; + VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel); + stateSetAccelNed_f(<p_accel); } diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h index 022364c0ce..4332fe0104 100644 --- a/sw/simulator/nps/nps_fdm.h +++ b/sw/simulator/nps/nps_fdm.h @@ -43,6 +43,7 @@ struct NpsFdm { double init_dt; double curr_dt; bool_t on_ground; + int nan_count; /* position */ struct EcefCoor_d ecef_pos; diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c index c2e5ebb95d..4487ae8c14 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.c +++ b/sw/simulator/nps/nps_fdm_jsbsim.c @@ -26,22 +26,41 @@ * This is an FDM for NPS that uses JSBSim as the simulation engine. */ +#include +#include +#include + #include #include #include #include #include -#include + #include "nps_fdm.h" -#include "generated/airframe.h" #include "math/pprz_geodetic.h" #include "math/pprz_geodetic_double.h" #include "math/pprz_geodetic_float.h" #include "math/pprz_algebra.h" #include "math/pprz_algebra_float.h" +#include "generated/airframe.h" +#include "generated/flight_plan.h" + /// Macro to convert from feet to metres #define MetersOfFeet(_f) ((_f)/3.2808399) +#define FeetOfMeters(_m) ((_m)*3.2808399) + +/** Name of the JSBSim model. + * Defaults to the AIRFRAME_NAME + */ +#ifndef NPS_JSBSIM_MODEL +#define NPS_JSBSIM_MODEL AIRFRAME_NAME +#endif + +#ifdef NPS_INITIAL_CONDITITONS +#warning NPS_INITIAL_CONDITITONS was replaced by NPS_JSBSIM_INIT! +#warning Defaulting to flight plan location. +#endif /** Minimum JSBSim timestep * Around 1/10000 seems to be good for ground impacts @@ -49,9 +68,11 @@ #define MIN_DT (1.0/10240.0) using namespace JSBSim; +using namespace std; static void feed_jsbsim(double* commands); static void fetch_state(void); +static int check_for_nan(void); static void jsbsimvec_to_vec(DoubleVect3* fdm_vector, const FGColumnVector3* jsb_vector); static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, const FGLocation* jsb_location); @@ -60,7 +81,6 @@ static void jsbsimvec_to_rate(DoubleRates* fdm_rate, const FGColumnVector3* jsb_ static void llh_from_jsbsim(LlaCoor_d* fdm_lla, FGPropagate* propagate); static void lla_from_jsbsim_geodetic(LlaCoor_d* fdm_lla, FGPropagate* propagate); static void lla_from_jsbsim_geocentric(LlaCoor_d* fdm_lla, FGPropagate* propagate); -//static void rate_to_vec(DoubleVect3* vector, DoubleRates* rate); static void init_jsbsim(double dt); static void init_ltp(void); @@ -87,6 +107,8 @@ void nps_fdm_init(double dt) { for (min_dt = (1.0/dt); min_dt < (1/MIN_DT); min_dt += (1/dt)){} min_dt = (1/min_dt); + fdm.nan_count = 0; + init_jsbsim(dt); FDMExec->RunIC(); @@ -147,6 +169,14 @@ void nps_fdm_run_step(double* commands) { fetch_state(); + /* Check the current state to make sure it is valid (no NaNs) */ + if (check_for_nan()) { + printf("Error: FDM simulation encountered a total of %i NaN values at simulation time %f.\n", fdm.nan_count, fdm.time); + printf("It is likely the simulation diverged and gave non-physical results. If you did\n"); + printf("not crash, check your model and/or initial conditions. Exiting with status 1.\n"); + exit(1); + } + } /** @@ -284,9 +314,18 @@ static void init_jsbsim(double dt) { char buf[1024]; string rootdir; + string jsbsim_ic_name; sprintf(buf,"%s/conf/simulator/jsbsim/",getenv("PAPARAZZI_HOME")); rootdir = string(buf); + + /* if jsbsim initial conditions are defined, use them + * otherwise use flightplan location + */ +#ifdef NPS_JSBSIM_INIT + jsbsim_ic_name = NPS_JSBSIM_INIT; +#endif + FDMExec = new FGFDMExec(); FDMExec->Setsim_time(0.); @@ -298,7 +337,7 @@ static void init_jsbsim(double dt) { if ( ! FDMExec->LoadModel( rootdir + "aircraft", rootdir + "engine", rootdir + "systems", - AIRFRAME_NAME, + NPS_JSBSIM_MODEL, false)){ #ifdef DEBUG cerr << " JSBSim could not be started" << endl << endl; @@ -311,12 +350,39 @@ static void init_jsbsim(double dt) { FDMExec->GetPropulsion()->InitRunning(-1); JSBSim::FGInitialCondition *IC = FDMExec->GetIC(); - if ( ! IC->Load(NPS_INITIAL_CONDITITONS)) { + if(!jsbsim_ic_name.empty()) { + if ( ! IC->Load(jsbsim_ic_name)) { #ifdef DEBUG - cerr << "Initialization unsuccessful" << endl; + cerr << "Initialization unsuccessful" << endl; #endif - delete FDMExec; - exit(-1); + delete FDMExec; + exit(-1); + } + } + else { + // FGInitialCondition::SetAltitudeASLFtIC + // requires this function to be called + // before itself + IC->SetVgroundFpsIC(0.); + + // Use flight plan initial conditions + // convert geodetic lat from flight plan to geocentric + double gd_lat = RadOfDeg(NAV_LAT0 / 1e7); + double gc_lat = gc_of_gd_lat_d(gd_lat, GROUND_ALT); + IC->SetLatitudeDegIC(DegOfRad(gc_lat)); + IC->SetLongitudeDegIC(NAV_LON0 / 1e7); + + IC->SetAltitudeASLFtIC(FeetOfMeters(GROUND_ALT + 2.0)); + IC->SetTerrainElevationFtIC(FeetOfMeters(GROUND_ALT)); + IC->SetPsiDegIC(QFU); + IC->SetVgroundFpsIC(0.); + + //initRunning for all engines + FDMExec->GetPropulsion()->InitRunning(-1); + if (!FDMExec->RunIC()) { + cerr << "Initialization from flight plan unsuccessful" << endl; + exit(-1); + } } // calculate vehicle max radius in m @@ -473,13 +539,87 @@ void lla_from_jsbsim_geodetic(LlaCoor_d* fdm_lla, FGPropagate* propagate) { } - -#if 0 -static void rate_to_vec(DoubleVect3* vector, DoubleRates* rate) { - - vector->x = rate->p; - vector->y = rate->q; - vector->z = rate->r; - -} +#ifdef __APPLE__ +/* Why isn't this there when we include math.h (on osx with clang)? */ +/// Check if a double is NaN. +static int isnan(double f) { return (f != f); } #endif + +/** + * Checks NpsFdm struct for NaNs. + * + * Increments the NaN count on each new NaN + * + * @return Count of new NaNs. 0 for no new NaNs. + */ +static int check_for_nan(void) { + int orig_nan_count = fdm.nan_count; + /* Check all elements for nans */ + if (isnan(fdm.ecef_pos.x)) fdm.nan_count++; + if (isnan(fdm.ecef_pos.y)) fdm.nan_count++; + if (isnan(fdm.ecef_pos.z)) fdm.nan_count++; + if (isnan(fdm.ltpprz_pos.x)) fdm.nan_count++; + if (isnan(fdm.ltpprz_pos.y)) fdm.nan_count++; + if (isnan(fdm.ltpprz_pos.z)) fdm.nan_count++; + if (isnan(fdm.lla_pos.lon)) fdm.nan_count++; + if (isnan(fdm.lla_pos.lat)) fdm.nan_count++; + if (isnan(fdm.lla_pos.alt)) fdm.nan_count++; + if (isnan(fdm.hmsl)) fdm.nan_count++; + // Skip debugging elements + if (isnan(fdm.ecef_ecef_vel.x)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_vel.y)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_vel.z)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_accel.x)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_accel.y)) fdm.nan_count++; + if (isnan(fdm.ecef_ecef_accel.z)) fdm.nan_count++; + if (isnan(fdm.body_ecef_vel.x)) fdm.nan_count++; + if (isnan(fdm.body_ecef_vel.y)) fdm.nan_count++; + if (isnan(fdm.body_ecef_vel.z)) fdm.nan_count++; + if (isnan(fdm.body_ecef_accel.x)) fdm.nan_count++; + if (isnan(fdm.body_ecef_accel.y)) fdm.nan_count++; + if (isnan(fdm.body_ecef_accel.z)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_vel.x)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_vel.y)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_vel.z)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_accel.x)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_accel.y)) fdm.nan_count++; + if (isnan(fdm.ltp_ecef_accel.z)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_vel.x)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_vel.y)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_vel.z)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_accel.x)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_accel.y)) fdm.nan_count++; + if (isnan(fdm.ltpprz_ecef_accel.z)) fdm.nan_count++; + if (isnan(fdm.ecef_to_body_quat.qi)) fdm.nan_count++; + if (isnan(fdm.ecef_to_body_quat.qx)) fdm.nan_count++; + if (isnan(fdm.ecef_to_body_quat.qy)) fdm.nan_count++; + if (isnan(fdm.ecef_to_body_quat.qz)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_quat.qi)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_quat.qx)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_quat.qy)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_quat.qz)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_eulers.phi)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_eulers.theta)) fdm.nan_count++; + if (isnan(fdm.ltp_to_body_eulers.psi)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_quat.qi)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_quat.qx)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_quat.qy)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_quat.qz)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_eulers.phi)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_eulers.theta)) fdm.nan_count++; + if (isnan(fdm.ltpprz_to_body_eulers.psi)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotvel.p)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotvel.q)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotvel.r)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotaccel.p)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotaccel.q)) fdm.nan_count++; + if (isnan(fdm.body_ecef_rotaccel.r)) fdm.nan_count++; + if (isnan(fdm.ltp_g.x)) fdm.nan_count++; + if (isnan(fdm.ltp_g.y)) fdm.nan_count++; + if (isnan(fdm.ltp_g.z)) fdm.nan_count++; + if (isnan(fdm.ltp_h.x)) fdm.nan_count++; + if (isnan(fdm.ltp_h.y)) fdm.nan_count++; + if (isnan(fdm.ltp_h.z)) fdm.nan_count++; + + return (fdm.nan_count - orig_nan_count); +} diff --git a/sw/simulator/nps/nps_ivy.h b/sw/simulator/nps/nps_ivy.h index 64011b7a1f..db8b993365 100644 --- a/sw/simulator/nps/nps_ivy.h +++ b/sw/simulator/nps/nps_ivy.h @@ -1,6 +1,7 @@ #ifndef NPS_IVY #define NPS_IVY +extern void nps_ivy_common_init(char* ivy_bus); extern void nps_ivy_init(char* ivy_bus); extern void nps_ivy_display(void); diff --git a/sw/simulator/nps/nps_ivy.c b/sw/simulator/nps/nps_ivy_common.c similarity index 85% rename from sw/simulator/nps/nps_ivy.c rename to sw/simulator/nps/nps_ivy_common.c index 93847c425c..c93bd7d4eb 100644 --- a/sw/simulator/nps/nps_ivy.c +++ b/sw/simulator/nps/nps_ivy_common.c @@ -10,7 +10,7 @@ #include "nps_fdm.h" #include "nps_sensors.h" #include "subsystems/ins.h" -#include "firmwares/rotorcraft/navigation.h" +#include "subsystems/navigation/common_flight_plan.h" #ifdef RADIO_CONTROL_TYPE_DATALINK #include "subsystems/radio_control.h" @@ -36,10 +36,6 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]); -static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]); - #ifdef RADIO_CONTROL_TYPE_DATALINK static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), @@ -50,7 +46,7 @@ static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]); #endif -void nps_ivy_init(char* ivy_bus) { +void nps_ivy_common_init(char* ivy_bus) { const char* agent_name = AIRFRAME_NAME"_NPS"; const char* ready_msg = AIRFRAME_NAME"_NPS Ready"; IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL); @@ -58,7 +54,6 @@ void nps_ivy_init(char* ivy_bus) { IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)"); IvyBindMsg(on_DL_GET_SETTING, NULL, "^(\\S*) GET_DL_SETTING (\\S*) (\\S*)"); IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)"); - IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); #ifdef RADIO_CONTROL_TYPE_DATALINK IvyBindMsg(on_DL_RC_3CH, NULL, "^(\\S*) RC_3CH (\\S*) (\\S*) (\\S*) (\\S*)"); @@ -117,25 +112,6 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), printf("goto block %d\n", block); } -static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), - void *user_data __attribute__ ((unused)), - int argc __attribute__ ((unused)), char *argv[]) { - uint8_t wp_id = atoi(argv[1]); - - struct LlaCoor_i lla; - struct EnuCoor_i enu; - lla.lat = INT32_RAD_OF_DEG(atoi(argv[3])); - lla.lon = INT32_RAD_OF_DEG(atoi(argv[4])); - lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; - enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); - enu.x = POS_BFP_OF_REAL(enu.x)/100; - enu.y = POS_BFP_OF_REAL(enu.y)/100; - enu.z = POS_BFP_OF_REAL(enu.z)/100; - VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z); - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z); - printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z); -} - #ifdef RADIO_CONTROL_TYPE_DATALINK static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), diff --git a/sw/simulator/nps/nps_ivy_fixedwing.c b/sw/simulator/nps/nps_ivy_fixedwing.c new file mode 100644 index 0000000000..a7bd73cbba --- /dev/null +++ b/sw/simulator/nps/nps_ivy_fixedwing.c @@ -0,0 +1,56 @@ +#include "nps_ivy.h" + +#include +#include + +#include "generated/airframe.h" +#include "math/pprz_algebra_double.h" +#include "subsystems/ins.h" +#include "subsystems/navigation/common_nav.h" + +/* fixedwing specific Datalink Ivy functions */ +void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]); + +void nps_ivy_init(char* ivy_bus) { + /* init ivy and bind some messages common to fw and rotorcraft */ + nps_ivy_common_init(ivy_bus); + + IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); +} + +//TODO use datalink parsing from fixedwing instead of doing it here explicitly + +#include "generated/settings.h" +#include "dl_protocol.h" +#include "subsystems/datalink/downlink.h" + +#define MOfCm(_x) (((float)(_x))/100.) + +void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]) { + + if (atoi(argv[2]) == AC_ID) { + uint8_t wp_id = atoi(argv[1]); + float a = MOfCm(atoi(argv[5])); + + /* Computes from (lat, long) in the referenced UTM zone */ + struct LlaCoor_f lla; + lla.lat = RadOfDeg((float)(atoi(argv[3]) / 1e7)); + lla.lon = RadOfDeg((float)(atoi(argv[4]) / 1e7)); + //printf("move wp id=%d lat=%f lon=%f alt=%f\n", wp_id, lla.lat, lla.lon, a); + struct UtmCoor_f utm; + utm.zone = nav_utm_zone0; + utm_of_lla_f(&utm, &lla); + nav_move_waypoint(wp_id, utm.east, utm.north, a); + + /* Waypoint range is limited. Computes the UTM pos back from the relative + coordinates */ + utm.east = waypoints[wp_id].x + nav_utm_east0; + utm.north = waypoints[wp_id].y + nav_utm_north0; + DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); + printf("move wp id=%d east=%f north=%f zone=%i alt=%f\n", wp_id, utm.east, utm.north, utm.zone, a); + } +} diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c new file mode 100644 index 0000000000..bf0ef5562b --- /dev/null +++ b/sw/simulator/nps/nps_ivy_rotorcraft.c @@ -0,0 +1,58 @@ +#include "nps_ivy.h" + +#include +#include + +#include "generated/airframe.h" +#include "math/pprz_algebra_double.h" +#include "nps_autopilot.h" +#include "nps_fdm.h" +#include "nps_sensors.h" +#include "subsystems/ins.h" +#include "firmwares/rotorcraft/navigation.h" + +#ifdef RADIO_CONTROL_TYPE_DATALINK +#include "subsystems/radio_control.h" +#endif + +#include NPS_SENSORS_PARAMS + + +/* rotorcraft specificDatalink Ivy functions */ +static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]); + +void nps_ivy_init(char* ivy_bus) { + /* init ivy and bind some messages common to fw and rotorcraft */ + nps_ivy_common_init(ivy_bus); + + IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); + +} + +//TODO use datalink parsing from rotorcraft instead of doing it here explicitly + +#include "generated/settings.h" +#include "dl_protocol.h" +#include "subsystems/datalink/downlink.h" +static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), + void *user_data __attribute__ ((unused)), + int argc __attribute__ ((unused)), char *argv[]) { + if (atoi(argv[2]) == AC_ID) { + uint8_t wp_id = atoi(argv[1]); + + struct LlaCoor_i lla; + struct EnuCoor_i enu; + lla.lat = INT32_RAD_OF_DEG(atoi(argv[3])); + lla.lon = INT32_RAD_OF_DEG(atoi(argv[4])); + lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt; + enu_of_lla_point_i(&enu,&ins_ltp_def,&lla); + enu.x = POS_BFP_OF_REAL(enu.x)/100; + enu.y = POS_BFP_OF_REAL(enu.y)/100; + enu.z = POS_BFP_OF_REAL(enu.z)/100; + VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z); + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z); + printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z); + } +} diff --git a/sw/simulator/nps/nps_radio_control_joystick.c b/sw/simulator/nps/nps_radio_control_joystick.c index 746c04aed4..4b164777b2 100644 --- a/sw/simulator/nps/nps_radio_control_joystick.c +++ b/sw/simulator/nps/nps_radio_control_joystick.c @@ -27,13 +27,49 @@ * Simple DirectMedia Layer library is used for cross-platform support. * Joystick button and axes are mapped to RC commands directly with defines. * - * Currently it doesn't support different RC configurations. + * You must have a joystick with either: + * - 4 axes and 3 buttons, or + * - 5 axes + * + * First you should run sw/ground_segment/joystick/test_stick to determine + * the indices of the physical axes and buttons on your joystick (and to test + * that it actually works). Then you can assign each axis and button to a + * command in your airframe file. + * + * The default axes are Roll = 0, Pitch = 1, Yaw = 2 and Throttle = 3. + * The default buttons are Manual = 0, Auto1 = 1, Auto2 = 2. + * + * Example for 4 axes and 3 buttons using a joystick with 7 axes and 5 buttons: + * @verbatim + *
+ * ... + * + * + * + * + * + * + * + *
+ * @endverbatim + * + * One can define NPS_JS_AXIS_MODE to use an axis instead of buttons to change + * You will need a 5-axis joystick. + * + * If you need to reverse the direction of any axis, simply use: + * @verbatim + * + * + * @endverbatim + * + * At this point, no other functionality or channels are supported for R/C control. + * */ #include "nps_radio_control.h" #include "nps_radio_control_joystick.h" -// for NPS_JS_MODE_AXIS +// for NPS_JS_AXIS_MODE #include "generated/airframe.h" #include @@ -42,22 +78,37 @@ #include // axes indice -#define JS_ROLL 0 -#define JS_PITCH 1 -#define JS_YAW 2 -#define JS_THROTTLE 3 +#ifndef NPS_JS_AXIS_ROLL +#define NPS_JS_AXIS_ROLL 0 +#endif +#ifndef NPS_JS_AXIS_PITCH +#define NPS_JS_AXIS_PITCH 1 +#endif +#ifndef NPS_JS_AXIS_YAW +#define NPS_JS_AXIS_YAW 2 +#endif +#ifndef NPS_JS_AXIS_THROTTLE +#define NPS_JS_AXIS_THROTTLE 3 +#endif -#ifndef NPS_JS_MODE_AXIS +#ifndef NPS_JS_AXIS_MODE #define JS_NB_AXIS 4 #else #define JS_NB_AXIS 5 #endif // buttons to switch modes -#define JS_BUTTON_MODE_MANUAL 4 -#define JS_BUTTON_MODE_AUTO1 5 -#define JS_BUTTON_MODE_AUTO2 6 -#ifndef NPS_JS_MODE_AXIS +#ifndef NPS_JS_BUTTON_MODE_MANUAL +#define NPS_JS_BUTTON_MODE_MANUAL 1 +#endif +#ifndef NPS_JS_BUTTON_MODE_AUTO1 +#define NPS_JS_BUTTON_MODE_AUTO1 2 +#endif +#ifndef NPS_JS_BUTTON_MODE_AUTO2 +#define NPS_JS_BUTTON_MODE_AUTO2 3 +#endif + +#ifndef NPS_JS_AXIS_MODE #define JS_NB_BUTTONS 3 #else #define JS_NB_BUTTONS 0 @@ -132,6 +183,8 @@ int nps_radio_control_joystick_init(const char* device) { else if (SDL_JoystickNumAxes(sdl_joystick) < JS_NB_AXIS) { printf("Selected joystick does not support enough axes!\n"); + printf("Number of axes required: %i\n", JS_NB_AXIS); + printf("Number of axes available: %i\n",SDL_JoystickNumAxes(sdl_joystick)); SDL_JoystickClose(sdl_joystick); exit(-1); } @@ -155,13 +208,33 @@ int nps_radio_control_joystick_init(const char* device) { */ void nps_radio_control_joystick_update(void) { - nps_joystick.throttle = ((float)(SDL_JoystickGetAxis(sdl_joystick,JS_THROTTLE)) - 32767.)/-65534.; - nps_joystick.roll = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_ROLL))/32767.; - nps_joystick.pitch = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_PITCH))/32767.; - nps_joystick.yaw = (float)(SDL_JoystickGetAxis(sdl_joystick,JS_YAW))/32767.; +#if NPS_JS_AXIS_THROTTLE_REVERSED + nps_joystick.throttle = ((float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_THROTTLE)) - 32767.)/-65534.; +#else + nps_joystick.throttle = ((float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_THROTTLE)) - 32767.)/-65534.; +#endif +#if NPS_JS_AXIS_ROLL_REVERSED + nps_joystick.roll = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_ROLL))/32767.; +#else + nps_joystick.roll = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_ROLL))/32767.; +#endif +#if NPS_JS_AXIS_PITCH_REVERSED + nps_joystick.pitch = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_PITCH))/32767.; +#else + nps_joystick.pitch = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_PITCH))/32767.; +#endif +#if NPS_JS_AXIS_YAW_REVERSED + nps_joystick.yaw = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_YAW))/32767.; +#else + nps_joystick.yaw = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_YAW))/32767.; +#endif // if an axis is asigned to the mode, use it instead of the buttons -#ifdef NPS_JS_MODE_AXIS - nps_joystick.mode = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_MODE_AXIS))/32767.; +#ifdef NPS_JS_AXIS_MODE +#if NPS_JS_AXIS_MODE_REVERSED + nps_joystick.mode = (float)(-1 * SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_MODE))/32767.; +#else + nps_joystick.mode = (float)(SDL_JoystickGetAxis(sdl_joystick,NPS_JS_AXIS_MODE))/32767.; +#endif #endif while(SDL_PollEvent(&sdl_event)) @@ -172,16 +245,16 @@ void nps_radio_control_joystick_update(void) { { switch(sdl_event.jbutton.button) { -#ifndef NPS_JS_MODE_AXIS - case JS_BUTTON_MODE_MANUAL: +#ifndef NPS_JS_AXIS_MODE + case NPS_JS_BUTTON_MODE_MANUAL: nps_joystick.mode = MODE_SWITCH_MANUAL; break; - case JS_BUTTON_MODE_AUTO1: + case NPS_JS_BUTTON_MODE_AUTO1: nps_joystick.mode = MODE_SWITCH_AUTO1; break; - case JS_BUTTON_MODE_AUTO2: + case NPS_JS_BUTTON_MODE_AUTO2: nps_joystick.mode = MODE_SWITCH_AUTO2; break; #endif