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 new file mode 100644 index 0000000000..ace2179575 --- /dev/null +++ b/conf/airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml @@ -0,0 +1,223 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + +
+ + +
+ + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + + + + + + + + + +
+ +
+ + + +
+ +
+ + + +
+ +
+ +
+ +
+ + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/airframes/TUDelft/IMAV2013/conf.xml b/conf/airframes/TUDelft/IMAV2013/conf.xml index 76978844ac..5581e4ce28 100644 --- a/conf/airframes/TUDelft/IMAV2013/conf.xml +++ b/conf/airframes/TUDelft/IMAV2013/conf.xml @@ -1,45 +1,52 @@ - - + name="ChouChou_LisaS" + ac_id="5" + airframe="airframes/TUDelft/IMAV2013/chouchou_lisa_s.xml" + radio="radios/cockpitSX.xml" + telemetry="telemetry/default_rotorcraft.xml" + flight_plan="flight_plans/rotorcraft_basic.xml" + settings=" settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml settings/control/rotorcraft_guidance.xml" + gui_color="blue" + /> + name="MavRick_LisaS" + ac_id="1" + airframe="airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml" + radio="radios/cockpitSX.xml" + telemetry="telemetry/default_fixedwing_imu.xml" + flight_plan="flight_plans/basic.xml" + settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml" + gui_color="white" + /> + name="Quadrotor_LisaS" + ac_id="2" + airframe="airframes/examples/quadrotor_lisa_s.xml" + radio="radios/cockpitSX.xml" + telemetry="telemetry/default_rotorcraft.xml" + flight_plan="flight_plans/rotorcraft_basic.xml" + settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml" + gui_color="blue" + /> - + name="Walkera_GeniusV1" + ac_id="4" + airframe="airframes/TUDelft/IMAV2013/walkera_genius_v1.xml" + radio="radios/cockpitSX.xml" + telemetry="telemetry/default_rotorcraft.xml" + flight_plan="flight_plans/rotorcraft_basic.xml" + settings=" settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml" + gui_color="orange" + /> + diff --git a/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml index 4955b2c088..7f719564ee 100644 --- a/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/mavrick_lisa_s.xml @@ -23,7 +23,7 @@ - + @@ -42,8 +42,8 @@ - - + + @@ -86,17 +86,12 @@ - - - - - - - - - - - + + + + + + @@ -107,7 +102,10 @@
- + + + +
diff --git a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml index 69108b84fa..064c01ae5a 100644 --- a/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml +++ b/conf/airframes/TUDelft/IMAV2013/quadrotor_lisa_s.xml @@ -22,7 +22,7 @@ - + @@ -76,7 +76,7 @@
- +
@@ -184,7 +184,7 @@
- +
diff --git a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml index aa31e8d4ec..4e7dda0a2e 100644 --- a/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml +++ b/conf/airframes/TUDelft/IMAV2013/walkera_genius_v1.xml @@ -41,7 +41,7 @@
- + @@ -71,7 +71,7 @@
- +
@@ -90,27 +90,25 @@ - - - + - - + + - + - - + + - + - - + + @@ -178,10 +176,9 @@ - + - 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 d6b09dea15..99265a1638 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -10,6 +10,7 @@ + @@ -202,7 +203,7 @@
- +
diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml index 154b0126fb..73c21af476 100644 --- a/conf/airframes/ardrone2_sdk.xml +++ b/conf/airframes/ardrone2_sdk.xml @@ -6,6 +6,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 8881af6298..ea03629dd7 100644 --- a/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml +++ b/conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml @@ -38,6 +38,8 @@ + + @@ -200,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 640ec29d44..6cee5b9cfa 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -182,7 +182,7 @@
- +
@@ -196,7 +196,6 @@ - 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/boards/ardrone2_raw.makefile b/conf/boards/ardrone2_raw.makefile index 9e96052594..8fb9edacf0 100644 --- a/conf/boards/ardrone2_raw.makefile +++ b/conf/boards/ardrone2_raw.makefile @@ -35,8 +35,8 @@ $(TARGET).CFLAGS +=-DARDRONE2_RAW # ----------------------------------------------------------------------- # default LED configuration -RADIO_CONTROL_LED ?= 4 -BARO_LED ?= none -AHRS_ALIGNER_LED ?= 2 -GPS_LED ?= 3 -SYS_TIME_LED ?= 1 +RADIO_CONTROL_LED ?= 6 +BARO_LED ?= none +AHRS_ALIGNER_LED ?= 5 +GPS_LED ?= 3 +SYS_TIME_LED ?= 0 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 f6acdfe815..d9f33a408c 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 # @@ -222,12 +215,11 @@ 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 += $(SRC_BOARD)/baro_board.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 @@ -265,10 +257,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/fixedwing/telemetry_superbitrf.makefile b/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile new file mode 100644 index 0000000000..95e0156b6c --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/telemetry_superbitrf.makefile @@ -0,0 +1,14 @@ +# +# The superbitRF module as telemetry downlink/uplink +# +# +ap.CFLAGS += -DUSE_$(MODEM_PORT) +ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) + +ap.CFLAGS += -DDOWNLINK -DDOWNLINK_FBW_DEVICE=SuperbitRF -DDOWNLINK_AP_DEVICE=SuperbitRF +ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=SUPERBITRF +#ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI2 -DUSE_SPI_SLAVE2 + +ap.srcs += peripherals/cyrf6936.c +ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/superbitrf.c subsystems/datalink/pprz_transport.c +ap.srcs += $(SRC_FIRMWARE)/datalink.c diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index 42ee416ca0..9d63f232ae 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/telemetry_superbitrf.makefile b/conf/firmwares/subsystems/rotorcraft/telemetry_superbitrf.makefile similarity index 81% rename from conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile rename to conf/firmwares/subsystems/rotorcraft/telemetry_superbitrf.makefile index 01f59768ea..7c784cdcd2 100644 --- a/conf/firmwares/subsystems/shared/telemetry_superbitrf.makefile +++ b/conf/firmwares/subsystems/rotorcraft/telemetry_superbitrf.makefile @@ -7,10 +7,8 @@ ap.CFLAGS += -D$(MODEM_PORT)_BAUD=$(MODEM_BAUD) ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=SuperbitRF ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=SUPERBITRF - -#ap.CFLAGS += -DDOWNLINK -DDOWNLINK_DEVICE=SuperbitRF -#ap.CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DDATALINK=SUPERBITRF #ap.CFLAGS += -DUSE_SUPERBITRF -DUSE_SPI2 -DUSE_SPI_SLAVE2 + ap.srcs += peripherals/cyrf6936.c ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/superbitrf.c subsystems/datalink/pprz_transport.c ap.srcs += $(SRC_FIRMWARE)/datalink.c $(SRC_FIRMWARE)/telemetry.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/firmwares/subsystems/shared/radio_control_datalink.makefile b/conf/firmwares/subsystems/shared/radio_control_datalink.makefile index bb342a5c3e..6aed61a1a8 100644 --- a/conf/firmwares/subsystems/shared/radio_control_datalink.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_datalink.makefile @@ -11,9 +11,13 @@ endif ifeq ($(NORADIO), False) -ifdef (RADIO_CONTROL_DATALINK_LED) - ap.CFLAGS += -D(RADIO_CONTROL_DATALINK_LED=$((RADIO_CONTROL_DATALINK_LED) -endif + ifdef (RADIO_CONTROL_DATALINK_LED) + ap.CFLAGS += -D(RADIO_CONTROL_DATALINK_LED=$((RADIO_CONTROL_DATALINK_LED) + endif + ifneq ($(RADIO_CONTROL_LED),none) + ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) + fbw.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) + endif $(TARGET).CFLAGS += -DRADIO_CONTROL $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_H=\"radio_control/rc_datalink.h\" $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_DATALINK diff --git a/conf/flight_plans/rotorcraft_basic.xml b/conf/flight_plans/rotorcraft_basic.xml index e9798d0de1..de897364e2 100644 --- a/conf/flight_plans/rotorcraft_basic.xml +++ b/conf/flight_plans/rotorcraft_basic.xml @@ -71,8 +71,12 @@ + + + + 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/arch/omap/mcu_periph/sys_time_arch.c b/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c index 2d4c78cd04..277e1a7893 100644 --- a/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/omap/mcu_periph/sys_time_arch.c @@ -29,6 +29,11 @@ #include #include +#ifdef SYS_TIME_LED +#include "led.h" +#endif + + void sys_time_arch_init( void ) { sys_time.cpu_ticks_per_sec = 1e6; @@ -58,6 +63,10 @@ void sys_tick_handler( int signum ) { if (sys_time.nb_sec_rem >= sys_time.cpu_ticks_per_sec) { sys_time.nb_sec_rem -= sys_time.cpu_ticks_per_sec; sys_time.nb_sec++; +#ifdef SYS_TIME_LED + LED_TOGGLE(SYS_TIME_LED); +#endif + } for (unsigned int i=0; i /* Standard input/output definitions */ #include /* String function definitions */ @@ -65,6 +66,17 @@ int actuator_ardrone2_raw_fd; /**< File descriptor for the port */ uint32_t led_hw_values; +static inline void actuators_ardrone_reset_flipflop(void) +{ + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + int32_t stop = sys_time.nb_sec + 2; + while (sys_time.nb_sec < stop); + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); +} + + + void actuators_ardrone_init(void) { led_hw_values = 0; @@ -97,10 +109,14 @@ void actuators_ardrone_init(void) //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 gpio_setup_input(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_INPUT); - gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); - gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + actuators_ardrone_reset_flipflop(); - //all select lines inactive + + //all select lines active + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); @@ -150,29 +166,60 @@ int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) { void actuators_ardrone_motor_status(void); void actuators_ardrone_motor_status(void) { - // If a motor IRQ lines is set + static bool_t last_motor_on = FALSE; + + // Reset Flipflop sequence + static bool_t reset_flipflop_counter = 0; + if (reset_flipflop_counter > 0) + { + reset_flipflop_counter--; + + if (reset_flipflop_counter == 10) + { + // Reset flipflop + gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + } + else if (reset_flipflop_counter == 1) + { + // Listen to IRQ again + gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + } + return; + } + + // If a motor IRQ line is set if (gpio_get(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_INPUT) == 1) { if (autopilot_motors_on) { - // Tell paparazzi that one motor has stalled - autopilot_set_motors_on(FALSE); + if (last_motor_on) + { + // Tell paparazzi that one motor has stalled + autopilot_set_motors_on(FALSE); + } + else + { + // Toggle Flipflop reset so motors can be re-enabled + reset_flipflop_counter = 20; + } - // Toggle Flipflop reset so motors can be re-enabled - gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); - gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); } } + last_motor_on = autopilot_motors_on; + } +#define BIT_NUMBER(VAL,BIT) (((VAL)>>BIT)&0x03) + void actuators_ardrone_led_run(void); void actuators_ardrone_led_run(void) { - static uint32_t previous_led_hw_values; + static uint32_t previous_led_hw_values = 0x00; if (previous_led_hw_values != led_hw_values) { previous_led_hw_values = led_hw_values; - actuators_ardrone_set_leds(led_hw_values & 0x01, led_hw_values & 0x02, led_hw_values & 0x04, led_hw_values & 0x08); + actuators_ardrone_set_leds(BIT_NUMBER(led_hw_values,0), BIT_NUMBER(led_hw_values,2), BIT_NUMBER(led_hw_values,4), BIT_NUMBER(led_hw_values,6) ); } } @@ -213,6 +260,13 @@ void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_ { uint8_t cmd[2]; + led0 &= 0x03; + led1 &= 0x03; + led2 &= 0x03; + led3 &= 0x03; + + printf("LEDS: %d %d %d %d \n", led0, led1, led2, led3); + cmd[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1); cmd[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0); diff --git a/sw/airborne/boards/ardrone/at_com.c b/sw/airborne/boards/ardrone/at_com.c index b2c11e4d0b..eba448ee13 100644 --- a/sw/airborne/boards/ardrone/at_com.c +++ b/sw/airborne/boards/ardrone/at_com.c @@ -133,10 +133,14 @@ void init_at_config(void) { } //Recieve a navdata packet -void at_com_recieve_navdata(unsigned char* buffer) { - int l; - recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0, - (struct sockaddr *) &from, (socklen_t *) &l); +int at_com_recieve_navdata(unsigned char* buffer) { + int l = sizeof(from); + int n; + // FIXME(ben): not clear why recvfrom() and not recv() is used. + n = recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0, + (struct sockaddr *) &from, (socklen_t *) &l); + + return n; } //Send an AT command diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h index f2b04185b2..79084315ef 100644 --- a/sw/airborne/boards/ardrone/at_com.h +++ b/sw/airborne/boards/ardrone/at_com.h @@ -202,7 +202,7 @@ typedef struct _navdata_gps_t { //External functions extern void init_at_com(void); -extern void at_com_recieve_navdata(unsigned char* buffer); +extern int at_com_recieve_navdata(unsigned char* buffer); extern void at_com_send_config(char* key, char* value); extern void at_com_send_ftrim(void); extern void at_com_send_ref(int bits); diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index 6d2202557b..363ca87686 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -100,6 +100,7 @@ void gpio_setup_input(uint32_t port, uint16_t pin) void gpio_setup_output(uint32_t port, uint16_t pin) { + /* if (port != 0x32524) return; // protect ardrone board from unauthorized use struct gpio_direction dir; // Open the device if not open @@ -108,8 +109,9 @@ void gpio_setup_output(uint32_t port, uint16_t pin) // Read the GPIO value dir.pin = pin; - dir.mode = GPIO_OUTPUT_HIGH; + dir.mode = GPIO_OUTPUT_LOW; ioctl(gpiofp, GPIO_DIRECTION, &dir); + */ } 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 c0de8a9fe1..796c89c206 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -88,6 +88,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/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 1b52e9a449..e49cd955b7 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -52,7 +52,22 @@ bool_t autopilot_power_switch; bool_t autopilot_detect_ground; bool_t autopilot_detect_ground_once; -#define AUTOPILOT_IN_FLIGHT_TIME 40 +#define AUTOPILOT_IN_FLIGHT_TIME 20 + +/** minimum vertical speed for in_flight condition in m/s */ +#ifndef AUTOPILOT_IN_FLIGHT_MIN_SPEED +#define AUTOPILOT_IN_FLIGHT_MIN_SPEED 0.2 +#endif + +/** minimum vertical acceleration for in_flight condition in m/s^2 */ +#ifndef AUTOPILOT_IN_FLIGHT_MIN_ACCEL +#define AUTOPILOT_IN_FLIGHT_MIN_ACCEL 2.0 +#endif + +/** minimum thrust for in_flight condition in pprz_t units */ +#ifndef AUTOPILOT_IN_FLIGHT_MIN_THRUST +#define AUTOPILOT_IN_FLIGHT_MIN_THRUST 500 +#endif #ifndef AUTOPILOT_DISABLE_AHRS_KILL #include "subsystems/ahrs.h" @@ -111,36 +126,6 @@ void autopilot_init(void) { } -static inline void autopilot_check_in_flight_no_rc( bool_t motors_on ) { - if (autopilot_in_flight) { - if (autopilot_in_flight_counter > 0) { - if (stabilization_cmd[COMMAND_THRUST] == 0) { - autopilot_in_flight_counter--; - if (autopilot_in_flight_counter == 0) { - autopilot_in_flight = FALSE; - } - } - else { /* !THROTTLE_STICK_DOWN */ - autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME; - } - } - } - else { /* not in flight */ - if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME && - motors_on) { - if (stabilization_cmd[COMMAND_THRUST] > 0) { - autopilot_in_flight_counter++; - if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) - autopilot_in_flight = TRUE; - } - else { /* THROTTLE_STICK_DOWN */ - autopilot_in_flight_counter = 0; - } - } - } -} - - void autopilot_periodic(void) { RunOnceEvery(NAV_PRESCALER, nav_periodic_task()); @@ -167,10 +152,6 @@ INFO("Using FAILSAFE_GROUND_DETECT") SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on); } - // when we dont have RC, check in flight by looking at throttle - if (radio_control.status != RC_OK) { - autopilot_check_in_flight_no_rc(autopilot_motors_on); - } } @@ -269,29 +250,37 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { } -static inline void autopilot_check_in_flight( bool_t motors_on ) { +void autopilot_check_in_flight(bool_t motors_on) { if (autopilot_in_flight) { if (autopilot_in_flight_counter > 0) { - if (THROTTLE_STICK_DOWN()) { + /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */ + if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) && + (abs(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) && + (abs(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) + { autopilot_in_flight_counter--; if (autopilot_in_flight_counter == 0) { autopilot_in_flight = FALSE; } } - else { /* !THROTTLE_STICK_DOWN */ + else { /* thrust, speed or accel not above min threshold, reset counter */ autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME; } } } - else { /* not in flight */ + else { /* currently not in flight */ if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME && - motors_on) { - if (!THROTTLE_STICK_DOWN()) { + motors_on) + { + /* if thrust above min threshold, assume in_flight. + * Don't check for velocity and acceleration above threshold here... + */ + if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) { autopilot_in_flight_counter++; if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) autopilot_in_flight = TRUE; } - else { /* THROTTLE_STICK_DOWN */ + else { /* currently not in_flight and thrust below threshold, reset counter */ autopilot_in_flight_counter = 0; } } @@ -344,8 +333,6 @@ void autopilot_on_rc_frame(void) { autopilot_arming_check_motors_on(); kill_throttle = ! autopilot_motors_on; - autopilot_check_in_flight(autopilot_motors_on); - guidance_v_read_rc(); guidance_h_read_rc(autopilot_in_flight); } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 7e874ef0f3..d970b9aba7 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -67,6 +67,7 @@ extern void autopilot_periodic(void); extern void autopilot_on_rc_frame(void); extern void autopilot_set_mode(uint8_t new_autopilot_mode); extern void autopilot_set_motors_on(bool_t motors_on); +extern void autopilot_check_in_flight(bool_t motors_on); extern bool_t autopilot_detect_ground; extern bool_t autopilot_detect_ground_once; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 1b5cdbf0b2..842f3ccf21 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -139,29 +139,35 @@ void guidance_h_mode_changed(uint8_t new_mode) { stabilization_attitude_reset_care_free_heading(); case GUIDANCE_H_MODE_FORWARD: case GUIDANCE_H_MODE_ATTITUDE: +#if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ - if (guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT || - guidance_h_mode == GUIDANCE_H_MODE_RATE) { + if (guidance_h_mode == GUIDANCE_H_MODE_KILL || + guidance_h_mode == GUIDANCE_H_MODE_RATE || + guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT) +#endif stabilization_attitude_enter(); - } break; case GUIDANCE_H_MODE_HOVER: guidance_h_hover_enter(); +#if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ - if (guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT || - guidance_h_mode == GUIDANCE_H_MODE_RATE) { + if (guidance_h_mode == GUIDANCE_H_MODE_KILL || + guidance_h_mode == GUIDANCE_H_MODE_RATE || + guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT) +#endif stabilization_attitude_enter(); - } break; case GUIDANCE_H_MODE_NAV: guidance_h_nav_enter(); +#if NO_ATTITUDE_RESET_ON_MODE_CHANGE /* reset attitude stabilization if previous mode was not using it */ - if (guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT || - guidance_h_mode == GUIDANCE_H_MODE_RATE) { + if (guidance_h_mode == GUIDANCE_H_MODE_KILL || + guidance_h_mode == GUIDANCE_H_MODE_RATE || + guidance_h_mode == GUIDANCE_H_MODE_RC_DIRECT) +#endif stabilization_attitude_enter(); - } break; default: diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c index b1db6ebcfa..34b23d212c 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c @@ -53,7 +53,7 @@ struct Int64Vect2 gh_pos_ref; #ifndef GUIDANCE_H_REF_MAX_ACCEL #define GUIDANCE_H_REF_MAX_ACCEL 5.66 #endif -#define GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC) +static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC); /** Speed saturation */ #ifndef GUIDANCE_H_REF_MAX_SPEED @@ -61,7 +61,7 @@ struct Int64Vect2 gh_pos_ref; #endif /** @todo GH_MAX_SPEED must be limited to 2^14 to avoid overflow */ #define GH_MAX_SPEED_REF_FRAC 7 -#define GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC) +static const int32_t gh_max_speed = BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC); /** second order model natural frequency */ #ifndef GUIDANCE_H_REF_OMEGA @@ -72,14 +72,16 @@ struct Int64Vect2 gh_pos_ref; #define GUIDANCE_H_REF_ZETA 0.85 #endif #define GH_ZETA_OMEGA_FRAC 10 -#define GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC) #define GH_OMEGA_2_FRAC 7 -#define GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC) +static const int32_t gh_zeta_omega = BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC); +static const int32_t gh_omega_2= BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC); /** first order time constant */ -#define GH_REF_THAU_F 0.5 -#define GH_REF_INV_THAU_FRAC 16 -#define GH_REF_INV_THAU BFP_OF_REAL((1./GH_REF_THAU_F), GH_REF_INV_THAU_FRAC) +#ifndef GUIDANCE_H_REF_TAU +#define GUIDANCE_H_REF_TAU 0.5 +#endif +#define GH_REF_INV_TAU_FRAC 16 +static const int32_t gh_ref_inv_tau = BFP_OF_REAL((1./GUIDANCE_H_REF_TAU), GH_REF_INV_TAU_FRAC); static struct Int32Vect2 gh_max_speed_ref; static struct Int32Vect2 gh_max_accel_ref; @@ -88,6 +90,13 @@ static int32_t route_ref; static int32_t s_route_ref; static int32_t c_route_ref; +static void gh_compute_route_ref(struct Int32Vect2* ref_vector); +static void gh_compute_ref_max(struct Int32Vect2* ref_vector); +static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector); +static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector); +static void gh_saturate_ref_accel(void); +static void gh_saturate_ref_speed(void); + void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) { struct Int64Vect2 new_pos; new_pos.x = ((int64_t)pos.x)<<(GH_POS_REF_FRAC - INT32_POS_FRAC); @@ -105,7 +114,7 @@ void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) { // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp) struct Int32Vect2 speed; INT32_VECT2_RSHIFT(speed, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); - VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA); + VECT2_SMUL(speed, speed, -2 * gh_zeta_omega); INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC); // compute pos error in pos_sp resolution struct Int32Vect2 pos_err; @@ -115,65 +124,19 @@ void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) { INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC)); // compute the "pos part" of accel struct Int32Vect2 pos; - VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2)); + VECT2_SMUL(pos, pos_err, -gh_omega_2); INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC); // sum accel VECT2_SUM(gh_accel_ref, speed, pos); - /* Compute route reference before saturation */ - float f_route_ref = atan2f(-pos_err.y, -pos_err.x); - route_ref = ANGLE_BFP_OF_REAL(f_route_ref); - /* Compute North and East route components */ - PPRZ_ITRIG_SIN(s_route_ref, route_ref); - PPRZ_ITRIG_COS(c_route_ref, route_ref); - c_route_ref = abs(c_route_ref); - s_route_ref = abs(s_route_ref); - /* Compute maximum acceleration*/ - gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); - gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); - /* Compute maximum speed*/ - gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC); - gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC); - /* restore gh_speed_ref range (Q14.17) */ - INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); + /* Compute max ref accel/speed along route before saturation */ + gh_compute_ref_max(&pos_err); - /* Saturate accelerations */ - if (gh_accel_ref.x <= -gh_max_accel_ref.x) { - gh_accel_ref.x = -gh_max_accel_ref.x; - } - else if (gh_accel_ref.x >= gh_max_accel_ref.x) { - gh_accel_ref.x = gh_max_accel_ref.x; - } - if (gh_accel_ref.y <= -gh_max_accel_ref.y) { - gh_accel_ref.y = -gh_max_accel_ref.y; - } - else if (gh_accel_ref.y >= gh_max_accel_ref.y) { - gh_accel_ref.y = gh_max_accel_ref.y; - } - - /* Saturate speed and adjust acceleration accordingly */ - if (gh_speed_ref.x <= -gh_max_speed_ref.x) { - gh_speed_ref.x = -gh_max_speed_ref.x; - if (gh_accel_ref.x < 0) - gh_accel_ref.x = 0; - } - else if (gh_speed_ref.x >= gh_max_speed_ref.x) { - gh_speed_ref.x = gh_max_speed_ref.x; - if (gh_accel_ref.x > 0) - gh_accel_ref.x = 0; - } - if (gh_speed_ref.y <= -gh_max_speed_ref.y) { - gh_speed_ref.y = -gh_max_speed_ref.y; - if (gh_accel_ref.y < 0) - gh_accel_ref.y = 0; - } - else if (gh_speed_ref.y >= gh_max_speed_ref.y) { - gh_speed_ref.y = gh_max_speed_ref.y; - if (gh_accel_ref.y > 0) - gh_accel_ref.y = 0; - } + gh_saturate_ref_accel(); + gh_saturate_ref_speed(); } + void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { /* WARNING: SPEED SATURATION UNTESTED */ VECT2_ADD(gh_pos_ref, gh_speed_ref); @@ -186,26 +149,80 @@ void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { // convert to accel resolution INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC)); // compute accel from speed_sp - VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_THAU); - INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_THAU_FRAC); + VECT2_SMUL(gh_accel_ref, speed_err, -gh_ref_inv_tau); + INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_TAU_FRAC); - /* Compute route reference before saturation */ - float f_route_ref = atan2f(-speed_sp.y, -speed_sp.x); + /* Compute max ref accel/speed along route before saturation */ + gh_compute_ref_max_speed(&speed_sp); + gh_compute_ref_max_accel(&speed_err); + + gh_saturate_ref_accel(); + gh_saturate_ref_speed(); +} + +static void gh_compute_route_ref(struct Int32Vect2* ref_vector) { + float f_route_ref = atan2f(-ref_vector->y, -ref_vector->x); route_ref = ANGLE_BFP_OF_REAL(f_route_ref); /* Compute North and East route components */ PPRZ_ITRIG_SIN(s_route_ref, route_ref); PPRZ_ITRIG_COS(c_route_ref, route_ref); c_route_ref = abs(c_route_ref); s_route_ref = abs(s_route_ref); - /* Compute maximum acceleration*/ - gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC); - gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC); - /* Compute maximum speed*/ - gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC); - gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC); - /* restore gh_speed_ref range (Q14.17) */ - INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); +} +static void gh_compute_ref_max(struct Int32Vect2* ref_vector) { + /* Compute route reference before saturation */ + if (ref_vector->x == 0 && ref_vector->y == 0) { + gh_max_accel_ref.x = 0; + gh_max_accel_ref.y = 0; + gh_max_speed_ref.x = 0; + gh_max_speed_ref.y = 0; + } + else { + gh_compute_route_ref(ref_vector); + /* Compute maximum acceleration*/ + gh_max_accel_ref.x = INT_MULT_RSHIFT(gh_max_accel, c_route_ref, INT32_TRIG_FRAC); + gh_max_accel_ref.y = INT_MULT_RSHIFT(gh_max_accel, s_route_ref, INT32_TRIG_FRAC); + /* Compute maximum speed*/ + gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed, c_route_ref, INT32_TRIG_FRAC); + gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed, s_route_ref, INT32_TRIG_FRAC); + /* restore gh_speed_ref range (Q14.17) */ + INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); + } +} + +static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector) { + /* Compute route reference before saturation */ + if (ref_vector->x == 0 && ref_vector->y == 0) { + gh_max_accel_ref.x = 0; + gh_max_accel_ref.y = 0; + } + else { + gh_compute_route_ref(ref_vector); + /* Compute maximum acceleration*/ + gh_max_accel_ref.x = INT_MULT_RSHIFT(gh_max_accel, c_route_ref, INT32_TRIG_FRAC); + gh_max_accel_ref.y = INT_MULT_RSHIFT(gh_max_accel, s_route_ref, INT32_TRIG_FRAC); + } +} + +static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector) { + /* Compute route reference before saturation */ + if (ref_vector->x == 0 && ref_vector->y == 0) { + gh_max_speed_ref.x = 0; + gh_max_speed_ref.y = 0; + } + else { + gh_compute_route_ref(ref_vector); + /* Compute maximum speed*/ + gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed, c_route_ref, INT32_TRIG_FRAC); + gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed, s_route_ref, INT32_TRIG_FRAC); + /* restore gh_speed_ref range (Q14.17) */ + INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); + } +} + +/** saturate reference accelerations */ +static void gh_saturate_ref_accel(void) { /* Saturate accelerations */ if (gh_accel_ref.x <= -gh_max_accel_ref.x) { gh_accel_ref.x = -gh_max_accel_ref.x; @@ -219,8 +236,10 @@ void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { else if (gh_accel_ref.y >= gh_max_accel_ref.y) { gh_accel_ref.y = gh_max_accel_ref.y; } +} - /* Saturate speed and adjust acceleration accordingly */ +/** Saturate ref speed and adjust acceleration accordingly */ +static void gh_saturate_ref_speed(void) { if (gh_speed_ref.x <= -gh_max_speed_ref.x) { gh_speed_ref.x = -gh_max_speed_ref.x; if (gh_accel_ref.x < 0) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 81c72e79bf..c45947c472 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -237,13 +237,16 @@ void guidance_v_run(bool_t in_flight) { run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_CLIMB) { + guidance_v_z_sp = stateGetPositionNed_i()->z; guidance_v_zd_sp = -nav_climb; gv_update_ref_from_zd_sp(guidance_v_zd_sp); - nav_flight_altitude = -guidance_v_z_sp; run_hover_loop(in_flight); } else if (vertical_mode == VERTICAL_MODE_MANUAL) { - guidance_v_z_sp = -nav_flight_altitude; // For display only + guidance_v_z_sp = stateGetPositionNed_i()->z; + guidance_v_zd_sp = stateGetSpeedNed_i()->z; + GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0); + guidance_v_z_sum_err = 0; guidance_v_delta_t = nav_throttle; } #if NO_RC_THRUST_LIMIT diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index a39c5a884e..4ee0f227b7 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -254,6 +254,8 @@ STATIC_INLINE void failsafe_check( void ) { autopilot_set_mode(AP_MODE_FAILSAFE); } #endif + + autopilot_check_in_flight(autopilot_motors_on); } STATIC_INLINE void main_event( void ) { @@ -307,6 +309,9 @@ static inline void on_gyro_event( void ) { if (nps_bypass_ahrs) sim_overwrite_ahrs(); #endif ins_propagate(); +#ifdef SITL + if (nps_bypass_ins) sim_overwrite_ins(); +#endif } #ifdef USE_VEHICLE_INTERFACE vi_notify_imu_available(); diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 3f87aaef37..62fb47b6ab 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -341,4 +341,11 @@ bool_t nav_detect_ground(void) { return TRUE; } +bool_t nav_is_in_flight(void) { + if (autopilot_in_flight) + return TRUE; + else + return FALSE; +} + void nav_home(void) {} diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index e992f9f311..b8df4ee553 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -81,6 +81,7 @@ unit_t nav_reset_alt( void ) __attribute__ ((unused)); void nav_periodic_task(void); void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos); bool_t nav_detect_ground(void); +bool_t nav_is_in_flight(void); void nav_home(void); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index d6c83774b6..a5764fa859 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -162,7 +162,8 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { * onto the horizontal plane with the psi setpoint. * * angle between two vectors a and b: - * angle = atan2(norm(cross(a,b)), dot(a,b)) + * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) + * where n is the thrust vector (i.e. both a and b lie in that plane) */ const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; @@ -174,14 +175,17 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { psi_vect.x = cosf(stab_att_sp_euler.psi); psi_vect.y = sinf(stab_att_sp_euler.psi); psi_vect.z = 0.0; + // normal is the direction of the thrust vector struct FloatVect3 normal; FLOAT_QUAT_VMULT(normal, q_rp, zaxis); + // projection of desired heading onto body x-y plane // b = v - dot(v,n)*n float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); struct FloatVect3 dotn; FLOAT_VECT3_SMUL(dotn, normal, dot); + // b = v - dot(v,n)*n struct FloatVect3 b; FLOAT_VECT3_DIFF(b, psi_vect, dotn); dot = FLOAT_VECT3_DOT_PRODUCT(a, b); @@ -189,14 +193,23 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { VECT3_CROSS_PRODUCT(cross, a, b); // norm of the cross product float nc = FLOAT_VECT3_NORM(cross); + // angle = atan2(norm(cross(a,b)), dot(a,b)) float yaw2 = atan2(nc, dot) / 2.0; + // negative angle if needed + // sign(dot(cross(a,b), n) + float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); + if (dot_cross_ab < 0) { + yaw2 = -yaw2; + } + /* quaternion with yaw command */ struct FloatQuat q_yaw; QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ FLOAT_QUAT_COMP(stab_att_sp_quat, q_yaw, q_rp); + FLOAT_QUAT_NORMALIZE(stab_att_sp_quat); FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index b0b5b3ae6e..f709f9f5c0 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -117,7 +117,8 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { * onto the horizontal plane with the psi setpoint. * * angle between two vectors a and b: - * angle = atan2(norm(cross(a,b)), dot(a,b)) + * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) + * where n is the thrust vector (i.e. both a and b lie in that plane) */ const struct FloatVect3 xaxis = {1.0, 0.0, 0.0}; const struct FloatVect3 zaxis = {0.0, 0.0, 1.0}; @@ -129,23 +130,35 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { psi_vect.x = cosf(psi_sp); psi_vect.y = sinf(psi_sp); psi_vect.z = 0.0; + // normal is the direction of the thrust vector struct FloatVect3 normal; FLOAT_QUAT_VMULT(normal, q_rp, zaxis); + // projection of desired heading onto body x-y plane // b = v - dot(v,n)*n float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal); struct FloatVect3 dotn; FLOAT_VECT3_SMUL(dotn, normal, dot); + // b = v - dot(v,n)*n struct FloatVect3 b; FLOAT_VECT3_DIFF(b, psi_vect, dotn); + dot = FLOAT_VECT3_DOT_PRODUCT(a, b); struct FloatVect3 cross; VECT3_CROSS_PRODUCT(cross, a, b); // norm of the cross product float nc = FLOAT_VECT3_NORM(cross); + // angle = atan2(norm(cross(a,b)), dot(a,b)) float yaw2 = atan2(nc, dot) / 2.0; + // negative angle if needed + // sign(dot(cross(a,b), n) + float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, normal); + if (dot_cross_ab < 0) { + yaw2 = -yaw2; + } + /* quaternion with yaw command */ struct FloatQuat q_yaw; QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); @@ -153,6 +166,7 @@ void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) { /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ struct FloatQuat q_sp; FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp); + FLOAT_QUAT_NORMALIZE(q_sp); FLOAT_QUAT_WRAP_SHORTEST(q_sp); /* convert to fixed point */ diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h index 4bf69fc197..25439b28c5 100644 --- a/sw/airborne/math/pprz_algebra.h +++ b/sw/airborne/math/pprz_algebra.h @@ -626,6 +626,18 @@ (_ri).r = RATE_BFP_OF_REAL((_rf).r); \ } +#define POSITIONS_FLOAT_OF_BFP(_ef, _ei) { \ + (_ef).x = POS_FLOAT_OF_BFP((_ei).x); \ + (_ef).y = POS_FLOAT_OF_BFP((_ei).y); \ + (_ef).z = POS_FLOAT_OF_BFP((_ei).z); \ + } + +#define POSITIONS_BFP_OF_REAL(_ef, _ei) { \ + (_ef).x = POS_BFP_OF_REAL((_ei).x); \ + (_ef).y = POS_BFP_OF_REAL((_ei).y); \ + (_ef).z = POS_BFP_OF_REAL((_ei).z); \ + } + #define SPEEDS_FLOAT_OF_BFP(_ef, _ei) { \ (_ef).x = SPEED_FLOAT_OF_BFP((_ei).x); \ (_ef).y = SPEED_FLOAT_OF_BFP((_ei).y); \ 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/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c index afed7a6202..1e5317e937 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c +++ b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c @@ -27,6 +27,11 @@ * and also sets battery level. */ +#ifdef ARDRONE2_DEBUG +# include +# include +#endif + #include "ahrs_ardrone2.h" #include "state.h" #include "math/pprz_algebra_float.h" @@ -55,15 +60,42 @@ void ahrs_align(void) { } +#ifdef ARDRONE2_DEBUG +static void dump(const void *_b, size_t s) { + const unsigned char *b = _b; + size_t n; + + for(n = 0; n < s; ++n) { + printf("%02x ", b[n]); + if (n%16 == 15) + printf("\n"); + } + if (n%16 != 0) + printf("\n"); +} +#endif + void ahrs_propagate(void) { + int l; + //Recieve the main packet - at_com_recieve_navdata(buffer); + l = at_com_recieve_navdata(buffer); navdata_t* main_packet = (navdata_t*) &buffer; +#ifdef ARDRONE2_DEBUG + if (l < 0) + printf("errno = %d\n", errno); +#endif + //When this isn't a valid packet return - if(main_packet->header != NAVDATA_HEADER) + if(l < 0 || main_packet->header != NAVDATA_HEADER) return; +#ifdef ARDRONE2_DEBUG + printf("Read %d\n", l); + dump(buffer, l); +#endif + //Set the state ahrs_impl.state = main_packet->ardrone_state; @@ -78,6 +110,9 @@ void ahrs_propagate(void) { //Read the navdata until packet is fully readed while(!full_read && navdata_option->size > 0) { +#ifdef ARDRONE2_DEBUG + printf ("tag = %d\n", navdata_option->tag); +#endif //Check the tag for the right option switch(navdata_option->tag) { case 0: //NAVDATA_DEMO @@ -112,6 +147,9 @@ void ahrs_propagate(void) { break; #ifdef USE_GPS_ARDRONE2 case 27: //NAVDATA_GPS +# ifdef ARDRONE2_DEBUG + dump(navdata_option, navdata_option->size); +# endif navdata_gps = (navdata_gps_t*) navdata_option; // Send the data to the gps parser @@ -123,7 +161,9 @@ void ahrs_propagate(void) { full_read = TRUE; break; default: - //printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size); +#ifdef ARDRONE2_DEBUG + printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size); +#endif break; } navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size); diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index 5c6d27ecfc..fc8efbb801 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -476,16 +476,19 @@ void superbitrf_event(void) { /* Switch the different states */ switch (superbitrf.state) { case 0: + // Fixing timer overflow + if(superbitrf.timer_overflow && get_sys_time_usec() <= superbitrf.timer) + superbitrf.timer_overflow = FALSE; + // When there is a timeout - if (superbitrf.timer < get_sys_time_usec()) { - superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? (superbitrf.channel_idx + 2) %2 : (superbitrf.channel_idx + 2) %23; + if(superbitrf.timer < get_sys_time_usec() && !superbitrf.timer_overflow) { superbitrf.transfer_timeouts++; superbitrf.timeouts++; superbitrf.state++; } // We really lost the communication - if(superbitrf.timeouts > 2) { + if(superbitrf.timeouts > 100) { superbitrf.state = 0; superbitrf.resync_count++; superbitrf.status = SUPERBITRF_SYNCING_A; @@ -496,22 +499,20 @@ void superbitrf_event(void) { cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); superbitrf.state++; - // Only send on channel 2 - if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) { - superbitrf.state = 8; - // Set the timer - superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECV_TIME) % 0xFFFFFFFF; - break; - } + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECV_TIME) % 0xFFFFFFFF; + if(superbitrf.timer < get_sys_time_usec()) + superbitrf.timer_overflow = TRUE; + else + superbitrf.timer_overflow = FALSE; - // Set the timer for sending - superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATAWAIT_TIME) % 0xFFFFFFFF; + // Only send on channel 2 + if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) + superbitrf.state = 8; break; case 2: - // Wait before sending - //superbitrf.state++; - if (superbitrf.timer < get_sys_time_usec()) - superbitrf.state++; + // Wait before sending (FIXME??) + superbitrf.state++; break; case 3: // Create a new packet when no packet loss @@ -549,13 +550,14 @@ void superbitrf_event(void) { // Start receiving cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); superbitrf.state++; - - // Set the timer - superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATAWAIT_TIME + SUPERBITRF_DATARECV_TIME) % 0xFFFFFFFF; break; case 6: + // Fixing timer overflow + if(superbitrf.timer_overflow && get_sys_time_usec() <= superbitrf.timer) + superbitrf.timer_overflow = FALSE; + // Waiting for data receive - if (superbitrf.timer < get_sys_time_usec()) + if (superbitrf.timer < get_sys_time_usec() && !superbitrf.timer_overflow) superbitrf.state++; break; case 7: @@ -584,7 +586,15 @@ void superbitrf_event(void) { break; default: // Set the timer - superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_TIME) % 0xFFFFFFFF; + if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) + superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_TIME) % 0xFFFFFFFF; + else + superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_SHORT_TIME) % 0xFFFFFFFF; + if(superbitrf.timer < get_sys_time_usec()) + superbitrf.timer_overflow = TRUE; + else + superbitrf.timer_overflow = FALSE; + superbitrf.state = 0; break; } @@ -791,7 +801,7 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui /* When we receive a packet during transfer */ case SUPERBITRF_TRANSFER: // Check the MFG id - if(error && !(status & CYRF_BAD_CRC)) { + if(error) { // Start receiving TODO: Fix nicely cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); break; @@ -811,10 +821,6 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui break; } - // If the CRC is wrong invert - if (error && (status & CYRF_BAD_CRC)) - superbitrf.crc_seed = ~superbitrf.crc_seed; - // Check if it is a RC packet if(packet[1] == (~superbitrf.bind_mfg_id[3]&0xFF) || packet[1] == (superbitrf.bind_mfg_id[3]&0xFF)) { superbitrf.rc_count++; @@ -824,8 +830,10 @@ static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, ui superbitrf.rc_frame_available = TRUE; // Calculate the timing (seperately for the channel switches) - superbitrf.timing2 = superbitrf.timing1; - superbitrf.timing1 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_TIME); + if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) + superbitrf.timing2 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_TIME); + else + superbitrf.timing1 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_SHORT_TIME); // Go to next receive superbitrf.state = 1; diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h index 8baa147f1a..e1cbb6d3bf 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -35,8 +35,8 @@ /* The timings in microseconds */ #define SUPERBITRF_BIND_RECV_TIME 10000 /**< The time to wait for a bind packet on a channel in microseconds */ #define SUPERBITRF_SYNC_RECV_TIME 7000 /**< The time to wait for a sync packet on a channel in microseconds */ -#define SUPERBITRF_RECV_TIME 22000 /**< The time to wait for a transfer packet on a channel in microseconds */ -#define SUPERBITRF_DATAWAIT_TIME 100 /**< The time to wait after RC receive to send a data packet in microseconds */ +#define SUPERBITRF_RECV_TIME 20000 /**< The time to wait for a transfer packet on a channel in microseconds */ +#define SUPERBITRF_RECV_SHORT_TIME 6000 /**< The time to wait for a transfer packet short on a channel in microseconds */ #define SUPERBITRF_DATARECV_TIME 10000 /**< The time to wait for a data packet on a channel in microseconds */ #define SUPERBITRF_DATARECVB_TIME 6000 /**< The time to wait for a data packet on a channel during bind in microseconds */ @@ -75,6 +75,7 @@ struct SuperbitRF { volatile enum SuperbitRFStatus status; /**< The status of the superbitRF */ uint8_t state; /**< The states each status can be in */ uint32_t timer; /**< The timer in microseconds */ + bool_t timer_overflow; /**< When the timer overflows */ uint8_t timeouts; /**< The amount of timeouts */ uint32_t transfer_timeouts; /**< The amount of timeouts during transfer */ uint32_t resync_count; /**< The amount of resyncs needed during transfer */ diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c index 0326f0dd64..2feba4c765 100644 --- a/sw/airborne/subsystems/gps/gps_ardrone2.c +++ b/sw/airborne/subsystems/gps/gps_ardrone2.c @@ -26,6 +26,10 @@ * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2. */ +#ifdef ARDRONE2_DEBUG +# include +#endif + #include "subsystems/gps.h" #include "math/pprz_geodetic_double.h" @@ -37,6 +41,10 @@ void gps_impl_init( void ) { void gps_ardrone2_parse(navdata_gps_t *navdata_gps) { int i; + +#ifdef ARDRONE2_DEBUG + printf("state = %d\n", navdata_gps->gps_state); +#endif // Set the lla double struct from the navdata struct LlaCoor_d gps_lla_d; gps_lla_d.lat = RadOfDeg(navdata_gps->lat); 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 f87e000747..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,22 +33,31 @@ #include "subsystems/electrical.h" #include "mcu_periph/sys_time.h" #include "state.h" +#include "subsystems/ahrs.h" +#include "subsystems/ins.h" +#include "math/pprz_algebra.h" #include "subsystems/actuators/motor_mixing.h" struct NpsAutopilot autopilot; bool_t nps_bypass_ahrs; +bool_t nps_bypass_ins; #ifndef NPS_BYPASS_AHRS #define NPS_BYPASS_AHRS FALSE #endif +#ifndef NPS_BYPASS_INS +#define NPS_BYPASS_INS FALSE +#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; + nps_bypass_ins = NPS_BYPASS_INS; main_init(); @@ -98,6 +109,10 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { sim_overwrite_ahrs(); } + if (nps_bypass_ins) { + sim_overwrite_ins(); + } + handle_periodic_tasks(); /* scale final motor commands to 0-1 for feeding the fdm */ @@ -107,17 +122,31 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) { } -#include "nps_fdm.h" -#include "subsystems/ahrs.h" -#include "math/pprz_algebra.h" + 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_f ltp_pos; + VECT3_COPY(ltp_pos, fdm.ltpprz_pos); + stateSetPositionNed_f(<p_pos); + + struct NedCoor_f ltp_speed; + VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel); + stateSetSpeedNed_f(<p_speed); + + struct NedCoor_f ltp_accel; + VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel); + stateSetAccelNed_f(<p_accel); } diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.h b/sw/simulator/nps/nps_autopilot_rotorcraft.h index 9d04b7213c..b71ffd7eb8 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.h +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.h @@ -5,6 +5,8 @@ extern bool_t nps_bypass_ahrs; +extern bool_t nps_bypass_ins; extern void sim_overwrite_ahrs(void); +extern void sim_overwrite_ins(void); #endif /* NPS_AUTOPILOT_ROTORCRAFT_H */ 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 6104766d07..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_impl.ltp_def.hmsl + ins_impl.ltp_def.lla.alt; - enu_of_lla_point_i(&enu,&ins_impl.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 diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py index 9aaf3940b6..c67aa7df0b 100755 --- a/sw/tools/calibration/calibrate.py +++ b/sw/tools/calibration/calibrate.py @@ -43,6 +43,9 @@ def main(): parser.add_option("-p", "--plot", help="Show resulting plots", action="store_true", dest="plot") + parser.add_option("-a", "--auto_threshold", + help="Try to automatically determine noise threshold", + action="store_true", dest="auto_threshold") parser.add_option("-v", "--verbose", action="store_true", dest="verbose") (options, args) = parser.parse_args() @@ -67,12 +70,12 @@ def main(): sensor_ref = 9.81 sensor_res = 10 noise_window = 20; - #noise_threshold = 40; + noise_threshold = 40; elif options.sensor == "MAG": sensor_ref = 1. sensor_res = 11 noise_window = 10; - #noise_threshold = 1000; + noise_threshold = 1000; if not filename.endswith(".data"): parser.error("Please specify a *.data log file") @@ -89,9 +92,10 @@ def main(): # estimate the noise threshold # find the median of measurement vector lenght - meas_median = scipy.median(scipy.array([scipy.linalg.norm(v) for v in measurements])) - # set noise threshold to be below 10% of that - noise_threshold = meas_median * 0.1 + if options.auto_threshold: + meas_median = scipy.median(scipy.array([scipy.linalg.norm(v) for v in measurements])) + # set noise threshold to be below 10% of that + noise_threshold = meas_median * 0.1 if options.verbose: print("Using noise threshold of", noise_threshold, "for filtering.")