diff --git a/CHANGELOG.md b/CHANGELOG.md
index cbcf94891d5..e5962ee45c4 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,7 +1,7 @@
-Paparazzi v5.9_devel
-====================
+Paparazzi v5.9.0_testing
+========================
-Currently ongoing development.
+First release candidate for v5.10
General
-------
@@ -25,6 +25,7 @@ General
[#1675] (https://github.com/paparazzi/paparazzi/pull/1675)
- flight plans: simpler geofence safety behaviour definition
[#1664] (https://github.com/paparazzi/paparazzi/pull/1644)
+ [#1802] (https://github.com/paparazzi/paparazzi/pull/1802)
- module build system: improvements to efficiently replace subsystems
[#1534] (https://github.com/paparazzi/paparazzi/pull/1534)
[#1590] (https://github.com/paparazzi/paparazzi/pull/1590)
@@ -60,6 +61,12 @@ General
[#1758] (https://github.com/paparazzi/paparazzi/pull/1758)
- cockpit: workaround lablgtk 2.18 Debian/Ubuntu bug
[#1647] (https://github.com/paparazzi/paparazzi/pull/1647)
+- GCS: plot shapes on the map
+ [#1809] (https://github.com/paparazzi/paparazzi/pull/1809)
+- paparazzicenter: experimental Python version
+ [#1811] (https://github.com/paparazzi/paparazzi/pull/1811)
+- try to prevent speech spam in GCS
+ [#1842] (https://github.com/paparazzi/paparazzi/pull/1842)
Airborne
--------
@@ -104,11 +111,14 @@ Airborne
[#1648] (https://github.com/paparazzi/paparazzi/pull/1648)
[#1735] (https://github.com/paparazzi/paparazzi/pull/1735)
[#1764] (https://github.com/paparazzi/paparazzi/pull/1764)
+ [#1806] (https://github.com/paparazzi/paparazzi/pull/1806)
- computer vision: optical flow landing module
[#1611] (https://github.com/paparazzi/paparazzi/pull/1611)
- computer vision: openCV examples
[#1663] (https://github.com/paparazzi/paparazzi/pull/1663)
[#1789] (https://github.com/paparazzi/paparazzi/pull/1789)
+ [#1798] (https://github.com/paparazzi/paparazzi/pull/1798)
+ [#1824] (https://github.com/paparazzi/paparazzi/pull/1824)
- computer vision: image appearance representation using texton distribution
[#1692] (https://github.com/paparazzi/paparazzi/pull/1692)
- UTM handling upgrades
@@ -124,6 +134,11 @@ Airborne
[#1625] (https://github.com/paparazzi/paparazzi/pull/1625)
[#1638] (https://github.com/paparazzi/paparazzi/pull/1638)
[#1740] (https://github.com/paparazzi/paparazzi/pull/1740)
+ [#1788] (https://github.com/paparazzi/paparazzi/pull/1788)
+- telemetry over intermcu
+ [#1821] (https://github.com/paparazzi/paparazzi/pull/1821)
+- unified sys_mon and rtos_mon modules
+ [#1846] (https://github.com/paparazzi/paparazzi/pull/1846)
Rotorcraft firmware
-------------------
@@ -149,6 +164,15 @@ Rotorcraft firmware
- guidance for hybrid drones
[#1769] (https://github.com/paparazzi/paparazzi/pull/1769)
+Fixed-wing firmware
+-------------------
+
+- split AP and FBW taks when using ChibiOS
+ [#1767] (https://github.com/paparazzi/paparazzi/pull/1767)
+- Hard-fault recovery wen using ChibiOS
+ [#1815] (https://github.com/paparazzi/paparazzi/pull/1815)
+
+
Drivers/HW support
------------------
@@ -167,8 +191,14 @@ Drivers/HW support
[#1562] (https://github.com/paparazzi/paparazzi/pull/1562)
- Parrot Bebop: fix for firmware 3.2.0
[#1745] (https://github.com/paparazzi/paparazzi/pull/1745)
+ [#1828] (https://github.com/paparazzi/paparazzi/pull/1828)
- Parrot Bebop: camera upgrade
[#1750] (https://github.com/paparazzi/paparazzi/pull/1750)
+ [#1766] (https://github.com/paparazzi/paparazzi/pull/1766)
+ [#1818] (https://github.com/paparazzi/paparazzi/pull/1818)
+ [#1825] (https://github.com/paparazzi/paparazzi/pull/1825)
+ [#1826] (https://github.com/paparazzi/paparazzi/pull/1826)
+ [#1834] (https://github.com/paparazzi/paparazzi/pull/1834)
- Parrot ARDrone2/Bebop: startup fixes
[#1571] (https://github.com/paparazzi/paparazzi/pull/1571)
[#1588] (https://github.com/paparazzi/paparazzi/pull/1588)
diff --git a/Doxyfile b/Doxyfile
index 71883fb6342..a80e1a7f986 100644
--- a/Doxyfile
+++ b/Doxyfile
@@ -1964,7 +1964,9 @@ PREDEFINED = USE_GPS \
RADIO_CONTROL \
GUIDANCE_V_C \
DATALINK \
+ DOWNLINK \
USE_PERIODIC_TELEMETRY_REPORT \
+ PERIODIC_TELEMETRY \
__attribute__(x)=
# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
diff --git a/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml b/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml
index 320c7eeb7ce..3086b7767d0 100644
--- a/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml
@@ -35,6 +35,7 @@
+
diff --git a/conf/airframes/BR/conf.xml b/conf/airframes/BR/conf.xml
index ceb8bf1cfc0..4a479ce003c 100644
--- a/conf/airframes/BR/conf.xml
+++ b/conf/airframes/BR/conf.xml
@@ -17,7 +17,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_survey.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/estimation/ahrs_float_mlkf.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/control/stabilization_indi.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/cv_blob_locator.xml modules/video_rtp_stream.xml modules/nav_survey_rectangle_rotorcraft.xml modules/nav_survey_poly_rotorcraft.xml modules/video_capture.xml modules/digital_cam_video.xml"
gui_color="#ffff0689b7a1"
/>
@@ -28,7 +28,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/TUDELFT/tudelft_rotorcraft_survey_delft.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/estimation/ahrs_float_mlkf.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/control/stabilization_indi.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/cv_blob_locator.xml modules/video_rtp_stream.xml modules/nav_survey_rectangle_rotorcraft.xml modules/nav_survey_poly_rotorcraft.xml modules/video_capture.xml modules/digital_cam_video.xml"
gui_color="#ffff0689b7a1"
/>
@@ -39,7 +39,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_survey.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_float_mlkf.xml settings/control/stabilization_att_int_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/cv_colorfilter.xml modules/video_capture.xml modules/video_rtp_stream.xml modules/nav_survey_rectangle_rotorcraft.xml modules/digital_cam_video.xml"
gui_color="green"
/>
@@ -105,7 +105,7 @@
radio="radios/dummy.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/modules/config_asctec_v2.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_float_mlkf.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/modules/config_asctec_v2.xml settings/control/rotorcraft_guidance.xml "
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/servo_switch.xml modules/gps_ubx_ucenter.xml"
gui_color="blue"
/>
diff --git a/conf/airframes/CDW/cdw_conf.xml b/conf/airframes/CDW/cdw_conf.xml
index 7ce94eb0b0a..7d9dc606b00 100644
--- a/conf/airframes/CDW/cdw_conf.xml
+++ b/conf/airframes/CDW/cdw_conf.xml
@@ -17,7 +17,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/TUDELFT/tudelft_rotorcraft_survey_delft.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_speed.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/cv_blob_locator.xml modules/video_rtp_stream.xml [modules/nav_survey_rectangle_rotorcraft.xml] modules/nav_survey_poly_rotorcraft.xml modules/digital_cam_video.xml"
gui_color="#ffff0689b7a1"
/>
diff --git a/conf/airframes/ENAC/conf_enac.xml b/conf/airframes/ENAC/conf_enac.xml
index 71307e0118b..b9314ca17f4 100644
--- a/conf/airframes/ENAC/conf_enac.xml
+++ b/conf/airframes/ENAC/conf_enac.xml
@@ -6,7 +6,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic_safety.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml [settings/estimation/ins_float_invariant.xml] settings/estimation/ahrs_float_invariant.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml [settings/estimation/ins_float_invariant.xml] "
gui_color="red"
settings_modules=""
/>
@@ -39,7 +39,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings="settings/rotorcraft_basic.xml [settings/estimation/ahrs_int_cmpl_quat.xml] [settings/control/stabilization_att_int_quat.xml] settings/control/rotorcraft_guidance.xml [settings/control/stabilization_indi.xml]"
+ settings="settings/rotorcraft_basic.xml [settings/control/stabilization_att_int_quat.xml] settings/control/rotorcraft_guidance.xml [settings/control/stabilization_indi.xml]"
settings_modules=""
gui_color="#ffff00000000"
/>
@@ -61,7 +61,7 @@
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/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml "
settings_modules="modules/servo_switch.xml modules/rotorcraft_cam.xml"
gui_color="#3cf2d51335a7"
/>
diff --git a/conf/airframes/ENAC/fixed-wing/apogee.xml b/conf/airframes/ENAC/fixed-wing/apogee.xml
index f3674c2d2fe..b0548d69571 100644
--- a/conf/airframes/ENAC/fixed-wing/apogee.xml
+++ b/conf/airframes/ENAC/fixed-wing/apogee.xml
@@ -39,7 +39,7 @@
-
+
diff --git a/conf/airframes/ENAC/fixed-wing/jp.xml b/conf/airframes/ENAC/fixed-wing/jp.xml
index 1d66ef51969..ca73462a39e 100644
--- a/conf/airframes/ENAC/fixed-wing/jp.xml
+++ b/conf/airframes/ENAC/fixed-wing/jp.xml
@@ -69,7 +69,7 @@
-
+
diff --git a/conf/airframes/ESDEN/esden_conf.xml b/conf/airframes/ESDEN/esden_conf.xml
index 3417ed29796..639928d201a 100644
--- a/conf/airframes/ESDEN/esden_conf.xml
+++ b/conf/airframes/ESDEN/esden_conf.xml
@@ -138,7 +138,7 @@
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/estimation/ahrs_float_mlkf.xml settings/nps.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/nps.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="white"
/>
@@ -149,7 +149,7 @@
radio="radios/dummy.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/estimation/ahrs_float_mlkf.xml settings/nps.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/nps.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="blue"
/>
@@ -160,7 +160,7 @@
radio="radios/dummy.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/estimation/ahrs_int_cmpl_quat.xml settings/nps.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/nps.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="white"
/>
diff --git a/conf/airframes/FLIXR/flixr_conf.xml b/conf/airframes/FLIXR/flixr_conf.xml
index daaa91b9af0..7e347030d6e 100644
--- a/conf/airframes/FLIXR/flixr_conf.xml
+++ b/conf/airframes/FLIXR/flixr_conf.xml
@@ -6,7 +6,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings="settings/rotorcraft_basic.xml settings/nps.xml settings/persistent_settings.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/superbitrf.xml"
+ settings="settings/rotorcraft_basic.xml settings/nps.xml settings/persistent_settings.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/superbitrf.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/geo_mag.xml"
gui_color="blue"
/>
@@ -17,7 +17,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_float_mlkf.xml settings/control/stabilization_att_int_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml"
settings_modules="modules/gps.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="blue"
/>
@@ -28,7 +28,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/demo.xml"
flight_plan="flight_plans/dummy.xml"
- settings="settings/estimation/ahrs_int_cmpl_quat.xml settings/test_actuators_pwm.xml"
+ settings=" settings/test_actuators_pwm.xml"
settings_modules="modules/imu_common.xml"
gui_color="blue"
/>
@@ -50,7 +50,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings="settings/rotorcraft_basic.xml settings/setup_actuators.xml settings/estimation/ahrs_int_cmpl_quat.xml [settings/estimation/ahrs_float_mlkf.xml] settings/estimation/ahrs_secondary.xml settings/control/rotorcraft_guidance.xml"
+ settings="settings/rotorcraft_basic.xml settings/setup_actuators.xml settings/estimation/ahrs_secondary.xml settings/control/rotorcraft_guidance.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="blue"
/>
@@ -61,7 +61,7 @@
radio="radios/TGY9x_jeti.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/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml "
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml"
gui_color="blue"
/>
@@ -72,7 +72,7 @@
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/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml "
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml"
gui_color="blue"
/>
@@ -83,7 +83,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
- settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml "
settings_modules=""
gui_color="blue"
/>
@@ -94,7 +94,7 @@
radio="radios/TGY9x_jeti.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/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml "
settings_modules="modules/gps.xml modules/imu_common.xml modules/gps_ubx_ucenter.xml"
gui_color="blue"
/>
@@ -116,7 +116,7 @@
radio="radios/dummy.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_quat.xml settings/estimation/ahrs_float_mlkf.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml "
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="blue"
/>
diff --git a/conf/airframes/HOOPERFLY/hooperfly_conf.xml b/conf/airframes/HOOPERFLY/hooperfly_conf.xml
index 60ab4c492e4..48ea4a13c6a 100644
--- a/conf/airframes/HOOPERFLY/hooperfly_conf.xml
+++ b/conf/airframes/HOOPERFLY/hooperfly_conf.xml
@@ -6,7 +6,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/HOOPERFLY/hooperfly_gsa_one.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml "
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml modules/nav_survey_rectangle_rotorcraft.xml"
gui_color="blue"
/>
@@ -17,7 +17,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/HOOPERFLY/hooperfly_gsa_one.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml "
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml modules/nav_survey_rectangle_rotorcraft.xml"
gui_color="purple"
/>
@@ -28,7 +28,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/HOOPERFLY/hooperfly_gsa_one.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml "
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml modules/nav_survey_rectangle_rotorcraft.xml"
gui_color="green"
/>
@@ -39,7 +39,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/HOOPERFLY/hooperfly_gsa_one.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml "
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml modules/nav_survey_rectangle_rotorcraft.xml"
gui_color="red"
/>
@@ -50,7 +50,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/HOOPERFLY/hooperfly_gsa_one.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml "
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml modules/nav_survey_rectangle_rotorcraft.xml"
gui_color="lime"
/>
@@ -61,7 +61,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/HOOPERFLY/hooperfly_gsa_one.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml "
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml modules/nav_survey_rectangle_rotorcraft.xml"
gui_color="cyan"
/>
@@ -72,7 +72,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/HOOPERFLY/hooperfly_gsa_one.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml "
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml modules/nav_survey_rectangle_rotorcraft.xml"
gui_color="yellow"
/>
@@ -83,7 +83,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/HOOPERFLY/hooperfly_gsa_one.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml "
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml modules/nav_survey_rectangle_rotorcraft.xml"
gui_color="orange"
/>
@@ -94,7 +94,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/HOOPERFLY/hooperfly_gsa_one.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml "
settings_modules="modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml modules/nav_survey_rectangle_rotorcraft.xml"
gui_color="orangered"
/>
diff --git a/conf/airframes/LS/ls_conf.xml b/conf/airframes/LS/ls_conf.xml
index b396eaa8653..8c07daa9e87 100644
--- a/conf/airframes/LS/ls_conf.xml
+++ b/conf/airframes/LS/ls_conf.xml
@@ -17,7 +17,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_optitrack.xml"
- settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int_quat.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/rotorcraft_guidance.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int_quat.xml settings/control/rotorcraft_guidance.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml"
gui_color="blue"
/>
diff --git a/conf/airframes/OPENUAS/openuas_conf.xml b/conf/airframes/OPENUAS/openuas_conf.xml
index 2d00ce29725..f9d349e2884 100644
--- a/conf/airframes/OPENUAS/openuas_conf.xml
+++ b/conf/airframes/OPENUAS/openuas_conf.xml
@@ -17,7 +17,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_survey.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/control/stabilization_indi.xml"
settings_modules="modules/gps.xml modules/geo_mag.xml modules/air_data.xml modules/cv_blob_locator.xml modules/video_rtp_stream.xml modules/nav_survey_rectangle_rotorcraft.xml modules/nav_survey_poly_rotorcraft.xml modules/video_capture.xml modules/digital_cam_video.xml"
gui_color="#ffffe8b36503"
/>
@@ -50,7 +50,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/air_data.xml"
gui_color="#fffffe72b9fd"
/>
@@ -61,7 +61,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings="settings/rotorcraft_basic.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/rotorcraft_guidance.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/gps.xml"
gui_color="#ffffffffffff"
/>
diff --git a/conf/airframes/TUDELFT/tudelft_KM_conf.xml b/conf/airframes/TUDELFT/tudelft_KM_conf.xml
index 58699f8871e..605c5d9e4aa 100644
--- a/conf/airframes/TUDELFT/tudelft_KM_conf.xml
+++ b/conf/airframes/TUDELFT/tudelft_KM_conf.xml
@@ -6,7 +6,7 @@
radio="radios/dummy.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_quat.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml "
settings_modules="modules/gps_ubx_ucenter.xml modules/video_thread.xml modules/cv_opticflow.xml"
gui_color="red"
/>
diff --git a/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_stereo.xml b/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_stereo.xml
index 28ce1ec6209..43af767324a 100644
--- a/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_stereo.xml
+++ b/conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_stereo.xml
@@ -23,8 +23,8 @@
-
-
+
diff --git a/conf/airframes/TUDELFT/tudelft_bebop2_no_damping.xml b/conf/airframes/TUDELFT/tudelft_bebop2_no_damping.xml
new file mode 100644
index 00000000000..97911bc1fa4
--- /dev/null
+++ b/conf/airframes/TUDELFT/tudelft_bebop2_no_damping.xml
@@ -0,0 +1,208 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/TUDELFT/tudelft_bebop_indi.xml b/conf/airframes/TUDELFT/tudelft_bebop_indi.xml
index 8c9663be577..09e8bdfad8b 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop_indi.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop_indi.xml
@@ -26,7 +26,9 @@
-
+
+
+
diff --git a/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml b/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml
index 3b7e5513ed3..6d8f5997970 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml
@@ -35,13 +35,13 @@
-
-
-
+
+
+
@@ -144,19 +144,11 @@
-
-
-
-
-
+
+
+
+
+
diff --git a/conf/airframes/TUDELFT/tudelft_conf.xml b/conf/airframes/TUDELFT/tudelft_conf.xml
index e0e7e9818d0..0dc976e781f 100644
--- a/conf/airframes/TUDELFT/tudelft_conf.xml
+++ b/conf/airframes/TUDELFT/tudelft_conf.xml
@@ -6,7 +6,7 @@
radio="radios/dummy.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_quat.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml "
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/air_data.xml modules/geo_mag.xml modules/video_rtp_stream.xml"
gui_color="#ffffd633d633"
/>
@@ -17,7 +17,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml"
settings_modules="modules/gps.xml modules/air_data.xml"
gui_color="#ffffcd49cd49"
/>
@@ -28,7 +28,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/air_data.xml"
gui_color="#ffffc457c457"
/>
@@ -39,7 +39,7 @@
radio="radios/dummy.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_quat.xml settings/estimation/ahrs_int_cmpl_quat.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml "
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/cv_opticflow.xml modules/video_capture.xml"
gui_color="#ffffccc2ccc2"
/>
@@ -50,7 +50,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_optitrack.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_indi.xml"
settings_modules="modules/gps.xml"
gui_color="#fffff996b847"
/>
@@ -94,7 +94,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft_mavlink.xml"
flight_plan="flight_plans/TUDELFT/tudelft_rotorcraft_survey_delft.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_speed.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/gps.xml modules/geo_mag.xml modules/air_data.xml modules/cv_blob_locator.xml [modules/nav_survey_rectangle_rotorcraft.xml] [modules/nav_survey_poly_rotorcraft.xml] modules/video_capture.xml modules/digital_cam_video.xml"
gui_color="#ffff0689b7a1"
/>
@@ -270,7 +270,7 @@
radio="radios/dummy.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] settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_indi.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int.xml] [settings/control/stabilization_rate.xml] settings/control/stabilization_indi.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml"
gui_color="#f1d3c1b7ffff"
/>
@@ -362,6 +362,17 @@
settings_modules="modules/gps.xml modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
gui_color="#f6d5baaaffff"
/>
+
diff --git a/conf/boards/opa_ap_1.0.makefile b/conf/boards/opa_ap_1.0.makefile
index 7648433c5ba..131c5874740 100644
--- a/conf/boards/opa_ap_1.0.makefile
+++ b/conf/boards/opa_ap_1.0.makefile
@@ -42,7 +42,7 @@ GPS_PORT ?= UART1
GPS_BAUD ?= B57600
INTERMCU_PORT ?= UART3
-INTERMCU_BAUD ?= B230400
+INTERMCU_BAUD ?= B460800
#
# default IMU configuration
diff --git a/conf/boards/opa_ftd_1.0.makefile b/conf/boards/opa_ftd_1.0.makefile
index e2b9abea6e2..f396daff0cb 100644
--- a/conf/boards/opa_ftd_1.0.makefile
+++ b/conf/boards/opa_ftd_1.0.makefile
@@ -40,7 +40,10 @@ MODEM_PORT ?= UART3
MODEM_BAUD ?= B19200
INTERMCU_PORT ?= UART2
-INTERMCU_BAUD ?= B230400
+INTERMCU_BAUD ?= B460800
+
+GPS_PORT ?= UART4
+GPS_BAUD ?= B57600
#
# default actuator configuration
diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index d3ac8ec8d44..1340c1f156d 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -18,7 +18,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml"
- settings_modules="modules/imu_common.xml modules/gps.xml"
+ settings_modules="modules/imu_common.xml modules/gps.xml modules/ahrs_int_cmpl_euler.xml"
gui_color="white"
/>
diff --git a/conf/conf_tests_coverity.xml b/conf/conf_tests_coverity.xml
index 721b138bfcb..d582da49f5c 100644
--- a/conf/conf_tests_coverity.xml
+++ b/conf/conf_tests_coverity.xml
@@ -226,7 +226,7 @@
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/estimation/ahrs_float_mlkf.xml settings/nps.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/nps.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="white"
/>
@@ -248,7 +248,7 @@
radio="radios/dummy.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/estimation/ahrs_float_mlkf.xml settings/nps.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/nps.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="blue"
/>
@@ -259,7 +259,7 @@
radio="radios/dummy.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/estimation/ahrs_int_cmpl_quat.xml settings/nps.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/nps.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="white"
/>
@@ -281,7 +281,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/nps.xml settings/control/stabilization_indi.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml settings/control/stabilization_indi.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml"
gui_color="#710080"
/>
@@ -369,7 +369,7 @@
radio="radios/dummy.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_quat.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/nps.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml settings/nps.xml"
settings_modules="modules/gps.xml modules/gps_ubx_ucenter.xml modules/air_data.xml modules/geo_mag.xml"
gui_color="red"
/>
@@ -391,7 +391,7 @@
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
- settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_float_mlkf.xml settings/control/stabilization_att_int_quat.xml settings/nps.xml"
+ settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml settings/nps.xml"
settings_modules="modules/gps.xml modules/geo_mag.xml modules/air_data.xml"
gui_color="red"
/>
diff --git a/conf/control_panel_example.xml b/conf/control_panel_example.xml
index 57090eb27fd..370bab42472 100644
--- a/conf/control_panel_example.xml
+++ b/conf/control_panel_example.xml
@@ -64,6 +64,7 @@
+
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
deleted file mode 100644
index d8e29177bdb..00000000000
--- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile
+++ /dev/null
@@ -1,32 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-# AHRS_H_X
-# AHRS_H_Y
-# AHRS_H_Z
-#
-
-# for fixedwings disable mag by default
-USE_MAGNETOMETER ?= 0
-
-
-include $(CFG_SHARED)/ahrs_float_cmpl_quat.makefile
-
-# add some fixedwing specific flags
-ifeq (,$(findstring $(TARGET),sim fbw))
-$(TARGET).CFLAGS += -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
-ifneq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
-$(TARGET).CFLAGS += -DAHRS_USE_GPS_HEADING
-endif
-endif
-
-#
-# Simple simulation of the AHRS result
-#
-ahrssim_CFLAGS = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS
-
-ahrssim_srcs = $(SRC_SUBSYSTEMS)/ahrs.c
-ahrssim_srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
-
-sim.CFLAGS += $(ahrssim_CFLAGS)
-sim.srcs += $(ahrssim_srcs)
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
deleted file mode 100644
index d24e5218146..00000000000
--- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile
+++ /dev/null
@@ -1,32 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-# AHRS_H_X
-# AHRS_H_Y
-# AHRS_H_Z
-#
-
-# for fixedwings disable mag by default
-USE_MAGNETOMETER ?= 0
-
-
-include $(CFG_SHARED)/ahrs_float_cmpl_rmat.makefile
-
-# add some fixedwing specific flags
-ifeq (,$(findstring $(TARGET),sim fbw))
-$(TARGET).CFLAGS += -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
-ifneq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
-$(TARGET).CFLAGS += -DAHRS_USE_GPS_HEADING
-endif
-endif
-
-#
-# Simple simulation of the AHRS result
-#
-ahrssim_CFLAGS = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS
-
-ahrssim_srcs = $(SRC_SUBSYSTEMS)/ahrs.c
-ahrssim_srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
-
-sim.CFLAGS += $(ahrssim_CFLAGS)
-sim.srcs += $(ahrssim_srcs)
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_infrared.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_infrared.makefile
deleted file mode 100644
index d7f4d2bb819..00000000000
--- a/conf/firmwares/subsystems/fixedwing/ahrs_infrared.makefile
+++ /dev/null
@@ -1 +0,0 @@
-$(error The ahrs_infrared subsystem has converted to a module, please remove it and add to your module section.)
diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile
deleted file mode 100644
index d354c659f8f..00000000000
--- a/conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_quat.makefile
+++ /dev/null
@@ -1,33 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-# AHRS_H_X
-# AHRS_H_Y
-# AHRS_H_Z
-#
-
-# for fixedwings disable mag by default
-USE_MAGNETOMETER ?= 0
-
-
-include $(CFG_SHARED)/ahrs_int_cmpl_quat.makefile
-
-# add some fixedwing specific flags
-ifeq (,$(findstring $(TARGET),sim fbw))
-$(TARGET).CFLAGS += -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
-ifneq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
-$(TARGET).CFLAGS += -DAHRS_USE_GPS_HEADING
-endif
-endif
-
-#
-# Simple simulation of the AHRS result
-#
-ahrssim_CFLAGS = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS
-
-ahrssim_srcs = $(SRC_SUBSYSTEMS)/ahrs.c
-ahrssim_srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
-
-sim.CFLAGS += $(ahrssim_CFLAGS)
-sim.srcs += $(ahrssim_srcs)
-
diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_float_invariant.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_float_invariant.makefile
deleted file mode 100644
index ce4cadb62ae..00000000000
--- a/conf/firmwares/subsystems/rotorcraft/ahrs_float_invariant.makefile
+++ /dev/null
@@ -1,46 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-# AHRS_H_X
-# AHRS_H_Y
-# AHRS_H_Z
-#
-
-USE_MAGNETOMETER ?= 1
-AHRS_ALIGNER_LED ?= none
-
-AHRS_FINV_CFLAGS = -DUSE_AHRS
-AHRS_FINV_CFLAGS += -DUSE_AHRS_ALIGNER
-
-ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
- AHRS_FINV_CFLAGS += -DUSE_MAGNETOMETER
-endif
-
-ifneq ($(AHRS_ALIGNER_LED),none)
- AHRS_FINV_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
-endif
-
-ifdef SECONDARY_AHRS
-ifneq (,$(findstring $(SECONDARY_AHRS), fcq float_cmpl_quat))
-# this is the secondary AHRS
-AHRS_FINV_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_invariant_wrapper.h\"
-AHRS_FINV_CFLAGS += -DSECONDARY_AHRS=ahrs_float_invariant
-else
-# this is the primary AHRS
-AHRS_FINV_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_invariant_wrapper.h\"
-AHRS_FINV_CFLAGS += -DPRIMARY_AHRS=ahrs_float_invariant
-endif
-else
-# plain old single AHRS usage
-AHRS_FINV_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_invariant_wrapper.h\"
-endif
-
-AHRS_FINV_SRCS += subsystems/ahrs.c
-AHRS_FINV_SRCS += subsystems/ahrs/ahrs_float_invariant.c
-AHRS_FINV_SRCS += subsystems/ahrs/ahrs_float_invariant_wrapper.c
-AHRS_FINV_SRCS += subsystems/ahrs/ahrs_aligner.c
-
-# add it for all targets except sim and fbw
-ifeq (,$(findstring $(TARGET),sim fbw))
-$(TARGET).CFLAGS += $(AHRS_FINV_CFLAGS)
-$(TARGET).srcs += $(AHRS_FINV_SRCS)
-endif
diff --git a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile
deleted file mode 100644
index a98395b1079..00000000000
--- a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile
+++ /dev/null
@@ -1,47 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-# AHRS_H_X
-# AHRS_H_Y
-# AHRS_H_Z
-#
-
-USE_MAGNETOMETER ?= 1
-AHRS_ALIGNER_LED ?= none
-
-AHRS_FC_CFLAGS = -DUSE_AHRS
-AHRS_FC_CFLAGS += -DUSE_AHRS_ALIGNER
-
-ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
- AHRS_FC_CFLAGS += -DUSE_MAGNETOMETER
-endif
-
-ifneq ($(AHRS_ALIGNER_LED),none)
- AHRS_FC_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
-endif
-
-ifdef SECONDARY_AHRS
-ifneq (,$(findstring $(SECONDARY_AHRS), fcq float_cmpl_quat))
-# this is the secondary AHRS
-AHRS_FC_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
-AHRS_FC_CFLAGS += -DSECONDARY_AHRS=ahrs_fc
-else
-# this is the primary AHRS
-AHRS_FC_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
-AHRS_FC_CFLAGS += -DPRIMARY_AHRS=ahrs_fc
-endif
-else
-# plain old single AHRS usage
-AHRS_FC_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
-endif
-
-AHRS_FC_CFLAGS += -DAHRS_PROPAGATE_QUAT
-AHRS_FC_SRCS += subsystems/ahrs.c
-AHRS_FC_SRCS += subsystems/ahrs/ahrs_float_cmpl.c
-AHRS_FC_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
-AHRS_FC_SRCS += subsystems/ahrs/ahrs_aligner.c
-
-# add it for all targets except sim and fbw
-ifeq (,$(findstring $(TARGET),sim fbw))
-$(TARGET).CFLAGS += $(AHRS_FC_CFLAGS)
-$(TARGET).srcs += $(AHRS_FC_SRCS)
-endif
diff --git a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile
deleted file mode 100644
index 3b742585f15..00000000000
--- a/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile
+++ /dev/null
@@ -1,47 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-# AHRS_H_X
-# AHRS_H_Y
-# AHRS_H_Z
-#
-
-USE_MAGNETOMETER ?= 1
-
-AHRS_FC_CFLAGS = -DUSE_AHRS
-AHRS_FC_CFLAGS += -DUSE_AHRS_ALIGNER
-
-ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
- AHRS_FC_CFLAGS += -DUSE_MAGNETOMETER
-endif
-
-ifneq ($(AHRS_ALIGNER_LED),none)
- AHRS_FC_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
-endif
-
-ifdef SECONDARY_AHRS
-ifneq (,$(findstring $(SECONDARY_AHRS), fcr float_cmpl_rmat))
-# this is the secondary AHRS
-AHRS_FC_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
-AHRS_FC_CFLAGS += -DSECONDARY_AHRS=ahrs_fc
-else
-# this is the primary AHRS
-AHRS_FC_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
-AHRS_FC_CFLAGS += -DPRIMARY_AHRS=ahrs_fc
-endif
-else
-# plain old single AHRS usage
-AHRS_FC_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
-endif
-
-AHRS_FC_CFLAGS += -DAHRS_PROPAGATE_RMAT
-AHRS_FC_SRCS += subsystems/ahrs.c
-AHRS_FC_SRCS += subsystems/ahrs/ahrs_float_cmpl.c
-AHRS_FC_SRCS += subsystems/ahrs/ahrs_float_cmpl_wrapper.c
-AHRS_FC_SRCS += subsystems/ahrs/ahrs_aligner.c
-
-
-# add it for all targets except sim and fbw
-ifeq (,$(findstring $(TARGET),sim fbw))
-$(TARGET).CFLAGS += $(AHRS_FC_CFLAGS)
-$(TARGET).srcs += $(AHRS_FC_SRCS)
-endif
diff --git a/conf/firmwares/subsystems/shared/ahrs_float_dcm.makefile b/conf/firmwares/subsystems/shared/ahrs_float_dcm.makefile
deleted file mode 100644
index 9629a3ae6f4..00000000000
--- a/conf/firmwares/subsystems/shared/ahrs_float_dcm.makefile
+++ /dev/null
@@ -1,57 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-
-# attitude estimation for fixedwings via dcm algorithm
-
-USE_MAGNETOMETER ?= 0
-AHRS_ALIGNER_LED ?= none
-
-AHRS_DCM_CFLAGS = -DUSE_AHRS
-AHRS_DCM_CFLAGS += -DUSE_AHRS_ALIGNER
-
-ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
- AHRS_DCM_CFLAGS += -DUSE_MAGNETOMETER
-endif
-
-ifneq ($(AHRS_ALIGNER_LED),none)
- AHRS_DCM_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
-endif
-
-ifdef SECONDARY_AHRS
-ifneq (,$(findstring $(SECONDARY_AHRS), dcm float_dcm))
-# this is the secondary AHRS
-AHRS_DCM_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm_wrapper.h\"
-AHRS_DCM_CFLAGS += -DSECONDARY_AHRS=ahrs_dcm
-else
-# this is the primary AHRS
-AHRS_DCM_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm_wrapper.h\"
-AHRS_DCM_CFLAGS += -DPRIMARY_AHRS=ahrs_dcm
-endif
-else
-# plain old single AHRS usage
-AHRS_DCM_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm_wrapper.h\"
-endif
-
-AHRS_DCM_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
-AHRS_DCM_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
-AHRS_DCM_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
-AHRS_DCM_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm_wrapper.c
-
-# add it for all targets except sim and fbw
-ifeq (,$(findstring $(TARGET),sim fbw))
-$(TARGET).CFLAGS += $(AHRS_DCM_CFLAGS)
-$(TARGET).srcs += $(AHRS_DCM_SRCS)
-endif
-
-
-#
-# Simple simulation of the AHRS result
-#
-ahrssim_CFLAGS = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS
-
-ahrssim_srcs = $(SRC_SUBSYSTEMS)/ahrs.c
-ahrssim_srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
-
-sim.CFLAGS += $(ahrssim_CFLAGS)
-sim.srcs += $(ahrssim_srcs)
-
diff --git a/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile b/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile
deleted file mode 100644
index b55c467a5f3..00000000000
--- a/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile
+++ /dev/null
@@ -1,45 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-#
-
-USE_MAGNETOMETER ?= 1
-AHRS_ALIGNER_LED ?= none
-
-AHRS_MLKF_CFLAGS = -DUSE_AHRS
-AHRS_MLKF_CFLAGS += -DUSE_AHRS_ALIGNER
-
-ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
- AHRS_MLKF_CFLAGS += -DUSE_MAGNETOMETER
-else
-$(error ahrs_float_mlkf needs a magnetometer)
-endif
-
-ifneq ($(AHRS_ALIGNER_LED),none)
- AHRS_MLKF_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
-endif
-
-ifdef SECONDARY_AHRS
-ifneq (,$(findstring $(SECONDARY_AHRS), mlkf))
-# this is the secondary AHRS
-AHRS_MLKF_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
-AHRS_MLKF_CFLAGS += -DSECONDARY_AHRS=ahrs_mlkf
-else
-# this is the primary AHRS
-AHRS_MLKF_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
-AHRS_MLKF_CFLAGS += -DPRIMARY_AHRS=ahrs_mlkf
-endif
-else
-# plain old single AHRS usage
-AHRS_MLKF_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
-endif
-
-AHRS_MLKF_SRCS += subsystems/ahrs.c
-AHRS_MLKF_SRCS += subsystems/ahrs/ahrs_float_mlkf.c
-AHRS_MLKF_SRCS += subsystems/ahrs/ahrs_float_mlkf_wrapper.c
-AHRS_MLKF_SRCS += subsystems/ahrs/ahrs_aligner.c
-
-# add it for all targets except sim and fbw
-ifeq (,$(findstring $(TARGET),sim fbw))
-$(TARGET).CFLAGS += $(AHRS_MLKF_CFLAGS)
-$(TARGET).srcs += $(AHRS_MLKF_SRCS)
-endif
diff --git a/conf/firmwares/subsystems/shared/ahrs_gx3.makefile b/conf/firmwares/subsystems/shared/ahrs_gx3.makefile
deleted file mode 100644
index 6223239c8c2..00000000000
--- a/conf/firmwares/subsystems/shared/ahrs_gx3.makefile
+++ /dev/null
@@ -1,20 +0,0 @@
-# AHRS subsystem for GX3
-# 2013, Utah State University, http://aggieair.usu.edu/
-
-GX3_PORT ?= UART3
-GX3_BAUD ?= B921600
-
-AHRS_CFLAGS = -DUSE_AHRS
-AHRS_CFLAGS += -DUSE_IMU
-
-AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\"
-AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
-AHRS_SRCS += $(SRC_SUBSYSTEMS)/imu.c
-AHRS_SRCS += subsystems/ahrs/ahrs_gx3.c
-
-GX3_PORT_LOWER=$(shell echo $(GX3_PORT) | tr A-Z a-z)
-AHRS_CFLAGS += -DUSE_$(GX3_PORT) -D$(GX3_PORT)_BAUD=$(GX3_BAUD)
-AHRS_CFLAGS += -DGX3_PORT=$(GX3_PORT_LOWER)
-
-ap.CFLAGS += $(AHRS_CFLAGS)
-ap.srcs += $(AHRS_SRCS)
diff --git a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile
deleted file mode 100644
index 8c3fcc6b380..00000000000
--- a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile
+++ /dev/null
@@ -1,57 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-# Fixed point complementary filter using euler angles for attitude estimation
-#
-
-USE_MAGNETOMETER ?= 1
-AHRS_ALIGNER_LED ?= none
-
-AHRS_ICE_CFLAGS = -DUSE_AHRS
-AHRS_ICE_CFLAGS += -DUSE_AHRS_ALIGNER
-
-ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
- AHRS_ICE_CFLAGS += -DUSE_MAGNETOMETER
-endif
-
-ifneq ($(AHRS_ALIGNER_LED),none)
- AHRS_ICE_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
-endif
-
-ifdef SECONDARY_AHRS
-ifneq (,$(findstring $(SECONDARY_AHRS), ice int_cmpl_euler))
-# this is the secondary AHRS
-AHRS_ICE_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
-AHRS_ICE_CFLAGS += -DSECONDARY_AHRS=ahrs_ice
-else
-# this is the primary AHRS
-AHRS_ICE_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
-AHRS_ICE_CFLAGS += -DPRIMARY_AHRS=ahrs_ice
-endif
-else
-# plain old single AHRS usage
-AHRS_ICE_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
-endif
-
-AHRS_ICE_SRCS += subsystems/ahrs.c
-AHRS_ICE_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler.c
-AHRS_ICE_SRCS += subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
-AHRS_ICE_SRCS += subsystems/ahrs/ahrs_aligner.c
-
-# add it for all targets except sim and fbw
-ifeq (,$(findstring $(TARGET),sim fbw))
-$(TARGET).CFLAGS += $(AHRS_ICE_CFLAGS)
-$(TARGET).srcs += $(AHRS_ICE_SRCS)
-endif
-
-
-#
-# Simple simulation of the AHRS result
-#
-ahrssim_CFLAGS = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
-ahrssim_CFLAGS += -DUSE_AHRS
-
-ahrssim_srcs = $(SRC_SUBSYSTEMS)/ahrs.c
-ahrssim_srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
-
-sim.CFLAGS += $(ahrssim_CFLAGS)
-sim.srcs += $(ahrssim_srcs)
diff --git a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile
deleted file mode 100644
index 3e4a1442fda..00000000000
--- a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile
+++ /dev/null
@@ -1,46 +0,0 @@
-# Hey Emacs, this is a -*- makefile -*-
-#
-# AHRS_H_X
-# AHRS_H_Y
-# AHRS_H_Z
-#
-
-USE_MAGNETOMETER ?= 1
-AHRS_ALIGNER_LED ?= none
-
-AHRS_ICQ_CFLAGS = -DUSE_AHRS
-AHRS_ICQ_CFLAGS += -DUSE_AHRS_ALIGNER
-
-ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE))
- AHRS_ICQ_CFLAGS += -DUSE_MAGNETOMETER
-endif
-
-ifneq ($(AHRS_ALIGNER_LED),none)
- AHRS_ICQ_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
-endif
-
-ifdef SECONDARY_AHRS
-ifneq (,$(findstring $(SECONDARY_AHRS),ahrs_icq int_cmpl_quat))
-# this is the secondary AHRS
-AHRS_ICQ_CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
-AHRS_ICQ_CFLAGS += -DSECONDARY_AHRS=ahrs_icq
-else
-# this is the primary AHRS
-AHRS_ICQ_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
-AHRS_ICQ_CFLAGS += -DPRIMARY_AHRS=ahrs_icq
-endif
-else
-# plain old single AHRS usage
-AHRS_ICQ_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
-endif
-
-AHRS_ICQ_SRCS += subsystems/ahrs.c
-AHRS_ICQ_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat.c
-AHRS_ICQ_SRCS += subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.c
-AHRS_ICQ_SRCS += subsystems/ahrs/ahrs_aligner.c
-
-# add it for all targets except sim and fbw
-ifeq (,$(findstring $(TARGET),sim fbw))
-$(TARGET).CFLAGS += $(AHRS_ICQ_CFLAGS)
-$(TARGET).srcs += $(AHRS_ICQ_SRCS)
-endif
diff --git a/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile b/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile
index 9ae81994293..3b3a152067a 100644
--- a/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile
+++ b/conf/firmwares/subsystems/shared/radio_control_spektrum.makefile
@@ -1,6 +1,7 @@
#
# Makefile for shared radio_control spektrum susbsytem
#
+# Define USE_DSMX on STM32 microcontrollers to bind in DSMX instead of DSM2
RADIO_CONTROL_LED ?= none
diff --git a/conf/firmwares/test_progs.makefile b/conf/firmwares/test_progs.makefile
index 1813b07a925..d88a435ec85 100644
--- a/conf/firmwares/test_progs.makefile
+++ b/conf/firmwares/test_progs.makefile
@@ -86,6 +86,24 @@ COMMON_TELEMETRY_CFLAGS += -D$(MODEM_DEV)_BROADCAST=$(MODEM_BROADCAST) -D$(MODEM
COMMON_TELEMETRY_CFLAGS += -DPPRZ_UART=$(UDP_MODEM_PORT_LOWER)
COMMON_TELEMETRY_CFLAGS += -DDOWNLINK_DEVICE=$(UDP_MODEM_PORT_LOWER)
else
+ifneq (,$(findstring usb, $(MODEM_DEV)))
+# via USB
+COMMON_TELEMETRY_CFLAGS += -DUSE_USB_SERIAL
+COMMON_TELEMETRY_CFLAGS += -DPPRZ_UART=usb_serial
+COMMON_TELEMETRY_CFLAGS += -DDOWNLINK_DEVICE=usb_serial
+ifeq ($(ARCH), lpc21)
+COMMON_TELEMETRY_SRCS += $(SRC_ARCH)/usb_ser_hw.c $(SRC_ARCH)/lpcusb/usbhw_lpc.c $(SRC_ARCH)/lpcusb/usbcontrol.c
+COMMON_TELEMETRY_SRCS += $(SRC_ARCH)/lpcusb/usbstdreq.c $(SRC_ARCH)/lpcusb/usbinit.c
+else
+ifeq ($(ARCH), stm32)
+COMMON_TELEMETRY_SRCS += $(SRC_ARCH)/usb_ser_hw.c
+else
+ifneq ($(ARCH), sim)
+$(error telemetry_transparent_usb currently only implemented for the lpc21 and stm32)
+endif
+endif
+endif
+else
# via UART
#ifeq ($(MODEM_PORT),)
#$(error MODEM_PORT not defined)
@@ -99,6 +117,7 @@ COMMON_TELEMETRY_SRCS += $(SRC_ARCH)/mcu_periph/uart_arch.c
ifeq ($(ARCH), linux)
COMMON_TELEMETRY_SRCS += $(SRC_ARCH)/serial_port.c
endif
+endif
endif #UART
#COMMON_TEST_SRCS += math/pprz_trig_int.c
diff --git a/conf/flight_plans/mission_fw.xml b/conf/flight_plans/mission_fw.xml
index 5c53485f747..e7dc6195724 100644
--- a/conf/flight_plans/mission_fw.xml
+++ b/conf/flight_plans/mission_fw.xml
@@ -10,6 +10,9 @@
+
+
+
diff --git a/conf/joystick/n64_gamepad.xml b/conf/joystick/n64_gamepad.xml
new file mode 100644
index 00000000000..082045c46e9
--- /dev/null
+++ b/conf/joystick/n64_gamepad.xml
@@ -0,0 +1,163 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/nes_gamepad.xml b/conf/joystick/nes_gamepad.xml
new file mode 100644
index 00000000000..61f21e55437
--- /dev/null
+++ b/conf/joystick/nes_gamepad.xml
@@ -0,0 +1,121 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/joystick/snes_gamepad.xml b/conf/joystick/snes_gamepad.xml
new file mode 100644
index 00000000000..18733adc3ae
--- /dev/null
+++ b/conf/joystick/snes_gamepad.xml
@@ -0,0 +1,129 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/ahrs_chimu_spi.xml b/conf/modules/ahrs_chimu_spi.xml
index 50277cc1600..f420b5d478d 100644
--- a/conf/modules/ahrs_chimu_spi.xml
+++ b/conf/modules/ahrs_chimu_spi.xml
@@ -5,6 +5,7 @@
CHimu (SPI)
+
diff --git a/conf/modules/ahrs_float_cmpl_quat.xml b/conf/modules/ahrs_float_cmpl_quat.xml
new file mode 100644
index 00000000000..82101958780
--- /dev/null
+++ b/conf/modules/ahrs_float_cmpl_quat.xml
@@ -0,0 +1,77 @@
+
+
+
+
+
+ AHRS using complementary filter in floating point.
+ Propagation is done in quaternion representation.
+ Estimates the gyro bias.
+ By default uses magnetometer for heading for rotorcrafts.
+ For fixedwing firmware AHRS_GRAVITY_UPDATE_COORDINATED_TURN is enabled by default.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ifdef SECONDARY_AHRS
+ifneq (,$(findstring $(SECONDARY_AHRS), fcq float_cmpl_quat))
+# this is the secondary AHRS
+$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
+$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_fc
+else
+# this is the primary AHRS
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
+$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_fc
+endif
+else
+# plain old single AHRS usage
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
+endif
+
+
+
diff --git a/conf/modules/ahrs_float_cmpl_rmat.xml b/conf/modules/ahrs_float_cmpl_rmat.xml
new file mode 100644
index 00000000000..b09200799e0
--- /dev/null
+++ b/conf/modules/ahrs_float_cmpl_rmat.xml
@@ -0,0 +1,73 @@
+
+
+
+
+
+ AHRS using complementary filter in floating point.
+ Propagation is done in rotation matrix representation.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ifdef SECONDARY_AHRS
+ifneq (,$(findstring $(SECONDARY_AHRS), fcr float_cmpl_rmat))
+# this is the secondary AHRS
+$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
+$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_fc
+else
+# this is the primary AHRS
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
+$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_fc
+endif
+else
+# plain old single AHRS usage
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_wrapper.h\"
+endif
+
+
+
diff --git a/conf/modules/ahrs_float_dcm.xml b/conf/modules/ahrs_float_dcm.xml
new file mode 100644
index 00000000000..d6a5a2ec9de
--- /dev/null
+++ b/conf/modules/ahrs_float_dcm.xml
@@ -0,0 +1,67 @@
+
+
+
+
+
+ AHRS using DCM filter.
+ No direct gyro bias estimation, but also compensates for attitude drift.
+ Uses GPS speed for heading.
+ Compensation of centrifugal force via GPS speed (to fly in circles with a fixedwing).
+ Careful, it doesn't handle all BODY_TO_IMU rotations (mounting positions) correctly!
+
+ The algorithm was developed by William Premerlani and Paul Bizard.
+ The algorithm is also used in the AHRS systems of the AdruIMU.
+ The name DCM for the algorithm is really a misnomer, as that just means that the orientation is represented as a DirectionCosineMatrix (rotation matrix).
+ But since people already know it under that name, we kept it.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ifdef SECONDARY_AHRS
+ifneq (,$(findstring $(SECONDARY_AHRS), dcm float_dcm))
+# this is the secondary AHRS
+$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm_wrapper.h\"
+$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_dcm
+else
+# this is the primary AHRS
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm_wrapper.h\"
+$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_dcm
+endif
+else
+# plain old single AHRS usage
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm_wrapper.h\"
+endif
+
+
+
diff --git a/conf/modules/ahrs_float_invariant.xml b/conf/modules/ahrs_float_invariant.xml
new file mode 100644
index 00000000000..353ae4fb2f4
--- /dev/null
+++ b/conf/modules/ahrs_float_invariant.xml
@@ -0,0 +1,65 @@
+
+
+
+
+
+ AHRS using invariant filter.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ifdef SECONDARY_AHRS
+ifneq (,$(findstring $(SECONDARY_AHRS), invariant float_invariant))
+# this is the secondary AHRS
+$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_invariant_wrapper.h\"
+$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_float_invariant
+else
+# this is the primary AHRS
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_invariant_wrapper.h\"
+$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_float_invariant
+endif
+else
+# plain old single AHRS usage
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_invariant_wrapper.h\"
+endif
+
+
+
diff --git a/conf/modules/ahrs_float_mlkf.xml b/conf/modules/ahrs_float_mlkf.xml
new file mode 100644
index 00000000000..273621d11a6
--- /dev/null
+++ b/conf/modules/ahrs_float_mlkf.xml
@@ -0,0 +1,63 @@
+
+
+
+
+
+ AHRS using MLKF filter.
+ Multiplicative Linearized Kalman Filter in quaternion formulation.
+ Estimates the gyro bias and needs magnetometer to update all 3 axes.
+ Not suitable for fixedwings!
+ Suitable for rotorcraft. The magnetometer is used and needs to be well calibrated. Estimates attitude and heading. Does not use GPS.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ifdef SECONDARY_AHRS
+ifneq (,$(findstring $(SECONDARY_AHRS), mlkf float_mlkf))
+# this is the secondary AHRS
+$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
+$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_mlkf
+else
+# this is the primary AHRS
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
+$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_mlkf
+endif
+else
+# plain old single AHRS usage
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_mlkf_wrapper.h\"
+endif
+
+
+
diff --git a/conf/modules/ahrs_gx3.xml b/conf/modules/ahrs_gx3.xml
new file mode 100644
index 00000000000..1b9d47e90eb
--- /dev/null
+++ b/conf/modules/ahrs_gx3.xml
@@ -0,0 +1,29 @@
+
+
+
+
+
+ AHRS driver for GX3.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/ahrs_int_cmpl_euler.xml b/conf/modules/ahrs_int_cmpl_euler.xml
new file mode 100644
index 00000000000..d0e3f285d65
--- /dev/null
+++ b/conf/modules/ahrs_int_cmpl_euler.xml
@@ -0,0 +1,65 @@
+
+
+
+
+
+ AHRS using complementary filter in fixed point.
+ Propagation is done in euler angles representation.
+ Not recommended for fixedwings, as this filter doesn't compensate for centrifugal force when flying turns.
+ Magnetometer is always only used for heading (yaw).
+ Does not handle the accel and mag updates correctly if BODY_TO_IMU is used for more than just adjustment by a few degrees.
+ In general, rather use int_cmpl_quat
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ifdef SECONDARY_AHRS
+ifneq (,$(findstring $(SECONDARY_AHRS), ice int_cmpl_euler))
+# this is the secondary AHRS
+$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
+$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_ice
+else
+# this is the primary AHRS
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
+$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_ice
+endif
+else
+# plain old single AHRS usage
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h\"
+endif
+
+
+
diff --git a/conf/modules/ahrs_int_cmpl_quat.xml b/conf/modules/ahrs_int_cmpl_quat.xml
new file mode 100644
index 00000000000..9d5171c9f06
--- /dev/null
+++ b/conf/modules/ahrs_int_cmpl_quat.xml
@@ -0,0 +1,89 @@
+
+
+
+
+
+ AHRS using complementary filter in fixed point.
+ Propagation is done in quaternion representation.
+
+ By default uses magnetometer for heading for rotorcrafts.
+ For fixedwing firmware AHRS_GRAVITY_UPDATE_COORDINATED_TURN is enabled by default and magnetometer disabled (needs GPS).
+
+ To measure attitude angles, gyrometers measurements are integrated.
+ The result of integration is accurate for short term, but gyro bias is accumulated, which results in long term errors (drift).
+ On the other hand, accelerometers can be used to infer pitch/roll angles utilizing gravity vector measurement, but they suffer from noise due to vibrations.
+ The measurement is then only accurate when averaged over a long term.
+ Also, accelerometers alone are unable to give accurate angles when the vehicle is accelerating.
+ Complementary filter takes advantage of both sensors, using a low-pass filter on accelerometer readings and high pass filter on gyrometers readings, to estimate attitude angles.
+
+ - No danger of gimbal lock, since quaternions are used.
+ - The arithmetic is fixed point and is thus suitable if the processor (on your board) has no FPU.
+ - Estimates the gyro bias.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ifdef SECONDARY_AHRS
+ifneq (,$(findstring $(SECONDARY_AHRS), icq int_cmpl_quat))
+# this is the secondary AHRS
+$(TARGET).CFLAGS += -DAHRS_SECONDARY_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
+$(TARGET).CFLAGS += -DSECONDARY_AHRS=ahrs_icq
+else
+# this is the primary AHRS
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
+$(TARGET).CFLAGS += -DPRIMARY_AHRS=ahrs_icq
+endif
+else
+# plain old single AHRS usage
+$(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat_wrapper.h\"
+endif
+
+
+
diff --git a/conf/modules/ahrs_sim.xml b/conf/modules/ahrs_sim.xml
new file mode 100644
index 00000000000..cf8ce4e474b
--- /dev/null
+++ b/conf/modules/ahrs_sim.xml
@@ -0,0 +1,20 @@
+
+
+
+
+
+ Simple simulation of the AHRS result.
+ Only for the simple fixedwing sim.
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/airspeed_ms45xx_i2c.xml b/conf/modules/airspeed_ms45xx_i2c.xml
index 94bb9bcaeeb..4413565da2e 100644
--- a/conf/modules/airspeed_ms45xx_i2c.xml
+++ b/conf/modules/airspeed_ms45xx_i2c.xml
@@ -24,7 +24,7 @@
-
+
diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml
index 7da3a88438e..d8273e1fbc8 100644
--- a/conf/modules/cv_opticflow.xml
+++ b/conf/modules/cv_opticflow.xml
@@ -10,7 +10,10 @@
-
+
+
+
+
@@ -20,6 +23,8 @@
+
+
@@ -27,6 +32,9 @@
+
+
+
@@ -52,6 +60,8 @@
+
+
@@ -100,6 +110,7 @@
+
diff --git a/conf/modules/gps_ublox.xml b/conf/modules/gps_ublox.xml
index ed7e0a9c521..dba5b8870ca 100644
--- a/conf/modules/gps_ublox.xml
+++ b/conf/modules/gps_ublox.xml
@@ -1,6 +1,6 @@
-
+
U-blox GPS (UART)
diff --git a/conf/modules/gps_ubx_i2c.xml b/conf/modules/gps_ubx_i2c.xml
index 760231411c2..3ab8e9f00a5 100644
--- a/conf/modules/gps_ubx_i2c.xml
+++ b/conf/modules/gps_ubx_i2c.xml
@@ -10,7 +10,7 @@
gps_ublox
-
+
@@ -22,6 +22,7 @@
+
diff --git a/conf/modules/imu_aspirin_v2_common.xml b/conf/modules/imu_aspirin_v2_common.xml
index 6fd88bb6526..3edb2191c02 100644
--- a/conf/modules/imu_aspirin_v2_common.xml
+++ b/conf/modules/imu_aspirin_v2_common.xml
@@ -7,7 +7,6 @@
- Accelerometer/Gyroscope: MPU6000 via SPI
- Magnetometer: HMC5883 as I2C slave of MPU
-
@@ -31,12 +30,9 @@
-
-
-
diff --git a/conf/modules/imu_lisa_s_v1.0.xml b/conf/modules/imu_lisa_s_v1.0.xml
index 7641305b51b..22125737a8b 100644
--- a/conf/modules/imu_lisa_s_v1.0.xml
+++ b/conf/modules/imu_lisa_s_v1.0.xml
@@ -18,8 +18,7 @@
-
-
+
diff --git a/conf/modules/ins_extended.xml b/conf/modules/ins_extended.xml
index 24df9dbb99e..0c4c39d3439 100644
--- a/conf/modules/ins_extended.xml
+++ b/conf/modules/ins_extended.xml
@@ -11,7 +11,6 @@
-
diff --git a/conf/modules/rtos_mon.xml b/conf/modules/rtos_mon.xml
deleted file mode 100644
index 15dd2d27809..00000000000
--- a/conf/modules/rtos_mon.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-
-
-
-
-
- RTOS monitoring tool
-
- Reports:
- - CPU load
- - core and heap free memory
- - number of threads, their load and free stack.
-
- Can be used with ChibiOS implementation
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/sys_mon.xml b/conf/modules/sys_mon.xml
index 88964bba1c1..14e96a3198c 100644
--- a/conf/modules/sys_mon.xml
+++ b/conf/modules/sys_mon.xml
@@ -5,8 +5,9 @@
System monitor.
The sys_mon module gives you some information about the timing of the periodic tasks and a rough estimate of cpu load (averaged over 1 sec).
+For systems with RTOS it also gives you stats about threads and memory usage.
-The SYS_MON message contains the following information (all times are given in microseconds):
+For bare-metal systems, the SYS_MON message contains the following information (all times are given in microseconds):
- @b periodic_time : time between two calls of the modules_periodic_task (averaged over 1s)
- @b periodic_time_min : minimum time between two calls of the modules_periodic_task() during the last second
- @b periodic_time_max : maximum time between two calls of the modules_periodic_task() during the last second
@@ -15,15 +16,22 @@ The SYS_MON message contains the following information (all times are given in m
- @b periodic_cycle_max : maximum time it took to execute the main periodic functions during the last second
- @b event_number : number of times the event loop was called during the last second
- @b cpu_load : rough estimate of cpu load (averaged over 1 sec)
-
+- @b cpu_time : time in seconds since start-up
So your periodic_time should be 1/MODULES_FREQUENCY, which should be the same as 1/PERIODIC_FREQUENCY
The periodic_cycle_max should not be over the periodic_time, otherwise in at least one cycle it took longer to calculate everything and the next one was slightly delayed.
+The sys_mon module has to run at the full main frequency!
+So either don't specify a main_freq parameter for the modules node or set your actual main frequency
-The sys_mon module has to run at the full main frequency!
+For systems with RTOS, RTOS_MON message is sent instead and the following information are shown:
+- @b nb_thread : number of threads running
+- @b cpu_load : rough estimate of cpu load (averaged over 1 sec)
+- @b core_free : Core free memory (bytes)
+- @b heap_free : Heap free memory (bytes)
+- @b cpu_time : time in seconds since start-up
-So either don't specify a main_freq parameter for the modules node or set your actual main frequency
+Again, the sys_mon module has to run at the full main frequency (so the reports are generated at 1 second intervals).
@@ -33,8 +41,23 @@ So either don't specify a main_freq parameter for the modules node or set your a
+
-
+
+ # for ChibiOS arch include rtos_mon.c and rtos_mon_arch.c
+ ifeq ($(ARCH), chibios)
+ $(TARGET).srcs += $(SRC_MODULES)/core/rtos_mon.c
+ $(TARGET).srcs += $(SRC_ARCH)/modules/core/rtos_mon_arch.c
+ else
+ # for all other architecture use existing sys_mon.c
+ $(TARGET).srcs += $(SRC_MODULES)/core/sys_mon.c
+ endif
+
+
+
+
+
+
diff --git a/conf/settings/control/gain_scheduling.xml b/conf/settings/control/gain_scheduling.xml
index a131c39e1ad..afadcc32004 100644
--- a/conf/settings/control/gain_scheduling.xml
+++ b/conf/settings/control/gain_scheduling.xml
@@ -2,15 +2,15 @@
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
diff --git a/conf/settings/estimation/ahrs_float_cmpl.xml b/conf/settings/estimation/ahrs_float_cmpl.xml
deleted file mode 100644
index fdc35edd638..00000000000
--- a/conf/settings/estimation/ahrs_float_cmpl.xml
+++ /dev/null
@@ -1,15 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/settings/estimation/ahrs_float_dcm.xml b/conf/settings/estimation/ahrs_float_dcm.xml
deleted file mode 100644
index e0866debcd8..00000000000
--- a/conf/settings/estimation/ahrs_float_dcm.xml
+++ /dev/null
@@ -1,16 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/settings/estimation/ahrs_float_invariant.xml b/conf/settings/estimation/ahrs_float_invariant.xml
deleted file mode 100644
index b938bf7c6d8..00000000000
--- a/conf/settings/estimation/ahrs_float_invariant.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/settings/estimation/ahrs_float_mlkf.xml b/conf/settings/estimation/ahrs_float_mlkf.xml
deleted file mode 100644
index b90f6a459ff..00000000000
--- a/conf/settings/estimation/ahrs_float_mlkf.xml
+++ /dev/null
@@ -1,13 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/settings/estimation/ahrs_int_cmpl_euler.xml b/conf/settings/estimation/ahrs_int_cmpl_euler.xml
deleted file mode 100644
index ce664913ef4..00000000000
--- a/conf/settings/estimation/ahrs_int_cmpl_euler.xml
+++ /dev/null
@@ -1,11 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/settings/estimation/ahrs_int_cmpl_quat.xml b/conf/settings/estimation/ahrs_int_cmpl_quat.xml
deleted file mode 100644
index 50f8e0066d9..00000000000
--- a/conf/settings/estimation/ahrs_int_cmpl_quat.xml
+++ /dev/null
@@ -1,15 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/settings/rotorcraft_basic.xml b/conf/settings/rotorcraft_basic.xml
index 4a8d3d2ba3f..38b8fd69af7 100644
--- a/conf/settings/rotorcraft_basic.xml
+++ b/conf/settings/rotorcraft_basic.xml
@@ -10,7 +10,7 @@
-
+
diff --git a/conf/settings/rotorcraft_basic_multi.xml b/conf/settings/rotorcraft_basic_multi.xml
index 82a50451007..cbf7f753015 100644
--- a/conf/settings/rotorcraft_basic_multi.xml
+++ b/conf/settings/rotorcraft_basic_multi.xml
@@ -10,7 +10,7 @@
-
+
diff --git a/docker/run.sh b/docker/run.sh
index 22c5949d6d0..4305cada002 100755
--- a/docker/run.sh
+++ b/docker/run.sh
@@ -82,8 +82,15 @@ fi
# try to detect which USB devices to pass to the container automatically
# set DISABLE_USB=1 to turn it off
if [ -z "$DISABLE_USB" ]; then
- # find on OSX doesn't have the -printf option... so use exec echo instead
- USB_OPTS=$(find /dev -maxdepth 1 \( -name "ttyACM?" -or -name "ttyUSB?" \) -exec echo -n "--device={} " \;)
+ # get major/minor docker version without the dot
+ DOCKER_VERSION=$(docker version | grep -m 1 Version: | awk '{print $2}' | cut -f1,2 -d. | tr -d .)
+ # docker should support relative symlinks for --device options since 1.12
+ if [ "$DOCKER_VERSION" -ge 112 ]; then
+ # find on OSX doesn't have the -printf option... so use exec echo instead
+ USB_OPTS=$(find /dev -maxdepth 2 \( -name "ttyACM?" -or -name "ttyUSB?" -or -name "bmp-*" -or -path /dev/paparazzi/* \) -exec echo -n "--device={} " \; 2> /dev/null)
+ else
+ USB_OPTS=$(find /dev -maxdepth 1 \( -name "ttyACM?" -or -name "ttyUSB?" \) -exec echo -n "--device={} " \; 2> /dev/null)
+ fi
if [ -n "$USB_OPTS" ]; then
echo Passing auto-detected USB devices: $USB_OPTS
fi
diff --git a/paparazzi_version b/paparazzi_version
index 6a9592a76db..c6be91b3dd8 100755
--- a/paparazzi_version
+++ b/paparazzi_version
@@ -1,6 +1,6 @@
#!/bin/sh
-DEF_VER=v5.9_devel
+DEF_VER=v5.11_devel
# First try git describe (if running on a git repo),
# then use default version from above (for release tarballs).
diff --git a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c b/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c
index 8606ab52ee3..b69b29e60a3 100644
--- a/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c
+++ b/sw/airborne/arch/chibios/mcu_periph/i2c_arch.c
@@ -69,13 +69,22 @@ static void handle_i2c_thd(struct i2c_periph *p)
struct i2c_transaction *t = p->trans[p->trans_extract_idx];
p->status = I2CStartRequested;
- // submit i2c transaction
- msg_t status = i2cMasterTransmitTimeout(
- (I2CDriver*)p->reg_addr,
- (i2caddr_t)((t->slave_addr)>>1),
- (uint8_t*)t->buf, (size_t)(t->len_w),
- (uint8_t*)t->buf, (size_t)(t->len_r),
- tmo);
+ msg_t status;
+ // submit i2c transaction (R/W or R only depending of len_w)
+ if (t->len_w > 0) {
+ status = i2cMasterTransmitTimeout(
+ (I2CDriver*)p->reg_addr,
+ (i2caddr_t)((t->slave_addr)>>1),
+ (uint8_t*)t->buf, (size_t)(t->len_w),
+ (uint8_t*)t->buf, (size_t)(t->len_r),
+ tmo);
+ } else {
+ status = i2cMasterReceiveTimeout(
+ (I2CDriver*)p->reg_addr,
+ (i2caddr_t)((t->slave_addr)>>1),
+ (uint8_t*)t->buf, (size_t)(t->len_r),
+ tmo);
+ }
chSysLock();
// end of transaction, handle fifo
@@ -94,7 +103,7 @@ static void handle_i2c_thd(struct i2c_periph *p)
break;
case MSG_TIMEOUT:
//if a timeout occurred before operation end
- // mark as failed
+ // mark as failed and restart
t->status = I2CTransFailed;
break;
case MSG_RESET:
diff --git a/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.c b/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.c
index 1576999d6d5..262f358e93e 100644
--- a/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.c
+++ b/sw/airborne/arch/chibios/mcu_periph/sys_time_arch.c
@@ -37,6 +37,8 @@
#include
#include "led.h"
+static MUTEX_DECL(sys_time_mtx);
+
/*
* Sys_tick handler thread
*/
@@ -65,16 +67,22 @@ void sys_time_arch_init(void)
*/
uint32_t get_sys_time_usec(void)
{
- return sys_time.nb_sec * 1000000 +
- usec_of_sys_time_ticks(sys_time.nb_sec_rem) +
- usec_of_sys_time_ticks(chVTGetSystemTime() - sys_time.nb_tick);
+ chMtxLock(&sys_time_mtx);
+ uint32_t t = sys_time.nb_sec * 1000000 +
+ usec_of_sys_time_ticks(sys_time.nb_sec_rem) +
+ usec_of_sys_time_ticks(chVTGetSystemTime() - sys_time.nb_tick);
+ chMtxUnlock(&sys_time_mtx);
+ return t;
}
uint32_t get_sys_time_msec(void)
{
- return sys_time.nb_sec * 1000 +
- msec_of_sys_time_ticks(sys_time.nb_sec_rem) +
- msec_of_sys_time_ticks(chVTGetSystemTime() - sys_time.nb_tick);
+ chMtxLock(&sys_time_mtx);
+ uint32_t t = sys_time.nb_sec * 1000 +
+ msec_of_sys_time_ticks(sys_time.nb_sec_rem) +
+ msec_of_sys_time_ticks(chVTGetSystemTime() - sys_time.nb_tick);
+ chMtxUnlock(&sys_time_mtx);
+ return t;
}
/**
@@ -117,6 +125,7 @@ static __attribute__((noreturn)) void thd_sys_tick(void *arg)
static void sys_tick_handler(void)
{
+ chMtxLock(&sys_time_mtx);
/* current time in sys_ticks */
sys_time.nb_tick = chVTGetSystemTime();
/* max time is 2^32 / CH_CFG_ST_FREQUENCY, i.e. around 10 days at 10kHz */
@@ -141,5 +150,6 @@ static void sys_tick_handler(void)
}
}
}
+ chMtxUnlock(&sys_time_mtx);
}
diff --git a/sw/airborne/arch/chibios/mcu_periph/uart_arch.c b/sw/airborne/arch/chibios/mcu_periph/uart_arch.c
index aeed25137c8..45de78daf79 100644
--- a/sw/airborne/arch/chibios/mcu_periph/uart_arch.c
+++ b/sw/airborne/arch/chibios/mcu_periph/uart_arch.c
@@ -37,8 +37,10 @@
#include "mcu_periph/uart_arch.h"
#include
#include
+#include "mcu_periph/gpio.h"
struct SerialInit {
+ SerialConfig *conf;
semaphore_t *rx_sem;
semaphore_t *tx_sem;
mutex_t *rx_mtx;
@@ -101,14 +103,14 @@ static void handle_uart_tx(struct uart_periph *p)
#define USE_UART1_RX TRUE
#endif
-static const SerialConfig usart1_config = {
+static SerialConfig usart1_config = {
UART1_BAUD, /* BITRATE */
0, /* USART CR1 */
USART_CR2_STOP1_BITS, /* USART CR2 */
0 /* USART CR3 */
};
-static struct SerialInit uart1_init_struct = { NULL, NULL, NULL, NULL };
+static struct SerialInit uart1_init_struct = { NULL, NULL, NULL, NULL, NULL };
// Threads RX and TX
#if USE_UART1_RX
@@ -147,9 +149,19 @@ static THD_WORKING_AREA(wa_thd_uart1_tx, 1024);
void uart1_init(void)
{
uart_periph_init(&uart1);
+
+ // Only set pin if enabled and not statically defined in board file
+#if USE_UART1_TX && defined UART1_GPIO_PORT_TX
+ gpio_setup_pin_af(UART1_GPIO_PORT_TX, UART1_GPIO_TX, UART1_GPIO_AF, TRUE);
+#endif
+#if USE_UART1_RX && defined UART1_GPIO_PORT_RX
+ gpio_setup_pin_af(UART1_GPIO_PORT_RX, UART1_GPIO_RX, UART1_GPIO_AF, FALSE);
+#endif
+
sdStart(&SD1, &usart1_config);
uart1.reg_addr = &SD1;
uart1.init_struct = &uart1_init_struct;
+ uart1_init_struct.conf = &usart1_config;
// Create threads
#if USE_UART1_RX
@@ -183,14 +195,14 @@ void uart1_init(void)
#define USE_UART2_RX TRUE
#endif
-static const SerialConfig usart2_config = {
+static SerialConfig usart2_config = {
UART2_BAUD, /* BITRATE */
0, /* USART CR1 */
USART_CR2_STOP1_BITS, /* USART CR2 */
0 /* USART CR3 */
};
-static struct SerialInit uart2_init_struct = { NULL, NULL, NULL, NULL };
+static struct SerialInit uart2_init_struct = { NULL, NULL, NULL, NULL, NULL };
// Threads RX and TX
#if USE_UART2_RX
@@ -229,9 +241,19 @@ static THD_WORKING_AREA(wa_thd_uart2_tx, 1024);
void uart2_init(void)
{
uart_periph_init(&uart2);
+
+ // Only set pin if enabled and not statically defined in board file
+#if USE_UART2_TX && defined UART2_GPIO_PORT_TX
+ gpio_setup_pin_af(UART2_GPIO_PORT_TX, UART2_GPIO_TX, UART2_GPIO_AF, TRUE);
+#endif
+#if USE_UART2_RX && defined UART2_GPIO_PORT_RX
+ gpio_setup_pin_af(UART2_GPIO_PORT_RX, UART2_GPIO_RX, UART2_GPIO_AF, FALSE);
+#endif
+
sdStart(&SD2, &usart2_config);
uart2.reg_addr = &SD2;
uart2.init_struct = &uart2_init_struct;
+ uart2_init_struct.conf = &usart2_config;
// Create threads
#if USE_UART2_RX
@@ -264,14 +286,14 @@ void uart2_init(void)
#define USE_UART3_RX TRUE
#endif
-static const SerialConfig usart3_config = {
+static SerialConfig usart3_config = {
UART3_BAUD, /* BITRATE */
0, /* USART CR1 */
USART_CR2_STOP1_BITS, /* USART CR2 */
0 /* USART CR3 */
};
-static struct SerialInit uart3_init_struct = { NULL, NULL, NULL, NULL };
+static struct SerialInit uart3_init_struct = { NULL, NULL, NULL, NULL, NULL };
// Threads RX and TX
#if USE_UART3_RX
@@ -310,9 +332,19 @@ static THD_WORKING_AREA(wa_thd_uart3_tx, 1024);
void uart3_init(void)
{
uart_periph_init(&uart3);
+
+ // Only set pin if enabled and not statically defined in board file
+#if USE_UART3_TX && defined UART3_GPIO_PORT_TX
+ gpio_setup_pin_af(UART3_GPIO_PORT_TX, UART3_GPIO_TX, UART3_GPIO_AF, TRUE);
+#endif
+#if USE_UART3_RX && defined UART3_GPIO_PORT_RX
+ gpio_setup_pin_af(UART3_GPIO_PORT_RX, UART3_GPIO_RX, UART3_GPIO_AF, FALSE);
+#endif
+
sdStart(&SD3, &usart3_config);
uart3.reg_addr = &SD3;
uart3.init_struct = &uart3_init_struct;
+ uart3_init_struct.conf = &usart3_config;
// Create threads
#if USE_UART3_RX
@@ -345,14 +377,14 @@ void uart3_init(void)
#define USE_UART4_RX TRUE
#endif
-static const SerialConfig usart4_config = {
+static SerialConfig usart4_config = {
UART4_BAUD, /* BITRATE */
0, /* USART CR1 */
USART_CR2_STOP1_BITS, /* USART CR2 */
0 /* USART CR3 */
};
-static struct SerialInit uart4_init_struct = { NULL, NULL, NULL, NULL };
+static struct SerialInit uart4_init_struct = { NULL, NULL, NULL, NULL, NULL };
// Threads RX and TX
#if USE_UART4_RX
@@ -391,9 +423,19 @@ static THD_WORKING_AREA(wa_thd_uart4_tx, 1024);
void uart4_init(void)
{
uart_periph_init(&uart4);
+
+ // Only set pin if enabled and not statically defined in board file
+#if USE_UART4_TX && defined UART4_GPIO_PORT_TX
+ gpio_setup_pin_af(UART4_GPIO_PORT_TX, UART4_GPIO_TX, UART4_GPIO_AF, TRUE);
+#endif
+#if USE_UART4_RX && defined UART4_GPIO_PORT_RX
+ gpio_setup_pin_af(UART4_GPIO_PORT_RX, UART4_GPIO_RX, UART4_GPIO_AF, FALSE);
+#endif
+
sdStart(&SD4, &usart4_config);
uart4.reg_addr = &SD4;
uart4.init_struct = &uart4_init_struct;
+ uart4_init_struct.conf = &usart4_config;
// Create threads
#if USE_UART4_RX
@@ -426,14 +468,14 @@ void uart4_init(void)
#define USE_UART5_RX TRUE
#endif
-static const SerialConfig usart5_config = {
+static SerialConfig usart5_config = {
UART5_BAUD, /* BITRATE */
0, /* USART CR1 */
USART_CR2_STOP1_BITS, /* USART CR2 */
0 /* USART CR3 */
};
-static struct SerialInit uart5_init_struct = { NULL, NULL, NULL, NULL };
+static struct SerialInit uart5_init_struct = { NULL, NULL, NULL, NULL, NULL };
// Threads RX and TX
#if USE_UART5_RX
@@ -472,9 +514,19 @@ static THD_WORKING_AREA(wa_thd_uart5_tx, 1024);
void uart5_init(void)
{
uart_periph_init(&uart5);
+
+ // Only set pin if enabled and not statically defined in board file
+#if USE_UART5_TX && defined UART5_GPIO_PORT_TX
+ gpio_setup_pin_af(UART5_GPIO_PORT_TX, UART5_GPIO_TX, UART5_GPIO_AF, TRUE);
+#endif
+#if USE_UART5_RX && defined UART5_GPIO_PORT_RX
+ gpio_setup_pin_af(UART5_GPIO_PORT_RX, UART5_GPIO_RX, UART5_GPIO_AF, FALSE);
+#endif
+
sdStart(&SD5, &usart5_config);
uart5.reg_addr = &SD5;
uart5.init_struct = &uart5_init_struct;
+ uart5_init_struct.conf = &usart5_config;
// Create threads
#if USE_UART5_RX
@@ -507,14 +559,14 @@ void uart5_init(void)
#define USE_UART6_RX TRUE
#endif
-static const SerialConfig usart6_config = {
+static SerialConfig usart6_config = {
UART6_BAUD, /* BITRATE */
0, /* USART CR1 */
USART_CR2_STOP1_BITS, /* USART CR2 */
0 /* USART CR3 */
};
-static struct SerialInit uart6_init_struct = { NULL, NULL, NULL, NULL };
+static struct SerialInit uart6_init_struct = { NULL, NULL, NULL, NULL, NULL };
// Threads RX and TX
#if USE_UART6_RX
@@ -553,9 +605,19 @@ static THD_WORKING_AREA(wa_thd_uart6_tx, 1024);
void uart6_init(void)
{
uart_periph_init(&uart6);
+
+ // Only set pin if enabled and not statically defined in board file
+#if USE_UART6_TX && defined UART6_GPIO_PORT_TX
+ gpio_setup_pin_af(UART6_GPIO_PORT_TX, UART6_GPIO_TX, UART6_GPIO_AF, TRUE);
+#endif
+#if USE_UART6_RX && defined UART6_GPIO_PORT_RX
+ gpio_setup_pin_af(UART6_GPIO_PORT_RX, UART6_GPIO_RX, UART6_GPIO_AF, FALSE);
+#endif
+
sdStart(&SD6, &usart6_config);
uart6.reg_addr = &SD6;
uart6.init_struct = &uart6_init_struct;
+ uart6_init_struct.conf = &usart6_config;
// Create threads
#if USE_UART6_RX
@@ -574,6 +636,188 @@ void uart6_init(void)
#endif
+#if USE_UART7
+
+#ifndef UART7_BAUD
+#define UART7_BAUD SERIAL_DEFAULT_BITRATE
+#endif
+
+/* by default enable UART Tx and Rx */
+#ifndef USE_UART7_TX
+#define USE_UART7_TX TRUE
+#endif
+#ifndef USE_UART7_RX
+#define USE_UART7_RX TRUE
+#endif
+
+static SerialConfig usart7_config = {
+ UART7_BAUD, /* BITRATE */
+ 0, /* USART CR1 */
+ USART_CR2_STOP1_BITS, /* USART CR2 */
+ 0 /* USART CR3 */
+};
+
+static struct SerialInit uart7_init_struct = { NULL, NULL, NULL, NULL, NULL };
+
+// Threads RX and TX
+#if USE_UART7_RX
+static MUTEX_DECL(uart7_rx_mtx);
+static SEMAPHORE_DECL(uart7_rx_sem, 0);
+
+static __attribute__((noreturn)) void thd_uart7_rx(void *arg)
+{
+ (void) arg;
+ chRegSetThreadName("uart7_rx");
+
+ while (TRUE) {
+ handle_uart_rx(&uart7);
+ }
+}
+
+static THD_WORKING_AREA(wa_thd_uart7_rx, 1024);
+#endif
+
+#if USE_UART7_TX
+static MUTEX_DECL(uart7_tx_mtx);
+static SEMAPHORE_DECL(uart7_tx_sem, 0);
+
+static __attribute__((noreturn)) void thd_uart7_tx(void *arg)
+{
+ (void) arg;
+ chRegSetThreadName("uart7_tx");
+
+ while (TRUE) {
+ handle_uart_tx(&uart7);
+ }
+}
+static THD_WORKING_AREA(wa_thd_uart7_tx, 1024);
+#endif
+
+void uart7_init(void)
+{
+ uart_periph_init(&uart7);
+
+ // Only set pin if enabled and not statically defined in board file
+#if USE_UART7_TX && defined UART7_GPIO_PORT_TX
+ gpio_setup_pin_af(UART7_GPIO_PORT_TX, UART7_GPIO_TX, UART7_GPIO_AF, TRUE);
+#endif
+#if USE_UART7_RX && defined UART7_GPIO_PORT_RX
+ gpio_setup_pin_af(UART7_GPIO_PORT_RX, UART7_GPIO_RX, UART7_GPIO_AF, FALSE);
+#endif
+
+ sdStart(&SD7, &usart7_config);
+ uart7.reg_addr = &SD7;
+ uart7.init_struct = &uart7_init_struct;
+ uart7_init_struct.conf = &usart7_config;
+
+ // Create threads
+#if USE_UART7_RX
+ uart7_init_struct.rx_mtx = &uart7_rx_mtx;
+ uart7_init_struct.rx_sem = &uart7_rx_sem;
+ chThdCreateStatic(wa_thd_uart7_rx, sizeof(wa_thd_uart7_rx),
+ NORMALPRIO, thd_uart7_rx, NULL);
+#endif
+#if USE_UART7_TX
+ uart7_init_struct.tx_mtx = &uart7_tx_mtx;
+ uart7_init_struct.tx_sem = &uart7_tx_sem;
+ chThdCreateStatic(wa_thd_uart7_tx, sizeof(wa_thd_uart7_tx),
+ NORMALPRIO, thd_uart7_tx, NULL);
+#endif
+}
+
+#endif
+
+#if USE_UART8
+
+#ifndef UART8_BAUD
+#define UART8_BAUD SERIAL_DEFAULT_BITRATE
+#endif
+
+/* by default enable UART Tx and Rx */
+#ifndef USE_UART8_TX
+#define USE_UART8_TX TRUE
+#endif
+#ifndef USE_UART8_RX
+#define USE_UART8_RX TRUE
+#endif
+
+static SerialConfig usart8_config = {
+ UART8_BAUD, /* BITRATE */
+ 0, /* USART CR1 */
+ USART_CR2_STOP1_BITS, /* USART CR2 */
+ 0 /* USART CR3 */
+};
+
+static struct SerialInit uart8_init_struct = { NULL, NULL, NULL, NULL, NULL };
+
+// Threads RX and TX
+#if USE_UART8_RX
+static MUTEX_DECL(uart8_rx_mtx);
+static SEMAPHORE_DECL(uart8_rx_sem, 0);
+
+static __attribute__((noreturn)) void thd_uart8_rx(void *arg)
+{
+ (void) arg;
+ chRegSetThreadName("uart8_rx");
+
+ while (TRUE) {
+ handle_uart_rx(&uart8);
+ }
+}
+
+static THD_WORKING_AREA(wa_thd_uart8_rx, 1024);
+#endif
+
+#if USE_UART8_TX
+static MUTEX_DECL(uart8_tx_mtx);
+static SEMAPHORE_DECL(uart8_tx_sem, 0);
+
+static __attribute__((noreturn)) void thd_uart8_tx(void *arg)
+{
+ (void) arg;
+ chRegSetThreadName("uart8_tx");
+
+ while (TRUE) {
+ handle_uart_tx(&uart8);
+ }
+}
+static THD_WORKING_AREA(wa_thd_uart8_tx, 1024);
+#endif
+
+void uart8_init(void)
+{
+ uart_periph_init(&uart8);
+
+ // Only set pin if enabled and not statically defined in board file
+#if USE_UART8_TX && defined UART8_GPIO_PORT_TX
+ gpio_setup_pin_af(UART8_GPIO_PORT_TX, UART8_GPIO_TX, UART8_GPIO_AF, TRUE);
+#endif
+#if USE_UART8_RX && defined UART8_GPIO_PORT_RX
+ gpio_setup_pin_af(UART8_GPIO_PORT_RX, UART8_GPIO_RX, UART8_GPIO_AF, FALSE);
+#endif
+
+ sdStart(&SD8, &usart8_config);
+ uart8.reg_addr = &SD8;
+ uart8.init_struct = &uart8_init_struct;
+ uart8_init_struct.conf = &usart8_config;
+
+ // Create threads
+#if USE_UART8_RX
+ uart8_init_struct.rx_mtx = &uart8_rx_mtx;
+ uart8_init_struct.rx_sem = &uart8_rx_sem;
+ chThdCreateStatic(wa_thd_uart8_rx, sizeof(wa_thd_uart8_rx),
+ NORMALPRIO, thd_uart8_rx, NULL);
+#endif
+#if USE_UART8_TX
+ uart8_init_struct.tx_mtx = &uart8_tx_mtx;
+ uart8_init_struct.tx_sem = &uart8_tx_sem;
+ chThdCreateStatic(wa_thd_uart8_tx, sizeof(wa_thd_uart8_tx),
+ NORMALPRIO, thd_uart8_tx, NULL);
+#endif
+}
+
+#endif
+
uint8_t uart_getch(struct uart_periph *p)
{
@@ -589,10 +833,18 @@ uint8_t uart_getch(struct uart_periph *p)
}
/**
- * Set baudrate (from the serialConfig)
- * @note Baudrate is set in sdStart, no need for implementation
+ * Set baudrate
*/
-void uart_periph_set_baudrate(struct uart_periph *p __attribute__((unused)), uint32_t baud __attribute__((unused))) {}
+void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud )
+{
+ struct SerialInit *init_struct = (struct SerialInit*)(p->init_struct);
+ SerialConfig *conf = init_struct->conf;
+ // set new baudrate
+ conf->speed = baud;
+ // restart periph
+ sdStop((SerialDriver*)(p->reg_addr));
+ sdStart((SerialDriver*)(p->reg_addr), conf);
+}
/**
* Set mode (not necessary, or can be set by SerialConfig)
@@ -600,10 +852,46 @@ void uart_periph_set_baudrate(struct uart_periph *p __attribute__((unused)), uin
void uart_periph_set_mode(struct uart_periph *p __attribute__((unused)), bool tx_enabled __attribute__((unused)),
bool rx_enabled __attribute__((unused)), bool hw_flow_control __attribute__((unused))) {}
-void uart_periph_set_bits_stop_parity(struct uart_periph *p __attribute__((unused)),
- uint8_t bits __attribute__((unused)), uint8_t stop __attribute__((unused)), uint8_t __attribute__((unused)) parity)
+/**
+ * Set parity and stop bits
+ */
+void uart_periph_set_bits_stop_parity(struct uart_periph *p,
+ uint8_t bits, uint8_t stop, uint8_t parity)
{
- // TBD
+ struct SerialInit *init_struct = (struct SerialInit*)(p->init_struct);
+ SerialConfig *conf = init_struct->conf;
+
+ /* Configure USART parity and data bits */
+ if (parity == UPARITY_EVEN) {
+ conf->cr1 |= USART_CR1_PCE; // set parity control bit
+ conf->cr1 &= ~USART_CR1_PS; // clear parity selection bit
+ if (bits == UBITS_7) {
+ conf->cr1 &= ~USART_CR1_M; // clear word length bit
+ } else { // 8 data bits by default
+ conf->cr1 |= USART_CR1_M; // set word length bit
+ }
+ } else if (parity == UPARITY_ODD) {
+ conf->cr1 |= USART_CR1_PCE; // set parity control bit
+ conf->cr1 |= USART_CR1_PS; // set parity selection bit
+ if (bits == UBITS_7) {
+ conf->cr1 &= ~USART_CR1_M; // clear word length bit
+ } else { // 8 data bits by default
+ conf->cr1 |= USART_CR1_M; // set word length bit
+ }
+ } else { // 8 data bist, NO_PARITY by default
+ conf->cr1 &= ~USART_CR1_PCE; // clear parity control bit
+ conf->cr1 &= ~USART_CR1_M; // clear word length bit
+ }
+ /* Configure USART stop bits */
+ conf->cr2 &= ~USART_CR2_STOP; // clear stop bits
+ if (stop == USTOP_2) {
+ conf-> cr2 |= USART_CR2_STOP2_BITS; // set bits for 2 stops
+ } else { // 1 stop bit by default
+ conf-> cr2 |= USART_CR2_STOP1_BITS; // set bits for 1 stop
+ }
+
+ sdStop((SerialDriver*)(p->reg_addr));
+ sdStart((SerialDriver*)(p->reg_addr), conf);
}
// Check free space and set a positive value for fd if valid
diff --git a/sw/airborne/arch/chibios/modules/core/rtos_mon_arch.c b/sw/airborne/arch/chibios/modules/core/rtos_mon_arch.c
index 5fce7d08720..78894bac171 100644
--- a/sw/airborne/arch/chibios/modules/core/rtos_mon_arch.c
+++ b/sw/airborne/arch/chibios/modules/core/rtos_mon_arch.c
@@ -24,11 +24,8 @@
* ChibiOS implementation
*/
-#include "modules/core/rtos_mon.h"
-#include "modules/core/rtos_mon_arch.h"
-#include "subsystems/datalink/downlink.h"
+#include "modules/core/sys_mon_rtos.h"
#include
-#include
#if !CH_DBG_THREADS_PROFILING
#error CH_DBG_THREADS_PROFILING should be defined to TRUE to use this monitoring tool
@@ -91,11 +88,9 @@ void rtos_mon_periodic_arch(void)
// assume we call the counter once a second
// so the difference in seconds is always one
- // NOTE: not perfectly precise
- // FIXME: add finer resolution than seconds?
+ // NOTE: not perfectly precise, +-5% on average so take it into consideration
rtos_mon.cpu_load = (1 - (float)(idle_counter - last_idle_counter) / CH_CFG_ST_FREQUENCY) * 100;
last_idle_counter = idle_counter;
-
}
static uint16_t get_stack_free(const thread_t *tp)
diff --git a/sw/airborne/arch/chibios/modules/core/rtos_mon_arch.h b/sw/airborne/arch/sim/modules/core/rtos_mon_arch.c
similarity index 52%
rename from sw/airborne/arch/chibios/modules/core/rtos_mon_arch.h
rename to sw/airborne/arch/sim/modules/core/rtos_mon_arch.c
index 88b3e06832a..eff5e5b4078 100644
--- a/sw/airborne/arch/chibios/modules/core/rtos_mon_arch.h
+++ b/sw/airborne/arch/sim/modules/core/rtos_mon_arch.c
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2016 Gautier Hattenberger
+ * Copyright (C) 2016 Michal Podhradsky
*
* This file is part of paparazzi
*
@@ -18,19 +19,32 @@
* .
*/
/**
- * @file "arch/chibios/modules/core/rtos_mon_arch.h"
- * @author Gautier Hattenberger
- * RTOS monitoring tool
- * ChibiOS implementation
+ * @file "arch/sim/modules/core/rtos_mon_arch.c"
+ * @author Michal Podhradsky
+ * System monitoring tool
+ * SIM implementation
*/
-#ifndef RTOS_MON_ARCH_H
-#define RTOS_MON_ARCH_H
+#include "modules/core/sys_mon_rtos.h"
+#include /* atof */
+#include /* printf, fgets */
+#include
-// Init function
-extern void rtos_mon_init_arch(void);
-// Periodic report
-extern void rtos_mon_periodic_arch(void);
+void rtos_mon_init_arch(void) {}
-#endif
+// Ask for CPU usage of the process
+void rtos_mon_periodic_arch(void)
+{
+ char line[20];
+ FILE *cmd = popen("ps -C simsitl -o %CPU", "r");
+
+ char *ret;
+ ret = fgets(line, sizeof(line), cmd);
+ ret = fgets(line, sizeof(line), cmd);
+ if (ret != NULL) {
+ double cpu = atof(ret);
+ rtos_mon.cpu_load = (uint8_t)cpu;
+ }
+ pclose(cmd);
+}
diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c
index a229d74eb2f..b509cd69dcb 100644
--- a/sw/airborne/arch/sim/sim_ap.c
+++ b/sw/airborne/arch/sim/sim_ap.c
@@ -138,7 +138,7 @@ value set_datalink_message(value s)
}
dl_msg_available = true;
- DlCheckAndParse();
+ DlCheckAndParse(&(DOWNLINK_DEVICE).device, &ivy_tp.trans_tx, dl_buffer);
return Val_unit;
}
diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h
index eff41e75202..28ee4499070 100644
--- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.h
+++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.h
@@ -39,6 +39,7 @@
#define B100000 100000
#define B115200 115200
#define B230400 230400
+#define B460800 460800
#define B921600 921600
#define B1500000 1500000
#define B3000000 3000000
diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
index e016973889b..68fcd9250dc 100644
--- a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
+++ b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c
@@ -49,8 +49,13 @@ INFO("Radio-Control now follows PPRZ sign convention: this means you might need
#define ONE_MHZ 1000000
/* Number of low pulses sent to satellite receivers */
+#if USE_DSMX
+#define MASTER_RECEIVER_PULSES 9
+#define SLAVE_RECEIVER_PULSES 10
+#else
#define MASTER_RECEIVER_PULSES 5
#define SLAVE_RECEIVER_PULSES 6
+#endif
#define TIM_TICS_FOR_100us 100
#define MIN_FRAME_SPACE 70 // 7ms
diff --git a/sw/airborne/arch/stm32/usb_ser_hw.c b/sw/airborne/arch/stm32/usb_ser_hw.c
index dfed19ddc97..42654e4232e 100644
--- a/sw/airborne/arch/stm32/usb_ser_hw.c
+++ b/sw/airborne/arch/stm32/usb_ser_hw.c
@@ -36,6 +36,7 @@
#include
#include
#include
+#include
#include "mcu_periph/usb_serial.h"
@@ -564,6 +565,10 @@ void VCOM_init(void)
usbd_register_set_config_callback(my_usbd_dev, cdcacm_set_config);
+ // disable VBUS monitoring
+ OTG_FS_GCCFG = 0;
+ OTG_FS_GCCFG |= OTG_GCCFG_NOVBUSSENS | OTG_GCCFG_PWRDWN;
+
// disconnected by default
usb_connected = false;
diff --git a/sw/airborne/boards/bebop.h b/sw/airborne/boards/bebop.h
index 34314c3a06a..e1be34d71d4 100644
--- a/sw/airborne/boards/bebop.h
+++ b/sw/airborne/boards/bebop.h
@@ -82,6 +82,8 @@
/** uart connected to GPS internally */
#define UART1_DEV /dev/ttyPA1
+/** FTDI cable for stereoboard or external GPS */
+#define UART2_DEV /dev/ttyUSB0
/* Default actuators driver */
#define DEFAULT_ACTUATORS "boards/bebop/actuators.h"
diff --git a/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c b/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
index d111a660fcf..896693a0ba2 100644
--- a/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
+++ b/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
@@ -60,30 +60,30 @@ uint8_t joystick_block;
}
#endif
-void firmware_parse_msg(void)
+void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
- uint8_t msg_id = IdOfPprzMsg(dl_buffer);
+ uint8_t msg_id = IdOfPprzMsg(buf);
/* parse telemetry messages coming from ground station */
switch (msg_id) {
#ifdef NAV
case DL_BLOCK: {
- if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
- nav_goto_block(DL_BLOCK_block_id(dl_buffer));
- SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
+ if (DL_BLOCK_ac_id(buf) != AC_ID) { break; }
+ nav_goto_block(DL_BLOCK_block_id(buf));
+ SEND_NAVIGATION(trans, dev);
}
break;
case DL_MOVE_WP: {
- if (DL_MOVE_WP_ac_id(dl_buffer) != AC_ID) { break; }
- uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
+ if (DL_MOVE_WP_ac_id(buf) != AC_ID) { break; }
+ uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla;
- lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7));
- lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7));
- lla.alt = MOfMM(DL_MOVE_WP_alt(dl_buffer));
+ lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(buf) / 1e7));
+ lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(buf) / 1e7));
+ lla.alt = MOfMM(DL_MOVE_WP_alt(buf));
struct UtmCoor_f utm;
utm.zone = nav_utm_zone0;
utm_of_lla_f(&utm, &lla);
@@ -93,34 +93,34 @@ void firmware_parse_msg(void)
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, &utm.alt, &nav_utm_zone0);
+ pprz_msg_send_WP_MOVED(trans, dev, AC_ID, &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0);
}
break;
#endif /** NAV */
#ifdef WIND_INFO
case DL_WIND_INFO: {
- if (DL_WIND_INFO_ac_id(dl_buffer) != AC_ID) { break; }
- uint8_t flags = DL_WIND_INFO_flags(dl_buffer);
+ if (DL_WIND_INFO_ac_id(buf) != AC_ID) { break; }
+ uint8_t flags = DL_WIND_INFO_flags(buf);
struct FloatVect2 wind = { 0.f, 0.f };
float upwind = 0.f;
if (bit_is_set(flags, 0)) {
- wind.x = DL_WIND_INFO_north(dl_buffer);
- wind.y = DL_WIND_INFO_east(dl_buffer);
+ wind.x = DL_WIND_INFO_north(buf);
+ wind.y = DL_WIND_INFO_east(buf);
stateSetHorizontalWindspeed_f(&wind);
}
if (bit_is_set(flags, 1)) {
- upwind = DL_WIND_INFO_up(dl_buffer);
+ upwind = DL_WIND_INFO_up(buf);
stateSetVerticalWindspeed_f(upwind);
}
#if !USE_AIRSPEED
if (bit_is_set(flags, 2)) {
- stateSetAirspeed_f(DL_WIND_INFO_airspeed(dl_buffer));
+ stateSetAirspeed_f(DL_WIND_INFO_airspeed(buf));
}
#endif
#ifdef WIND_INFO_RET
float airspeed = stateGetAirspeed_f();
- DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &flags, &wind.y, &wind.x, &upwind, &airspeed);
+ pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags, &wind.y, &wind.x, &upwind, &airspeed);
#endif
}
break;
@@ -130,9 +130,9 @@ void firmware_parse_msg(void)
/** Infrared and GPS sensors are replaced by messages on the datalink */
case DL_HITL_INFRARED: {
/** This code simulates infrared.c:ir_update() */
- infrared.roll = DL_HITL_INFRARED_roll(dl_buffer);
- infrared.pitch = DL_HITL_INFRARED_pitch(dl_buffer);
- infrared.top = DL_HITL_INFRARED_top(dl_buffer);
+ infrared.roll = DL_HITL_INFRARED_roll(buf);
+ infrared.pitch = DL_HITL_INFRARED_pitch(buf);
+ infrared.top = DL_HITL_INFRARED_top(buf);
}
break;
@@ -141,10 +141,10 @@ void firmware_parse_msg(void)
if (gps_msg_received) {
gps_nb_ovrn++;
} else {
- ubx_class = DL_HITL_UBX_class(dl_buffer);
- ubx_id = DL_HITL_UBX_id(dl_buffer);
- uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer);
- uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer);
+ ubx_class = DL_HITL_UBX_class(buf);
+ ubx_id = DL_HITL_UBX_id(buf);
+ uint8_t l = DL_HITL_UBX_ubx_payload_length(buf);
+ uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(buf);
memcpy(ubx_msg_buf, ubx_payload, l);
gps_msg_received = true;
}
@@ -154,10 +154,10 @@ void firmware_parse_msg(void)
#if USE_JOYSTICK
case DL_JOYSTICK_RAW: {
- if (DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
- JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer),
- DL_JOYSTICK_RAW_pitch(dl_buffer),
- DL_JOYSTICK_RAW_throttle(dl_buffer));
+ if (DL_JOYSTICK_RAW_ac_id(buf) == AC_ID) {
+ JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(buf),
+ DL_JOYSTICK_RAW_pitch(buf),
+ DL_JOYSTICK_RAW_throttle(buf));
}
}
break;
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index 6f78f5e40c8..557a2bca878 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -401,6 +401,25 @@ void autopilot_periodic(void)
}
+/** AP mode setting handler
+ *
+ * Checks RC status before calling autopilot_set_mode function
+ */
+void autopilot_SetModeHandler(float mode)
+{
+ if (mode == AP_MODE_KILL || mode == AP_MODE_FAILSAFE || mode == AP_MODE_HOME) {
+ // safety modes are always accessible via settings
+ autopilot_set_mode(mode);
+ } else {
+ if (radio_control.status != RC_OK &&
+ (mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
+ // without RC, only nav-like modes are accessible
+ autopilot_set_mode(mode);
+ }
+ }
+ // with RC, other modes can only be changed from the RC
+}
+
void autopilot_set_mode(uint8_t new_autopilot_mode)
{
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h
index 3c7e35c2cf6..3e1d2902481 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.h
@@ -67,6 +67,7 @@ extern void autopilot_init(void);
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_SetModeHandler(float new_autopilot_mode); // handler for dl_setting
extern void autopilot_set_motors_on(bool motors_on);
extern void autopilot_check_in_flight(bool motors_on);
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
index e2d3ebe7ae7..f4a2dd550ec 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
@@ -23,7 +23,7 @@
* @file firmwares/rotorcraft/guidance/guidance_indi.c
*
* A guidance mode based on Incremental Nonlinear Dynamic Inversion
- * Come to ICRA2016 to learn more!
+ * Come to IROS2016 to learn more!
*
*/
@@ -40,10 +40,25 @@
#include "mcu_periph/sys_time.h"
#include "autopilot.h"
#include "stabilization/stabilization_attitude_ref_quat_int.h"
+#include "firmwares/rotorcraft/stabilization.h"
+#include "stdio.h"
float guidance_indi_pos_gain = 0.5;
float guidance_indi_speed_gain = 1.8;
struct FloatVect3 sp_accel = {0.0,0.0,0.0};
+#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
+float thrust_in_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
+
+#ifndef GUIDANCE_INDI_THRUST_DYNAMICS
+#ifndef STABILIZATION_INDI_ACT_DYN_P
+#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS to be able to use indi vertical control"
+#else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
+#define GUIDANCE_INDI_THRUST_DYNAMICS STABILIZATION_INDI_ACT_DYN_P
+#endif
+#endif //GUIDANCE_INDI_THRUST_DYNAMICS
+
+#endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
+
struct FloatVect3 filt_accel_ned;
struct FloatVect3 filt_accel_ned_d;
@@ -57,6 +72,10 @@ float roll_filtdd = 0;
float pitch_filt = 0;
float pitch_filtd = 0;
float pitch_filtdd = 0;
+float thrust_act = 0;
+float thrust_filt = 0;
+float thrust_filtd = 0;
+float thrust_filtdd = 0;
struct FloatMat33 Ga;
struct FloatMat33 Ga_inv;
@@ -66,7 +85,12 @@ float filter_omega = 20.0;
float filter_zeta = 0.65;
struct FloatEulers guidance_euler_cmd;
+float thrust_in;
+/**
+ *
+ * Call upon entering indi guidance
+ */
void guidance_indi_enter(void) {
filt_accelzbody = 0;
filt_accelzbodyd = 0;
@@ -80,71 +104,137 @@ void guidance_indi_enter(void) {
FLOAT_VECT3_ZERO(filt_accel_ned);
FLOAT_VECT3_ZERO(filt_accel_ned_d);
FLOAT_VECT3_ZERO(filt_accel_ned_dd);
+ thrust_in = 0.0;
+ thrust_act = 0;
+ thrust_filt = 0;
+ thrust_filtd = 0;
+ thrust_filtdd = 0;
}
+/**
+ * @param in_flight in flight boolean
+ * @param heading the desired heading [rad]
+ *
+ * main indi guidance function
+ */
void guidance_indi_run(bool in_flight, int32_t heading) {
//filter accel to get rid of noise
+ guidance_indi_filter_accel();
//filter attitude to synchronize with accel
guidance_indi_filter_attitude();
- guidance_indi_filter_accel();
- float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x; //+-ov.y/ (STABILIZATION_ATTITUDE_SP_MAX_THETA)*3.0;
- float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y; //+ ov.x/ (STABILIZATION_ATTITUDE_SP_MAX_PHI)*3.0;
+ //Linear controller to find the acceleration setpoint from position and velocity
+ float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x;
+ float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y;
+ float pos_z_err = POS_FLOAT_OF_BFP(guidance_v_z_ref - stateGetPositionNed_i()->z);
+
+ float speed_sp_x = pos_x_err * guidance_indi_pos_gain;
+ float speed_sp_y = pos_y_err * guidance_indi_pos_gain;
+ float speed_sp_z = pos_z_err * guidance_indi_pos_gain;
+
+ sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain;
+ sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain;
+ sp_accel.z = (speed_sp_z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain;
- float speed_sp_x = pos_x_err*guidance_indi_pos_gain;
- float speed_sp_y = pos_y_err*guidance_indi_pos_gain;
+#ifdef GUIDANCE_INDI_RC_DEBUG
+ //for rc control horizontal, rotate from body axes to NED
+ float psi = stateGetNedToBodyEulers_f()->psi;
+ float rc_x = -(radio_control.values[RADIO_PITCH]/9600.0)*8.0;
+ float rc_y = (radio_control.values[RADIO_ROLL]/9600.0)*8.0;
+ sp_accel.x = cosf(psi) * rc_x - sinf(psi) * rc_y;
+ sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y;
- sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x)*guidance_indi_speed_gain;
- sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y)*guidance_indi_speed_gain;
-// sp_accel.x = (radio_control.values[RADIO_PITCH]/9600.0)*8.0;
-// sp_accel.y = -(radio_control.values[RADIO_ROLL]/9600.0)*8.0;
+ //for rc vertical control
+ sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*5.0/9600.0;
+#endif
- // struct FloatMat33 Ga;
+ //Calculate matrix of partial derivatives
guidance_indi_calcG(&Ga);
+ //Invert this matrix
MAT33_INV(Ga_inv, Ga);
- float altitude_sp = POS_FLOAT_OF_BFP(guidance_v_z_sp);
- float vertical_velocity_sp = guidance_indi_pos_gain*(altitude_sp - stateGetPositionNed_f()->z);
-// float vertical_velocity_rc_euler = -(stabilization_cmd[COMMAND_THRUST]-4500.0)/4500.0*2.0;
- float vertical_velocity_err_euler = vertical_velocity_sp - stateGetSpeedNed_f()->z;
- sp_accel.z = vertical_velocity_err_euler*guidance_indi_speed_gain;
-
- struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned.x, sp_accel.y -filt_accel_ned.y, 0.0};
+ struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned.x, sp_accel.y -filt_accel_ned.y, sp_accel.z -filt_accel_ned.z};
+ //Bound the acceleration error so that the linearization still holds
Bound(a_diff.x, -6.0, 6.0);
Bound(a_diff.y, -6.0, 6.0);
Bound(a_diff.z, -9.0, 9.0);
+ //If the thrust to specific force ratio has been defined, include vertical control
+ //else ignore the vertical acceleration error
+#ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
+ a_diff.z = 0.0;
+#endif
+
+ //Calculate roll,pitch and thrust command
MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
guidance_euler_cmd.phi = roll_filt + euler_cmd.x;
guidance_euler_cmd.theta = pitch_filt + euler_cmd.y;
- guidance_euler_cmd.psi = 0;//stateGetNedToBodyEulers_f()->psi;
+ //zero psi command, because a roll/pitch quat will be constructed later
+ guidance_euler_cmd.psi = 0;
+
+#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
+ guidance_indi_filter_thrust();
+
+ //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
+ thrust_in = thrust_filt + euler_cmd.z*thrust_in_specific_force_gain;
+ Bound(thrust_in, 0, 9600);
+
+#ifdef GUIDANCE_INDI_RC_DEBUG
+ if(radio_control.values[RADIO_THROTTLE]<300) {
+ thrust_in = 0;
+ }
+#endif
- //Bound euler angles to prevent flipping and keep upright
+ //Overwrite the thrust command from guidance_v
+ stabilization_cmd[COMMAND_THRUST] = thrust_in;
+#endif
+
+ //Bound euler angles to prevent flipping
Bound(guidance_euler_cmd.phi, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);
Bound(guidance_euler_cmd.theta, -GUIDANCE_H_MAX_BANK, GUIDANCE_H_MAX_BANK);
- stabilization_attitude_set_setpoint_rp_quat_f(in_flight, heading);
+ //set the quat setpoint with the calculated roll and pitch
+ stabilization_attitude_set_setpoint_rp_quat_f(&guidance_euler_cmd, in_flight, heading);
+}
+
+#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
+/**
+ *
+ * Filter the thrust, such that it corresponds to the filtered measured acceleration
+ */
+void guidance_indi_filter_thrust(void)
+{
+ thrust_act = thrust_act + GUIDANCE_INDI_THRUST_DYNAMICS * (thrust_in - thrust_act);
+ thrust_filt = thrust_filt + thrust_filtd / PERIODIC_FREQUENCY;
+ thrust_filtd = thrust_filtd + thrust_filtdd / PERIODIC_FREQUENCY;
+ thrust_filtdd = -thrust_filtd * 2 * filter_zeta * filter_omega + (thrust_act - thrust_filt) * filter_omega*filter_omega;
}
+#endif
-//low pass the accelerometer measurements with a second order filter to remove noise from vibrations
+/**
+ *
+ * low pass the accelerometer measurements with a second order filter
+ * to remove noise from vibrations
+ */
void guidance_indi_filter_accel(void)
{
VECT3_ADD_SCALED(filt_accel_ned, filt_accel_ned_d, 1.0/PERIODIC_FREQUENCY);
-// filt_accelzbody = filt_accelzbody + filt_accelzbodyd / PERIODIC_FREQUENCY; //also do body z accel
VECT3_ADD_SCALED(filt_accel_ned_d, filt_accel_ned_dd, 1.0/PERIODIC_FREQUENCY);
-// filt_accelzbodyd = filt_accelzbodyd + filt_accelzbodydd / PERIODIC_FREQUENCY; //also do body z accel
filt_accel_ned_dd.x = -filt_accel_ned_d.x * 2 * filter_zeta * filter_omega + (stateGetAccelNed_f()->x - filt_accel_ned.x) * filter_omega*filter_omega;
filt_accel_ned_dd.y = -filt_accel_ned_d.y * 2 * filter_zeta * filter_omega + (stateGetAccelNed_f()->y - filt_accel_ned.y) * filter_omega*filter_omega;
filt_accel_ned_dd.z = -filt_accel_ned_d.z * 2 * filter_zeta * filter_omega + (stateGetAccelNed_f()->z - filt_accel_ned.z) * filter_omega*filter_omega;
-// filt_accelzbodydd= -filt_accelzbodyd * 2 * filter_zeta * filter_omega + (accel_meas_body_f.z - filt_accelzbody) * filter_omega*filter_omega;
}
+/**
+ *
+ * Filter the attitude, such that it corresponds to the filtered measured acceleration
+ */
void guidance_indi_filter_attitude(void)
{
roll_filt = roll_filt + roll_filtd / PERIODIC_FREQUENCY;
@@ -152,13 +242,17 @@ void guidance_indi_filter_attitude(void)
roll_filtd = roll_filtd + roll_filtdd / PERIODIC_FREQUENCY;
pitch_filtd = pitch_filtd + pitch_filtdd / PERIODIC_FREQUENCY;
-// float cospsi = cosf(stateGetNedToBodyEulers_f()->psi);
-// float sinpsi = sinf(stateGetNedToBodyEulers_f()->psi);
roll_filtdd = -roll_filtd * 2 * filter_zeta * filter_omega + (stateGetNedToBodyEulers_f()->phi - roll_filt) * filter_omega*filter_omega;
pitch_filtdd = -pitch_filtd * 2 * filter_zeta * filter_omega + (stateGetNedToBodyEulers_f()->theta - pitch_filt) * filter_omega*filter_omega;
}
+/**
+ * @param Gmat array to write the matrix to [3x3]
+ *
+ * Calculate the matrix of partial derivatives of the roll, pitch and thrust
+ * w.r.t. the NED accelerations
+ */
void guidance_indi_calcG(struct FloatMat33 *Gmat) {
struct FloatEulers *euler = stateGetNedToBodyEulers_f();
@@ -169,9 +263,8 @@ void guidance_indi_calcG(struct FloatMat33 *Gmat) {
float ctheta = cosf(euler->theta);
float spsi = sinf(euler->psi);
float cpsi = cosf(euler->psi);
- float T = -9.81; //minus gravity is a good guesstimate of the thrust force
-// float T = (filt_accelzn-9.81)/(cphi*ctheta); //calculate specific force in body z axis by using the accelerometer
-// float T = filt_accelzbody; //if body acceleration is available, use that!
+ //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
+ float T = -9.81;
RMAT_ELMT(*Gmat, 0, 0) = (cphi*spsi - sphi*cpsi*stheta)*T;
RMAT_ELMT(*Gmat, 1, 0) = (-sphi*spsi*stheta - cpsi*cphi)*T;
@@ -184,10 +277,18 @@ void guidance_indi_calcG(struct FloatMat33 *Gmat) {
RMAT_ELMT(*Gmat, 2, 2) = cphi*ctheta;
}
-void stabilization_attitude_set_setpoint_rp_quat_f(bool in_flight, int32_t heading)
+/**
+ * @param indi_rp_cmd roll/pitch command from indi guidance [rad] (float)
+ * @param in_flight in flight boolean
+ * @param heading the desired heading [rad] in BFP with INT32_ANGLE_FRAC
+ *
+ * function that creates a quaternion from a roll, pitch and yaw setpoint
+ */
+void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, int32_t heading)
{
struct FloatQuat q_rp_cmd;
- float_quat_of_eulers(&q_rp_cmd, &guidance_euler_cmd); //TODO this is a quaternion without yaw! add the desired yaw before you use it!
+ //this is a quaternion without yaw! add the desired yaw before you use it!
+ float_quat_of_eulers(&q_rp_cmd, indi_rp_cmd);
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
index 5eac0f542da..0504dec775a 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
@@ -39,6 +39,9 @@ extern void guidance_indi_run(bool in_flight, int32_t heading);
extern void guidance_indi_filter_attitude(void);
extern void guidance_indi_calcG(struct FloatMat33 *Gmat);
extern void guidance_indi_filter_accel(void);
-extern void stabilization_attitude_set_setpoint_rp_quat_f(bool in_flight, int32_t heading);
+extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, int32_t heading);
+extern void guidance_indi_filter_thrust(void);
+
+extern float guidance_indi_thrust_specific_force_gain;
#endif /* GUIDANCE_INDI_H */
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index a54c4297513..c2dc884f962 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -310,7 +310,10 @@ STATIC_INLINE void failsafe_check(void)
autopilot_mode != AP_MODE_KILL &&
autopilot_mode != AP_MODE_HOME &&
autopilot_mode != AP_MODE_FAILSAFE &&
- autopilot_mode != AP_MODE_NAV) {
+ autopilot_mode != AP_MODE_NAV &&
+ autopilot_mode != AP_MODE_MODULE &&
+ autopilot_mode != AP_MODE_FLIP &&
+ autopilot_mode != AP_MODE_GUIDED) {
autopilot_set_mode(RC_LOST_MODE);
}
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index 3fd8dc24669..cbcc99f575b 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -307,7 +307,7 @@ void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
{
- int32_t dist_to_point;
+ float dist_to_point;
struct Int32Vect2 diff;
struct EnuCoor_i *pos = stateGetPositionEnu_i();
@@ -326,13 +326,14 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t
VECT2_DIFF(diff, *wp, *pos);
}
/* compute distance of estimated/current pos to target wp
- * distance with half metric precision (6.25 cm)
+ * POS_FRAC resolution
+ * convert to float to compute the norm without overflow in 32bit
*/
- INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
- dist_to_point = int32_vect2_norm(&diff);
+ struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
+ dist_to_point = float_vect2_norm(&diff_f);
/* return TRUE if we have arrived */
- if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
+ if (dist_to_point < ARRIVED_AT_WAYPOINT) {
return true;
}
@@ -341,8 +342,8 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t
/* return TRUE if normal line at the end of the segment is crossed */
struct Int32Vect2 from_diff;
VECT2_DIFF(from_diff, *wp, *from);
- INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC / 2);
- return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
+ struct FloatVect2 from_diff_f = {POS_FLOAT_OF_BFP(from_diff.x), POS_FLOAT_OF_BFP(from_diff.y)};
+ return (diff_f.x * from_diff_f.x + diff_f.y * from_diff_f.y < 0);
}
return false;
@@ -351,7 +352,7 @@ bool nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t
bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
{
uint16_t time_at_wp;
- uint32_t dist_to_point;
+ float dist_to_point;
static uint16_t wp_entry_time = 0;
static bool wp_reached = false;
static struct EnuCoor_i wp_last = { 0, 0, 0 };
@@ -361,10 +362,11 @@ bool nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
wp_reached = false;
wp_last = *wp;
}
+
VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
- INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
- dist_to_point = int32_vect2_norm(&diff);
- if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
+ struct FloatVect2 diff_f = {POS_FLOAT_OF_BFP(diff.x), POS_FLOAT_OF_BFP(diff.y)};
+ dist_to_point = float_vect2_norm(&diff_f);
+ if (dist_to_point < ARRIVED_AT_WAYPOINT) {
if (!wp_reached) {
wp_reached = true;
wp_entry_time = autopilot_flight_time;
diff --git a/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c b/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
index 931ab3b1eff..20e3f122d0b 100644
--- a/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
+++ b/sw/airborne/firmwares/rotorcraft/rotorcraft_datalink.c
@@ -36,32 +36,32 @@
#include "firmwares/rotorcraft/autopilot.h"
-void firmware_parse_msg(void)
+void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
- uint8_t msg_id = IdOfPprzMsg(dl_buffer);
+ uint8_t msg_id = IdOfPprzMsg(buf);
/* parse telemetry messages coming from ground station */
switch (msg_id) {
#ifdef USE_NAVIGATION
case DL_BLOCK : {
- if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
- nav_goto_block(DL_BLOCK_block_id(dl_buffer));
+ if (DL_BLOCK_ac_id(buf) != AC_ID) { break; }
+ nav_goto_block(DL_BLOCK_block_id(buf));
}
break;
case DL_MOVE_WP : {
- uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
+ uint8_t ac_id = DL_MOVE_WP_ac_id(buf);
if (ac_id != AC_ID) { break; }
if (stateIsLocalCoordinateValid()) {
- uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
+ uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
struct LlaCoor_i lla;
- lla.lat = DL_MOVE_WP_lat(dl_buffer);
- lla.lon = DL_MOVE_WP_lon(dl_buffer);
+ lla.lat = DL_MOVE_WP_lat(buf);
+ lla.lon = DL_MOVE_WP_lon(buf);
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
- lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl +
+ lla.alt = DL_MOVE_WP_alt(buf) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
waypoint_move_lla(wp_id, &lla);
}
@@ -70,13 +70,13 @@ void firmware_parse_msg(void)
#endif /* USE_NAVIGATION */
case DL_GUIDED_SETPOINT_NED:
- if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
+ if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID) { break; }
- autopilot_guided_update(DL_GUIDED_SETPOINT_NED_flags(dl_buffer),
- DL_GUIDED_SETPOINT_NED_x(dl_buffer),
- DL_GUIDED_SETPOINT_NED_y(dl_buffer),
- DL_GUIDED_SETPOINT_NED_z(dl_buffer),
- DL_GUIDED_SETPOINT_NED_yaw(dl_buffer));
+ autopilot_guided_update(DL_GUIDED_SETPOINT_NED_flags(buf),
+ DL_GUIDED_SETPOINT_NED_x(buf),
+ DL_GUIDED_SETPOINT_NED_y(buf),
+ DL_GUIDED_SETPOINT_NED_z(buf),
+ DL_GUIDED_SETPOINT_NED_yaw(buf));
break;
default:
diff --git a/sw/airborne/firmwares/setup/setup_actuators.c b/sw/airborne/firmwares/setup/setup_actuators.c
index df756b49e83..8f8d6b82c18 100644
--- a/sw/airborne/firmwares/setup/setup_actuators.c
+++ b/sw/airborne/firmwares/setup/setup_actuators.c
@@ -104,12 +104,12 @@ static inline void main_event(void)
#define IdOfMsg(x) (x[1])
-void dl_parse_msg(void)
+void dl_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
- uint8_t msg_id = IdOfMsg(dl_buffer);
+ uint8_t msg_id = IdOfMsg(buf);
if (msg_id == DL_SET_ACTUATOR) {
- uint8_t actuator_no = DL_SET_ACTUATOR_no(dl_buffer);
- uint16_t actuator_value __attribute__((unused)) = DL_SET_ACTUATOR_value(dl_buffer);
+ uint8_t actuator_no = DL_SET_ACTUATOR_no(buf);
+ uint16_t actuator_value __attribute__((unused)) = DL_SET_ACTUATOR_value(buf);
LED_TOGGLE(2);
/* bad hack:
@@ -152,9 +152,9 @@ void dl_parse_msg(void)
//}
}
#ifdef DlSetting
- else if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) {
- uint8_t i = DL_SETTING_index(dl_buffer);
- float val = DL_SETTING_value(dl_buffer);
+ else if (msg_id == DL_SETTING && DL_SETTING_ac_id(buf) == AC_ID) {
+ uint8_t i = DL_SETTING_index(buf);
+ float val = DL_SETTING_value(buf);
DlSetting(i, val);
LED_TOGGLE(2);
@@ -187,8 +187,8 @@ void dl_parse_msg(void)
#endif
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
- } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) {
- uint8_t i = DL_GET_SETTING_index(dl_buffer);
+ } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(buf) == AC_ID) {
+ uint8_t i = DL_GET_SETTING_index(buf);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
diff --git a/sw/airborne/firmwares/wind_tunnel/main.c b/sw/airborne/firmwares/wind_tunnel/main.c
index 7f121c345cf..9338b106b0f 100644
--- a/sw/airborne/firmwares/wind_tunnel/main.c
+++ b/sw/airborne/firmwares/wind_tunnel/main.c
@@ -82,12 +82,12 @@ static inline void main_event_task(void)
#define IdOfMsg(x) (x[1])
-void dl_parse_msg(void)
+void dl_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
LED_TOGGLE(1);
- uint8_t msg_id = IdOfMsg(dl_buffer);
+ uint8_t msg_id = IdOfMsg(buf);
switch (msg_id) {
case DL_PING: {
@@ -96,8 +96,8 @@ void dl_parse_msg(void)
}
case DL_SETTING : {
- uint8_t i = DL_SETTING_index(dl_buffer);
- float var = DL_SETTING_value(dl_buffer);
+ uint8_t i = DL_SETTING_index(buf);
+ float var = DL_SETTING_value(buf);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(&i, &var);
break;
diff --git a/sw/airborne/firmwares/wind_tunnel/main_mb.c b/sw/airborne/firmwares/wind_tunnel/main_mb.c
index ff7122cf1a2..5f6d5801e9f 100644
--- a/sw/airborne/firmwares/wind_tunnel/main_mb.c
+++ b/sw/airborne/firmwares/wind_tunnel/main_mb.c
@@ -79,12 +79,12 @@ static inline void main_event_task(void)
#define IdOfMsg(x) (x[1])
-void dl_parse_msg(void)
+void dl_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
LED_TOGGLE(1);
- uint8_t msg_id = IdOfMsg(dl_buffer);
+ uint8_t msg_id = IdOfMsg(buf);
switch (msg_id) {
case DL_PING: {
@@ -93,8 +93,8 @@ void dl_parse_msg(void)
}
case DL_SETTING : {
- uint8_t i = DL_SETTING_index(dl_buffer);
- float var = DL_SETTING_value(dl_buffer);
+ uint8_t i = DL_SETTING_index(buf);
+ float var = DL_SETTING_value(buf);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(&i, &var);
break;
diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c
index 40bde9e2486..f59d25c5b83 100644
--- a/sw/airborne/math/pprz_geodetic_int.c
+++ b/sw/airborne/math/pprz_geodetic_int.c
@@ -311,6 +311,30 @@ void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaC
ned_of_ecef_point_i(ned, def, &ecef);
}
+/** Convert a point from LLA to local ENU.
+ * @param[out] enu ENU point in meter << #INT32_POS_FRAC
+ * @param[in] def local coordinate system definition
+ * @param[in] lla LLA point in 1e7deg and mm
+ */
+void enu_of_lla_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
+{
+ struct EcefCoor_i ecef;
+ ecef_of_lla_i(&ecef, lla);
+ enu_of_ecef_pos_i(enu, def, &ecef);
+}
+
+/** Convert a point from LLA to local NED.
+ * @param[out] ned NED point in meter << #INT32_POS_FRAC
+ * @param[in] def local coordinate system definition
+ * @param[in] lla LLA point in 1e7deg and mm
+ */
+void ned_of_lla_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
+{
+ struct EcefCoor_i ecef;
+ ecef_of_lla_i(&ecef, lla);
+ ned_of_ecef_pos_i(ned, def, &ecef);
+}
+
void enu_of_lla_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
{
struct EcefCoor_i ecef;
diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h
index 64d4641f098..3c235ce35c7 100644
--- a/sw/airborne/math/pprz_geodetic_int.h
+++ b/sw/airborne/math/pprz_geodetic_int.h
@@ -117,6 +117,8 @@ extern void enu_of_ecef_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, stru
extern void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef);
extern void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla);
extern void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla);
+extern void enu_of_lla_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla);
+extern void ned_of_lla_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla);
extern void enu_of_lla_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla);
extern void ned_of_lla_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla);
extern void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu);
diff --git a/sw/airborne/modules/computer_vision/lib/filters/kalman_filter_vision.c b/sw/airborne/modules/computer_vision/lib/filters/kalman_filter_vision.c
new file mode 100644
index 00000000000..9010cbfadd5
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/lib/filters/kalman_filter_vision.c
@@ -0,0 +1,205 @@
+/*
+ * kalman_filter_vision.c
+ *
+ * Created on: Sep 12, 2016
+ * Author: knmcguire
+ */
+#include "kalman_filter_vision.h"
+#include "math/pprz_algebra_float.h"
+
+//TODO: implement kalman filter for larger states
+//TODO: implement extended kalman filter
+
+
+
+/**
+ * A simple linear 2D kalman filter, computed using floats and matrices. To be used for vision task like optical flow, tracking markers etc.
+ * @param[in] *model The process model for the prediction of the next state (array size of [1 x 4])
+ * @param[in] *measurements Values of the measurements (array size of [1 x 2])
+ * @param[in] *covariance Covariance matrix of previous iteration (array size of [1 x 4])
+ * @param[out] *covariance Updated Covariance matrix (array size of [1 x 4])
+ * @param[in] *state Previous state vector with estimates(array size of [1 x 2])
+ * @param[out] *state Updated state vector with estimates
+ * @param[in] *measurement_noise Expected variance of the noise of the measurements
+ * @param[in] *process_noise Expected variance of the noise of the process model
+ */
+void kalman_filter_linear_2D_float(float *model, float *measurements, float *covariance, float *state
+ , float *process_noise, float *measurement_noise)
+{
+ //......................Preparation kalman filter ..................... //
+
+ // process model (linear)
+ float _G[2][2];
+ MAKE_MATRIX_PTR(G, _G, 2);
+ G[0][0] = model[0];
+ G[0][1] = model[1];
+ G[1][0] = model[2];
+ G[1][1] = model[3];
+
+ // transpose of G
+ float _Gtrans[2][2];
+ MAKE_MATRIX_PTR(Gtrans, _Gtrans, 2);
+ float_mat_copy(Gtrans, G, 2, 2);
+ float_mat_transpose(Gtrans, 2);
+
+ // Observation model (linear)
+ // note: right now both velocity and acceleration are observed
+ float _H[2][2];
+ MAKE_MATRIX_PTR(H, _H, 2);
+ H[0][0] = 1.0f;
+ H[0][1] = 0.0f;
+ H[1][0] = 0.0f;
+ H[1][1] = 1.0f;
+
+ //transpose of H
+ float _Htrans[2][2];
+ MAKE_MATRIX_PTR(Htrans, _Htrans, 2);
+ float_mat_copy(Htrans, H, 2, 2);
+ float_mat_transpose(Htrans, 2);
+
+ //Previous state
+ float _Xprev[2][1];
+ MAKE_MATRIX_PTR(Xprev, _Xprev, 2);
+ Xprev[0][0] = state[0];
+ Xprev[1][0] = state[1]; //state[1];
+
+ //Previous covariance
+ float _Pprevious[2][2];
+ MAKE_MATRIX_PTR(Pprevious, _Pprevious, 2);
+ Pprevious[0][0] = covariance[0];
+ Pprevious[0][1] = covariance[1];
+ Pprevious[1][0] = covariance[2];
+ Pprevious[1][1] = covariance[3];
+
+ //measurements;
+ float _Z[2][1];
+ MAKE_MATRIX_PTR(Z, _Z, 2);
+ Z[0][0] = measurements[0];
+ Z[1][0] = measurements[1];
+
+ //Process noise model
+ float _Q[2][2];
+ MAKE_MATRIX_PTR(Q, _Q, 2);
+ Q[0][0] = process_noise[0];
+ Q[0][1] = 0.0f;
+ Q[1][0] = 0.0f;
+ Q[1][1] = process_noise[1];
+
+ //measurement nosie model
+ float _R[2][2];
+ MAKE_MATRIX_PTR(R, _R, 2);
+ R[0][0] = measurement_noise[0];
+ R[0][1] = 0.0f;
+ R[1][0] = 0.0f;
+ R[1][1] = measurement_noise[1];
+
+ //Variables during kalman computation:
+ float _Xpredict[2][1];
+ MAKE_MATRIX_PTR(Xpredict, _Xpredict, 2);
+ float _Xnext[2][1];
+ MAKE_MATRIX_PTR(Xnext, _Xnext, 2);
+
+ float _Ppredict[2][2];
+ MAKE_MATRIX_PTR(Ppredict, _Ppredict, 2);
+ float _Pnext[2][2];
+ MAKE_MATRIX_PTR(Pnext, _Pnext, 2);
+
+ float _K[2][2];
+ MAKE_MATRIX_PTR(K, _K, 2);
+
+ float _eye[2][2];
+ MAKE_MATRIX_PTR(eye, _eye, 2);
+ eye[0][0] = 1.0f;
+ eye[0][1] = 0.0f;
+ eye[1][0] = 0.0f;
+ eye[1][1] = 1.0f;
+
+ float _temp_mat[2][2];
+ MAKE_MATRIX_PTR(temp_mat, _temp_mat, 2);
+ float _temp_mat2[2][2];
+ MAKE_MATRIX_PTR(temp_mat2, _temp_mat2, 2)
+ float _temp_mat3[2][2];
+ MAKE_MATRIX_PTR(temp_mat3, _temp_mat3, 2)
+
+ float _temp_vec[2][1];
+ MAKE_MATRIX_PTR(temp_vec, _temp_vec, 2);
+ float _temp_vec2[2][1];
+ MAKE_MATRIX_PTR(temp_vec2, _temp_vec2, 2);
+
+
+ //......................KALMAN FILTER ..................... //
+
+
+ // 1. calculate state predict
+
+ //Xpredict = G* Xprev;
+ float_mat_mul(Xpredict, G, Xprev, 2, 2, 1);
+ //......................KALMAN FILTER ..................... //
+
+ // 2. calculate covariance predict
+
+ // Ppredict = G*Pprevious*Gtrans + Q
+ //...Pprevious*Gtrans...
+ float_mat_mul(temp_mat, Pprevious, Gtrans, 2, 2, 2);
+ //G*Pprevious*Gtrans...
+ float_mat_mul(temp_mat2, G, temp_mat, 2, 2, 2);
+ //G*Pprevious*Gtrans+Q
+ float_mat_sum(Ppredict, temp_mat2, Q, 2, 2);
+
+ // 3. Calculate Kalman gain
+
+ // K = Ppredict * Htrans /( H * Ppredict * Htrans + R)
+ // ... Ppredict * Htrans ...
+ float_mat_mul(temp_mat, Ppredict, Htrans, 2, 2, 2);
+ //... H * Predict * Htrans
+ float_mat_mul(temp_mat2, H, temp_mat, 2, 2, 2);
+ //..( H * Ppredict * Htrans + R)
+ float_mat_sum(temp_mat3, temp_mat2, R, 2, 2);
+ //...inv( H * Ppredict * Htrans + R)
+ //TODO: Make a matrix inverse function for more than 2x2 matrix!
+
+ float det_temp2 = 1 / (temp_mat3[0][0] * temp_mat3[1][1] - temp_mat3[0][1] * temp_mat3[1][0]);
+ temp_mat2[0][0] = det_temp2 * (temp_mat3[1][1]);
+ temp_mat2[0][1] = det_temp2 * (-1 * temp_mat3[1][0]);
+ temp_mat2[1][0] = det_temp2 * (-1 * temp_mat3[0][1]);
+ temp_mat2[1][1] = det_temp2 * (temp_mat3[0][0]);
+ // K = Ppredict * Htrans / *inv( H * Ppredict * Htrans + R)
+ float_mat_mul(K, temp_mat, temp_mat2, 2, 2, 2);
+
+ // 4. Update state estimate
+
+ //Xnext = Xpredict + K *(Z - Htrans * Xpredict)
+ // ... Htrans * Xpredict)
+ float_mat_mul(temp_vec, Htrans, Xpredict, 2, 2, 1);
+
+ //... (Z - Htrans * Xpredict)
+ float_mat_diff(temp_vec2, Z, temp_vec, 2, 1);
+
+ // ... K *(Z - Htrans * Xpredict)
+ float_mat_mul(temp_vec, K, temp_vec2, 2, 2, 1);
+
+
+ //Xnext = Xpredict + K *(Z - Htrans * Xpredict)
+ float_mat_sum(Xnext, Xpredict, temp_vec, 2, 1);
+
+ // 5. Update covariance matrix
+
+ // Pnext = (eye(2) - K*H)*P_predict
+ // ...K*H...
+ float_mat_mul(temp_mat, K, H, 2, 2, 2);
+ //(eye(2) - K*H)
+ float_mat_diff(temp_mat2, eye, temp_mat, 2, 2);
+ // Pnext = (eye(2) - K*H)*P_predict
+ float_mat_mul(Pnext, temp_mat2, Ppredict, 2, 2, 2);
+
+
+ //save values for next state
+ covariance[0] = Pnext[0][0];
+ covariance[1] = Pnext[0][1];;
+ covariance[2] = Pnext[1][0];;
+ covariance[3] = Pnext[1][1];;
+
+
+ state[0] = Xnext[0][0];
+ state[1] = Xnext[1][0];
+}
diff --git a/sw/airborne/modules/computer_vision/lib/filters/kalman_filter_vision.h b/sw/airborne/modules/computer_vision/lib/filters/kalman_filter_vision.h
new file mode 100644
index 00000000000..1761f4e2a91
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/lib/filters/kalman_filter_vision.h
@@ -0,0 +1,15 @@
+/*
+ * kalman_filter_vision.h
+ *
+ * Created on: Sep 12, 2016
+ * Author: knmcguire
+ */
+
+#ifndef KALMAN_FILTER_VISION_H_
+#define KALMAN_FILTER_VISION_H_
+
+void kalman_filter_linear_2D_float(float *model, float *measurements, float *covariance, float *state,
+ float *process_noise, float *measurement_noise);
+
+
+#endif /* KALMAN_FILTER_VISION_H_ */
diff --git a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
index 073f8962441..14a878d6637 100644
--- a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
+++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
@@ -30,6 +30,8 @@
#define _INTER_THREAD_DATA_H
#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_int.h"
+
/* The result calculated from the opticflow */
struct opticflow_result_t {
@@ -60,6 +62,8 @@ struct opticflow_result_t {
struct opticflow_state_t {
struct FloatRates rates; ///< Body rates
float agl; ///< height above ground [m]
+ struct FloatQuat imu_to_body_quat; ///< imu to body quaternion
+ struct Int32Vect3 accel_imu_meas; ///< imu acceleration in imu's coordinates
};
#endif
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
index c4fa7d50b3a..67c3a6b7883 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
@@ -44,6 +44,11 @@
#include "size_divergence.h"
#include "linear_flow_fit.h"
+// Kalman filter
+#include "lib/filters/kalman_filter_vision.h"
+#include "subsystems/imu.h"
+
+
// whether to show the flow and corners:
#define OPTICFLOW_SHOW_FLOW 0
#define OPTICFLOW_SHOW_CORNERS 0
@@ -150,11 +155,31 @@ PRINT_CONFIG_VAR(OPTICFLOW_METHOD)
#endif
PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION)
+#ifndef OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X
+#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X 1.0
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X)
+
+#ifndef OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y
+#define OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y 1.0
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y)
+
#ifndef OPTICFLOW_MEDIAN_FILTER
-#define OPTICFLOW_MEDIAN_FILTER TRUE
+#define OPTICFLOW_MEDIAN_FILTER FALSE
#endif
PRINT_CONFIG_VAR(OPTICFLOW_MEDIAN_FILTER)
+#ifndef OPTICFLOW_KALMAN_FILTER
+#define OPTICFLOW_KALMAN_FILTER TRUE
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_KALMAN_FILTER)
+
+#ifndef OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE
+#define OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE 0.01
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE)
+
//Include median filter
#include "filters/median_filter.h"
struct MedianFilterInt vel_x_filt, vel_y_filt;
@@ -191,6 +216,8 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
opticflow->window_size = OPTICFLOW_WINDOW_SIZE;
opticflow->search_distance = OPTICFLOW_SEARCH_DISTANCE;
opticflow->derotation = OPTICFLOW_DEROTATION; //0 = OFF, 1 = ON
+ opticflow->derotation_correction_factor_x = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_X;
+ opticflow->derotation_correction_factor_y = OPTICFLOW_DEROTATION_CORRECTION_FACTOR_Y;
opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
@@ -198,7 +225,8 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC;
opticflow->pyramid_level = OPTICFLOW_PYRAMID_LEVEL;
opticflow->median_filter = OPTICFLOW_MEDIAN_FILTER;
-
+ opticflow->kalman_filter = OPTICFLOW_KALMAN_FILTER;
+ opticflow->kalman_filter_process_noise = OPTICFLOW_KALMAN_FILTER_PROCESS_NOISE;
opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE;
opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD;
@@ -352,14 +380,16 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / OPTICFLOW_FOV_H;*/
if (opticflow->derotation && result->tracked_cnt > 5) {
- diff_flow_x = (state->rates.p + opticflow->prev_rates.p) / 2.0f / result->fps * img->w /
+ diff_flow_x = (state->rates.p) / result->fps * img->w /
OPTICFLOW_FOV_W;// * img->w / OPTICFLOW_FOV_W;
- diff_flow_y = (state->rates.q + opticflow->prev_rates.q) / 2.0f / result->fps * img->h /
+ diff_flow_y = (state->rates.q) / result->fps * img->h /
OPTICFLOW_FOV_H;// * img->h / OPTICFLOW_FOV_H;
}
- result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor;
- result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor;
+ result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor *
+ opticflow->derotation_correction_factor_x;
+ result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor *
+ opticflow->derotation_correction_factor_y;
opticflow->prev_rates = state->rates;
// Velocity calculation
@@ -469,12 +499,12 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *
int16_t der_shift_y = 0;
if (opticflow->derotation) {
- der_shift_x = (int16_t)((edge_hist[previous_frame_nr[0]].rates.p + edge_hist[current_frame_nr].rates.p) / 2.0f /
+ der_shift_x = (int16_t)(edge_hist[current_frame_nr].rates.p /
result->fps *
- (float)img->w / (OPTICFLOW_FOV_W));
- der_shift_y = (int16_t)((edge_hist[previous_frame_nr[1]].rates.q + edge_hist[current_frame_nr].rates.q) / 2.0f /
+ (float)img->w / (OPTICFLOW_FOV_W) * opticflow->derotation_correction_factor_x);
+ der_shift_y = (int16_t)(edge_hist[current_frame_nr].rates.q /
result->fps *
- (float)img->h / (OPTICFLOW_FOV_H));
+ (float)img->h / (OPTICFLOW_FOV_H) * opticflow->derotation_correction_factor_y);
}
// Estimate pixel wise displacement of the edge histograms for x and y direction
@@ -585,6 +615,112 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
calc_edgeflow_tot(opticflow, state, img, result);
}
+ /* Rotate velocities from camera frame coordinates to body coordinates for control
+ * IMPORTANT!!! This frame to body orientation should be the case for the Parrot
+ * ARdrone and Bebop, however this can be different for other quadcopters
+ * ALWAYS double check!
+ */
+ result->vel_body_x = result->vel_y;
+ result->vel_body_y = - result->vel_x;
+
+ // KALMAN filter on velocity
+ float measurement_noise[2] = {result->noise_measurement, 1.0f};
+ static bool reinitialize_kalman = true;
+
+ static uint8_t wait_filter_counter =
+ 0; // When starting up the opticalflow module, or switching between methods, wait for a bit to prevent bias
+
+
+ if (opticflow->kalman_filter == true) {
+ if (opticflow->just_switched_method == true) {
+ wait_filter_counter = 0;
+ reinitialize_kalman = true;
+ }
+
+ if (wait_filter_counter > 50) {
+
+ // Get accelerometer values rotated to body axis
+ // TODO: use acceleration from the state ?
+ struct FloatVect3 accel_imu_f;
+ ACCELS_FLOAT_OF_BFP(accel_imu_f, state->accel_imu_meas);
+ struct FloatVect3 accel_meas_body;
+ float_quat_vmult(&accel_meas_body, &state->imu_to_body_quat, &accel_imu_f);
+
+ float acceleration_measurement[2];
+ acceleration_measurement[0] = accel_meas_body.x;
+ acceleration_measurement[1] = accel_meas_body.y;
+
+ kalman_filter_opticflow_velocity(&result->vel_body_x, &result->vel_body_y, acceleration_measurement, result->fps,
+ measurement_noise, opticflow->kalman_filter_process_noise, reinitialize_kalman);
+ if (reinitialize_kalman) {
+ reinitialize_kalman = false;
+ }
+
+ } else {
+ wait_filter_counter++;
+ }
+ } else {
+ reinitialize_kalman = true;
+ }
+
+}
+
+/**
+ * Filter the velocity with a simple linear kalman filter, together with the accelerometers
+ * @param[in] *velocity_x Velocity in x direction of body fixed coordinates
+ * @param[in] *velocity_y Belocity in y direction of body fixed coordinates
+ * @param[in] *acceleration_measurement Measurements of the accelerometers
+ * @param[in] fps Frames per second
+ * @param[in] *measurement_noise Expected variance of the noise of the measurements
+ * @param[in] *measurement_noise Expected variance of the noise of the model prediction
+ * @param[in] reinitialize_kalman Boolean to reinitialize the kalman filter
+ */
+void kalman_filter_opticflow_velocity(float *velocity_x, float *velocity_y, float *acceleration_measurement, float fps,
+ float *measurement_noise, float kalman_process_noise, bool reinitialize_kalman)
+{
+ // Initialize variables
+ static float covariance_x[4], covariance_y[4], state_estimate_x[2], state_estimate_y[2];
+ float measurements_x[2], measurements_y[2];
+
+ if (reinitialize_kalman) {
+ state_estimate_x[0] = 0.0f;
+ state_estimate_x[1] = 0.0f;
+ covariance_x[0] = 1.0f;
+ covariance_x[1] = 1.0f;
+ covariance_x[2] = 1.0f;
+ covariance_x[3] = 1.0f;
+
+ state_estimate_y[0] = 0.0f;
+ state_estimate_y[1] = 0.0f;
+ covariance_y[0] = 1.0f;
+ covariance_y[1] = 1.0f;
+ covariance_y[2] = 1.0f;
+ covariance_y[3] = 1.0f;
+ }
+
+ /*Model for velocity estimation
+ * state = [ velocity; acceleration]
+ * Velocity_prediction = last_velocity_estimate + acceleration * dt
+ * Acceleration_prediction = last_acceleration
+ * model = Jacobian([vel_prediction; accel_prediction],state)
+ * = [1 dt ; 0 1];
+ * */
+ float model[4] = {1.0f, 1.0f / fps , 0.0f , 1.0f};
+ float process_noise[2] = {kalman_process_noise, kalman_process_noise};
+
+ // Measurements from velocity_x of optical flow and acceleration directly from scaled accelerometers
+ measurements_x[0] = *velocity_x;
+ measurements_x[1] = acceleration_measurement[0];
+
+ measurements_y[0] = *velocity_y;
+ measurements_y[1] = acceleration_measurement[1];
+
+ // 2D linear kalman filter
+ kalman_filter_linear_2D_float(model, measurements_x, covariance_x, state_estimate_x, process_noise, measurement_noise);
+ kalman_filter_linear_2D_float(model, measurements_y, covariance_y, state_estimate_y, process_noise, measurement_noise);
+
+ *velocity_x = state_estimate_x[0];
+ *velocity_y = state_estimate_y[0];
}
/**
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
index e573a1e0ac0..10a68056701 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
@@ -38,32 +38,37 @@
#include "lib/v4l/v4l2.h"
struct opticflow_t {
- bool got_first_img; ///< If we got a image to work with
- bool just_switched_method;
- struct FloatRates prev_rates; ///< Gyro Rates from the previous image frame
- struct image_t img_gray; ///< Current gray image frame
- struct image_t prev_img_gray; ///< Previous gray image frame
- struct timeval prev_timestamp; ///< Timestamp of the previous frame, used for FPS calculation
-
- uint8_t method; ///< Method to use to calculate the optical flow
- uint16_t window_size; ///< Window size for the blockmatching algorithm (general value for all methods)
- uint16_t search_distance; ///< Search distance for blockmatching alg.
- bool derotation; ///< Derotation switched on or off (depended on the quality of the gyroscope measurement)
- bool median_filter; ///< Decides to use a median filter on the velocity
-
- uint16_t subpixel_factor; ///< The amount of subpixels per pixel
- uint8_t max_iterations; ///< The maximum amount of iterations the Lucas Kanade algorithm should do
- uint8_t threshold_vec; ///< The threshold in x, y subpixels which the algorithm should stop
- uint8_t pyramid_level; ///< Number of pyramid levels used in Lucas Kanade algorithm (0 == no pyramids used)
-
- uint8_t max_track_corners; ///< Maximum amount of corners Lucas Kanade should track
- bool fast9_adaptive; ///< Whether the FAST9 threshold should be adaptive
- uint8_t fast9_threshold; ///< FAST9 corner detection threshold
- uint16_t fast9_min_distance; ///< Minimum distance in pixels between corners
- uint16_t fast9_padding; ///< Padding used in FAST9 detector
-
- uint16_t fast9_rsize; ///< Amount of corners allocated
- struct point_t *fast9_ret_corners; ///< Corners
+ bool got_first_img; ///< If we got a image to work with
+ bool just_switched_method; ///< Boolean to check if methods has been switched (for reinitialization)
+ struct FloatRates prev_rates; ///< Gyro Rates from the previous image frame
+ struct image_t img_gray; ///< Current gray image frame
+ struct image_t prev_img_gray; ///< Previous gray image frame
+ struct timeval prev_timestamp; ///< Timestamp of the previous frame, used for FPS calculation
+
+ uint8_t method; ///< Method to use to calculate the optical flow
+ uint16_t window_size; ///< Window size for the blockmatching algorithm (general value for all methods)
+ uint16_t search_distance; ///< Search distance for blockmatching alg.
+ bool derotation; ///< Derotation switched on or off (depended on the quality of the gyroscope measurement)
+ bool median_filter; ///< Decides to use a median filter on the velocity
+ bool kalman_filter; ///< Decide to use Kalman filter to filter velocity with accelerometers
+ float kalman_filter_process_noise; ///< Process noise of the model used in the kalman filter
+
+ float derotation_correction_factor_x; ///< Correction factor for derotation in x axis, determined from a fit from the gyros and flow rotation. (wrong FOV, camera not in center)
+ float derotation_correction_factor_y; ///< Correction factor for derotation in Y axis, determined from a fit from the gyros and flow rotation. (wrong FOV, camera not in center)
+
+ uint16_t subpixel_factor; ///< The amount of subpixels per pixel
+ uint8_t max_iterations; ///< The maximum amount of iterations the Lucas Kanade algorithm should do
+ uint8_t threshold_vec; ///< The threshold in x, y subpixels which the algorithm should stop
+ uint8_t pyramid_level; ///< Number of pyramid levels used in Lucas Kanade algorithm (0 == no pyramids used)
+
+ uint8_t max_track_corners; ///< Maximum amount of corners Lucas Kanade should track
+ bool fast9_adaptive; ///< Whether the FAST9 threshold should be adaptive
+ uint8_t fast9_threshold; ///< FAST9 corner detection threshold
+ uint16_t fast9_min_distance; ///< Minimum distance in pixels between corners
+ uint16_t fast9_padding; ///< Padding used in FAST9 detector
+
+ uint16_t fast9_rsize; ///< Amount of corners allocated
+ struct point_t *fast9_ret_corners; ///< Corners
};
@@ -75,6 +80,10 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta
struct opticflow_result_t *result);
void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result);
+
+void kalman_filter_opticflow_velocity(float *velocity_x, float *velocity_y, float *acceleration_measurement, float fps,
+ float *measurement_noise, float process_noise, bool reinitialize_kalman);
+
#endif /* OPTICFLOW_CALCULATOR_H */
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index 0ff463006e0..c279fe67b4b 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/sw/airborne/modules/computer_vision/opticflow_module.c
@@ -40,27 +40,46 @@
#include "cv.h"
-/* Default sonar/agl to use in opticflow visual_estimator */
+/* ABI messages sender ID */
#ifndef OPTICFLOW_AGL_ID
#define OPTICFLOW_AGL_ID ABI_BROADCAST ///< Default sonar/agl to use in opticflow visual_estimator
#endif
PRINT_CONFIG_VAR(OPTICFLOW_AGL_ID)
-#ifndef OPTICFLOW_SENDER_ID
-#define OPTICFLOW_SENDER_ID 1
+#ifndef OPTICFLOW_IMU_ID
+#define OPTICFLOW_IMU_ID ABI_BROADCAST ///< Default IMU (accelerometers) to use in opticflow visual_estimator
#endif
+PRINT_CONFIG_VAR(OPTICFLOW_IMU_ID)
+
+#ifndef OPTICFLOW_BODY_TO_IMU_ID
+#define OPTICFLOW_BODY_TO_IMU_ID ABI_BROADCAST ///< Default body to IMU to use in opticflow visual_estimator
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_BODY_TO_IMU_ID)
+
+#ifndef OPTICFLOW_SEND_ABI_ID
+#define OPTICFLOW_SEND_ABI_ID 1 ///< Default ID to send abi messages
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_SEND_ABI_ID)
+
/* The main opticflow variables */
struct opticflow_t opticflow; ///< Opticflow calculations
static struct opticflow_result_t opticflow_result; ///< The opticflow result
static struct opticflow_state_t opticflow_state; ///< State of the drone to communicate with the opticflow
-static abi_event opticflow_agl_ev; ///< The altitude ABI event
+static abi_event opticflow_imu_accel_ev; ///< The altitude ABI event
+static abi_event opticflow_agl_ev; ///< The accelerometers ABI event
+static abi_event opticflow_body_to_imu_ev; ///< The body-to-imu ABI event
+
static bool opticflow_got_result; ///< When we have an optical flow calculation
static pthread_mutex_t opticflow_mutex; ///< Mutex lock fo thread safety
/* Static functions */
struct image_t *opticflow_module_calc(struct image_t *img); ///< The main optical flow calculation thread
static void opticflow_agl_cb(uint8_t sender_id, float distance); ///< Callback function of the ground altitude
+static void opticflow_imu_accel_cb(uint8_t sender_id, uint32_t stamp,
+ struct Int32Vect3 *accel); ///< Callback function of the IMU's accelerometers
+static void opticflow_body_to_imu_cb(uint8_t sender_id,
+ struct FloatQuat *q_b2i_f); ///< Callback function of imu to body
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -90,11 +109,17 @@ static void opticflow_telem_send(struct transport_tx *trans, struct link_device
*/
void opticflow_module_init(void)
{
- // Subscribe to the altitude above ground level ABI messages
- AbiBindMsgAGL(OPTICFLOW_AGL_ID, &opticflow_agl_ev, opticflow_agl_cb);
+ // Subscribe ABI messages
+ AbiBindMsgAGL(OPTICFLOW_AGL_ID, &opticflow_agl_ev, opticflow_agl_cb); // ABI to the altitude above ground level
+ AbiBindMsgIMU_ACCEL_INT32(OPTICFLOW_IMU_ID, &opticflow_imu_accel_ev,
+ &opticflow_imu_accel_cb); // ABI to the IMU accelerometer measurements
+ AbiBindMsgBODY_TO_IMU_QUAT(OPTICFLOW_BODY_TO_IMU_ID, &opticflow_body_to_imu_ev,
+ &opticflow_body_to_imu_cb); // ABI to the quaternion of body to imu
// Set the opticflow state to 0
FLOAT_RATES_ZERO(opticflow_state.rates);
+ float_quat_identity(&opticflow_state.imu_to_body_quat);
+ INT_VECT3_ZERO(opticflow_state.accel_imu_meas);
opticflow_state.agl = 0;
// Initialize the opticflow calculation
@@ -118,7 +143,7 @@ void opticflow_module_run(void)
// Update the stabilization loops on the current calculation
if (opticflow_got_result) {
uint32_t now_ts = get_sys_time_usec();
- AbiSendMsgOPTICAL_FLOW(OPTICFLOW_SENDER_ID, now_ts,
+ AbiSendMsgOPTICAL_FLOW(OPTICFLOW_SEND_ABI_ID, now_ts,
opticflow_result.flow_x,
opticflow_result.flow_y,
opticflow_result.flow_der_x,
@@ -128,7 +153,7 @@ void opticflow_module_run(void)
opticflow_state.agl);
//TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt
if (opticflow_result.noise_measurement < 0.8) {
- AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
+ AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SEND_ABI_ID, now_ts,
opticflow_result.vel_body_x,
opticflow_result.vel_body_y,
0.0f,
@@ -150,9 +175,10 @@ void opticflow_module_run(void)
struct image_t *opticflow_module_calc(struct image_t *img)
{
// Copy the state
- struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts);
+ // TODO : put accelerometer values at pose of img timestamp
struct opticflow_state_t temp_state;
- temp_state.agl = opticflow_state.agl;
+ struct pose_t pose = get_rotation_at_timestamp(img->pprz_ts);
+ temp_state = opticflow_state;
temp_state.rates = pose.rates;
// Do the optical flow calculation
@@ -161,16 +187,9 @@ struct image_t *opticflow_module_calc(struct image_t *img)
// Copy the result if finished
pthread_mutex_lock(&opticflow_mutex);
- memcpy(&opticflow_result, &temp_result, sizeof(struct opticflow_result_t));
+ opticflow_result = temp_result;
opticflow_got_result = true;
- /* Rotate velocities from camera frame coordinates to body coordinates for control
- * IMPORTANT!!! This frame to body orientation should be the case for the Parrot
- * ARdrone and Bebop, however this can be different for other quadcopters
- * ALWAYS double check!
- */
- opticflow_result.vel_body_x = opticflow_result.vel_y;
- opticflow_result.vel_body_y = - opticflow_result.vel_x;
// release the mutex as we are done with editing the opticflow result
pthread_mutex_unlock(&opticflow_mutex);
@@ -189,3 +208,27 @@ static void opticflow_agl_cb(uint8_t sender_id __attribute__((unused)), float di
opticflow_state.agl = distance;
}
}
+
+/**
+ * Get the accelerometer measurements of the imu
+ * @param[in] sender_id The id that send the ABI message (unused)
+ * @param[in] stamp The timestamp of when the message is send
+ * @param[in] accel The accelerometer measurements of the imu
+ */
+static void opticflow_imu_accel_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp, struct Int32Vect3 *accel)
+{
+ opticflow_state.accel_imu_meas = *accel;
+}
+
+/**
+ * Get the body-to-imu quaternion
+ * @param[in] sender_id The id that send the ABI message (unused)
+ * @param[in] q_b2i_f The body-to-imu quaternion
+ */
+static void opticflow_body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
+ struct FloatQuat *q_b2i_f)
+{
+ struct FloatQuat imu_to_body_quat_temp;
+ float_quat_invert(&imu_to_body_quat_temp, q_b2i_f); // invert quaternion for body-to-imu to imu-to-body
+ opticflow_state.imu_to_body_quat = imu_to_body_quat_temp;
+}
diff --git a/sw/airborne/modules/core/rtos_mon.c b/sw/airborne/modules/core/rtos_mon.c
index b4516f89129..78ab63474e6 100644
--- a/sw/airborne/modules/core/rtos_mon.c
+++ b/sw/airborne/modules/core/rtos_mon.c
@@ -23,14 +23,13 @@
* RTOS monitoring tool
*/
-#include "modules/core/rtos_mon.h"
-#include "modules/core/rtos_mon_arch.h"
+#include "modules/core/sys_mon.h"
+#include "modules/core/sys_mon_rtos.h"
#include "subsystems/datalink/downlink.h"
-#include
struct rtos_monitoring rtos_mon;
-void rtos_mon_init(void)
+void init_sysmon(void)
{
// zero structure
memset(&rtos_mon, 0, sizeof(struct rtos_monitoring));
@@ -41,18 +40,24 @@ void rtos_mon_init(void)
// Periodic report of RTOS parameters
// This function is actually arch dependent
-void rtos_mon_periodic(void)
+void periodic_report_sysmon(void)
{
// update struct
rtos_mon_periodic_arch();
+ // update cpu time
+ rtos_mon.cpu_time = get_sys_time_float();
+
// send report
DOWNLINK_SEND_RTOS_MON(DefaultChannel, DefaultDevice,
&rtos_mon.thread_counter,
&rtos_mon.cpu_load,
&rtos_mon.core_free_memory,
- &rtos_mon.heap_free_memory);
+ &rtos_mon.heap_free_memory,
+ &rtos_mon.cpu_time);
}
+void periodic_sysmon(void) {}
+void event_sysmon(void) {}
diff --git a/sw/airborne/modules/core/sys_mon.c b/sw/airborne/modules/core/sys_mon.c
index cdf68ee1a74..d3b70c44196 100644
--- a/sw/airborne/modules/core/sys_mon.c
+++ b/sw/airborne/modules/core/sys_mon.c
@@ -19,10 +19,20 @@
* Boston, MA 02111-1307, USA.
*
*/
+/** \file sys_mon.c
+ *
+ * System monitoring for bare metal targets
+ * return cpu load, average exec time, ...
+ */
#include "core/sys_mon.h"
+#include "core/sys_mon_bare_metal.h"
#include "mcu_periph/sys_time.h"
+#include "mcu_periph/uart.h"
+#include "pprzlink/messages.h"
+#include "subsystems/datalink/downlink.h"
+
/** Global system monitor data (averaged over 1 sec) */
struct SysMon sys_mon;
@@ -48,6 +58,7 @@ void init_sysmon(void)
sys_mon.periodic_cycle_min = 0xFFFF;
sys_mon.periodic_cycle_max = 0;
sys_mon.event_number = 0;
+ sys_mon.cpu_time = 0;
n_periodic = 0;
n_event = 0;
@@ -59,10 +70,6 @@ void init_sysmon(void)
periodic_timer = 0;
}
-#include "mcu_periph/uart.h"
-#include "pprzlink/messages.h"
-#include "subsystems/datalink/downlink.h"
-
void periodic_report_sysmon(void)
{
/** Report system status at low frequency */
@@ -71,12 +78,13 @@ void periodic_report_sysmon(void)
sys_mon.periodic_cycle = sum_cycle_periodic / n_periodic;
sys_mon.cpu_load = 100 * sys_mon.periodic_cycle / sys_mon.periodic_time;
sys_mon.event_number = sum_n_event / n_periodic;
+ sys_mon.cpu_time = get_sys_time_float();
DOWNLINK_SEND_SYS_MON(DefaultChannel, DefaultDevice, &sys_mon.periodic_time,
&sys_mon.periodic_time_min, &sys_mon.periodic_time_max,
&sys_mon.periodic_cycle, &sys_mon.periodic_cycle_min,
&sys_mon.periodic_cycle_max, &sys_mon.event_number,
- &sys_mon.cpu_load);
+ &sys_mon.cpu_load, &sys_mon.cpu_time);
}
n_periodic = 0;
diff --git a/sw/airborne/modules/core/sys_mon.h b/sw/airborne/modules/core/sys_mon.h
index aec9286d7b0..750ca87e049 100644
--- a/sw/airborne/modules/core/sys_mon.h
+++ b/sw/airborne/modules/core/sys_mon.h
@@ -22,7 +22,7 @@
/** \file sys_mon.h
*
- * System monitoring
+ * System monitoring common header
* return cpu load, average exec time, ...
*/
@@ -31,19 +31,6 @@
#include "std.h"
-struct SysMon {
- uint8_t cpu_load;
- uint16_t periodic_time; ///< in usec
- uint16_t periodic_time_min; ///< in usec
- uint16_t periodic_time_max; ///< in usec
- uint16_t periodic_cycle; ///< in usec
- uint16_t periodic_cycle_min; ///< in usec
- uint16_t periodic_cycle_max; ///< in usec
- uint16_t event_number;
-};
-
-extern struct SysMon sys_mon;
-
/** Init system monitoring
*/
void init_sysmon(void);
diff --git a/sw/airborne/modules/core/sys_mon_bare_metal.h b/sw/airborne/modules/core/sys_mon_bare_metal.h
new file mode 100644
index 00000000000..858fba8e3b1
--- /dev/null
+++ b/sw/airborne/modules/core/sys_mon_bare_metal.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2016 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file sys_mon.h
+ *
+ * System monitoring
+ * return cpu load, average exec time, ...
+ */
+
+#ifndef SYS_MON_BARE_METAL_H
+#define SYS_MON_BARE_METAL_H
+
+#include "core/sys_mon.h"
+
+struct SysMon {
+ uint8_t cpu_load;
+ uint16_t periodic_time; ///< in usec
+ uint16_t periodic_time_min; ///< in usec
+ uint16_t periodic_time_max; ///< in usec
+ uint16_t periodic_cycle; ///< in usec
+ uint16_t periodic_cycle_min; ///< in usec
+ uint16_t periodic_cycle_max; ///< in usec
+ uint16_t event_number;
+ float cpu_time; // in secs since startup
+};
+
+extern struct SysMon sys_mon;
+
+#endif /* SYS_MON_BARE_METAL_H */
diff --git a/sw/airborne/modules/core/rtos_mon.h b/sw/airborne/modules/core/sys_mon_rtos.h
similarity index 77%
rename from sw/airborne/modules/core/rtos_mon.h
rename to sw/airborne/modules/core/sys_mon_rtos.h
index b85f01e96de..20a404b80aa 100644
--- a/sw/airborne/modules/core/rtos_mon.h
+++ b/sw/airborne/modules/core/sys_mon_rtos.h
@@ -1,7 +1,7 @@
/*
* Copyright (C) 2016 Gautier Hattenberger
*
- * This file is part of paparazzi
+ * 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
@@ -14,19 +14,22 @@
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, see
- * .
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
*/
-/**
- * @file "modules/core/rtos_mon.h"
- * @author Gautier Hattenberger
- * RTOS monitoring tool
+
+/** \file sys_mon_rtos.h
+ *
+ * System monitoring for RTOS targets
+ * return cpu load, average exec time, ...
*/
-#ifndef RTOS_MON_H
-#define RTOS_MON_H
+#ifndef SYS_MON_RTOS_H
+#define SYS_MON_RTOS_H
-#include "std.h"
+#include "core/sys_mon.h"
// Maximum number of threads
// The limit is related to the max size of the report message
@@ -49,14 +52,14 @@ struct rtos_monitoring {
uint16_t thread_free_stack[RTOS_MON_MAX_THREADS]; ///< individual thread free stack in bytes
char thread_names[RTOS_MON_THREAD_NAMES+1]; ///< string of thread names / identifiers
uint8_t thread_name_idx; ///< length of the string in thread_names buffer
+ float cpu_time; // in secs since startup
};
extern struct rtos_monitoring rtos_mon;
// Init function
-extern void rtos_mon_init(void);
+extern void rtos_mon_init_arch(void);
// Periodic report
-extern void rtos_mon_periodic(void);
-
-#endif
+extern void rtos_mon_periodic_arch(void);
+#endif /* SYS_MON_RTOS_H */
diff --git a/sw/airborne/modules/datalink/extra_pprz_dl.h b/sw/airborne/modules/datalink/extra_pprz_dl.h
index 40c92dd3376..3752d81f8f8 100644
--- a/sw/airborne/modules/datalink/extra_pprz_dl.h
+++ b/sw/airborne/modules/datalink/extra_pprz_dl.h
@@ -40,9 +40,9 @@ extern struct pprz_transport extra_pprz_tp;
/* Datalink Event */
-#define ExtraDatalinkEvent() { \
+#define ExtraDatalinkEvent() { \
pprz_check_and_parse(&EXTRA_DOWNLINK_DEVICE.device, &extra_pprz_tp, dl_buffer, &dl_msg_available); \
- DlCheckAndParse(); \
+ DlCheckAndParse(&EXTRA_DOWNLINK_DEVICE.device, &extra_pprz_tp.trans_tx, dl_buffer); \
}
/** Init function */
diff --git a/sw/airborne/modules/gps/gps_ubx_i2c.c b/sw/airborne/modules/gps/gps_ubx_i2c.c
index 00b1d98b978..8698ae59d2a 100644
--- a/sw/airborne/modules/gps/gps_ubx_i2c.c
+++ b/sw/airborne/modules/gps/gps_ubx_i2c.c
@@ -38,7 +38,7 @@
#define GPS_I2C_SLAVE_ADDR (0x42 << 1)
#ifndef GPS_UBX_I2C_DEV
-#define GPS_UBX_I2C_DEV i2c2
+#error "GPS_UBX_I2C_DEV needs to be defined (e.g. to i2c1)"
#endif
PRINT_CONFIG_VAR(GPS_UBX_I2C_DEV)
@@ -95,7 +95,12 @@ uint8_t gps_i2c_getch(struct GpsUbxI2C *p);
void gps_ubx_i2c_init(void)
{
+#if GPS_UBX_UCENTER
gps_ubx_i2c_ucenter_done = FALSE;
+#else
+ gps_ubx_i2c_ucenter_done = TRUE;
+#endif
+
gps_i2c.read_state = gps_i2c_read_standby;
gps_i2c.write_state = gps_i2c_write_standby;
@@ -187,7 +192,7 @@ void gps_ubx_i2c_periodic(void)
break;
case gps_i2c_write_cfg:
- memcpy(&gps_i2c.trans.buf, gps_i2c.tx_buf, gps_i2c.tx_buf_idx);
+ memcpy((uint8_t *) & (gps_i2c.trans.buf), gps_i2c.tx_buf, gps_i2c.tx_buf_idx);
i2c_transmit(&GPS_UBX_I2C_DEV, &gps_i2c.trans, GPS_I2C_SLAVE_ADDR, gps_i2c.tx_buf_idx);
gps_i2c.tx_buf_idx = 0;
gps_i2c.read_state = gps_i2c_read_standby;
@@ -232,12 +237,12 @@ void gps_ubx_i2c_read_event(void)
case gps_i2c_read_data:
if (gps_ubx_i2c_bytes_to_read > GPS_I2C_BUF_SIZE)
{
- memcpy(&gps_i2c.rx_buf, &gps_i2c.trans.buf, GPS_I2C_BUF_SIZE);
+ memcpy(&gps_i2c.rx_buf, (uint8_t *) & (gps_i2c.trans.buf), GPS_I2C_BUF_SIZE);
gps_i2c.rx_buf_idx = 0;
gps_i2c.rx_buf_avail = GPS_I2C_BUF_SIZE;
} else
{
- memcpy(&gps_i2c.rx_buf, &gps_i2c.trans.buf, gps_ubx_i2c_bytes_to_read);
+ memcpy(&gps_i2c.rx_buf, (uint8_t *) & (gps_i2c.trans.buf), gps_ubx_i2c_bytes_to_read);
gps_i2c.rx_buf_idx = 0;
gps_i2c.rx_buf_avail = gps_ubx_i2c_bytes_to_read;
}
diff --git a/sw/airborne/modules/loggers/sdlog_chibios/printf.c b/sw/airborne/modules/loggers/sdlog_chibios/printf.c
index 8ee9fca7e54..335c850a00f 100644
--- a/sw/airborne/modules/loggers/sdlog_chibios/printf.c
+++ b/sw/airborne/modules/loggers/sdlog_chibios/printf.c
@@ -29,15 +29,51 @@
* @{
*/
-//#include
-#include "std.h"
+#include
+
#include
#include "printf.h"
+#include
+
#define MAX_FILLER 11
#define FLOAT_PRECISION 100000
+typedef struct {
+ union {
+ BaseSequentialStream *chp;
+ size_t size;
+ };
+ char *destBuf;
+ const char *fmt;
+ va_list ap;
+} synchronous_print_arg_t;
+
+static thread_t *printThreadPtr = NULL;
+
+static THD_WORKING_AREA(waSerialPrint, 512);
+
+static noreturn void serialPrint(void *arg)
+{
+
+ (void)arg;
+ chRegSetThreadName("serialPrint");
+
+ while (true) {
+ thread_t *sender = chMsgWait();
+ synchronous_print_arg_t *spat = (synchronous_print_arg_t *) chMsgGet(sender);
+ // do the print
+ if (spat->destBuf == NULL) {
+ directchvprintf(spat->chp, spat->fmt, spat->ap);
+ } else {
+ chvsnprintf(spat->destBuf, spat->size, spat->fmt, spat->ap);
+ }
+ chMsgRelease(sender, MSG_OK);
+ }
+}
+
+
#if CHPRINTF_USE_FLOAT
static int intPow(int a, int b)
{
@@ -48,16 +84,7 @@ static int intPow(int a, int b)
#endif
-static bool writeBufferWithinSize(char **buffer, const char c, size_t *size)
-{
- if (*size) {
- **buffer = c;
- (*buffer)++;
- return (--(*size) == 0);
- } else {
- return true;
- }
-}
+
static char *long_to_string_with_divisor(char *p,
@@ -137,205 +164,64 @@ static char *ftoa(char *p, double num, uint32_t precision)
* @param[in] fmt formatting string
*/
-void chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap)
+
+
+static void _chvsnprintf(char *buffer, BaseSequentialStream *chp, size_t size, const char *fmt, va_list ap)
{
char *p, *s, c, filler;
int i, precision, width;
- bool is_long, left_align;
+ bool is_long, left_align, plus_on_float;
long l;
#if CHPRINTF_USE_FLOAT
+ int fprec = 0;
double d;
char tmpbuf[2 * MAX_FILLER + 1];
- int fprec = 0;
#else
char tmpbuf[MAX_FILLER + 1];
#endif
- while (true) {
- c = *fmt++;
- if (c == 0) {
- return;
- }
- if (c != '%') {
- chSequentialStreamPut(chp, (uint8_t)c);
- continue;
- }
- p = tmpbuf;
- s = tmpbuf;
- left_align = false;
- if (*fmt == '-') {
- fmt++;
- left_align = true;
- }
- filler = ' ';
- if (*fmt == '.') {
- fmt++;
- filler = '0';
-#if CHPRINTF_USE_FLOAT
- fprec = intPow(10, (*fmt) - '0');
-#endif
- }
- width = 0;
- while (true) {
- c = *fmt++;
- if (c >= '0' && c <= '9') {
- c -= '0';
- } else if (c == '*') {
- c = va_arg(ap, int);
+ // return true if space exhausted
+ bool _putChar(const char _c) {
+ if (buffer != NULL) {
+ if (size) {
+ *buffer = _c;
+ buffer++;
+ return (--size == 0);
} else {
- break;
- }
- width = width * 10 + c;
- }
- precision = 0;
- if (c == '.') {
- while (true) {
- c = *fmt++;
- if (c >= '0' && c <= '9') {
- c -= '0';
-#if CHPRINTF_USE_FLOAT
- fprec = intPow(10, c);
-#endif
- } else if (c == '*') {
- c = va_arg(ap, int);
- } else {
- break;
- }
- precision *= 10;
- precision += c;
- }
- }
- /* Long modifier.*/
- if (c == 'l' || c == 'L') {
- is_long = true;
- if (*fmt) {
- c = *fmt++;
+ return true;
}
- } else {
- is_long = (c >= 'A') && (c <= 'Z');
- }
-
- /* Command decoding.*/
- switch (c) {
- case 'c':
- filler = ' ';
- *p++ = va_arg(ap, int);
- break;
- case 's':
- filler = ' ';
- if ((s = va_arg(ap, char *)) == 0) {
- s = "(null)";
- }
- if (precision == 0) {
- precision = 32767;
- }
- for (p = s; *p && (--precision >= 0); p++)
- ;
- break;
- case 'D':
- case 'd':
- if (is_long) {
- l = va_arg(ap, long);
- } else {
- l = va_arg(ap, int);
- }
- if (l < 0) {
- *p++ = '-';
- l = -l;
- }
- p = ltoa(p, l, 10);
- break;
-#if CHPRINTF_USE_FLOAT
- case 'f':
- d = (double) va_arg(ap, double);
- if (d < 0) {
- *p++ = '-';
- d = -d;
- }
- p = ftoa(p, d, fprec);
- break;
-#endif
- case 'X':
- case 'x':
- c = 16;
- goto unsigned_common;
- case 'U':
- case 'u':
- c = 10;
- goto unsigned_common;
- case 'O':
- case 'o':
- c = 8;
-unsigned_common:
- if (is_long) {
- l = va_arg(ap, long);
- } else {
- l = va_arg(ap, int);
- }
- p = ltoa(p, l, c);
- break;
- default:
- *p++ = c;
- break;
- }
- i = (int)(p - s);
- if ((width -= i) < 0) {
- width = 0;
- }
- if (left_align == false) {
- width = -width;
- }
- if (width < 0) {
- if (*s == '-' && filler == '0') {
- chSequentialStreamPut(chp, (uint8_t)*s++);
- i--;
- }
- do {
- chSequentialStreamPut(chp, (uint8_t)filler);
- } while (++width != 0);
- }
- while (--i >= 0) {
- chSequentialStreamPut(chp, (uint8_t)*s++);
- }
-
- while (width) {
- chSequentialStreamPut(chp, (uint8_t)filler);
- width--;
+ } else if (chp != NULL) {
+ chSequentialStreamPut(chp, _c);
+ return false;
}
+ return false;
}
-}
-void chvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
-{
- char *p, *s, c, filler;
- int i, precision, width;
- bool is_long, left_align;
- long l;
-#if CHPRINTF_USE_FLOAT
- double d;
- char tmpbuf[2 * MAX_FILLER + 1];
- int fprec = 0;
-#else
- char tmpbuf[MAX_FILLER + 1];
-#endif
+
while (true) {
c = *fmt++;
if (c == 0) {
- writeBufferWithinSize(&buffer, 0, &size);
+ // only add end of string marker when filling buffer, not when outputing on I/O channel
+ if (buffer != NULL) {
+ _putChar(0);
+ }
return;
}
if (c != '%') {
- if (writeBufferWithinSize(&buffer, c, &size)) { return; }
+ if (_putChar(c)) { return; }
continue;
}
p = tmpbuf;
s = tmpbuf;
- left_align = false;
+ left_align = plus_on_float = false;
if (*fmt == '-') {
fmt++;
left_align = true;
+ } else if (*fmt == '+') {
+ fmt++;
+ plus_on_float = true;
}
filler = ' ';
if (*fmt == '.') {
@@ -421,6 +307,8 @@ void chvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
if (d < 0) {
*p++ = '-';
d = -d;
+ } else if (plus_on_float) {
+ *p++ = '+';
}
p = ftoa(p, d, fprec);
break;
@@ -457,22 +345,33 @@ void chvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
}
if (width < 0) {
if (*s == '-' && filler == '0') {
- if (writeBufferWithinSize(&buffer, (uint8_t)*s++, &size)) { return; }
+ if (_putChar((uint8_t)*s++)) { return; }
i--;
}
do
- if (writeBufferWithinSize(&buffer, (uint8_t)filler, &size)) { return; }
+ if (_putChar((uint8_t)filler)) { return; }
while (++width != 0);
}
while (--i >= 0)
- if (writeBufferWithinSize(&buffer, (uint8_t)*s++, &size)) { return; }
+ if (_putChar((uint8_t)*s++)) { return; }
while (width) {
- if (writeBufferWithinSize(&buffer, (uint8_t)filler, &size)) { return; }
+ if (_putChar((uint8_t)filler)) { return; }
width--;
}
}
- writeBufferWithinSize(&buffer, 0, &size) ;
+ _putChar(0) ;
+}
+
+
+void directchvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap)
+{
+ _chvsnprintf(NULL, chp, 0, fmt, ap);
+}
+
+void chvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
+{
+ _chvsnprintf(buffer, NULL, size, fmt, ap);
}
void chsnprintf(char *buffer, size_t size, const char *fmt, ...)
@@ -480,16 +379,91 @@ void chsnprintf(char *buffer, size_t size, const char *fmt, ...)
va_list ap;
va_start(ap, fmt);
- chvsnprintf(buffer, size, fmt, ap);
+ _chvsnprintf(buffer, NULL, size, fmt, ap);
+ va_end(ap);
+}
+
+void directchprintf(BaseSequentialStream *chp, const char *fmt, ...)
+{
+ va_list ap;
+
+ va_start(ap, fmt);
+ _chvsnprintf(NULL, chp, 0, fmt, ap);
+ va_end(ap);
+}
+
+void chprintf(BaseSequentialStream *lchp, const char *fmt, ...)
+{
+ va_list ap;
+
+ if (printThreadPtr == NULL) {
+ printThreadPtr = chThdCreateStatic(waSerialPrint, sizeof(waSerialPrint), NORMALPRIO + 1, serialPrint, NULL);
+ }
+
+ va_start(ap, fmt);
+ synchronous_print_arg_t spat = {.chp = lchp,
+ .destBuf = NULL,
+ .fmt = fmt,
+ .ap = ap
+ };
+
+ chMsgSend(printThreadPtr, (msg_t) &spat);
+
va_end(ap);
}
-void chprintf(BaseSequentialStream *chp, const char *fmt, ...)
+void chvprintf(BaseSequentialStream *lchp, const char *fmt, va_list ap)
+{
+ if (printThreadPtr == NULL) {
+ printThreadPtr = chThdCreateStatic(waSerialPrint, sizeof(waSerialPrint), NORMALPRIO + 1, serialPrint, NULL);
+ }
+
+ synchronous_print_arg_t spat = {.chp = lchp,
+ .destBuf = NULL,
+ .fmt = fmt,
+ .ap = ap
+ };
+
+ chMsgSend(printThreadPtr, (msg_t) &spat);
+}
+
+
+
+void smchsnprintf(char *buffer, size_t size, const char *fmt, ...)
{
va_list ap;
+ if (printThreadPtr == NULL) {
+ printThreadPtr = chThdCreateStatic(waSerialPrint, sizeof(waSerialPrint), NORMALPRIO + 1, serialPrint, NULL);
+ }
+
va_start(ap, fmt);
- chvprintf(chp, fmt, ap);
+ synchronous_print_arg_t spat = {.size = size,
+ .destBuf = buffer,
+ .fmt = fmt,
+ .ap = ap
+ };
+
+ chMsgSend(printThreadPtr, (msg_t) &spat);
+
va_end(ap);
}
+
+
+void smchvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap)
+{
+ if (printThreadPtr == NULL) {
+ printThreadPtr = chThdCreateStatic(waSerialPrint, sizeof(waSerialPrint), NORMALPRIO + 1, serialPrint, NULL);
+ }
+
+ synchronous_print_arg_t spat = {.size = size,
+ .destBuf = buffer,
+ .fmt = fmt,
+ .ap = ap
+ };
+
+ chMsgSend(printThreadPtr, (msg_t) &spat);
+}
+
+
/** @} */
diff --git a/sw/airborne/modules/loggers/sdlog_chibios/printf.h b/sw/airborne/modules/loggers/sdlog_chibios/printf.h
index cef9cf5d93e..0abd8891fea 100644
--- a/sw/airborne/modules/loggers/sdlog_chibios/printf.h
+++ b/sw/airborne/modules/loggers/sdlog_chibios/printf.h
@@ -34,18 +34,23 @@
* @brief Float type support.
*/
#if !defined(CHPRINTF_USE_FLOAT) || defined(__DOXYGEN__)
-#define CHPRINTF_USE_FLOAT FALSE
+#define CHPRINTF_USE_FLOAT TRUE
#endif
#ifdef __cplusplus
extern "C" {
#endif
-void chprintf(BaseSequentialStream *chp, const char *fmt, ...);
-void chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap);
+void directchprintf(BaseSequentialStream *chp, const char *fmt, ...);
+void directchvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap);
// __attribute__ ((format (printf, 2, 3)));
void chvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap);
void chsnprintf(char *buffer, size_t size, const char *fmt, ...);
// __attribute__ ((format (printf, 3, 4)));
+void chprintf(BaseSequentialStream *lchp, const char *fmt, ...) ;
+void chvprintf(BaseSequentialStream *lchp, const char *fmt, va_list ap);
+void smchsnprintf(char *buffer, size_t size, const char *fmt, ...);
+void smchvsnprintf(char *buffer, size_t size, const char *fmt, va_list ap);
+
#ifdef __cplusplus
}
#endif
diff --git a/sw/airborne/modules/nav/nav_heli_spinup.c b/sw/airborne/modules/nav/nav_heli_spinup.c
index 353d76d2671..bfdee0cd91a 100644
--- a/sw/airborne/modules/nav/nav_heli_spinup.c
+++ b/sw/airborne/modules/nav/nav_heli_spinup.c
@@ -29,28 +29,39 @@
static struct nav_heli_spinup_t nav_heli_spinup;
+/**
+ * @param duration duration over which the heli should spin up [s]
+ * @param throttle the final throttle at the end of the spinup
+ *
+ * Sets the heli spinup routine parameters and sets the horizontal and
+ * vertical modes to manual, with zero commands
+ */
void nav_heli_spinup_setup(uint16_t duration, float throttle)
{
nav_heli_spinup.duration = (duration > 0) ? duration : 1;
nav_heli_spinup.throttle = throttle * MAX_PPRZ;
nav_throttle = 0;
- nav_roll = 0;
- nav_pitch = 0;
- nav_heading = 0;
+ nav_cmd_roll = 0;
+ nav_cmd_pitch = 0;
+ nav_cmd_yaw= 0;
horizontal_mode = HORIZONTAL_MODE_MANUAL;
vertical_mode = VERTICAL_MODE_MANUAL;
}
+/**
+ * Runs the heli spinup routine, with the parameters set by
+ * nav_heli_spinup_setup
+ */
bool nav_heli_spinup_run(void)
{
if (stage_time > nav_heli_spinup.duration) {
return false;
}
- nav_roll = 0;
- nav_pitch = 0;
- nav_heading = 0;
+ nav_cmd_roll = 0;
+ nav_cmd_pitch = 0;
+ nav_cmd_yaw= 0;
horizontal_mode = HORIZONTAL_MODE_MANUAL;
vertical_mode = VERTICAL_MODE_MANUAL;
nav_throttle = stage_time * nav_heli_spinup.throttle / nav_heli_spinup.duration;
diff --git a/sw/airborne/modules/optical_flow/px4flow.c b/sw/airborne/modules/optical_flow/px4flow.c
index 72f673e2144..ce4ac68132b 100644
--- a/sw/airborne/modules/optical_flow/px4flow.c
+++ b/sw/airborne/modules/optical_flow/px4flow.c
@@ -51,12 +51,17 @@ static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((u
{
optical_flow_available = true;
- // Y negated to get to the body of the drone
- AbiSendMsgVELOCITY_ESTIMATE(PX4FLOW_VELOCITY_ID, 0,
- (optical_flow.flow_x / optical_flow.ground_distance),
- -1.0 * (optical_flow.flow_y / optical_flow.ground_distance),
- 0.0f,
- PX4FLOW_NOISE);
+ // positive distance means it's known/valid
+ if (optical_flow.ground_distance > 0) {
+ // Y negated to get to the body of the drone
+ AbiSendMsgVELOCITY_ESTIMATE(PX4FLOW_VELOCITY_ID, 0,
+ (optical_flow.flow_x / optical_flow.ground_distance),
+ -1.0 * (optical_flow.flow_y / optical_flow.ground_distance),
+ 0.0f,
+ PX4FLOW_NOISE);
+
+ AbiSendMsgAGL(AGL_SONAR_PX4FLOW_ID, optical_flow.ground_distance);
+ }
}
/** Initialization function
diff --git a/sw/airborne/modules/sensors/aoa_pwm.c b/sw/airborne/modules/sensors/aoa_pwm.c
index d742ed1215b..da051350602 100644
--- a/sw/airborne/modules/sensors/aoa_pwm.c
+++ b/sw/airborne/modules/sensors/aoa_pwm.c
@@ -124,7 +124,7 @@ void aoa_pwm_update(void) {
prev_aoa = aoa_pwm.angle;
#if USE_AOA
- stateSetAngleOfAttack_f(aoa_adc.angle);
+ stateSetAngleOfAttack_f(aoa_pwm.angle);
#endif
#if SEND_SYNC_AOA
diff --git a/sw/airborne/state.c b/sw/airborne/state.c
index 894c717f509..044744f0be0 100644
--- a/sw/airborne/state.c
+++ b/sw/airborne/state.c
@@ -127,7 +127,7 @@ void stateCalcPositionNed_i(void)
SetBit(state.pos_status, POS_NED_F);
NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
- ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
+ ned_of_lla_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i);
} else { /* could not get this representation, set errno */
errno = 1;
}
@@ -206,7 +206,7 @@ void stateCalcPositionEnu_i(void)
SetBit(state.pos_status, POS_ENU_F);
ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f);
} else if (bit_is_set(state.pos_status, POS_LLA_I)) {
- enu_of_lla_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_i);
+ enu_of_lla_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_i);
} else { /* could not get this representation, set errno */
errno = 1;
}
diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h
index 483fdee1a9a..1648f6876eb 100644
--- a/sw/airborne/subsystems/abi_sender_ids.h
+++ b/sw/airborne/subsystems/abi_sender_ids.h
@@ -103,6 +103,10 @@
#define AGL_SONAR_NPS_ID 3
#endif
+#ifndef AGL_SONAR_PX4FLOW_ID
+#define AGL_SONAR_PX4FLOW_ID 4
+#endif
+
/*
* IDs of magnetometer sensors (including IMUs with mag)
*/
diff --git a/sw/airborne/subsystems/actuators/motor_mixing.c b/sw/airborne/subsystems/actuators/motor_mixing.c
index 0e15e9e6b3f..3f587ee3b26 100644
--- a/sw/airborne/subsystems/actuators/motor_mixing.c
+++ b/sw/airborne/subsystems/actuators/motor_mixing.c
@@ -93,6 +93,19 @@ static const int32_t thrust_coef[MOTOR_MIXING_NB_MOTOR] = MOTOR_MIXING_THRUST_CO
struct MotorMixing motor_mixing;
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+static void send_motor_mixing(struct transport_tx *trans, struct link_device *dev)
+{
+ int16_t motors[MOTOR_MIXING_NB_MOTOR];
+ for (uint8_t i = 0; i < MOTOR_MIXING_NB_MOTOR; i++)
+ {
+ motors[i] = (int16_t)motor_mixing.commands[i];
+ }
+ pprz_msg_send_MOTOR_MIXING(trans, dev, AC_ID , MOTOR_MIXING_NB_MOTOR, motors);
+}
+#endif
+
void motor_mixing_init(void)
{
uint8_t i;
@@ -107,6 +120,9 @@ void motor_mixing_init(void)
}
motor_mixing.nb_failure = 0;
motor_mixing.nb_saturation = 0;
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_MOTOR_MIXING, send_motor_mixing);
+#endif
}
static void offset_commands(int32_t offset)
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
index c27ec7f856f..656353e9632 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
@@ -46,10 +46,6 @@
#error "You have to define either AHRS_PROPAGATE_RMAT or AHRS_PROPAGATE_QUAT"
#endif
-#ifdef AHRS_MAG_UPDATE_YAW_ONLY
-#warning "AHRS_MAG_UPDATE_YAW_ONLY is deprecated, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw."
-#endif
-
#if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
#warning "Using magnetometer _and_ GPS course to update heading. Probably better to if you want to use GPS course."
#endif
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index d7afd6a5934..0129706a053 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -432,7 +432,7 @@ void Drift_correction()
Vector_Scale(&Scaled_Omega_I[0], &errorYaw[0], Ki_YAW);
Vector_Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
}
-#if USE_MAGNETOMETER_ONGROUND == 1
+#if USE_MAGNETOMETER_ONGROUND
PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight")
else if (launch == FALSE) {
float COGX = mag->x; // Non-Tilt-Compensated (for filter stability reasons)
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
index ded7b75416b..1b846d1b899 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
@@ -43,10 +43,6 @@
PRINT_CONFIG_MSG("LOW PASS FILTER ON GYRO RATES")
#endif
-#ifdef AHRS_MAG_UPDATE_YAW_ONLY
-#error "The define AHRS_MAG_UPDATE_YAW_ONLY doesn't exist anymore, please remove it. This is the default behaviour. Define AHRS_MAG_UPDATE_ALL_AXES to use mag for all axes and not only yaw."
-#endif
-
#if USE_MAGNETOMETER && AHRS_USE_GPS_HEADING
#warning "Using both magnetometer and GPS course to update heading. Probably better to configure USE_MAGNETOMETER=0 if you want to use GPS course."
#endif
diff --git a/sw/airborne/subsystems/datalink/datalink.c b/sw/airborne/subsystems/datalink/datalink.c
index 2f1ef982218..d67594ca1b0 100644
--- a/sw/airborne/subsystems/datalink/datalink.c
+++ b/sw/airborne/subsystems/datalink/datalink.c
@@ -52,10 +52,11 @@
bool datalink_enabled = true;
#endif
-void dl_parse_msg(void)
+
+void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
{
- uint8_t sender_id = SenderIdOfPprzMsg(dl_buffer);
- uint8_t msg_id = IdOfPprzMsg(dl_buffer);
+ uint8_t sender_id = SenderIdOfPprzMsg(buf);
+ uint8_t msg_id = IdOfPprzMsg(buf);
/* parse telemetry messages coming from other AC */
if (sender_id != 0) {
@@ -68,24 +69,24 @@ void dl_parse_msg(void)
/* parse telemetry messages coming from ground station */
switch (msg_id) {
case DL_PING: {
- DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
+ pprz_msg_send_PONG(trans, dev, AC_ID);
}
break;
case DL_SETTING : {
- if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
- uint8_t i = DL_SETTING_index(dl_buffer);
- float var = DL_SETTING_value(dl_buffer);
+ if (DL_SETTING_ac_id(buf) != AC_ID) { break; }
+ uint8_t i = DL_SETTING_index(buf);
+ float var = DL_SETTING_value(buf);
DlSetting(i, var);
- DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
+ pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &var);
}
break;
case DL_GET_SETTING : {
- if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
- uint8_t i = DL_GET_SETTING_index(dl_buffer);
+ if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
+ uint8_t i = DL_GET_SETTING_index(buf);
float val = settings_get_value(i);
- DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
+ pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &val);
}
break;
@@ -95,20 +96,20 @@ void dl_parse_msg(void)
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
- DL_RC_3CH_throttle_mode(dl_buffer),
- DL_RC_3CH_roll(dl_buffer),
- DL_RC_3CH_pitch(dl_buffer));
+ DL_RC_3CH_throttle_mode(buf),
+ DL_RC_3CH_roll(buf),
+ DL_RC_3CH_pitch(buf));
break;
case DL_RC_4CH :
- if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
+ if (DL_RC_4CH_ac_id(buf) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
- parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer),
- DL_RC_4CH_throttle(dl_buffer),
- DL_RC_4CH_roll(dl_buffer),
- DL_RC_4CH_pitch(dl_buffer),
- DL_RC_4CH_yaw(dl_buffer));
+ parse_rc_4ch_datalink(DL_RC_4CH_mode(buf),
+ DL_RC_4CH_throttle(buf),
+ DL_RC_4CH_roll(buf),
+ DL_RC_4CH_pitch(buf),
+ DL_RC_4CH_yaw(buf));
}
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
@@ -116,13 +117,13 @@ void dl_parse_msg(void)
#if USE_GPS
case DL_GPS_INJECT : {
// Check if the GPS is for this AC
- if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
+ if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; }
// GPS parse data
gps_inject_data(
- DL_GPS_INJECT_packet_id(dl_buffer),
- DL_GPS_INJECT_data_length(dl_buffer),
- DL_GPS_INJECT_data(dl_buffer)
+ DL_GPS_INJECT_packet_id(buf),
+ DL_GPS_INJECT_data_length(buf),
+ DL_GPS_INJECT_data(buf)
);
}
break;
@@ -133,13 +134,13 @@ void dl_parse_msg(void)
}
}
/* Parse firmware specific datalink */
- firmware_parse_msg();
+ firmware_parse_msg(dev, trans, buf);
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
/* default empty WEAK implementation for firmwares without an extra firmware_parse_msg */
-WEAK void firmware_parse_msg(void)
+WEAK void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf __attribute__((unused)))
{
-}
\ No newline at end of file
+}
diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h
index e8c77a69154..212ffc7e03c 100644
--- a/sw/airborne/subsystems/datalink/datalink.h
+++ b/sw/airborne/subsystems/datalink/datalink.h
@@ -60,10 +60,10 @@ EXTERN uint16_t datalink_nb_msgs;
EXTERN uint8_t dl_buffer[MSG_SIZE] __attribute__((aligned));
/** Should be called when chars are available in dl_buffer */
-EXTERN void dl_parse_msg(void);
+EXTERN void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf);
/** Firmware specfic msg handler */
-EXTERN void firmware_parse_msg(void);
+EXTERN void firmware_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *buf);
#if USE_NPS
EXTERN bool datalink_enabled;
@@ -79,7 +79,7 @@ EXTERN bool datalink_enabled;
}
/** Check for new message and parse */
-static inline void DlCheckAndParse(void)
+static inline void DlCheckAndParse(struct link_device *dev, struct transport_tx *trans, uint8_t *buf)
{
// make it possible to disable datalink in NPS sim
#if USE_NPS
@@ -91,7 +91,7 @@ static inline void DlCheckAndParse(void)
if (dl_msg_available) {
datalink_time = 0;
datalink_nb_msgs++;
- dl_parse_msg();
+ dl_parse_msg(dev, trans, buf);
dl_msg_available = false;
}
}
@@ -100,35 +100,35 @@ static inline void DlCheckAndParse(void)
#define DatalinkEvent() { \
pprz_check_and_parse(&(PPRZ_UART).device, &pprz_tp, dl_buffer, &dl_msg_available); \
- DlCheckAndParse(); \
+ DlCheckAndParse(&(DOWNLINK_DEVICE).device, &pprz_tp.trans_tx, dl_buffer); \
}
#elif defined DATALINK && DATALINK == XBEE
#define DatalinkEvent() { \
xbee_check_and_parse(&(XBEE_UART).device, &xbee_tp, dl_buffer, &dl_msg_available); \
- DlCheckAndParse(); \
+ DlCheckAndParse(&(DOWNLINK_DEVICE).device, &xbee_tp.trans_tx, dl_buffer); \
}
#elif defined DATALINK && DATALINK == W5100
#define DatalinkEvent() { \
W5100CheckAndParse(W5100, pprz_tp); \
- DlCheckAndParse(); \
+ DlCheckAndParse(&(DOWNLINK_DEVICE).device, &pprz_tp.trans_tx, dl_buffer); \
}
#elif defined DATALINK && DATALINK == SUPERBITRF
#define DatalinkEvent() { \
SuperbitRFCheckAndParse(); \
- DlCheckAndParse(); \
+ DlCheckAndParse(&(DOWNLINK_DEVICE).device, &pprz_tp.trans_tx, dl_buffer); \
}
#elif defined DATALINK && DATALINK == BLUEGIGA
#define DatalinkEvent() { \
pprz_check_and_parse(&(DOWNLINK_DEVICE).device, &pprz_tp, dl_buffer, &dl_msg_available); \
- DlCheckAndParse(); \
+ DlCheckAndParse(&(DOWNLINK_DEVICE).device, &pprz_tp.trans_tx, dl_buffer); \
}
#else
diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c
index f9d43d11cbb..facf24e8631 100644
--- a/sw/airborne/subsystems/gps.c
+++ b/sw/airborne/subsystems/gps.c
@@ -175,12 +175,6 @@ static void send_gps_sol(struct transport_tx *trans, struct link_device *dev)
}
#endif
-void gps_periodic_check(struct GpsState *gps_s)
-{
- if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) {
- gps_s->fix = GPS_FIX_NONE;
- }
-}
#ifdef SECONDARY_GPS
static uint8_t gps_multi_switch(struct GpsState *gps_s) {
@@ -211,6 +205,24 @@ static uint8_t gps_multi_switch(struct GpsState *gps_s) {
}
#endif /*SECONDARY_GPS*/
+
+void gps_periodic_check(struct GpsState *gps_s)
+{
+ if (sys_time.nb_sec - gps_s->last_msg_time > GPS_TIMEOUT) {
+ gps_s->fix = GPS_FIX_NONE;
+ }
+
+#ifdef SECONDARY_GPS
+ current_gps_id = gps_multi_switch(gps_s);
+ if (gps_s->comp_id == current_gps_id) {
+ gps = *gps_s;
+ }
+#else
+ gps = *gps_s;
+#endif
+}
+
+
static abi_event gps_ev;
static void gps_cb(uint8_t sender_id,
uint32_t stamp __attribute__((unused)),
diff --git a/sw/airborne/subsystems/imu/imu_bebop.c b/sw/airborne/subsystems/imu/imu_bebop.c
index b22d67f0116..ee7ad825ce8 100644
--- a/sw/airborne/subsystems/imu/imu_bebop.c
+++ b/sw/airborne/subsystems/imu/imu_bebop.c
@@ -70,6 +70,8 @@ PRINT_CONFIG_VAR(BEBOP_GYRO_RANGE)
#endif
PRINT_CONFIG_VAR(BEBOP_ACCEL_RANGE)
+struct OrientationReps imu_to_mag_bebop; ///< IMU to magneto rotation
+
/** Basic Navstik IMU data */
struct ImuBebop imu_bebop;
@@ -87,6 +89,15 @@ void imu_bebop_init(void)
/* AKM8963 */
ak8963_init(&imu_bebop.ak, &(BEBOP_MAG_I2C_DEV), AK8963_ADDR);
+
+#if BEBOP_VERSION2
+ //the magnetometer of the bebop2 is located on the gps board,
+ //which is under a slight angle
+ struct FloatEulers imu_to_mag_eulers =
+ {0.0, RadOfDeg(8.5), 0.0};
+ orientationSetEulers_f(&imu_to_mag_bebop, &imu_to_mag_eulers);
+#endif
+
}
/**
@@ -132,8 +143,13 @@ void imu_bebop_event(void)
if (imu_bebop.ak.data_available) {
#if BEBOP_VERSION2
+ struct Int32Vect3 mag_temp;
// In the second bebop version the magneto is turned 90 degrees
- VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.x, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z);
+ VECT3_ASSIGN(mag_temp, -imu_bebop.ak.data.vect.x, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.z);
+
+ // Rotate the magneto
+ struct Int32RMat *imu_to_mag_rmat = orientationGetRMat_i(&imu_to_mag_bebop);
+ int32_rmat_vmult(&imu.mag_unscaled, imu_to_mag_rmat, &mag_temp);
#else //BEBOP regular first verion
VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z);
#endif
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 584f9c2080c..637b7d90426 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -71,9 +71,6 @@ static void sonar_cb(uint8_t sender_id, float distance);
#include "firmwares/rotorcraft/stabilization.h"
#endif
-#ifndef INS_SONAR_OFFSET
-#define INS_SONAR_OFFSET 0.
-#endif
#ifndef INS_SONAR_MIN_RANGE
#define INS_SONAR_MIN_RANGE 0.001
#endif
diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c
index 3c307afb3f6..3e2f9557f72 100644
--- a/sw/airborne/test/subsystems/test_ahrs.c
+++ b/sw/airborne/test/subsystems/test_ahrs.c
@@ -107,9 +107,9 @@ static inline void main_report(void)
periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
}
-void dl_parse_msg(void)
+void dl_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
- uint8_t msg_id = dl_buffer[1];
+ uint8_t msg_id = buf[1];
switch (msg_id) {
case DL_PING: {
@@ -117,16 +117,16 @@ void dl_parse_msg(void)
}
break;
case DL_SETTING:
- if (DL_SETTING_ac_id(dl_buffer) == AC_ID) {
- uint8_t i = DL_SETTING_index(dl_buffer);
- float val = DL_SETTING_value(dl_buffer);
+ if (DL_SETTING_ac_id(buf) == AC_ID) {
+ uint8_t i = DL_SETTING_index(buf);
+ float val = DL_SETTING_value(buf);
DlSetting(i, val);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
case DL_GET_SETTING : {
- if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
- uint8_t i = DL_GET_SETTING_index(dl_buffer);
+ if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
+ uint8_t i = DL_GET_SETTING_index(buf);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
diff --git a/sw/airborne/test/subsystems/test_settings.c b/sw/airborne/test/subsystems/test_settings.c
index 3be146a995b..0aae5ff4bf3 100644
--- a/sw/airborne/test/subsystems/test_settings.c
+++ b/sw/airborne/test/subsystems/test_settings.c
@@ -92,9 +92,9 @@ static inline void main_event(void)
DatalinkEvent();
}
-void dl_parse_msg(void)
+void dl_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
- uint8_t msg_id = dl_buffer[1];
+ uint8_t msg_id = buf[1];
switch (msg_id) {
case DL_PING: {
@@ -102,16 +102,16 @@ void dl_parse_msg(void)
}
break;
case DL_SETTING:
- if (DL_SETTING_ac_id(dl_buffer) == AC_ID) {
- uint8_t i = DL_SETTING_index(dl_buffer);
- float val = DL_SETTING_value(dl_buffer);
+ if (DL_SETTING_ac_id(buf) == AC_ID) {
+ uint8_t i = DL_SETTING_index(buf);
+ float val = DL_SETTING_value(buf);
DlSetting(i, val);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
case DL_GET_SETTING : {
- if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
- uint8_t i = DL_GET_SETTING_index(dl_buffer);
+ if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
+ uint8_t i = DL_GET_SETTING_index(buf);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
diff --git a/sw/airborne/test/test_actuators_pwm.c b/sw/airborne/test/test_actuators_pwm.c
index 9e583ae8d83..37f4ed44c6a 100644
--- a/sw/airborne/test/test_actuators_pwm.c
+++ b/sw/airborne/test/test_actuators_pwm.c
@@ -92,9 +92,9 @@ static inline void main_event(void)
#define IdOfMsg(x) (x[1])
-void dl_parse_msg(void)
+void dl_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
- uint8_t msg_id = IdOfMsg(dl_buffer);
+ uint8_t msg_id = IdOfMsg(buf);
switch (msg_id) {
case DL_PING: {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
@@ -102,8 +102,8 @@ void dl_parse_msg(void)
break;
case DL_SET_ACTUATOR: {
- uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer);
- uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer);
+ uint8_t servo_no = DL_SET_ACTUATOR_no(buf);
+ uint16_t servo_value = DL_SET_ACTUATOR_value(buf);
#ifdef LED_2
LED_TOGGLE(2);
#endif
@@ -114,17 +114,17 @@ void dl_parse_msg(void)
break;
case DL_SETTING: {
- if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
- uint8_t i = DL_SETTING_index(dl_buffer);
- float var = DL_SETTING_value(dl_buffer);
+ if (DL_SETTING_ac_id(buf) != AC_ID) { break; }
+ uint8_t i = DL_SETTING_index(buf);
+ float var = DL_SETTING_value(buf);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_GET_SETTING : {
- if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
- uint8_t i = DL_GET_SETTING_index(dl_buffer);
+ if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
+ uint8_t i = DL_GET_SETTING_index(buf);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
diff --git a/sw/airborne/test/test_datalink.c b/sw/airborne/test/test_datalink.c
index 01d2137f593..cbf6fe66bf8 100644
--- a/sw/airborne/test/test_datalink.c
+++ b/sw/airborne/test/test_datalink.c
@@ -69,9 +69,9 @@ static inline void main_event(void)
DatalinkEvent();
}
-void dl_parse_msg(void)
+void dl_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
- uint8_t msg_id = dl_buffer[1];
+ uint8_t msg_id = buf[1];
switch (msg_id) {
case DL_PING: {
diff --git a/sw/airborne/test/test_manual.c b/sw/airborne/test/test_manual.c
index e241d6a8d38..49875165dac 100644
--- a/sw/airborne/test/test_manual.c
+++ b/sw/airborne/test/test_manual.c
@@ -115,9 +115,9 @@ static inline void main_event(void)
#define IdOfMsg(x) (x[1])
-void dl_parse_msg(void)
+void dl_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
{
- uint8_t msg_id = IdOfMsg(dl_buffer);
+ uint8_t msg_id = IdOfMsg(buf);
switch (msg_id) {
case DL_PING: {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
@@ -125,17 +125,17 @@ void dl_parse_msg(void)
break;
case DL_SETTING: {
- if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
- uint8_t i = DL_SETTING_index(dl_buffer);
- float var = DL_SETTING_value(dl_buffer);
+ if (DL_SETTING_ac_id(buf) != AC_ID) { break; }
+ uint8_t i = DL_SETTING_index(buf);
+ float var = DL_SETTING_value(buf);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_GET_SETTING : {
- if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
- uint8_t i = DL_GET_SETTING_index(dl_buffer);
+ if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
+ uint8_t i = DL_GET_SETTING_index(buf);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
@@ -146,20 +146,20 @@ void dl_parse_msg(void)
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
- DL_RC_3CH_throttle_mode(dl_buffer),
- DL_RC_3CH_roll(dl_buffer),
- DL_RC_3CH_pitch(dl_buffer));
+ DL_RC_3CH_throttle_mode(buf),
+ DL_RC_3CH_roll(buf),
+ DL_RC_3CH_pitch(buf));
break;
case DL_RC_4CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(
- DL_RC_4CH_mode(dl_buffer),
- DL_RC_4CH_throttle(dl_buffer),
- DL_RC_4CH_roll(dl_buffer),
- DL_RC_4CH_pitch(dl_buffer),
- DL_RC_4CH_yaw(dl_buffer));
+ DL_RC_4CH_mode(buf),
+ DL_RC_4CH_throttle(buf),
+ DL_RC_4CH_roll(buf),
+ DL_RC_4CH_pitch(buf),
+ DL_RC_4CH_yaw(buf));
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index d5624139971..a5296431f96 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit d562413997195f2176dec7ae9140a64e4c4a3051
+Subproject commit a5296431f96587364f55270a19c05b4dd9395164
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index 1398e1933b8..75c6f9b05ec 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -38,6 +38,9 @@ let (//) = Filename.concat
let gcs_id = "GCS"
let approaching_alert_time = 3.
+let approaching_alert_dmin = 0.5
+let approaching_alert_slmin = 20.
+
let track_size = ref 500
let _auto_hide_fp = ref false
@@ -961,12 +964,14 @@ let highlight_fp = fun ac b s ->
end
-let check_approaching = fun ac geo alert ->
+let check_approaching = fun ac geo1 geo2 alert ->
match ac.track#last with
None -> ()
| Some ac_pos ->
- let d = LL.wgs84_distance ac_pos geo in
- if d < ac.speed *. approaching_alert_time then
+ let s_len = LL.wgs84_distance geo1 geo2 in (* length of the segment *)
+ let d = LL.wgs84_distance ac_pos geo2 in (* distance to end of the segment *)
+ (* only log_and_say "approaching" if close enough but not too much and when flying long segments *)
+ if d < ac.speed *. approaching_alert_time && d > approaching_alert_dmin && s_len > approaching_alert_slmin then
log_and_say alert ac.ac_name (sprintf "%s, approaching" ac.ac_speech_name)
@@ -1266,7 +1271,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph ->
ac.track#draw_segment geo1 geo2;
(* Check if approaching the end of the segment *)
- check_approaching ac geo2 alert
+ check_approaching ac geo1 geo2 alert
in
safe_bind "SEGMENT_STATUS" get_segment_status;
diff --git a/sw/ground_segment/joystick/gb2ivy.py b/sw/ground_segment/joystick/gb2ivy.py
index b15bee9723f..9b84e3c4c39 100755
--- a/sw/ground_segment/joystick/gb2ivy.py
+++ b/sw/ground_segment/joystick/gb2ivy.py
@@ -35,17 +35,17 @@ def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
- self.auto2_index = None
+ self.ap_mode = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
- self.auto2_index = settings.name_lookup['auto2'].index
+ self.ap_mode = settings.name_lookup['mode'].index
except Exception as e:
print(e)
- print("auto2 setting not found, mode change not possible.")
+ print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("gb2ivy")
def shutdown(self):
@@ -66,24 +66,24 @@ def bat_cb(ac_id, msg):
def set_guided_mode(self):
"""
- change auto2 mode to GUIDED.
+ change mode to GUIDED.
"""
- if self.auto2_index is not None:
+ if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
- msg['index'] = self.auto2_index
+ msg['index'] = self.ap_mode
msg['value'] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
- change auto2 mode to NAV.
+ change mode to NAV.
"""
- if self.auto2_index is not None:
+ if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
- msg['index'] = self.auto2_index
+ msg['index'] = self.ap_mode
msg['value'] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
diff --git a/sw/ground_segment/python/svinfo/svinfo.ico b/sw/ground_segment/python/svinfo/svinfo.ico
new file mode 100755
index 00000000000..3dab1f64df6
Binary files /dev/null and b/sw/ground_segment/python/svinfo/svinfo.ico differ
diff --git a/sw/ground_segment/python/svinfo/svinfo.py b/sw/ground_segment/python/svinfo/svinfo.py
new file mode 100755
index 00000000000..9f64b8cdc64
--- /dev/null
+++ b/sw/ground_segment/python/svinfo/svinfo.py
@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+
+import wx
+import svinfoviewer
+
+class SVInfoFrame(wx.App):
+ def OnInit(self):
+ self.main = svinfoviewer.SVInfoFrame()
+ self.main.Show()
+ self.SetTopWindow(self.main)
+ return True
+
+def main():
+ application = SVInfoFrame(0)
+ application.MainLoop()
+
+if __name__ == '__main__':
+ main()
diff --git a/sw/ground_segment/python/svinfo/svinfoviewer.py b/sw/ground_segment/python/svinfo/svinfoviewer.py
new file mode 100644
index 00000000000..2f9e70660ef
--- /dev/null
+++ b/sw/ground_segment/python/svinfo/svinfoviewer.py
@@ -0,0 +1,138 @@
+import wx
+
+import sys
+import os
+import time
+import threading
+import math
+import pynotify
+
+PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),
+ '../../../..')))
+
+sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python")
+
+from pprzlink.ivy import IvyMessagesInterface
+
+WIDTH = 300
+BARH = 140
+CHANNEL = 16
+
+
+def QIColour(qi):
+ return {
+ 0: wx.Colour(64,64,64), # This channel is idle
+ 1: wx.Colour(128,128,128), # Searching
+ 2: wx.Colour(0,128,128), # Signal aquired
+ 3: wx.Colour(255,0,0), # Signal detected but unusable
+ 4: wx.Colour(0,0,255), # Code Lock on Signal
+ 5: wx.Colour(0,255,0), # Code and Carrier locked
+ 6: wx.Colour(0,255,0), # Code and Carrier locked
+ 7: wx.Colour(0,255,0), # Code and Carrier locked
+ }[qi]
+
+class SvChannel(object):
+ def __init__(self, chn, msg):
+ self.chn = chn
+ self.SVID = int(msg['SVID'])
+ self.Flags = int(msg['Flags'])
+ self.QI = int(msg['QI'])
+ self.CNO = int(msg['CNO'])
+ self.Elev = int(msg['Elev'])
+ self.Azim = int(msg['Azim'])
+
+
+class SVInfoFrame(wx.Frame):
+ def message_recv(self, ac_id, msg):
+ if msg.name == "SVINFO":
+ chn = int(msg['chn'])
+ self.sv[chn] = SvChannel(chn, msg)
+ wx.CallAfter(self.update)
+
+ def update(self):
+ self.Refresh()
+
+ def OnSize(self, event):
+ self.w = event.GetSize()[0]
+ self.h = event.GetSize()[1]
+ self.Refresh()
+
+ def OnPaint(self, e):
+ tdx = -5
+ tdy = -7
+ th = 15
+
+ w = self.w
+ h = self.w
+ if h < self.w + 50:
+ h = self.w + 50
+ bar = self.h-w-th-th
+
+
+ dc = wx.PaintDC(self)
+ brush = wx.Brush("white")
+ dc.SetBackground(brush)
+ dc.Clear()
+
+ # Background
+ dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
+ dc.DrawCircle(w/2,w/2,w/2-1)
+ dc.DrawCircle(w/2,w/2,w/4-1)
+ dc.DrawCircle(w/2,w/2,1)
+ font = wx.Font(11, wx.ROMAN, wx.BOLD, wx.NORMAL)
+ dc.SetFont(font)
+
+ dc.DrawText("N",w/2+tdx,2)
+ dc.DrawText("S",w/2+tdx,w-17)
+ dc.DrawText("E",w-15,w/2+tdy)
+ dc.DrawText("W",2,w/2+tdy)
+
+ # SV
+ for chn in self.sv:
+ sv = self.sv[chn]
+
+ c = QIColour(sv.QI)
+ used = sv.Flags & 1
+ s = 7 + used * 5
+
+ el = float(sv.Elev) / 90.0 * float(w) / 2.0
+ az = float(sv.Azim) * math.pi / 180.0
+
+ y = float(w)/2.0 - math.cos(az) * el
+ x = float(w)/2.0 + math.sin(az) * el
+
+ dc.SetBrush(wx.Brush(c, wx.SOLID))
+ dc.DrawCircle(int(x),int(y),s)
+
+ font = wx.Font(8, wx.ROMAN, wx.NORMAL, wx.NORMAL)
+ dc.SetFont(font)
+ dc.DrawText(str(sv.SVID),x+tdx,y+tdy)
+
+ bh = float(bar-th-th) * float(sv.CNO) / 55.0
+ dc.DrawRectangle(w/CHANNEL*chn+5 * (1-used),self.h-th-bh,w/CHANNEL-2 - 10 * (1-used),bh)
+ dc.DrawText(str(chn),w/CHANNEL*chn,self.h-th)
+ dc.DrawText(str(sv.CNO),w/CHANNEL*chn,self.h-bar)
+
+
+ def __init__(self):
+
+ self.w = WIDTH
+ self.h = WIDTH + BARH
+
+ wx.Frame.__init__(self, id=-1, parent=None, name=u'SVInfoFrame',
+ size=wx.Size(self.w, self.h), title=u'SV Info')
+ self.Bind(wx.EVT_PAINT, self.OnPaint)
+ self.Bind(wx.EVT_SIZE, self.OnSize)
+ self.Bind(wx.EVT_CLOSE, self.OnClose)
+
+ ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/svinfo/svinfo.ico", wx.BITMAP_TYPE_ICO)
+ self.SetIcon(ico)
+
+ self.sv = {}
+
+ self.interface = IvyMessagesInterface("svinfoframe")
+ self.interface.subscribe(self.message_recv)
+
+ def OnClose(self, event):
+ self.interface.shutdown()
+ self.Destroy()
diff --git a/sw/lib/ocaml/fp_proc.ml b/sw/lib/ocaml/fp_proc.ml
index e7ac6adbeeb..c38819dcda6 100644
--- a/sw/lib/ocaml/fp_proc.ml
+++ b/sw/lib/ocaml/fp_proc.ml
@@ -121,6 +121,9 @@ let transform_stage = fun prefix reroutes env xml ->
| "attitude" ->
let attribs = transform_values ["vmode"] env attribs in
Xml.Element (tag, attribs, children)
+ | "manual" ->
+ let attribs = transform_values [] env attribs in
+ Xml.Element (tag, attribs, children)
| "go" ->
assert (children=[]);
let attribs = transform_values ["wp";"from";"hmode";"vmode"] env attribs in
@@ -226,6 +229,7 @@ let parse_include = fun dir flight_plan include_xml ->
let waypoints = get_children "waypoints" proc
and exceptions = get_children "exceptions" proc
+ and modules = get_children "modules" proc
and blocks = get_children "blocks" proc
and sectors = get_children "sectors" proc
and header = get_pc_data "header" proc in
@@ -237,6 +241,7 @@ let parse_include = fun dir flight_plan include_xml ->
append_children
["waypoints", waypoints;
"blocks", blocks;
+ "modules", modules;
"exceptions", exceptions;
"sectors", sectors]
(append_pc_data "header" header flight_plan)
diff --git a/sw/lib/ocaml/papget_renderer.ml b/sw/lib/ocaml/papget_renderer.ml
index 876a16cea50..3ed89378a70 100644
--- a/sw/lib/ocaml/papget_renderer.ml
+++ b/sw/lib/ocaml/papget_renderer.ml
@@ -59,7 +59,7 @@ object (self)
PC.property "color" color ]
method update = fun (value : string) ->
let renderer = fun x ->
- try sprintf (Obj.magic format) (float_of_string x) with _ -> x in
+ try sprintf (Scanf.format_from_string format "%f") (float_of_string x) with _ -> x in
text#set [`SIZE_POINTS size; `TEXT (renderer value); `FILL_COLOR color; `ANCHOR `NW]
diff --git a/sw/logalizer/logplotter.ml b/sw/logalizer/logplotter.ml
index 31a331c004c..8a39392046d 100644
--- a/sw/logalizer/logplotter.ml
+++ b/sw/logalizer/logplotter.ml
@@ -547,7 +547,7 @@ let rec select_gps_values = function
let t = fst lats.(i)
and lat = snd lats.(i) /. 1e7
and lon = snd lons.(i) /. 1e7 in
- let wgs84 = make_geo lat lon in
+ let wgs84 = make_geo_deg lat lon in
l := (t, wgs84, a) :: !l
done;
List.rev !l
diff --git a/sw/tools/doxygen_gen/gen_modules_doc.py b/sw/tools/doxygen_gen/gen_modules_doc.py
index 628a07a30c0..27ff80abd7d 100755
--- a/sw/tools/doxygen_gen/gen_modules_doc.py
+++ b/sw/tools/doxygen_gen/gen_modules_doc.py
@@ -30,12 +30,17 @@ def dox_list_file(f):
def get_module_dir(module):
return module.get("dir", module.get("name")).strip()
+def get_module_dox_name(module_name):
+ return module_name.lower().replace('.', '_')
+
+def get_module_page_name(module_name):
+ return "module__" + get_module_dox_name(module_name)
def modules_category_list(category, modules):
s = "@subsection modules_category_" + category.lower() + " " + category.title() + " modules\n\n"
for (fname, m) in sorted(modules.items()):
mname = fname[:-4].lower()
- page_name = "module__" + mname
+ page_name = get_module_page_name(mname)
(brief, _) = get_module_description(m)
s += "- @subpage {} : {}\n".format(page_name, brief)
return s + "\n\n"
@@ -78,7 +83,7 @@ def modules_overview_page(modules_dict):
def module_page(filename, module):
(brief, details) = get_module_description(module)
mname = filename[:-4].lower()
- page_name = "module__" + mname
+ page_name = get_module_page_name(mname)
title = mname + " module"
s = dox_new_page(page_name, title)
s += "" + brief + "\n\n"
@@ -100,7 +105,7 @@ def module_page(filename, module):
def get_xml_example(filename, module):
module_name = filename[:-4]
opts = module.findall(".doc/define") + module.findall(".doc/configure")
- s = "\n@section module_load_example__{0} Example for airframe file\n".format(module_name.lower())
+ s = "\n@section module_load_example__{0} Example for airframe file\n".format(get_module_dox_name(module_name))
s += "Add to your firmware section:\n"
if opts:
s += "This example contains all possible configuration options, not all of them are mandatory!\n"
@@ -172,7 +177,7 @@ def module_configuration(module, mname):
doc += get_doc_config_option(module, 'define')
doc += get_doc_sections(module)
if doc:
- return "@section configuration__{0} Module configuration options\n\n".format(mname) + doc
+ return "@section configuration__{0} Module configuration options\n\n".format(get_module_dox_name(mname)) + doc
else:
return ""
@@ -181,14 +186,14 @@ def module_depends_conflicts(module, mname):
s = ""
deps = get_module_dependencies(module)
if deps:
- s += "@section dependencies__{} Dependencies\n".format(mname)
+ s += "@section dependencies__{} Dependencies\n".format(get_module_dox_name(mname))
for d in deps:
- s += "- @ref module__{}\n".format(d.lower())
+ s += "- @ref {}\n".format(get_module_page_name(d))
conflicts = get_module_conflicts(module)
if conflicts:
- s += "@section conflicts__{} Conflicts\n".format(mname)
+ s += "@section conflicts__{} Conflicts\n".format(get_module_dox_name(mname))
for c in conflicts:
- s += "- @ref module__{}\n".format(c.lower())
+ s += "- @ref {}\n".format(get_module_page_name(c))
return s
@@ -208,10 +213,10 @@ def module_autoloads(module, mname):
s = ""
autos = get_module_autoloads(module)
if autos:
- s += "@section autoloads__{} Auto-loaded modules\n".format(mname)
+ s += "@section autoloads__{} Auto-loaded modules\n".format(get_module_dox_name(mname))
s += "The following modules are automatically loaded (just as if you had added them in the airframe file)\n"
for a in autos:
- s += "- @ref module__{}\n".format(a.lower())
+ s += "- @ref {}\n".format(get_module_page_name(a))
return s
@@ -233,7 +238,7 @@ def get_module_description(module):
brief = module.get('name').replace('_', ' ').title()
else:
# treat first line until dot as brief
- d = re.split(r'\.|\n', desc.text.strip(), 1)
+ d = re.split(r'\. |\n', desc.text.strip(), 1)
brief = d[0].strip()
if len(d) > 1:
details = inspect.cleandoc(d[1]) + "\n"
@@ -339,7 +344,7 @@ def module_functions(module, mname):
fdoc += get_event_functions(module)
fdoc += get_periodic_functions(module)
if fdoc:
- return "@section functions__{0} Module functions\n\n".format(mname) + fdoc + "\n"
+ return "@section functions__{0} Module functions\n\n".format(get_module_dox_name(mname)) + fdoc + "\n"
else:
return ""
@@ -347,7 +352,7 @@ def module_datalink(module, mname):
s = ""
datalinks = module.findall("./datalink")
if datalinks:
- s += "@section datalink_functions__{0} Datalink Functions\n\n".format(mname)
+ s += "@section datalink_functions__{0} Datalink Functions\n\n".format(get_module_dox_name(mname))
s += "Whenever the specified datalink message is received, the corresponing handler function is called.\n\n"
for d in datalinks:
s += "- on message @b {0} call {1}\n".format(d.get('message'), d.get('fun'))
diff --git a/sw/tools/generators/gen_aircraft.ml b/sw/tools/generators/gen_aircraft.ml
index a8e851c7d16..810cd85b027 100644
--- a/sw/tools/generators/gen_aircraft.ml
+++ b/sw/tools/generators/gen_aircraft.ml
@@ -113,6 +113,39 @@ let file_xml2mk = fun f ?(arch = false) dir_name target xml ->
else format_of_string "%s.srcs += %s/%s\n" in
fprintf f fmt target dir_name name
+(* only print the configuration flags for a module
+ * 'raw' section are not handled here
+ *)
+let module_configure_xml2mk = fun f target firmware m ->
+ (* print global config flags *)
+ List.iter (fun flag ->
+ match Compat.bytes_lowercase (Xml.tag flag) with
+ | "configure" -> configure_xml2mk f flag
+ | _ -> ()) m.param;
+ (* Look for makefile section *)
+ ExtXml.iter_tag "makefile"
+ (fun section ->
+ (* Look for defines, flags, files, ... if target is matching *)
+ let section =
+ let targets = Gen_common.targets_of_field section Env.default_module_targets in
+ if Gen_common.test_targets target targets then section else Xml.Element ("makefile", [], [])
+ in
+ (* keep section if firmware is also matching or not speficied *)
+ let section = begin
+ try
+ if Xml.attrib section "firmware" = firmware then section
+ else Xml.Element ("makefile", [], [])
+ with _ -> section end
+ in
+ Xml.iter
+ (fun field ->
+ match Compat.bytes_lowercase (Xml.tag field) with
+ | "configure" -> configure_xml2mk f field
+ | _ -> ()
+ ) section
+ ) m.xml
+
+(* module files and flags except configuration flags *)
let module_xml2mk = fun f target firmware m ->
let name = ExtXml.attrib m.xml "name" in
let dir = try Xml.attrib m.xml "dir" with Xml.No_attribute _ -> name in
@@ -121,7 +154,6 @@ let module_xml2mk = fun f target firmware m ->
fprintf f "\n# makefile for module %s in modules/%s\n" name dir;
List.iter (fun flag ->
match Compat.bytes_lowercase (Xml.tag flag) with
- | "configure" -> configure_xml2mk f flag
| "define" -> define_xml2mk f ~target flag
| _ -> ()) m.param;
(* Look for makefile section *)
@@ -142,7 +174,6 @@ let module_xml2mk = fun f target firmware m ->
Xml.iter
(fun field ->
match Compat.bytes_lowercase (Xml.tag field) with
- | "configure" -> configure_xml2mk f field
| "define" -> define_xml2mk f ~target field
| "include" -> include_xml2mk f ~target ~vpath:m.vpath field
| "flag" ->
@@ -249,6 +280,7 @@ let parse_firmware = fun makefile_ac ac_xml firmware fp ->
List.iter (subsystem_configure_xml2mk makefile_ac) t_subsystems;
List.iter (subsystem_configure_xml2mk makefile_ac) mods;
List.iter (subsystem_configure_xml2mk makefile_ac) t_mods;
+ List.iter (module_configure_xml2mk makefile_ac target_name firmware_name) modules;
fprintf makefile_ac "\ninclude $(PAPARAZZI_SRC)/conf/boards/%s.makefile\n" (Xml.attrib target "board");
fprintf makefile_ac "include $(PAPARAZZI_SRC)/conf/firmwares/%s.makefile\n" (Xml.attrib firmware "name");
List.iter (fun def -> define_xml2mk makefile_ac def) defines;