diff --git a/conf/airframes/CDW/cdw_conf.xml b/conf/airframes/CDW/cdw_conf.xml
index 157369202b..5310a1efa1 100644
--- a/conf/airframes/CDW/cdw_conf.xml
+++ b/conf/airframes/CDW/cdw_conf.xml
@@ -11,15 +11,4 @@
gui_color="white"
release="33e02c9f1e831cb30ba9dc442e055b6e28c72a52"
/>
-
diff --git a/conf/airframes/CDW/cdw_tricopter.xml b/conf/airframes/CDW/cdw_tricopter.xml
deleted file mode 100644
index 08bbf7303a..0000000000
--- a/conf/airframes/CDW/cdw_tricopter.xml
+++ /dev/null
@@ -1,201 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/ENAC/conf_enac.xml b/conf/airframes/ENAC/conf_enac.xml
index 93db232a27..e706b1154f 100644
--- a/conf/airframes/ENAC/conf_enac.xml
+++ b/conf/airframes/ENAC/conf_enac.xml
@@ -32,17 +32,6 @@
settings_modules="modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
gui_color="yellow"
/>
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/examples/h_hex.xml b/conf/airframes/examples/h_hex.xml
deleted file mode 100644
index 355ee81fe7..0000000000
--- a/conf/airframes/examples/h_hex.xml
+++ /dev/null
@@ -1,179 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/examples/krooz_sd_quad_mkk.xml b/conf/airframes/examples/krooz_sd_quad_mkk.xml
deleted file mode 100644
index 6270fe8080..0000000000
--- a/conf/airframes/examples/krooz_sd_quad_mkk.xml
+++ /dev/null
@@ -1,212 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/examples/quadrotor_navstik.xml b/conf/airframes/examples/quadrotor_navstik.xml
deleted file mode 100644
index 2e7a49dadd..0000000000
--- a/conf/airframes/examples/quadrotor_navstik.xml
+++ /dev/null
@@ -1,217 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/examples/setup_apogee.xml b/conf/airframes/examples/setup_apogee.xml
index 547cf779b7..1163b3c580 100644
--- a/conf/airframes/examples/setup_apogee.xml
+++ b/conf/airframes/examples/setup_apogee.xml
@@ -1,6 +1,6 @@
-
+
diff --git a/conf/airframes/examples/setup_elle0.xml b/conf/airframes/examples/setup_elle0.xml
index 8f00cfb54e..653d839c9c 100644
--- a/conf/airframes/examples/setup_elle0.xml
+++ b/conf/airframes/examples/setup_elle0.xml
@@ -28,8 +28,6 @@
-
-
diff --git a/conf/airframes/examples/setup_lisam2.xml b/conf/airframes/examples/setup_lisam2.xml
index 5fa2b2a381..b39b8132ac 100644
--- a/conf/airframes/examples/setup_lisam2.xml
+++ b/conf/airframes/examples/setup_lisam2.xml
@@ -1,6 +1,6 @@
-
+
@@ -29,8 +29,6 @@
-
-
diff --git a/conf/airframes/testhardware/LisaL_v1.1_aspirin_v1.5_fw.xml b/conf/airframes/testhardware/LisaL_v1.1_aspirin_v1.5_fw.xml
deleted file mode 100644
index 16e8b7b05a..0000000000
--- a/conf/airframes/testhardware/LisaL_v1.1_aspirin_v1.5_fw.xml
+++ /dev/null
@@ -1,195 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/testhardware/LisaL_v1.1_aspirin_v1.5_rc.xml b/conf/airframes/testhardware/LisaL_v1.1_aspirin_v1.5_rc.xml
deleted file mode 100644
index f112773b81..0000000000
--- a/conf/airframes/testhardware/LisaL_v1.1_aspirin_v1.5_rc.xml
+++ /dev/null
@@ -1,202 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/testhardware/LisaL_v1.1_b2_v1.2_fw.xml b/conf/airframes/testhardware/LisaL_v1.1_b2_v1.2_fw.xml
deleted file mode 100644
index 5287899b2a..0000000000
--- a/conf/airframes/testhardware/LisaL_v1.1_b2_v1.2_fw.xml
+++ /dev/null
@@ -1,194 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/testhardware/LisaL_v1.1_b2_v1.2_rc.xml b/conf/airframes/testhardware/LisaL_v1.1_b2_v1.2_rc.xml
deleted file mode 100644
index 4670986243..0000000000
--- a/conf/airframes/testhardware/LisaL_v1.1_b2_v1.2_rc.xml
+++ /dev/null
@@ -1,227 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/tudelft/mavshot.xml b/conf/airframes/tudelft/mavshot.xml
deleted file mode 100644
index 6692f72ccd..0000000000
--- a/conf/airframes/tudelft/mavshot.xml
+++ /dev/null
@@ -1,289 +0,0 @@
-
-
- Quadshot vehicle equiped with Asctec motor controllers.
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/beagle_bone_black.xml b/conf/airframes/untested/beagle_bone_black.xml
deleted file mode 100644
index d8f5089201..0000000000
--- a/conf/airframes/untested/beagle_bone_black.xml
+++ /dev/null
@@ -1,44 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/demo_cc3d.xml b/conf/airframes/untested/demo_cc3d.xml
deleted file mode 100644
index c6fd76cfa1..0000000000
--- a/conf/airframes/untested/demo_cc3d.xml
+++ /dev/null
@@ -1,58 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/hex_naze32.xml b/conf/airframes/untested/hex_naze32.xml
deleted file mode 100644
index 2f80cfae37..0000000000
--- a/conf/airframes/untested/hex_naze32.xml
+++ /dev/null
@@ -1,189 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/krooz_sd_bre_hexa_mkk.xml b/conf/airframes/untested/krooz_sd_bre_hexa_mkk.xml
deleted file mode 100644
index f6b331f990..0000000000
--- a/conf/airframes/untested/krooz_sd_bre_hexa_mkk.xml
+++ /dev/null
@@ -1,212 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/krooz_sd_fw.xml b/conf/airframes/untested/krooz_sd_fw.xml
deleted file mode 100644
index e4fe89901e..0000000000
--- a/conf/airframes/untested/krooz_sd_fw.xml
+++ /dev/null
@@ -1,189 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/krooz_sd_hexa_mkk.xml b/conf/airframes/untested/krooz_sd_hexa_mkk.xml
deleted file mode 100644
index f6cf60d54a..0000000000
--- a/conf/airframes/untested/krooz_sd_hexa_mkk.xml
+++ /dev/null
@@ -1,212 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/krooz_sd_okto_mkk.xml b/conf/airframes/untested/krooz_sd_okto_mkk.xml
deleted file mode 100644
index 796a76b1b3..0000000000
--- a/conf/airframes/untested/krooz_sd_okto_mkk.xml
+++ /dev/null
@@ -1,217 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/krooz_sd_quad_pwm.xml b/conf/airframes/untested/krooz_sd_quad_pwm.xml
deleted file mode 100644
index 4f25780496..0000000000
--- a/conf/airframes/untested/krooz_sd_quad_pwm.xml
+++ /dev/null
@@ -1,201 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/lisa_asctec.xml b/conf/airframes/untested/lisa_asctec.xml
deleted file mode 100644
index 4289cab55c..0000000000
--- a/conf/airframes/untested/lisa_asctec.xml
+++ /dev/null
@@ -1,192 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/quad_cc3d.xml b/conf/airframes/untested/quad_cc3d.xml
deleted file mode 100644
index 7333a8d1df..0000000000
--- a/conf/airframes/untested/quad_cc3d.xml
+++ /dev/null
@@ -1,189 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/quad_cjmcu.xml b/conf/airframes/untested/quad_cjmcu.xml
deleted file mode 100644
index 36fc5d5e44..0000000000
--- a/conf/airframes/untested/quad_cjmcu.xml
+++ /dev/null
@@ -1,220 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/quad_flip32.xml b/conf/airframes/untested/quad_flip32.xml
deleted file mode 100644
index f397c36158..0000000000
--- a/conf/airframes/untested/quad_flip32.xml
+++ /dev/null
@@ -1,203 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/quad_revo_nano.xml b/conf/airframes/untested/quad_revo_nano.xml
deleted file mode 100644
index 9f3ebd634b..0000000000
--- a/conf/airframes/untested/quad_revo_nano.xml
+++ /dev/null
@@ -1,195 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/quadrotor_lisa_m_mkk.xml b/conf/airframes/untested/quadrotor_lisa_m_mkk.xml
deleted file mode 100644
index 6cc624d762..0000000000
--- a/conf/airframes/untested/quadrotor_lisa_m_mkk.xml
+++ /dev/null
@@ -1,179 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/quadrotor_mlkf.xml b/conf/airframes/untested/quadrotor_mlkf.xml
deleted file mode 100644
index c7d31d41d6..0000000000
--- a/conf/airframes/untested/quadrotor_mlkf.xml
+++ /dev/null
@@ -1,189 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/quadrotor_pixhawk_lite.xml b/conf/airframes/untested/quadrotor_pixhawk_lite.xml
deleted file mode 100644
index 9834707b87..0000000000
--- a/conf/airframes/untested/quadrotor_pixhawk_lite.xml
+++ /dev/null
@@ -1,225 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/airframes/untested/quadrotor_px4fmu.xml b/conf/airframes/untested/quadrotor_px4fmu.xml
deleted file mode 100644
index 897eacb09c..0000000000
--- a/conf/airframes/untested/quadrotor_px4fmu.xml
+++ /dev/null
@@ -1,184 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index e6e8993656..29450fb7fd 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -21,17 +21,6 @@
settings_modules="modules/gps_ubx_ucenter.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
gui_color="white"
/>
-
-
-
-
-
-
-
-
+
@@ -16,9 +17,5 @@
-
-
-
-
diff --git a/conf/modules/actuators_asctec.xml b/conf/modules/actuators_asctec.xml
deleted file mode 100644
index 3d631243e1..0000000000
--- a/conf/modules/actuators_asctec.xml
+++ /dev/null
@@ -1,35 +0,0 @@
-
-
-
-
-
- Actuators Driver for Asctec V1 ESC
-
- required xml configuration:
- - servo section with driver="Asctec"
- - command_laws section to map motor_mixing commands to servos
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/actuators_asctec_v2_new.xml b/conf/modules/actuators_asctec_v2_new.xml
deleted file mode 100644
index 117d5d6223..0000000000
--- a/conf/modules/actuators_asctec_v2_new.xml
+++ /dev/null
@@ -1,39 +0,0 @@
-
-
-
-
-
- New Actuators Driver for Asctec V2 ESC
-
- required xml configuration:
- - servo section with driver="Asctec_v2_new"
- - command_laws section to map motor_mixing commands to servos
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/actuators_bebop.xml b/conf/modules/actuators_bebop.xml
index b0fad99301..7638f6a6fa 100644
--- a/conf/modules/actuators_bebop.xml
+++ b/conf/modules/actuators_bebop.xml
@@ -7,6 +7,7 @@
+
diff --git a/conf/modules/actuators_mkk.xml b/conf/modules/actuators_mkk.xml
deleted file mode 100644
index e66e100fba..0000000000
--- a/conf/modules/actuators_mkk.xml
+++ /dev/null
@@ -1,49 +0,0 @@
-
-
-
-
-
- Actuators Driver for Mikrokopter V1 ESC
-
- required xml configuration:
- - configuration section (number of motors and addresses)
- - servo section with driver="Mkk"
- - command_laws section to map motor_mixing commands to servos (max command = 255)
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/actuators_mkk_v2.xml b/conf/modules/actuators_mkk_v2.xml
deleted file mode 100644
index 2a35cb7682..0000000000
--- a/conf/modules/actuators_mkk_v2.xml
+++ /dev/null
@@ -1,49 +0,0 @@
-
-
-
-
-
- Actuators Driver for Mikrokopter V2 ESC
-
- required xml configuration:
- - configuration section (number of motors and addresses)
- - servo section with driver="Mkk_v2"
- - command_laws section to map motor_mixing commands to servos (max command = 255)
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/actuators_nps.xml b/conf/modules/actuators_nps.xml
new file mode 100644
index 0000000000..aff3544fdb
--- /dev/null
+++ b/conf/modules/actuators_nps.xml
@@ -0,0 +1,14 @@
+
+
+
+
+
+ Actuators for NPS (dummy pwm)
+
+
+
+
+
+
+
+
diff --git a/conf/modules/ahrs_infrared.xml b/conf/modules/ahrs_infrared.xml
deleted file mode 100644
index 85f6c163f4..0000000000
--- a/conf/modules/ahrs_infrared.xml
+++ /dev/null
@@ -1,27 +0,0 @@
-
-
-
-
-
- AHRS infrared.
- Attitude estimation using infrared sensors detecting the horizon.
- For fixedwings only:
- - GPS course is used as heading.
- - ADC channels can be used for gyros.
-
-
-
- infrared_adc
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/ahrs_int_cmpl_euler.xml b/conf/modules/ahrs_int_cmpl_euler.xml
deleted file mode 100644
index f62bb26e49..0000000000
--- a/conf/modules/ahrs_int_cmpl_euler.xml
+++ /dev/null
@@ -1,68 +0,0 @@
-
-
-
-
-
- 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/cartography.xml b/conf/modules/cartography.xml
deleted file mode 100644
index 8e7f22bea8..0000000000
--- a/conf/modules/cartography.xml
+++ /dev/null
@@ -1,16 +0,0 @@
-
-
-
-
- Cartography sweeping pattern for navigation
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/configure_actuators_mkk_v2.xml b/conf/modules/configure_actuators_mkk_v2.xml
deleted file mode 100644
index 21b1e6c366..0000000000
--- a/conf/modules/configure_actuators_mkk_v2.xml
+++ /dev/null
@@ -1,33 +0,0 @@
-
-
-
-
- Configure Mikrokopter MKK v2.0 BLDC motor controllers (requires subsystem actuators_mkk_v2)
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/infrared_adc.xml b/conf/modules/infrared_adc.xml
deleted file mode 100644
index c662624692..0000000000
--- a/conf/modules/infrared_adc.xml
+++ /dev/null
@@ -1,55 +0,0 @@
-
-
-
-
- Infrared sensor using ADC
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-ADC_IR_TOP ?= ADC_0
-ADC_IR1 ?= ADC_1
-ADC_IR2 ?= ADC_2
-ADC_IR_NB_SAMPLES ?= 16
-
-ap.CFLAGS += -DADC_CHANNEL_IR1=$(ADC_IR1) -DUSE_$(ADC_IR1)
-ap.CFLAGS += -DADC_CHANNEL_IR2=$(ADC_IR2) -DUSE_$(ADC_IR2)
-ap.CFLAGS += -DADC_CHANNEL_IR_TOP=$(ADC_IR_TOP) -DUSE_$(ADC_IR_TOP)
-
-ap.CFLAGS += -DADC_CHANNEL_IR_NB_SAMPLES=$(ADC_IR_NB_SAMPLES)
-
-
-
-
-
diff --git a/conf/modules/infrared_i2c.xml b/conf/modules/infrared_i2c.xml
deleted file mode 100644
index 48dd84b439..0000000000
--- a/conf/modules/infrared_i2c.xml
+++ /dev/null
@@ -1,39 +0,0 @@
-
-
-
-
-
- I2C Infrared sensor
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/mag_micromag_fw.xml b/conf/modules/mag_micromag_fw.xml
deleted file mode 100644
index 91fe45e416..0000000000
--- a/conf/modules/mag_micromag_fw.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-
-
-
-
-
- Micromag magnetometer.
- for fixedwing
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/photogrammetry_calculator.xml b/conf/modules/photogrammetry_calculator.xml
deleted file mode 100644
index 9d3d96600a..0000000000
--- a/conf/modules/photogrammetry_calculator.xml
+++ /dev/null
@@ -1,41 +0,0 @@
-
-
-
-
- Photogrammetry navigation functions
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/modules/sim.xml b/conf/modules/sim.xml
index 3bc267035b..66502e0720 100644
--- a/conf/modules/sim.xml
+++ b/conf/modules/sim.xml
@@ -23,7 +23,7 @@
-
+
diff --git a/conf/settings/modules/config_asctec.xml b/conf/settings/modules/config_asctec.xml
deleted file mode 100644
index c4abb2588f..0000000000
--- a/conf/settings/modules/config_asctec.xml
+++ /dev/null
@@ -1,15 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/settings/modules/config_asctec_v2_new.xml b/conf/settings/modules/config_asctec_v2_new.xml
deleted file mode 100644
index 25f0b2b8d8..0000000000
--- a/conf/settings/modules/config_asctec_v2_new.xml
+++ /dev/null
@@ -1,11 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/settings/rc_settings_infrared.xml b/conf/settings/rc_settings_infrared.xml
deleted file mode 100644
index e2147cefd2..0000000000
--- a/conf/settings/rc_settings_infrared.xml
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/conf/userconf/OPENUAS/openuas_all_ac.xml b/conf/userconf/OPENUAS/openuas_all_ac.xml
index a494eb3778..1bd85fb771 100644
--- a/conf/userconf/OPENUAS/openuas_all_ac.xml
+++ b/conf/userconf/OPENUAS/openuas_all_ac.xml
@@ -241,17 +241,6 @@
settings_modules="modules/ahrs_float_cmpl_quat.xml modules/air_data.xml modules/photogrammetry_calculator.xml modules/geo_mag.xml modules/gps.xml modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/tune_airspeed.xml modules/nav_basic_fw.xml modules/nav_smooth.xml modules/nav_survey_poly_osam.xml modules/stabilization_adaptive_fw.xml"
gui_color="#fffff44a2254"
/>
-
-
-
-
+#include "generated/airframe.h"
+
+#include
+
+float sim_air_speed;
+
+value set_airspeed(value air_speed)
+{
+ sim_air_speed = Double_val(air_speed);
+ return Val_unit;
+}
+
diff --git a/sw/airborne/arch/sim/sim_ir.c b/sw/airborne/arch/sim/sim_ir.c
deleted file mode 100644
index 41223d34f9..0000000000
--- a/sw/airborne/arch/sim/sim_ir.c
+++ /dev/null
@@ -1,37 +0,0 @@
-/** \file sim_ir.c
- * \brief Regroup functions to simulate autopilot/infrared.c
- *
- * Infrared soft simulation. OCaml binding.
- */
-
-
-#include
-#include "subsystems/sensors/infrared.h"
-#include "generated/airframe.h"
-
-#include
-
-float sim_air_speed;
-
-void ir_gain_calib(void)
-{
-}
-
-value set_ir_and_airspeed(
- value roll __attribute__((unused)),
- value front __attribute__((unused)),
- value top __attribute__((unused)),
- value air_speed
-)
-{
- // USE_INFRARED : Stupid hack, since sim always calls this function,
- // but we don't always have an infrared module
-#if USE_INFRARED
- infrared.roll = Int_val(roll);
- infrared.pitch = Int_val(front);
- infrared.top = Int_val(top);
-#endif
- sim_air_speed = Double_val(air_speed);
- return Val_unit;
-}
-
diff --git a/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c b/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
index e28fa4d39d..21b01b9438 100644
--- a/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
+++ b/sw/airborne/firmwares/fixedwing/fixedwing_datalink.c
@@ -114,15 +114,7 @@ void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct
#endif /** WIND_INFO */
#ifdef HITL
- /** 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(buf);
- infrared.pitch = DL_HITL_INFRARED_pitch(buf);
- infrared.top = DL_HITL_INFRARED_top(buf);
- }
- break;
-
+ /** GPS sensors are replaced by messages on the datalink */
case DL_HITL_UBX: {
/** This code simulates gps_ubx.c:parse_ubx() */
if (gps_msg_received) {
diff --git a/sw/airborne/modules/ahrs/ahrs_infrared.c b/sw/airborne/modules/ahrs/ahrs_infrared.c
deleted file mode 100644
index e6a90fb742..0000000000
--- a/sw/airborne/modules/ahrs/ahrs_infrared.c
+++ /dev/null
@@ -1,142 +0,0 @@
-/*
- * Copyright (C) 2011 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/**
- * @file modules/ahrs/ahrs_infrared.c
- *
- * Attitude estimation using infrared sensors detecting the horizon.
- * For fixedwings only:
- * - GPS course is used as heading.
- * - ADC channels can be used for gyros.
- *
- */
-
-#include "modules/ahrs/ahrs_infrared.h"
-
-#include "subsystems/sensors/infrared.h"
-#include "subsystems/imu.h"
-#include "subsystems/gps.h"
-
-#include "state.h"
-#include "subsystems/abi.h"
-
-#ifndef INFRARED_FILTER_ID
-#define INFRARED_FILTER_ID 2
-#endif
-
-static float heading;
-
-/** ABI binding for gyro data.
- * Used for gyro ABI messages.
- */
-#ifndef AHRS_INFRARED_GYRO_ID
-#define AHRS_INFRARED_GYRO_ID ABI_BROADCAST
-#endif
-static abi_event gyro_ev;
-
-#ifndef AHRS_INFRARED_GPS_ID
-#define AHRS_INFRARED_GPS_ID GPS_MULTI_ID
-#endif
-static abi_event gps_ev;
-void ahrs_infrared_update_gps(struct GpsState *gps_s);
-
-static void gyro_cb(uint8_t sender_id __attribute__((unused)),
- uint32_t stamp __attribute__((unused)),
- struct Int32Rates *gyro)
-{
- stateSetBodyRates_i(gyro);
-}
-
-static void gps_cb(uint8_t sender_id __attribute__((unused)),
- uint32_t stamp __attribute__((unused)),
- struct GpsState *gps_s)
-{
- ahrs_infrared_update_gps(gps_s);
-}
-
-
-#if PERIODIC_TELEMETRY
-#include "subsystems/datalink/telemetry.h"
-
-static void send_infrared(struct transport_tx *trans, struct link_device *dev)
-{
- pprz_msg_send_IR_SENSORS(trans, dev, AC_ID,
- &infrared.value.ir1, &infrared.value.ir2, &infrared.pitch, &infrared.roll, &infrared.top);
-}
-
-static void send_status(struct transport_tx *trans, struct link_device *dev)
-{
- uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top);
- uint8_t mde = 3;
- uint8_t id = INFRARED_FILTER_ID;
- if (contrast < 50) { mde = 7; }
- pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &contrast);
-}
-#endif
-
-
-void ahrs_infrared_init(void)
-{
- heading = 0.;
-
- AbiBindMsgIMU_GYRO_INT32(AHRS_INFRARED_GYRO_ID, &gyro_ev, gyro_cb);
- AbiBindMsgGPS(AHRS_INFRARED_GPS_ID, &gps_ev, &gps_cb);
-
-#if PERIODIC_TELEMETRY
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_IR_SENSORS, send_infrared);
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_status);
-#endif
-}
-
-
-void ahrs_infrared_update_gps(struct GpsState *gps_s)
-{
- float hspeed_mod_f = gps_s->gspeed / 100.;
- float course_f = gps_s->course / 1e7;
-
- // Heading estimator from wind-information, usually computed with -DWIND_INFO
- // wind_north and wind_east initialized to 0, so still correct if not updated
- float w_vn = cosf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->x;
- float w_ve = sinf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->y;
- heading = atan2f(w_ve, w_vn);
- if (heading < 0.) {
- heading += 2 * M_PI;
- }
-
-}
-
-void ahrs_infrared_periodic(void)
-{
- float phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
- float theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
-
- if (theta < -M_PI_2) { theta += M_PI; }
- else if (theta > M_PI_2) { theta -= M_PI; }
-
- if (phi >= 0) { phi *= infrared.correction_right; }
- else { phi *= infrared.correction_left; }
-
- if (theta >= 0) { theta *= infrared.correction_up; }
- else { theta *= infrared.correction_down; }
-
- struct FloatEulers att = { phi, theta, heading };
- stateSetNedToBodyEulers_f(&att);
-}
diff --git a/sw/airborne/modules/ahrs/ahrs_infrared.h b/sw/airborne/modules/ahrs/ahrs_infrared.h
deleted file mode 100644
index 8776a8c34e..0000000000
--- a/sw/airborne/modules/ahrs/ahrs_infrared.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*
- * Copyright (C) 2011 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/**
- * @file modules/ahrs/ahrs_infrared.h
- *
- * Fixedwing attitude estimation using infrared sensors.
- *
- */
-
-#ifndef AHRS_INFRARED_H
-#define AHRS_INFRARED_H
-
-#include "std.h"
-
-extern void ahrs_infrared_init(void);
-extern void ahrs_infrared_periodic(void);
-
-#endif /* AHRS_INFRARED_H */
diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c
deleted file mode 100644
index 401d5c1567..0000000000
--- a/sw/airborne/modules/cartography/cartography.c
+++ /dev/null
@@ -1,723 +0,0 @@
-/*
- * Copyright (C) 2011 Vandeportaele Bertrand
- *
- * 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 cartography.c
- * \brief Navigation functions for cartography of the ground
- *
- */
-
-
-
-
-#include "state.h"
-#include "stdio.h"
-
-#include "firmwares/fixedwing/nav.h"
-#include "generated/flight_plan.h"
-
-#include "std.h" //macros pas mal dans sw/include
-
-////////////////////////////////////////////////////////////////////////////////////////////////
-//for fast debbuging, the simulation can be accelerated using the gaia software from an xterm console
-// /home/bvdp/paparazzi3/sw/simulator/gaia
-////////////////////////////////////////////////////////////////////////////////////////////////
-// for explanations about debugging macros:
-//http://gcc.gnu.org/onlinedocs/cpp/Stringification.html#Stringification
-
-// Be carefull not to use printf function in ap compilation, only use it in sim compilation
-// the DEBUG_PRINTF should be defined only in the sim part of the makefile airframe file
-#ifdef DEBUG_PRINTF
-int CPTDEBUG = 0;
-#define PRTDEB(TYPE,EXP) \
- printf("%5d: " #EXP ": %"#TYPE"\n",CPTDEBUG,EXP);fflush(stdout);CPTDEBUG++;
-#define PRTDEBSTR(EXP) \
- printf("%5d: STR: "#EXP"\n",CPTDEBUG);fflush(stdout);CPTDEBUG++;
-#else
-#define PRTDEB(TYPE,EXP) \
- ;
-
-#define PRTDEBSTR(EXP) \
- ;
-#endif
-
-/*
- exemple of use for theese macros
- PRTDEBSTR(Init polysurvey)
- PRTDEB(u,SurveySize)
-
- PRTDEB(lf,PolygonCenter.x)
- PRTDEB(lf,PolygonCenter.y)
- */
-////////////////////////////////////////////////////////////////////////////////////////////////
-
-#define DISTXY(P1X,P1Y,P2X,P2Y)\
- (sqrt( ( (P2X-P1X) * (P2X-P1X) ) + ( (P2Y-P1Y) * (P2Y-P1Y) ) ) )
-
-#define NORMXY(P1X,P1Y)\
- (sqrt( ( (P1X) * (P1X) ) + ( (P1Y) * (P1Y) ) ) )
-
-
-//max distance between the estimator position and an objective points to consider that the objective points is atteined
-
-#define DISTLIMIT 30
-
-////////////////////////////////////////////////////////////////////////////////////////////////
-
-uint16_t railnumberSinceBoot =
- 1; //used to count the number of rails the plane has achieved since the boot, to number the sequences of saved images
-//the number 1 is reserved for snapshot fonctions that take only one image, the 2-65535 numbers are used to number the following sequences
-
-////////////////////////////////////////////////////////////////////////////////////////////////
-#define USE_ONBOARD_CAMERA 1
-
-#if USE_ONBOARD_CAMERA
-uint16_t camera_snapshot_image_number = 0;
-#endif
-
-
-
-
-////////////////////////////////////////////////////////////////////////////////////////////////
-bool survey_losange_uturn;//this flag indicates if the aircraft is turning between 2 rails (1) or if it is flying straight forward in the rail direction (0)
-
-int railnumber; //indicate the number of the rail being acquired
-int numberofrailtodo;
-
-float distrail; //distance between adjacent rails in meters, the value is set in the init function
-float distplus; //distance that the aircraft should travel before and after a rail before turning to the next rails in meters, the value is set in the init function
-
-float distrailinteractif =
- 60; //a cheangable value that can be set interactively in the GCS, not used at that time, it can be used to choose the right value while the aircraft is flying
-
-
-static struct point point1, point2, point3; // 3 2D points used for navigation
-static struct point pointA, pointB, pointC; // 3 2D points used for navigation
-static struct point vec12, vec13;
-float norm12, norm13; // norms of 12 & 13 vectors
-
-
-
-
-float tempx, tempy; //temporary points for exchanges
-float angle1213; //angle between 12 & 13 vectors
-float signforturn; //can be +1 or -1, it is used to compute the direction of the UTURN between rails
-
-float tempcircleradius;// used to compute the radius of the UTURN after a rail
-float circleradiusmin = 40;
-
-////////////////////////////////////////////////////////////////////////////////////////////////
-float normBM, normAM, distancefromrail;
-
-
-float course_next_rail;
-float angle_between;
-
-bool ProjectionInsideLimitOfRail;
-
-
-
-
-#include "modules/cartography/cartography.h"
-#include "generated/modules.h"
-
-#include "subsystems/datalink/downlink.h"
-#include "mcu_periph/uart.h"
-#include "std.h"
-
-
-////////////////////////////////////////////////////////////////////////////////////////////////
-
-void init_carto(void)
-{
-}
-
-void periodic_downlink_carto(void)
-{
- static uint16_t dummy_id = 0;
- static uint8_t dummy_state = 0;
- static uint8_t snapshot_valid = 1;
- static float dummy_temp = NAN;
- DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,
- &dummy_d,
- &dummy_state,
- &camera_snapshot_image_number,
- &snapshot_valid,
- &dummy_temp);
-}
-
-void start_carto(void)
-{
-}
-
-void stop_carto(void)
-{
-}
-
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////
-bool nav_survey_Inc_railnumberSinceBoot(void)
-{
- railnumberSinceBoot++;
- return false;
-}
-///////////////////////////////////////////////////////////////////////////////////////////////
-bool nav_survey_Snapshoot(void)
-{
- camera_snapshot_image_number = railnumberSinceBoot;
- PRTDEBSTR(SNAPSHOT)
- cartography_periodic_downlink_carto_status = MODULES_START;
- return false;
-
-}
-///////////////////////////////////////////////////////////////////////////////////////////////
-bool nav_survey_Snapshoot_Continu(void)
-{
- camera_snapshot_image_number = railnumberSinceBoot;
- PRTDEBSTR(SNAPSHOT)
- cartography_periodic_downlink_carto_status = MODULES_START;
- return true;
-
-}
-///////////////////////////////////////////////////////////////////////////////////////////////
-bool nav_survey_StopSnapshoot(void)
-{
- camera_snapshot_image_number = 0;
- PRTDEBSTR(STOP SNAPSHOT)
- cartography_periodic_downlink_carto_status = MODULES_START;
- return false;
-
-}
-///////////////////////////////////////////////////////////////////////////////////////////////
-
-bool nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4)
-{
- waypoints[wp4].x = waypoints[wp2].x + waypoints[wp3].x - waypoints[wp1].x;
- waypoints[wp4].y = waypoints[wp2].y + waypoints[wp3].y - waypoints[wp1].y;
-
- PRTDEBSTR(nav_survey_computefourth_corner)
- PRTDEB(f, waypoints[wp4].x)
- PRTDEB(f, waypoints[wp4].y)
- return false;
-}
-
-////////////////////////////////////////////////////////////////////////////////////////////////
-bool nav_survey_ComputeProjectionOnLine(struct point pointAf, struct point pointBf, float pos_xf, float pos_yf,
- float *normAMf, float *normBMf, float *distancefromrailf);
-
-
-bool nav_survey_ComputeProjectionOnLine(struct point pointAf, struct point pointBf, float pos_xf, float pos_yf,
- float *normAMf, float *normBMf, float *distancefromrailf)
-//return if the projection of the estimator on the AB line is located inside the AB interval
-{
- float a, b, c, xa, xb, xc, ya, yb, yc;
- float f;
- float AA1;
- float BB1;
- float YP;
- float XP;
-
- float AMx, AMy, BMx, BMy;
- //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
-
-
-
- xb = pointAf.x;
- yb = pointAf.y;
-
- xc = pointBf.x;
- yc = pointBf.y;
-
- xa = pos_xf;
- ya = pos_yf;
-
- //calcul des parametres de la droite pointAf pointBf
- a = yc - yb;
- b = xb - xc;
- c = (yb - yc) * xb + (xc - xb) * yb ;
-
- //calcul de la distance de la droite à l'avion
-
-
- if (fabs(a) > 1e-10) {
- *distancefromrailf = fabs((a * xa + b * ya + c) / sqrt(a * a + b *
- b)); //denominateur =0 iniquement si a=b=0 //peut arriver si 2 waypoints sont confondus
- } else {
- return 0;
- }
-
- PRTDEB(f, a)
- PRTDEB(f, b)
- PRTDEB(f, c)
- PRTDEB(f, *distancefromrailf)
-
-
- // calcul des coordonnées du projeté orthogonal M(xx,y) de A sur (BC)
- AA1 = (xc - xb);
- BB1 = (yc - yb);
- if (fabs(AA1) > 1e-10) {
- f = (b - (a * BB1 / AA1));
- if (fabs(f) > 1e-10) {
- YP = (-(a * xa) - (a * BB1 * ya / AA1) - c) / f;
- } else {
- return 0;
- }
- } else {
- return 0;
- }
-
-
-
-
- XP = (-c - b * YP) / a ; //a !=0 deja testé avant
- //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
- //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
- //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!!
-
- PRTDEB(f, AA1)
- PRTDEB(f, BB1)
- PRTDEB(f, YP)
- PRTDEB(f, XP)
-
- AMx = XP - pointAf.x;
- AMy = YP - pointAf.y;
- BMx = XP - pointBf.x;
- BMy = YP - pointBf.y;
-
- *normAMf = NORMXY(AMx, AMy);
- *normBMf = NORMXY(BMx, BMy);
-
- PRTDEB(f, *normAMf)
- PRTDEB(f, *normBMf)
-
- if (((*normAMf) + (*normBMf)) > 1.05 * DISTXY(pointBf.x, pointBf.y, pointAf.x, pointAf.y)) {
- PRTDEBSTR(NOT INSIDE)
- return 0;
- } else {
- PRTDEBSTR(INSIDE)
- return 1;
- }
-}
-///////////////////////////////////////////////////////////////////////////
-//if distrailinit = 0, the aircraft travel from wp1 -> wp2 then do the inverse travel passing through the wp3,
-//This mode could be use to register bands of images aquired in a first nav_survey_losange_carto, done perpendicularly
-bool nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrailinit, float distplusinit)
-{
- //PRTDEBSTR(nav_survey_losange_carto_init)
- survey_losange_uturn = false;
-
-
- point1.x = waypoints[wp1].x; //the coordinates are in meter units, taken from the flight plan, in float type
- point1.y = waypoints[wp1].y;
- point2.x = waypoints[wp2].x;
- point2.y = waypoints[wp2].y;
- point3.x = waypoints[wp3].x;
- point3.y = waypoints[wp3].y;
-
- PRTDEB(u, wp1)
- PRTDEB(f, point1.x)
- PRTDEB(f, point1.y)
-
- PRTDEB(u, wp2)
- PRTDEB(f, point2.x)
- PRTDEB(f, point2.y)
-
- PRTDEB(u, wp3)
- PRTDEB(f, point3.x)
- PRTDEB(f, point3.y)
-
-
-
- vec12.x = point2.x - point1.x;
- vec12.y = point2.y - point1.y;
- PRTDEB(f, vec12.x)
- PRTDEB(f, vec12.y)
-
- //TODO gerer le cas ou un golio met les points à la meme position -> norm=0 > /0
- norm12 = NORMXY(vec12.x, vec12.y);
-
- PRTDEB(f, norm12)
-
-
- vec13.x = point3.x - point1.x;
- vec13.y = point3.y - point1.y;
- PRTDEB(f, vec13.x)
- PRTDEB(f, vec13.y)
-
- norm13 = NORMXY(vec13.x, vec13.y);
- PRTDEB(f, norm13)
-
- //if (distrail<1e-15) //inutile distrail=0 pour recollage et dans ce cas, il prend la valeur norm13
- // return false;
-
-
- if (fabs(distrailinit) <= 1) {
- //is distrailinit==0, then the aircraft should do 2 passes to register the bands
- distrail = norm13;
- numberofrailtodo = 1;
- } else {
- //no, so normal trajectory
- distrail = fabs(distrailinit);
- numberofrailtodo = ceil(norm13 / distrail); //round to the upper integer
- }
-
- distplus = fabs(distplusinit);
-
-
- PRTDEB(f, distrail)
- PRTDEB(f, distplus)
- PRTDEB(d, numberofrailtodo)
- PRTDEB(d, railnumber)
- PRTDEB(d, railnumberSinceBoot)
-
- railnumber = -1; // the state is before the first rail, which is numbered 0
-
- if (norm12 < 1e-15) {
- return false;
- }
- if (norm13 < 1e-15) {
- return false;
- }
-
-
- angle1213 = (180 / 3.14159) * acos((((vec12.x * vec13.x) + (vec12.y * vec13.y))) / (norm12 *
- norm13)); //oriented angle between 12 and 13 vectors
-
- angle1213 = atan2f(vec13.y, vec13.x) - atan2f(vec12.y, vec12.x);
- while (angle1213 >= M_PI) { angle1213 -= 2 * M_PI; }
- while (angle1213 <= -M_PI) { angle1213 += 2 * M_PI; }
-
- PRTDEB(f, angle1213)
-
- if (angle1213 > 0) {
- signforturn = -1;
- } else {
- signforturn = 1;
- }
-
-
- return false; //Init function must return false, so that the next function in the flight plan is automatically executed
- //dans le flight_plan.h
- // if (! (nav_survey_losange_carto()))
- // NextStageAndBreak();
-}
-////////////////////////////////////////////////////////////////////////////////////////////////
-bool nav_survey_losange_carto(void)
-{
- //test pour modifier en vol la valeur distrail
-
- //distrail=distrailinteractif;
-
-
- //by default, a 0 is sent in the message DOWNLINK_SEND_CAMERA_SNAPSHOT,
- //if the aircraft is inside the region to map, camera_snapshot_image_number will be equal to the number of rail since the last boot (not since the nav_survey_losange_carto_init, in order to get different values for differents calls to the cartography function (this number is used to name the images on the hard drive
- camera_snapshot_image_number = 0;
-
-
- PRTDEB(f, distrail)
-
-
-
- PRTDEBSTR(nav_survey_losange_carto)
- PRTDEB(d, railnumber)
-
- PRTDEB(d, railnumberSinceBoot)
-
- //PRTDEB(f,stateGetPositionEnu_f()->x)
- //PRTDEB(f,stateGetPositionEnu_f()->y)
-
- //sortir du bloc si données abhérantes
- if (norm13 < 1e-15) {
- PRTDEBSTR(norm13 < 1e-15)
- return false;
- }
- if (norm12 < 1e-15) {
- PRTDEBSTR(norm13 < 1e-15)
- return false;
- }
- if (distrail < 1e-15) {
- PRTDEBSTR(distrail < 1e-15)
- return false;
- }
-
- if (survey_losange_uturn == FALSE) {
-
- if (railnumber == -1) {
- //se diriger vers le début du 1°rail
- PRTDEBSTR(approche debut rail 0)
- pointA.x = point1.x - (vec12.x / norm12) * distplus * 1.2; //on prend une marge plus grande pour arriver en ce point
- pointA.y = point1.y - (vec12.y / norm12) * distplus * 1.2; //car le virage n'est pas tres bien géré
-
-
- pointB.x = point2.x + (vec12.x / norm12) * distplus * 1.2; //on prend une marge plus grande pour arriver en ce point
- pointB.y = point2.y + (vec12.y / norm12) * distplus * 1.2; //car le virage n'est pas tres bien géré
-
- //PRTDEB(f,pointA.x)
- //PRTDEB(f,pointA.y)
-
-
- //the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time
- //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT)
- //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT)
-
-
- nav_survey_ComputeProjectionOnLine(pointA, pointB, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, &normAM,
- &normBM, &distancefromrail);
-
- if ((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointA.x, pointA.y) > 2 * DISTLIMIT)
- || (normBM < (DISTXY(pointB.x, pointB.y, pointA.x, pointA.y)))) {
- nav_route_xy(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointA.x, pointA.y);
- //nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y);
- } else {
- PRTDEBSTR(debut rail 0)
- //un fois arrivé, on commence le 1° rail;
- railnumber = 0;
- railnumberSinceBoot++;
-
- }
- }
-
-
- if (railnumber >= 0) {
- pointA.x = (point1.x - ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
- pointA.y = (point1.y - ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
-
- pointB.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
- pointB.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
-
-
-
-
-
- if ((railnumber % 2) == 0) { //rail n0, 2, 4, donc premiere direction, de wp1 vers wp2
- //rien a faire
- } else { //if ((railnumber %2)==1) //rail n1, 3, 5, donc seconde direction, de wp2 vers wp1
- //echange pointA et B
- tempx = pointA.x;
- tempy = pointA.y;
- pointA.x = pointB.x;
- pointA.y = pointB.y;
- pointB.x = tempx;
- pointB.y = tempy;
-
- }
-
- // PRTDEB(f,pointA.x)
- // PRTDEB(f,pointA.y)
- // PRTDEB(f,pointB.x)
- // PRTDEB(f,pointB.y)
- ProjectionInsideLimitOfRail = nav_survey_ComputeProjectionOnLine(pointA, pointB, stateGetPositionEnu_f()->x,
- stateGetPositionEnu_f()->y, &normAM, &normBM, &distancefromrail);
-
-
-
- // if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) >DISTLIMIT) &&
- // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
-
-
- if (!((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT)
- || (ProjectionInsideLimitOfRail && (normAM > (DISTXY(pointB.x, pointB.y, pointA.x, pointA.y))))))
- // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y))))
- {
- nav_route_xy(pointA.x, pointA.y, pointB.x, pointB.y);
- PRTDEBSTR(NAVROUTE)
-
-
- //est ce que l'avion est dans la zone ou il doit prendre des images?
- //DEJA APPELE AVANT LE IF
- // nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail);
-
- if ((normAM > distplus) && (normBM > distplus) && (distancefromrail < distrail / 2)) {
- //CAMERA_SNAPSHOT_REQUIERED=true;
- //camera_snapshot_image_number++;
- camera_snapshot_image_number = railnumberSinceBoot;
- PRTDEBSTR(SNAPSHOT)
- }
-
- }
-
- else { // virage
- //PRTDEBSTR(debut rail suivant)
- railnumber++;
- railnumberSinceBoot++;
-
- PRTDEB(d, railnumber)
- PRTDEB(d, railnumberSinceBoot)
- //CAMERA_SNAPSHOT_REQUIERED=true;
- //camera_snapshot_image_number++;
-
- PRTDEBSTR(UTURN)
- survey_losange_uturn = true;
-
- }
-
- if (railnumber > numberofrailtodo) {
- PRTDEBSTR(fin nav_survey_losange_carto)
- return false; //apparament, l'avion va au bloc suivant lorsque la fonction renvoie false
- }
-
- }
- } else { // (survey_losange_uturn==TRUE)
-
-
- if (distrail < 200) {
- //tourne autour d'un point à mi chemin entre les 2 rails
-
- //attention railnumber a été incrémenté en fin du rail précédent
-
- if ((railnumber % 2) == 1) { //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
- PRTDEBSTR(UTURN - IMPAIR)
- //fin du rail précédent
- pointA.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber - 1) * (vec13.x / norm13) * distrail);
- pointA.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber - 1) * (vec13.y / norm13) * distrail);
- //début du rail suivant
- pointB.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
- pointB.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
- //milieu
- waypoints[0].x = (pointA.x + pointB.x) / 2;
- waypoints[0].y = (pointA.y + pointB.y) / 2;
-
- tempcircleradius = distrail / 2;
- if (tempcircleradius < circleradiusmin) {
- tempcircleradius = circleradiusmin;
- }
-
-
- //fin du rail suivant
- pointC.x = (point1.x + ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
- pointC.y = (point1.y + ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
-
-
- course_next_rail = atan2(pointC.x - pointB.x, pointC.y - pointB.y);
- PRTDEB(f, course_next_rail)
- PRTDEB(f, stateGetHorizontalSpeedDir_f())
-
- angle_between = (course_next_rail - stateGetHorizontalSpeedDir_f());
- while (angle_between > M_PI) { angle_between -= 2 * M_PI; }
- while (angle_between < -M_PI) { angle_between += 2 * M_PI; }
-
- angle_between = DegOfRad(angle_between);
- PRTDEB(f, angle_between)
- //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
-
- NavCircleWaypoint(0, signforturn * tempcircleradius);
- if ((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT)
- || (angle_between > -10 && angle_between < 10)) {
- //l'avion fait le rail suivant
- survey_losange_uturn = false;
- PRTDEBSTR(FIN UTURN - IMPAIR)
- }
- } else { //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
- PRTDEBSTR(UTURN - PAIR)
- //fin du rail précédent
- pointA.x = (point1.x - ((vec12.x / norm12) * distplus)) + ((railnumber - 1) * (vec13.x / norm13) * distrail);
- pointA.y = (point1.y - ((vec12.y / norm12) * distplus)) + ((railnumber - 1) * (vec13.y / norm13) * distrail);
- //début du rail suivant
- pointB.x = (point1.x - ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
- pointB.y = (point1.y - ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
- //milieu
- waypoints[0].x = (pointA.x + pointB.x) / 2;
- waypoints[0].y = (pointA.y + pointB.y) / 2;
-
- tempcircleradius = distrail / 2;
- if (tempcircleradius < circleradiusmin) {
- tempcircleradius = circleradiusmin;
- }
-
-
-
-
- //fin du rail suivant
- pointC.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
- pointC.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
-
-
- course_next_rail = atan2(pointC.x - pointB.x, pointC.y - pointB.y);
- PRTDEB(f, course_next_rail)
- PRTDEB(f, stateGetHorizontalSpeedDir_f())
-
- angle_between = (course_next_rail - stateGetHorizontalSpeedDir_f());
- while (angle_between > M_PI) { angle_between -= 2 * M_PI; }
- while (angle_between < -M_PI) { angle_between += 2 * M_PI; }
-
- angle_between = DegOfRad(angle_between);
- PRTDEB(f, angle_between)
- //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE)
-
- NavCircleWaypoint(0, signforturn * (-1)*tempcircleradius);
- if ((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT)
- || (angle_between > -10 && angle_between < 10)) {
- //l'avion fait le rail suivant
- survey_losange_uturn = false;
- PRTDEBSTR(FIN UTURN - PAIR)
- }
- }
- } else {
- //Le virage serait trop grand, on va en ligne droite pour ne pas trop éloigner l'avion
-
- if ((railnumber % 2) == 1) { //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2
- PRTDEBSTR(TRANSIT - IMPAIR)
- //fin du rail précédent
- pointA.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber - 1) * (vec13.x / norm13) * distrail);
- pointA.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber - 1) * (vec13.y / norm13) * distrail);
- //début du rail suivant
- pointB.x = (point2.x + ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
- pointB.y = (point2.y + ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
- nav_route_xy(pointA.x, pointA.y, pointB.x, pointB.y);
- if (DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT) {
- //l'avion fait le rail suivant
- survey_losange_uturn = false;
- PRTDEBSTR(FIN TRANSIT - IMPAIR)
- }
- } else { //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1
- PRTDEBSTR(TRANSIT - PAIR)
- //fin du rail précédent
- pointA.x = (point1.x - ((vec12.x / norm12) * distplus)) + ((railnumber - 1) * (vec13.x / norm13) * distrail);
- pointA.y = (point1.y - ((vec12.y / norm12) * distplus)) + ((railnumber - 1) * (vec13.y / norm13) * distrail);
- //début du rail suivant
- pointB.x = (point1.x - ((vec12.x / norm12) * distplus)) + ((railnumber) * (vec13.x / norm13) * distrail);
- pointB.y = (point1.y - ((vec12.y / norm12) * distplus)) + ((railnumber) * (vec13.y / norm13) * distrail);
- nav_route_xy(pointA.x, pointA.y, pointB.x, pointB.y);
- if (DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT) {
- //l'avion fait le rail suivant
- survey_losange_uturn = false;
- PRTDEBSTR(FIN TRANSIT - PAIR)
- }
-
- }
-
- }
- }
-
-
- //////////////// FAUT IL METTRE l'APPEL A CES FONCTIONS???????????????,
- //NavVerticalAutoThrottleMode(0.); /* No pitch */
- //NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
-
-
-
- cartography_periodic_downlink_carto_status = MODULES_START;
-
- return true; //apparament pour les fonctions de tache=> true
-}
-////////////////////////////////////////////////////////////////////////////////////////////////
-
-
diff --git a/sw/airborne/modules/cartography/cartography.h b/sw/airborne/modules/cartography/cartography.h
deleted file mode 100644
index b4c3233bf8..0000000000
--- a/sw/airborne/modules/cartography/cartography.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/** Automatic survey of an oriented rectangle (defined by three points) */
-
-#ifndef CARTOGRAPHY_H
-#define CARTOGRAPHY_H
-
-
-
-
-void init_carto(void);
-void periodic_downlink_carto(void);
-void start_carto(void);
-void stop_carto(void);
-
-/*
- typedef enum {NS, WE} survey_orientation_t;
- */
-
-
-#if USE_ONBOARD_CAMERA
-extern bool CAMERA_SNAPSHOT_REQUIERED;
-extern uint16_t camera_snapshot_image_number;
-#endif
-
-extern float distrailinteractif; //pour exporter la variable et pouvoir la changer depuis settings
-
-
-
-///////////////////////////////////////////////////////////////////////////////////////////////
-
-
-extern bool nav_survey_Inc_railnumberSinceBoot(void);
-extern bool nav_survey_Snapshoot(void);
-bool nav_survey_Snapshoot_Continu(void);
-extern bool nav_survey_StopSnapshoot(void);
-extern bool nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4);
-
-extern bool nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrail, float distplus);
-
-extern bool nav_survey_losange_carto(
- void); // !!!! important il faut mettre void en parametres d'entrée, sinon le compilo dit: attention : function declaration isn»t a prototype
-
-//(uint8_t wp1, uint8_t wp2, uint8_t wp3);
-
-/*
- #define NavSurveylosange_cartoInit(_wp1, _wp2, _grid, _distplus) nav_survey_losange_init(_wp1, _wp2, _wp3, _grid, _distplus)
- #define NavSurveylosange_carto nav_survey_losange
- */
-
-
-
-#endif
-
diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.c b/sw/airborne/modules/cartography/photogrammetry_calculator.c
deleted file mode 100644
index b09692b0ce..0000000000
--- a/sw/airborne/modules/cartography/photogrammetry_calculator.c
+++ /dev/null
@@ -1,122 +0,0 @@
-/*
- * Copyright (C) 2009 Christophe De Wagter
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include "photogrammetry_calculator.h"
-
-#include "generated/airframe.h"
-#include "generated/flight_plan.h"
-
-/** Default sweep angle in radians from north */
-#ifndef PHOTOGRAMMETRY_SWEEP_ANGLE
-#define PHOTOGRAMMETRY_SWEEP_ANGLE 0
-#endif
-
-/** overlap 1-99 percent */
-#ifndef PHOTOGRAMMETRY_OVERLAP
-#define PHOTOGRAMMETRY_OVERLAP 50
-#endif
-
-/** sidelap 1-99 percent */
-#ifndef PHOTOGRAMMETRY_SIDELAP
-#define PHOTOGRAMMETRY_SIDELAP 50
-#endif
-
-/** mm pixel projection size */
-#ifndef PHOTOGRAMMETRY_RESOLUTION
-#define PHOTOGRAMMETRY_RESOLUTION 50
-#endif
-
-
-// Flightplan Paramters
-float photogrammetry_sweep_angle = 0; // in rad
-
-int photogrammetry_sidestep = 0;
-int photogrammetry_triggerstep = 0;
-int photogrammetry_height = 0;
-
-// Photogrammetry Goals
-int photogrammetry_sidelap; // Percent 0 - 100
-int photogrammetry_overlap; // Percent 0 - 100
-int photogrammetry_resolution; // Millimeter per pixel
-
-// Safety Aspects
-int photogrammetry_height_min;
-int photogrammetry_height_max;
-int photogrammetry_radius_min;
-
-
-void init_photogrammetry_calculator(void)
-{
- photogrammetry_sweep_angle = PHOTOGRAMMETRY_SWEEP_ANGLE;
-
- photogrammetry_sidelap = PHOTOGRAMMETRY_SIDELAP;
- photogrammetry_overlap = PHOTOGRAMMETRY_OVERLAP;
- photogrammetry_resolution = PHOTOGRAMMETRY_RESOLUTION;
-
- photogrammetry_height_min = PHOTOGRAMMETRY_HEIGHT_MIN;
- photogrammetry_height_max = PHOTOGRAMMETRY_HEIGHT_MAX;
- photogrammetry_radius_min = PHOTOGRAMMETRY_RADIUS_MIN;
-
- photogrammetry_calculator_update_camera2flightplan();
-}
-
-void photogrammetry_calculator_update_camera2flightplan(void)
-{
-
- // Photogrammetry Goals
- float photogrammetry_sidelap_f = ((float) photogrammetry_sidelap) / 100.0f;
- float photogrammetry_overlap_f = ((float) photogrammetry_overlap) / 100.0f;
-
- // Linear Projection Camera Model
- float viewing_ratio_height = ((float) PHOTOGRAMMETRY_SENSOR_HEIGHT) / ((float)PHOTOGRAMMETRY_FOCAL_LENGTH);
- float viewing_ratio_width = ((float) PHOTOGRAMMETRY_SENSOR_WIDTH) / ((float)PHOTOGRAMMETRY_FOCAL_LENGTH);
- float pixel_projection_width = viewing_ratio_width / ((float)PHOTOGRAMMETRY_PIXELS_WIDTH);
-
- // Flightplan Variables
- photogrammetry_height = ((float) photogrammetry_resolution) / pixel_projection_width / 1000.0f;
-
- if (photogrammetry_height > photogrammetry_height_max) {
- photogrammetry_height = photogrammetry_height_max;
- } else if (photogrammetry_height < photogrammetry_height_min) {
- photogrammetry_height = photogrammetry_height_min;
- }
-
- photogrammetry_sidestep = viewing_ratio_width * photogrammetry_height * (1.0f - photogrammetry_sidelap_f);
- photogrammetry_triggerstep = viewing_ratio_height * photogrammetry_height * (1.0f - photogrammetry_overlap_f);
-}
-
-void photogrammetry_calculator_update_flightplan2camera(void)
-{
- // Linear Projection Camera Model
- float viewing_ratio_height = ((float) PHOTOGRAMMETRY_SENSOR_HEIGHT) / ((float)PHOTOGRAMMETRY_FOCAL_LENGTH);
- float viewing_ratio_width = ((float) PHOTOGRAMMETRY_SENSOR_WIDTH) / ((float)PHOTOGRAMMETRY_FOCAL_LENGTH);
- float pixel_projection_width = viewing_ratio_width / ((float)PHOTOGRAMMETRY_PIXELS_WIDTH);
-
- // Resolution <-> Height
- photogrammetry_resolution = photogrammetry_height * 1000.0f * pixel_projection_width;
-
- // Overlap <-> track width
- photogrammetry_sidelap = 100.0f - photogrammetry_sidestep / viewing_ratio_width / photogrammetry_height * 100.0f;
- photogrammetry_overlap = 100.0f - photogrammetry_triggerstep / viewing_ratio_height / photogrammetry_height * 100.0f;
-}
-
-
diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.h b/sw/airborne/modules/cartography/photogrammetry_calculator.h
deleted file mode 100644
index e3a49928af..0000000000
--- a/sw/airborne/modules/cartography/photogrammetry_calculator.h
+++ /dev/null
@@ -1,152 +0,0 @@
-/*
- * Copyright (C) 2009 Christophe De Wagter
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** @file modules/cartography/photogrammetry_calculator.h
-
-Add to airframe file:
-
-@verbatim
-
-
-
-
-
-@endverbatim
-
-Add to flightplan or airframe file:
-@verbatim
-
-
-
-
-@endverbatim
-
-Add to flightplan
-@verbatim
-
-#define PHOTOGRAMMETRY_SWEEP_ANGLE RadOfDeg(53) // angle in radians from the North
-#define PHOTOGRAMMETRY_OVERLAP 50 // 1-99 Procent
-#define PHOTOGRAMMETRY_SIDELAP 50 // 1-99 Procent
-#define PHOTOGRAMMETRY_RESOLUTION 80 // mm pixel projection size
-
-
-
-
-
-
-
-
-
-
-@endverbatim
-
- */
-
-#ifndef PHOTOGRAMMETRY_CALCULATOR_H
-#define PHOTOGRAMMETRY_CALCULATOR_H
-
-#include "std.h"
-#include "paparazzi.h"
-
-#include "modules/nav/nav_survey_poly_osam.h"
-#include "modules/nav/nav_survey_polygon.h"
-
-
-// Flightplan Variables
-extern float photogrammetry_sweep_angle;
-extern int photogrammetry_sidestep;
-extern int photogrammetry_triggerstep;
-extern int photogrammetry_height;
-
-extern int photogrammetry_height_min;
-extern int photogrammetry_height_max;
-extern int photogrammetry_radius_min;
-
-
-// Photogrammetry Goals
-extern int photogrammetry_sidelap;
-extern int photogrammetry_overlap;
-extern int photogrammetry_resolution;
-
-void init_photogrammetry_calculator(void);
-void photogrammetry_calculator_update_camera2flightplan(void);
-void photogrammetry_calculator_update_flightplan2camera(void);
-
-// Update Flightplan on Camera Change
-#define photogrammetry_calculator_UpdateSideLap(X) { \
- photogrammetry_sidelap = X; \
- photogrammetry_calculator_update_camera2flightplan(); \
- }
-
-#define photogrammetry_calculator_UpdateOverLap(X) { \
- photogrammetry_overlap = X; \
- photogrammetry_calculator_update_camera2flightplan(); \
- }
-
-#define photogrammetry_calculator_UpdateResolution(X) { \
- photogrammetry_resolution = X; \
- photogrammetry_calculator_update_camera2flightplan(); \
- }
-
-// Update Camera on Flightplan Change
-#define photogrammetry_calculator_UpdateHeight(X) { \
- photogrammetry_height = X; \
- photogrammetry_calculator_update_flightplan2camera(); \
- }
-
-#define photogrammetry_calculator_UpdateSideStep(X) { \
- photogrammetry_sidestep = X; \
- photogrammetry_calculator_update_flightplan2camera(); \
- }
-
-#define photogrammetry_calculator_UpdateTriggerStep(X) { \
- photogrammetry_triggerstep = X; \
- photogrammetry_calculator_update_flightplan2camera(); \
- }
-
-
-// Flightplan Routine Wrappers
-#define PhotogrammetryCalculatorPolygonSurveyOsam(_WP, _COUNT) { \
- WaypointAlt(_WP) = photogrammetry_height + GROUND_ALT; \
- int _ang = 90 - DegOfRad(photogrammetry_sweep_angle); \
- while (_ang > 90) _ang -= 180; while (_ang < -90) _ang += 180; \
- nav_survey_poly_osam_setup((_WP), (_COUNT), 2*photogrammetry_sidestep, _ang); \
- }
-
-#define PhotogrammetryCalculatorPolygonSurvey(_WP, _COUNT) { \
- nav_survey_polygon_setup((_WP), (_COUNT), DegOfRad(photogrammetry_sweep_angle), \
- photogrammetry_sidestep, photogrammetry_triggerstep, \
- photogrammetry_radius_min, photogrammetry_height + GROUND_ALT); \
- }
-
-#endif
diff --git a/sw/airborne/modules/config/config_mkk_v2.c b/sw/airborne/modules/config/config_mkk_v2.c
deleted file mode 100644
index 97f214408e..0000000000
--- a/sw/airborne/modules/config/config_mkk_v2.c
+++ /dev/null
@@ -1,191 +0,0 @@
-/*
- * Copyright (C) 2013 Christophe De Wagter
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include "config_mkk_v2.h"
-#include "generated/airframe.h"
-#include "subsystems/actuators.h"
-
-void config_mkk_v2_parse_eeprom(void);
-
-struct config_mkk_v2_struct config_mkk_v2;
-
-void config_mkk_v2_init(void)
-{
- config_mkk_v2.nb_err = 0;
- config_mkk_v2.read_config = 0;
-
- config_mkk_v2.trans.status = I2CTransSuccess;
-
-}
-
-#include "subsystems/actuators/actuators_mkk_v2.h"
-
-void config_mkk_v2_periodic_read_status(void)
-{
- // Read Config
- if (config_mkk_v2.read_config > 0) {
- switch (config_mkk_v2.trans.status) {
- case I2CTransFailed:
- config_mkk_v2.read_config = 0;
- break;
- case I2CTransSuccess:
- case I2CTransDone:
- config_mkk_v2_parse_eeprom();
- config_mkk_v2.read_config = 0;
- break;
- default:
- return;
- }
- }
-}
-
-#include "mcu_periph/uart.h"
-#include "pprzlink/messages.h"
-#include "subsystems/datalink/downlink.h"
-
-
-void config_mkk_v2_periodic_telemetry(void)
-{
- static uint8_t send_nr = 0;
-
- DOWNLINK_SEND_MKK(DefaultChannel, DefaultDevice, &send_nr, &actuators_mkk_v2.data[send_nr].MaxPWM,
- &actuators_mkk_v2.data[send_nr].Current, &actuators_mkk_v2.data[send_nr].Temperature);
-
- send_nr++;
- if (send_nr >= ACTUATORS_MKK_V2_NB) {
- send_nr = 0;
- }
-}
-
-
-
-//////////////////////////////////////////////////////////////////
-// MKK Config
-
-uint8_t config_mkk_v2_crc(uint8_t offset);
-
-// Following 2 structs are known from: http://mikrokopter.de/mikrosvn/FlightCtrl/tags/V0.88n/twimaster.h
-
-/*
-typedef struct
-{
- uint8_t revision; // must be BL_revision
- uint8_t SetMask; // settings mask
- uint8_t PwmScaling; // maximum value of control pwm, acts like a thrust limit
- uint8_t CurrentLimit; // current limit in A
- uint8_t TempLimit; // in °C
- uint8_t CurrentScaling; // scaling factor for current measurement
- uint8_t BitConfig; // see defines above
- uint8_t crc; // checksum
-} __attribute__((packed)) config_mkk_v2_eeprom_t;
-
-extern config_mkk_v2_eeprom_t config_mkk_v2_eeprom;
-*/
-
-uint8_t config_mkk_v2_crc(uint8_t offset)
-{
- uint8_t crc = 0xaa;
- for (int i = offset; i < (offset + 7); i++) {
- crc += config_mkk_v2.trans.buf[i];
- }
- return crc;
-}
-
-
-config_mkk_v2_eeprom_t config_mkk_v2_eeprom;
-
-
-#define BL_READMODE_CONFIG 16
-#define config_mkk_v2_EEPROM_REVISION 2
-
-
-#define RETURN_IF_NOT_KILLMODE() \
- { \
- if (!actuators_delay_done) \
- return; \
- }
-
-void config_mkk_v2_read_eeprom(void)
-{
- // Activate decoder
- config_mkk_v2.read_config = 1;
-
- // Do not read config while running
- RETURN_IF_NOT_KILLMODE();
-
- // New I2C Write/Read Transaction
- config_mkk_v2.trans.type = I2CTransTxRx;
- config_mkk_v2.trans.slave_addr = 0x52 + config_mkk_v2.addr * 2;
- config_mkk_v2.trans.len_w = 2;
- config_mkk_v2.trans.buf[0] = 0;
- config_mkk_v2.trans.buf[1] = (BL_READMODE_CONFIG << 3);
- config_mkk_v2.trans.len_r = 8;
-
- i2c_submit(&ACTUATORS_MKK_V2_I2C_DEV, &config_mkk_v2.trans);
-}
-
-void config_mkk_v2_parse_eeprom(void)
-{
- config_mkk_v2_eeprom.crc = config_mkk_v2.trans.buf[7]; // checksum
- if (config_mkk_v2_crc(0) != config_mkk_v2_eeprom.crc) {
- config_mkk_v2.nb_err++;
- } else {
- config_mkk_v2_eeprom.revision = config_mkk_v2.trans.buf[0]; // must be BL_revision
- config_mkk_v2_eeprom.SetMask = config_mkk_v2.trans.buf[1]; // settings mask
- config_mkk_v2_eeprom.PwmScaling =
- config_mkk_v2.trans.buf[2]; // maximum value of control pwm, acts like a thrust limit
- config_mkk_v2_eeprom.CurrentLimit = config_mkk_v2.trans.buf[3]; // current limit in A
- config_mkk_v2_eeprom.TempLimit = config_mkk_v2.trans.buf[4]; // in °C
- config_mkk_v2_eeprom.CurrentScaling = config_mkk_v2.trans.buf[5]; // scaling factor for current measurement
- config_mkk_v2_eeprom.BitConfig = config_mkk_v2.trans.buf[6]; // see defines above
- }
-}
-
-void config_mkk_v2_send_eeprom(void)
-{
- // Do not upload while running
- RETURN_IF_NOT_KILLMODE();
-
- // Do not upload bad data:
- if (config_mkk_v2_eeprom.revision != config_mkk_v2_EEPROM_REVISION) {
- return;
- }
-
- // New I2C Write Transaction
- config_mkk_v2.trans.type = I2CTransTx;
- config_mkk_v2.trans.slave_addr = 0x52 + config_mkk_v2.addr * 2;
- config_mkk_v2.trans.len_w = 10;
- config_mkk_v2.trans.buf[0] = 0;
- config_mkk_v2.trans.buf[1] = (BL_READMODE_CONFIG << 3);
-
- config_mkk_v2.trans.buf[2] = config_mkk_v2_eeprom.revision;
- config_mkk_v2.trans.buf[3] = config_mkk_v2_eeprom.SetMask;
- config_mkk_v2.trans.buf[4] = config_mkk_v2_eeprom.PwmScaling;
- config_mkk_v2.trans.buf[5] = config_mkk_v2_eeprom.CurrentLimit;
- config_mkk_v2.trans.buf[6] = config_mkk_v2_eeprom.TempLimit;
- config_mkk_v2.trans.buf[7] = config_mkk_v2_eeprom.CurrentScaling;
- config_mkk_v2.trans.buf[8] = config_mkk_v2_eeprom.BitConfig;
- config_mkk_v2.trans.buf[9] = config_mkk_v2_crc(2);
-
- i2c_submit(&ACTUATORS_MKK_V2_I2C_DEV, &config_mkk_v2.trans);
-
-}
diff --git a/sw/airborne/modules/config/config_mkk_v2.h b/sw/airborne/modules/config/config_mkk_v2.h
deleted file mode 100644
index 5a4da39694..0000000000
--- a/sw/airborne/modules/config/config_mkk_v2.h
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * Copyright (C) 2013 Christophe De Wagter
- *
- * 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 config_mkk_v2.h
- *
- * Read Status and Config from MKK (Mikrokopter) BLDC motor controllers
- */
-
-#ifndef config_mkk_v2_MODULE_H
-#define config_mkk_v2_MODULE_H
-
-#include "std.h"
-
-#include "mcu_periph/i2c.h"
-
-struct config_mkk_v2_struct {
- uint8_t read_config;
- uint8_t addr;
-
- int nb_err;
-
- struct i2c_transaction trans;
-};
-
-extern struct config_mkk_v2_struct config_mkk_v2;
-
-void config_mkk_v2_init(void);
-void config_mkk_v2_periodic_read_status(void);
-void config_mkk_v2_periodic_telemetry(void);
-
-//////////////////////////////////////////////////////////////////
-// MKK Config
-
-typedef struct {
- uint8_t revision;
- uint8_t SetMask;
- uint8_t PwmScaling;
- uint8_t CurrentLimit;
- uint8_t TempLimit;
- uint8_t CurrentScaling;
- uint8_t BitConfig;
- uint8_t crc;
-} config_mkk_v2_eeprom_t;
-
-extern config_mkk_v2_eeprom_t config_mkk_v2_eeprom;
-
-
-#define CONFIG_MKK_V2_MASK_SET_PWM_SCALING 0x01
-#define CONFIG_MKK_V2_MASK_SET_CURRENT_LIMIT 0x02
-#define CONFIG_MKK_V2_MASK_SET_TEMP_LIMIT 0x04
-#define CONFIG_MKK_V2_MASK_SET_CURRENT_SCALING 0x08
-#define CONFIG_MKK_V2_MASK_SET_BITCONFIG 0x10
-#define CONFIG_MKK_V2_MASK_RESET_CAPCOUNTER 0x20
-#define CONFIG_MKK_V2_MASK_SET_DEFAULT_PARAMS 0x40
-#define CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM 0x80
-
-#define BITCONF_REVERSE_ROTATION 0x01
-
-
-extern void config_mkk_v2_send_eeprom(void);
-extern void config_mkk_v2_read_eeprom(void);
-
-#define config_mkk_v2_ResetDefault(_v) { \
- config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_DEFAULT_PARAMS; \
- config_mkk_v2_send_eeprom(); \
- }
-
-#define config_mkk_v2_SetPwmScaling(_v) { \
- config_mkk_v2_eeprom.PwmScaling = _v; \
- config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_PWM_SCALING; \
- config_mkk_v2_send_eeprom(); \
- }
-
-#define config_mkk_v2_SetCurrentLimit(_v) { \
- config_mkk_v2_eeprom.CurrentLimit = _v; \
- config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_CURRENT_LIMIT; \
- config_mkk_v2_send_eeprom(); \
- }
-
-#define config_mkk_v2_SetTempLimit(_v) { \
- config_mkk_v2_eeprom.TempLimit = _v; \
- config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_TEMP_LIMIT; \
- config_mkk_v2_send_eeprom(); \
- }
-
-#define config_mkk_v2_SetCurrentScaling(_v) { \
- config_mkk_v2_eeprom.CurrentScaling = _v; \
- config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_CURRENT_SCALING; \
- config_mkk_v2_send_eeprom(); \
- }
-
-#define config_mkk_v2_SetBitConfig(_v) { \
- config_mkk_v2_eeprom.BitConfig = _v; \
- config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_BITCONFIG; \
- config_mkk_v2_send_eeprom(); \
- }
-
-#define config_mkk_v2_GetConfig(_v) { \
- config_mkk_v2.addr = _v; \
- config_mkk_v2_read_eeprom(); \
- }
-
-
-
-#endif
-
-
-
diff --git a/sw/airborne/modules/sensors/infrared_adc.h b/sw/airborne/modules/sensors/infrared_adc.h
deleted file mode 100644
index 9a0efb291a..0000000000
--- a/sw/airborne/modules/sensors/infrared_adc.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * Copyright (C) 2010 ENAC
- *
- * 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.
- *
- */
-
-/*
- * Wrapper for adc infrared sensor module
- */
-
-#ifndef INFRARED_ADC_H
-#define INFRARED_ADC_H
-
-#include "subsystems/sensors/infrared_adc.h"
-
-#endif // INFRARED_ADC_H
-
diff --git a/sw/airborne/modules/sensors/infrared_i2c.h b/sw/airborne/modules/sensors/infrared_i2c.h
deleted file mode 100644
index 95e9ea1d01..0000000000
--- a/sw/airborne/modules/sensors/infrared_i2c.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * Copyright (C) 2010 ENAC
- *
- * 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.
- *
- */
-
-/*
- * Wrapper for i2c infrared sensor module
- */
-
-#ifndef INFRARED_I2C_H
-#define INFRARED_I2C_H
-
-#include "subsystems/sensors/infrared_i2c.h"
-
-#endif // INFRARED_I2C_H
diff --git a/sw/airborne/modules/sensors/mag_micromag_fw.c b/sw/airborne/modules/sensors/mag_micromag_fw.c
deleted file mode 100644
index f6f6a44387..0000000000
--- a/sw/airborne/modules/sensors/mag_micromag_fw.c
+++ /dev/null
@@ -1,74 +0,0 @@
-#include "modules/sensors/mag_micromag_fw.h"
-#include "modules/sensors/mag_micromag_fw_hw.h"
-#include "led.h"
-
-#include "mcu_periph/uart.h"
-#include "pprzlink/messages.h"
-#include "subsystems/datalink/downlink.h"
-
-volatile uint8_t micromag_status;
-volatile int16_t micromag_values[MM_NB_AXIS];
-
-
-void micromag_periodic(void)
-{
-
- static uint8_t cnt = 0;
-
- if (micromag_status == MM_IDLE) {
- // uint8_t * tab = &cnt;
- // DOWNLINK_SEND_DEBUG(1,tab);
- cnt = 0;
- MmSendReq();
- } else if (micromag_status == MM_GOT_EOC) {
- MmReadRes();
- } else if (micromag_status == MM_WAITING_EOC) {
- cnt++;
- if (cnt > 50) {cnt = 0; micromag_status = MM_IDLE;}
- }
-}
-
-void micromag_event(void)
-{
-
- int32_t mx = micromag_values[0];
- int32_t my = micromag_values[1];
- int32_t mz = micromag_values[2];
-
- if (micromag_status == MM_DATA_AVAILABLE) {
- DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,
- &mx,
- &my,
- &mz);
- micromag_status = MM_IDLE;
- }
-}
-
-void micromag_init(void)
-{
-
- micromag_hw_init();
-
- uint8_t i;
- for (i = 0; i < MM_NB_AXIS; i++) {
- micromag_values[i] = 0;
- }
- micromag_status = MM_IDLE;
-}
-
-void micromag_reset()
-{
- micromag_status = MM_IDLE;
-}
-
-void micromag_read()
-{
- if (micromag_status == MM_IDLE) {
- MmSendReq();
- } else if (micromag_status == MM_GOT_EOC) {
- MmReadRes();
- } else if (micromag_status == MM_DATA_AVAILABLE) {
- micromag_status = MM_IDLE;
- }
-}
-
diff --git a/sw/airborne/modules/sensors/mag_micromag_fw.h b/sw/airborne/modules/sensors/mag_micromag_fw.h
deleted file mode 100644
index 8c1e01a9f4..0000000000
--- a/sw/airborne/modules/sensors/mag_micromag_fw.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#ifndef MICROMAG_FW_H
-#define MICROMAG_FW_H
-
-
-#include "std.h"
-#define MM_NB_AXIS 3
-
-extern void micromag_init(void);
-extern void micromag_read(void);
-
-extern void micromag_reset(void);
-extern void micromag_periodic(void);
-extern void micromag_event(void);
-
-#define MM_IDLE 0
-#define MM_BUSY 1
-#define MM_SENDING_REQ 2
-#define MM_WAITING_EOC 3
-#define MM_GOT_EOC 4
-#define MM_READING_RES 5
-#define MM_DATA_AVAILABLE 6
-
-/* ssp input clock 468.75kHz, clock that divided by SCR+1 */
-#define SSP_CLOCK 468750
-
-/* SSPCR0 settings */
-#define SSP_DDS 0x07 << 0 /* data size : 8 bits */
-#define SSP_FRF 0x00 << 4 /* frame format : SPI */
-#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */
-#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */
-#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16 */
-
-/* SSPCR1 settings */
-#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
-#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */
-#define SSP_MS 0x00 << 2 /* master slave mode : master */
-#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */
-
-#define SS_PIN 20
-#define SS_IODIR IO0DIR
-#define SS_IOSET IO0SET
-#define SS_IOCLR IO0CLR
-
-#define SSPCR0_VAL (SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR )
-#define SSPCR1_VAL (SSP_LBM | SSP_SSE | SSP_MS | SSP_SOD )
-
-#define SSP_PINSEL1_SCK (2<<2)
-#define SSP_PINSEL1_MISO (2<<4)
-#define SSP_PINSEL1_MOSI (2<<6)
-
-#define SSP_Enable() SetBit(SSPCR1, SSE);
-#define SSP_Disable() ClearBit(SSPCR1, SSE);
-#define SSP_EnableRxi() SetBit(SSPIMSC, RXIM)
-#define SSP_DisableRxi() ClearBit(SSPIMSC, RXIM)
-#define SSP_EnableTxi() SetBit(SSPIMSC, TXIM)
-#define SSP_DisableTxi() ClearBit(SSPIMSC, TXIM)
-#define SSP_EnableRti() SetBit(SSPIMSC, RTIM);
-#define SSP_DisableRti() ClearBit(SSPIMSC, RTIM);
-#define SSP_ClearRti() SetBit(SSPICR, RTIC);
-
-extern volatile uint8_t micromag_status;
-extern volatile int16_t micromag_values[MM_NB_AXIS];
-
-#endif /* MICROMAG_H */
diff --git a/sw/airborne/modules/settings/rc_settings.c b/sw/airborne/modules/settings/rc_settings.c
index 71e6ceb01f..ffd8aa85bb 100644
--- a/sw/airborne/modules/settings/rc_settings.c
+++ b/sw/airborne/modules/settings/rc_settings.c
@@ -27,7 +27,6 @@
#include "modules/settings/rc_settings.h"
#include "autopilot.h"
#include "firmwares/fixedwing/nav.h"
-#include "subsystems/sensors/infrared.h"
#include "inter_mcu.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
diff --git a/sw/airborne/subsystems/actuators/actuators_asctec.c b/sw/airborne/subsystems/actuators/actuators_asctec.c
deleted file mode 100644
index 5627cf17f1..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_asctec.c
+++ /dev/null
@@ -1,130 +0,0 @@
-/*
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * 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 actuators_asctec.c
- * Actuators driver for Asctec motor controllers.
- */
-
-#include "subsystems/actuators.h"
-#include "subsystems/actuators/actuators_asctec.h"
-
-#include "mcu_periph/i2c.h"
-#include "mcu_periph/sys_time.h"
-
-#define ASCTEC_MIN_CMD -100
-#define ASCTEC_MAX_CMD 100
-
-#define ASCTEC_MIN_THROTTLE 0
-#define ASCTEC_MAX_THROTTLE 200
-
-#define ACTUATORS_ASCTEC_SLAVE_ADDR 0x02
-
-PRINT_CONFIG_VAR(ACTUATORS_ASCTEC_I2C_DEV)
-
-struct ActuatorsAsctec actuators_asctec;
-
-void actuators_asctec_init(void)
-{
- actuators_asctec.cmd = NONE;
- actuators_asctec.cur_addr = FRONT;
- actuators_asctec.new_addr = FRONT;
- actuators_asctec.i2c_trans.status = I2CTransSuccess;
- actuators_asctec.i2c_trans.type = I2CTransTx;
- actuators_asctec.i2c_trans.slave_addr = ACTUATORS_ASCTEC_SLAVE_ADDR;
- actuators_asctec.i2c_trans.len_w = 4;
- actuators_asctec.nb_err = 0;
-}
-
-void actuators_asctec_set(bool motors_on)
-{
-#if defined ACTUATORS_START_DELAY && ! defined SITL
- if (!actuators_delay_done) {
- if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) { return; }
- else { actuators_delay_done = true; }
- }
-#endif
-
- switch (actuators_asctec.i2c_trans.status) {
- case I2CTransFailed:
- actuators_asctec.nb_err++;
- actuators_asctec.i2c_trans.status = I2CTransDone;
- break;
- case I2CTransSuccess:
- case I2CTransDone:
- actuators_asctec.i2c_trans.status = I2CTransDone;
- break;
- default:
- actuators_asctec.nb_err++;
- return;
- }
-
-#ifdef KILL_MOTORS
- actuators_asctec.cmds[PITCH] = 0;
- actuators_asctec.cmds[ROLL] = 0;
- actuators_asctec.cmds[YAW] = 0;
- actuators_asctec.cmds[THRUST] = 0;
-#else /* ! KILL_MOTORS */
- Bound(actuators_asctec.cmds[PITCH], ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
- Bound(actuators_asctec.cmds[ROLL], ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
- Bound(actuators_asctec.cmds[YAW], ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
- if (motors_on) {
- Bound(actuators_asctec.cmds[THRUST], ASCTEC_MIN_THROTTLE + 1, ASCTEC_MAX_THROTTLE);
- } else {
- actuators_asctec.cmds[THRUST] = 0;
- }
-#endif /* KILL_MOTORS */
-
- switch (actuators_asctec.cmd) {
- case TEST:
- actuators_asctec.i2c_trans.buf[0] = 251;
- actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cur_addr;
- actuators_asctec.i2c_trans.buf[2] = 0;
- actuators_asctec.i2c_trans.buf[3] = 231 + actuators_asctec.cur_addr;
- break;
- case REVERSE:
- actuators_asctec.i2c_trans.buf[0] = 254;
- actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cur_addr;
- actuators_asctec.i2c_trans.buf[2] = 0;
- actuators_asctec.i2c_trans.buf[3] = 234 + actuators_asctec.cur_addr;
- break;
- case SET_ADDR:
- actuators_asctec.i2c_trans.buf[0] = 250;
- actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cur_addr;
- actuators_asctec.i2c_trans.buf[2] = actuators_asctec.new_addr;
- actuators_asctec.i2c_trans.buf[3] = 230 + actuators_asctec.cur_addr +
- actuators_asctec.new_addr;
- actuators_asctec.cur_addr = actuators_asctec.new_addr;
- break;
- case NONE:
- actuators_asctec.i2c_trans.buf[0] = 100 - actuators_asctec.cmds[PITCH];
- actuators_asctec.i2c_trans.buf[1] = 100 + actuators_asctec.cmds[ROLL];
- actuators_asctec.i2c_trans.buf[2] = 100 - actuators_asctec.cmds[YAW];
- actuators_asctec.i2c_trans.buf[3] = actuators_asctec.cmds[THRUST];
- break;
- default:
- break;
- }
- actuators_asctec.cmd = NONE;
-
- i2c_transmit(&ACTUATORS_ASCTEC_I2C_DEV, &actuators_asctec.i2c_trans,
- ACTUATORS_ASCTEC_SLAVE_ADDR, 4);
-
-}
diff --git a/sw/airborne/subsystems/actuators/actuators_asctec.h b/sw/airborne/subsystems/actuators/actuators_asctec.h
deleted file mode 100644
index 3c15e96f4e..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_asctec.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * 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 actuators_asctec.h
- * Actuators driver for Asctec motor controllers.
- */
-
-#ifndef ACTUATORS_ASCTEC_H
-#define ACTUATORS_ASCTEC_H
-
-#include "mcu_periph/i2c.h"
-
-#include "generated/airframe.h"
-
-enum actuators_asctec_cmd { NONE,
- TEST,
- REVERSE,
- SET_ADDR
- };
-
-enum actuators_asctec_addr { FRONT,
- BACK,
- LEFT,
- RIGHT
- };
-
-/* this is for the v1 protocol which does its own mixing */
-enum actuators_asctec_cmds { PITCH,
- ROLL,
- YAW,
- THRUST,
- CMD_NB
- };
-
-struct ActuatorsAsctec {
- enum actuators_asctec_cmd cmd;
- enum actuators_asctec_addr cur_addr;
- enum actuators_asctec_addr new_addr;
- int32_t cmds[CMD_NB];
- struct i2c_transaction i2c_trans;
- volatile uint32_t nb_err;
-};
-
-
-extern struct ActuatorsAsctec actuators_asctec;
-
-#define actuators_asctec_SetCommand(_v) { \
- actuators_asctec.cmd = _v; \
- }
-
-#define actuators_asctec_SetNewAddr(_v) { \
- actuators_asctec.new_addr = _v; \
- }
-
-#define actuators_asctec_SetCurAddr(_v) { \
- actuators_asctec.cur_addr = _v; \
- }
-
-extern void actuators_asctec_init(void);
-extern void actuators_asctec_set(bool motors_on);
-
-#define ActuatorAsctecSet(_i, _v) { actuators_asctec.cmds[_i] = _v; }
-#define ActuatorsAsctecInit() actuators_asctec_init()
-#define ActuatorsAsctecCommit() actuators_asctec_set(autopilot_get_motors_on())
-
-
-#endif /* ACTUATORS_ASCTEC_H */
diff --git a/sw/airborne/subsystems/actuators/actuators_asctec_v2_new.c b/sw/airborne/subsystems/actuators/actuators_asctec_v2_new.c
deleted file mode 100644
index 9b8eccf5b8..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_asctec_v2_new.c
+++ /dev/null
@@ -1,189 +0,0 @@
-/*
- * Copyright (C) 2013 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/** @file actuators_asctec_v2_new.c
- * Actuators driver for Asctec v2 motor controllers with the new I2C protocol.
- */
-
-#include "subsystems/actuators.h"
-#include "subsystems/actuators/actuators_asctec_v2_new.h"
-
-#include "mcu_periph/i2c.h"
-#include "mcu_periph/sys_time.h"
-
-#define ACTUATORS_ASCTEC_V2_SLAVE_ADDR 0x00
-
-PRINT_CONFIG_VAR(ACTUATORS_ASCTEC_V2_I2C_DEV)
-
-struct ActuatorsAsctecV2 actuators_asctec_v2;
-
-static uint16_t crc_update(uint16_t crc, uint8_t data);
-
-void actuators_asctec_v2_init(void)
-{
- actuators_asctec_v2.cmd = NONE;
- actuators_asctec_v2.cur_addr = FRONT;
- actuators_asctec_v2.new_addr = FRONT;
- actuators_asctec_v2.i2c_trans.status = I2CTransSuccess;
- actuators_asctec_v2.i2c_trans.type = I2CTransTx;
- actuators_asctec_v2.i2c_trans.slave_addr = ACTUATORS_ASCTEC_V2_SLAVE_ADDR;
- actuators_asctec_v2.i2c_trans.len_w = 5;
- actuators_asctec_v2.nb_err = 0;
-}
-
-
-void actuators_asctec_v2_set(void)
-{
-#if defined ACTUATORS_START_DELAY && ! defined SITL
- if (!actuators_delay_done) {
- if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) {
-#ifdef USE_I2C_ACTUATORS_REBOOT_HACK
- //Lisa-L with Asctech v2 motors only start after reflashing when a bus error was sensed on stm32-i2c.
- //multiple re-init solves the problem.
- i2c1_init();
-#endif
- return;
- } else { actuators_delay_done = true; }
- }
-#endif
-
- switch (actuators_asctec_v2.i2c_trans.status) {
- case I2CTransFailed:
- actuators_asctec_v2.nb_err++;
- actuators_asctec_v2.i2c_trans.status = I2CTransDone;
- break;
- case I2CTransSuccess:
- case I2CTransDone:
- actuators_asctec_v2.i2c_trans.status = I2CTransDone;
- break;
- default:
- actuators_asctec_v2.nb_err++;
- return;
- }
-
- uint16_t crc=0xff;
-
-#ifdef KILL_MOTORS
- actuators_asctec_v2.i2c_trans.buf[0] = 0;
- actuators_asctec_v2.i2c_trans.buf[1] = 0;
- actuators_asctec_v2.i2c_trans.buf[2] = 0;
- actuators_asctec_v2.i2c_trans.buf[3] = 0;
- actuators_asctec_v2.i2c_trans.buf[4] = 0xC0;
-
- for (uint8_t i=0;i<5;i++)
- crc=crc_update(crc,actuators_asctec_v2.i2c_trans.buf[i]);
-
- actuators_asctec_v2.i2c_trans.buf[5] = crc;
- i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 6);
-#else
- switch (actuators_asctec_v2.cmd) {
- case TEST:
- actuators_asctec_v2.i2c_trans.buf[0] = 251;
- actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cur_addr;
- actuators_asctec_v2.i2c_trans.buf[2] = 0;
- actuators_asctec_v2.i2c_trans.buf[3] = 231 + actuators_asctec_v2.cur_addr;
-
- i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 4);
- actuators_asctec_v2.cmd = NONE;
- break;
- case REVERSE:
- actuators_asctec_v2.i2c_trans.buf[0] = 254;
- actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cur_addr;
- actuators_asctec_v2.i2c_trans.buf[2] = 0;
- actuators_asctec_v2.i2c_trans.buf[3] = 234 + actuators_asctec_v2.cur_addr;
-
- i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 4);
- actuators_asctec_v2.cmd = NONE;
- break;
- case SET_ADDR: {
- static uint8_t set_addr = 0;
- static uint8_t serial_num[2];
- switch(set_addr) {
-
- // Request address
- case 0:
- case 2:
- i2c_receive(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, 0x06 + actuators_asctec_v2.cur_addr*2, 3);
- set_addr++;
- break;
- case 1:
- if(actuators_asctec_v2.i2c_trans.buf[0] == 2) {
- serial_num[0] = actuators_asctec_v2.i2c_trans.buf[1];
- set_addr++;
- break;
- }
- set_addr--;
- break;
- case 3:
- if(actuators_asctec_v2.i2c_trans.buf[0] == 3) {
- serial_num[1] = actuators_asctec_v2.i2c_trans.buf[1];
- set_addr++;
- break;
- }
-
- set_addr--;
- break;
- case 4:
- //update ID of the motor based on the address
- // NOTE: the missing break; at the end of this case is intentional
- actuators_asctec_v2.i2c_trans.buf[0] = actuators_asctec_v2.new_addr;
- actuators_asctec_v2.i2c_trans.buf[1] = serial_num[0];
- actuators_asctec_v2.i2c_trans.buf[2] = serial_num[1];
- actuators_asctec_v2.i2c_trans.buf[3] = 252;
-
- i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 4);
- set_addr++;
- /* Falls through. */
- default:
- set_addr = 0;
- actuators_asctec_v2.cmd = NONE;
- actuators_asctec_v2.cur_addr = actuators_asctec_v2.new_addr;
- break;
- }
-
- break;
- }
- case NONE:
- actuators_asctec_v2.i2c_trans.buf[0] = actuators_asctec_v2.cmds[SERVO_FRONT];
- actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cmds[SERVO_BACK];
- actuators_asctec_v2.i2c_trans.buf[2] = actuators_asctec_v2.cmds[SERVO_LEFT];
- actuators_asctec_v2.i2c_trans.buf[3] = actuators_asctec_v2.cmds[SERVO_RIGHT];
- actuators_asctec_v2.i2c_trans.buf[4] = 0xC0;
-
- for (uint8_t i=0;i<5;i++)
- crc=crc_update(crc,actuators_asctec_v2.i2c_trans.buf[i]);
-
- actuators_asctec_v2.i2c_trans.buf[5] = crc;
- i2c_transmit(&ACTUATORS_ASCTEC_V2_I2C_DEV, &actuators_asctec_v2.i2c_trans, ACTUATORS_ASCTEC_V2_SLAVE_ADDR, 6);
- break;
- default:
- break;
- }
-#endif
-}
-
-static uint16_t crc_update(uint16_t crc, uint8_t data)
-{
- data ^= (crc & 0xff);
- data ^= data << 4;
-
- return ((((uint16_t)data << 8) | ((crc>>8)&0xff)) ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3));
-}
diff --git a/sw/airborne/subsystems/actuators/actuators_asctec_v2_new.h b/sw/airborne/subsystems/actuators/actuators_asctec_v2_new.h
deleted file mode 100644
index 853f0f7dba..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_asctec_v2_new.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * Copyright (C) 2009-2013 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/** @file actuators_asctec_v2_new.h
- * Actuators driver for Asctec v2 motor controllers with the new I2C protocol.
- */
-
-#ifndef ACTUATORS_ASCTEC_V2_NEW_H
-#define ACTUATORS_ASCTEC_V2_NEW_H
-
-#include "mcu_periph/i2c.h"
-
-#include "generated/airframe.h"
-
-enum actuators_asctec_v2_cmd { NONE,
- TEST,
- REVERSE,
- SET_ADDR
- };
-
-enum actuators_asctec_v2_addr { FRONT,
- BACK,
- LEFT,
- RIGHT
- };
-
-struct ActuatorsAsctecV2 {
- enum actuators_asctec_v2_cmd cmd;
- enum actuators_asctec_v2_addr cur_addr;
- enum actuators_asctec_v2_addr new_addr;
- int32_t cmds[4];
- struct i2c_transaction i2c_trans;
- volatile uint32_t nb_err;
-};
-
-
-extern struct ActuatorsAsctecV2 actuators_asctec_v2;
-
-#define actuators_asctec_v2_new_SetCommand(_v) { \
- actuators_asctec_v2.cmd = _v; \
- }
-
-#define actuators_asctec_v2_new_SetNewAddr(_v) { \
- actuators_asctec_v2.new_addr = _v; \
- }
-
-#define actuators_asctec_v2_new_SetCurAddr(_v) { \
- actuators_asctec_v2.cur_addr = _v; \
- }
-
-extern void actuators_asctec_v2_init(void);
-extern void actuators_asctec_v2_set(void);
-
-#define ActuatorAsctec_v2_newSet(_i, _v) { actuators_asctec_v2.cmds[_i] = _v; }
-#define ActuatorsAsctec_v2_newInit() actuators_asctec_v2_init()
-#define ActuatorsAsctec_v2_newCommit() actuators_asctec_v2_set()
-
-
-#endif /* ACTUATORS_ASCTEC_V2_NEW_H */
diff --git a/sw/airborne/subsystems/actuators/actuators_mkk.c b/sw/airborne/subsystems/actuators/actuators_mkk.c
deleted file mode 100644
index ca893b183d..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_mkk.c
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * 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 actuators_mkk.c
- * Actuators driver for Mikrokopter motor controllers.
- */
-
-#include "subsystems/actuators.h"
-#include "subsystems/actuators/actuators_mkk.h"
-
-#include "mcu_periph/i2c.h"
-#include "mcu_periph/sys_time.h"
-
-PRINT_CONFIG_VAR(ACTUATORS_MKK_I2C_DEV)
-
-struct ActuatorsMkk actuators_mkk;
-
-
-void actuators_mkk_init(void)
-{
- actuators_mkk.submit_err_cnt = 0;
-}
-
-
-void actuators_mkk_set(void)
-{
- const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR;
- static uint8_t last_idx = ACTUATORS_MKK_NB;
-
-#if defined ACTUATORS_START_DELAY && ! defined SITL
- if (!actuators_delay_done) {
- if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) {
- return;
- } else {
- actuators_delay_done = true;
- }
- }
-#endif
-
- uint8_t cur_idx = last_idx;
- for (uint8_t i = 0; i < ACTUATORS_MKK_NB; i++) {
- if (cur_idx >= ACTUATORS_MKK_NB) {
- cur_idx = 0;
- }
-#ifdef KILL_MOTORS
- actuators_mkk.trans[cur_idx].buf[0] = 0;
-#endif
- if (!i2c_transmit(&ACTUATORS_MKK_I2C_DEV, &actuators_mkk.trans[cur_idx],
- actuators_addr[cur_idx], 1)) {
- actuators_mkk.submit_err_cnt++;
- last_idx = cur_idx;
- return;
- }
- cur_idx++;
- }
- /* successfully submitted all transactions */
- last_idx = ACTUATORS_MKK_NB;
-}
diff --git a/sw/airborne/subsystems/actuators/actuators_mkk.h b/sw/airborne/subsystems/actuators/actuators_mkk.h
deleted file mode 100644
index 9574b19282..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_mkk.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * 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 actuators_mkk.h
- * Actuators driver for Mikrokopter motor controllers.
- */
-
-#ifndef ACTUATORS_MKK_H
-#define ACTUATORS_MKK_H
-
-#include "std.h"
-#include "mcu_periph/i2c.h"
-
-#include "generated/airframe.h"
-
-
-struct ActuatorsMkk {
- struct i2c_transaction trans[ACTUATORS_MKK_NB];
- uint16_t submit_err_cnt;
-};
-
-extern struct ActuatorsMkk actuators_mkk;
-
-extern void actuators_mkk_init(void);
-extern void actuators_mkk_set(void);
-
-#define ActuatorMkkSet(_i, _v) { actuators_mkk.trans[_i].buf[0] = _v; }
-#define ActuatorsMkkInit() actuators_mkk_init()
-#define ActuatorsMkkCommit() actuators_mkk_set()
-
-#endif /* ACTUATORS_MKK_H */
diff --git a/sw/airborne/subsystems/actuators/actuators_mkk_v2.c b/sw/airborne/subsystems/actuators/actuators_mkk_v2.c
deleted file mode 100644
index 392a61da07..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_mkk_v2.c
+++ /dev/null
@@ -1,101 +0,0 @@
-/*
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * 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 actuators_mkk_v2.c
- * Actuators driver for Mikrokopter motor controllers.
- */
-
-#include "subsystems/actuators.h"
-#include "subsystems/actuators/actuators_mkk_v2.h"
-
-#include "mcu_periph/i2c.h"
-#include "mcu_periph/sys_time.h"
-
-PRINT_CONFIG_VAR(ACTUATORS_MKK_V2_I2C_DEV)
-
-struct actuators_mkk_v2_struct actuators_mkk_v2;
-
-void actuators_mkk_v2_init(void)
-{
-
- const uint8_t actuators_addr[ACTUATORS_MKK_V2_NB] = ACTUATORS_MKK_V2_ADDR;
- for (uint8_t i = 0; i < ACTUATORS_MKK_V2_NB; i++) {
- actuators_mkk_v2.trans[i].type = I2CTransTx;
- actuators_mkk_v2.trans[i].len_w = 2;
- actuators_mkk_v2.trans[i].slave_addr = actuators_addr[i];
- actuators_mkk_v2.trans[i].status = I2CTransSuccess;
-
- actuators_mkk_v2.data[i].Version = 0;
- actuators_mkk_v2.data[i].Current = 0;
- actuators_mkk_v2.data[i].MaxPWM = 0;
- actuators_mkk_v2.data[i].Temperature = 0;
- }
-
- actuators_mkk_v2.read_number = 0;
-
-}
-
-static inline void actuators_mkk_v2_read(void)
-{
- actuators_mkk_v2.read_number++;
- if (actuators_mkk_v2.read_number >= ACTUATORS_MKK_V2_NB) {
- actuators_mkk_v2.read_number = 0;
- }
-
- actuators_mkk_v2.trans[actuators_mkk_v2.read_number].type = I2CTransTxRx;
- actuators_mkk_v2.trans[actuators_mkk_v2.read_number].len_r = 3;
-}
-
-void actuators_mkk_v2_set(void)
-{
-#if defined ACTUATORS_START_DELAY && ! defined SITL
- if (!actuators_delay_done) {
- if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) { return; }
- else { actuators_delay_done = true; }
- }
-#endif
-
- // Read result
- for (uint8_t i = 0; i < ACTUATORS_MKK_V2_NB; i++) {
- if (actuators_mkk_v2.trans[i].type != I2CTransTx) {
- actuators_mkk_v2.trans[i].type = I2CTransTx;
-
- actuators_mkk_v2.data[i].Current = actuators_mkk_v2.trans[i].buf[0];
- actuators_mkk_v2.data[i].MaxPWM = actuators_mkk_v2.trans[i].buf[1];
- actuators_mkk_v2.data[i].Temperature = actuators_mkk_v2.trans[i].buf[2];
- }
- }
-
- RunOnceEvery(10, actuators_mkk_v2_read());
-
- for (uint8_t i = 0; i < ACTUATORS_MKK_V2_NB; i++) {
-
-#ifdef KILL_MOTORS
- actuators_mkk_v2.trans[i].buf[0] = 0;
- actuators_mkk_v2.trans[i].buf[1] = 0;
-#else
- actuators_mkk_v2.trans[i].buf[0] = (actuators_mkk_v2.setpoint[i] >> 3);
- actuators_mkk_v2.trans[i].buf[1] = actuators_mkk_v2.setpoint[i] & 0x07;
-#endif
-
- i2c_submit(&ACTUATORS_MKK_V2_I2C_DEV, &actuators_mkk_v2.trans[i]);
- }
-}
diff --git a/sw/airborne/subsystems/actuators/actuators_mkk_v2.h b/sw/airborne/subsystems/actuators/actuators_mkk_v2.h
deleted file mode 100644
index 629480fa48..0000000000
--- a/sw/airborne/subsystems/actuators/actuators_mkk_v2.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * Copyright (C) 2008-2009 Antoine Drouin
- *
- * 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 actuators_mkk_v2.h
- * Actuators driver for Mikrokopter motor controllers.
- */
-
-#ifndef ACTUATORS_MKK_V2_H
-#define ACTUATORS_MKK_V2_H
-
-#include "std.h"
-#include "mcu_periph/i2c.h"
-
-#include "generated/airframe.h"
-
-struct actuators_mkk_v2_telemetry_struct {
- uint8_t Version; // Motor controller version
- uint8_t Current; // In 0.1 A steps, read back from BL
- uint8_t MaxPWM; // Read back from BL -> is less than 255 if BL is in current limit, not running (250) or starting (40)
- int8_t Temperature; // Old BL-Ctrl will return a 255 here, the new version the temp. in °C
-};
-
-struct actuators_mkk_v2_struct {
- uint8_t read_number;
- uint16_t setpoint[ACTUATORS_MKK_V2_NB];
- struct i2c_transaction trans[ACTUATORS_MKK_V2_NB];
- struct actuators_mkk_v2_telemetry_struct data[ACTUATORS_MKK_V2_NB];
-};
-
-extern struct actuators_mkk_v2_struct actuators_mkk_v2;
-
-extern void actuators_mkk_v2_init(void);
-extern void actuators_mkk_v2_set(void);
-
-#define ActuatorMkk_v2Set(_i, _v) { actuators_mkk_v2.setpoint[_i] = _v; }
-#define ActuatorsMkk_v2Init() actuators_mkk_v2_init()
-#define ActuatorsMkk_v2Commit() actuators_mkk_v2_set()
-
-#endif /* ACTUATORS_MKK_V2_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
deleted file mode 100644
index 3497b5fae3..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+++ /dev/null
@@ -1,290 +0,0 @@
-/*
- * Copyright (C) 2008-2010 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/**
- * @file subsystems/ahrs/ahrs_int_cmpl_euler.c
- *
- * Complementary filter in euler representation (fixed-point).
- *
- * Estimate the attitude, heading and gyro bias.
- *
- */
-
-#include "ahrs_int_cmpl_euler.h"
-
-#include "math/pprz_trig_int.h"
-#include "math/pprz_algebra_int.h"
-
-#include "generated/airframe.h"
-
-#ifndef FACE_REINJ_1
-#define FACE_REINJ_1 1024
-#endif
-
-#ifndef AHRS_MAG_OFFSET
-#define AHRS_MAG_OFFSET 0.
-#endif
-
-struct AhrsIntCmplEuler ahrs_ice;
-
-static inline void get_phi_theta_measurement_fom_accel(int32_t *phi_meas, int32_t *theta_meas,
- struct Int32Vect3 *accel);
-static inline void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est, int32_t theta_est,
- struct Int32Vect3 *mag);
-
-#define F_UPDATE 512
-
-#define PI_INTEG_EULER (INT32_ANGLE_PI * F_UPDATE)
-#define TWO_PI_INTEG_EULER (INT32_ANGLE_2_PI * F_UPDATE)
-#define INTEG_EULER_NORMALIZE(_a) { \
- while (_a > PI_INTEG_EULER) _a -= TWO_PI_INTEG_EULER; \
- while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \
- }
-
-
-void ahrs_ice_init(void)
-{
- ahrs_ice.status = AHRS_ICE_UNINIT;
- ahrs_ice.is_aligned = false;
-
- /* init ltp_to_imu to zero */
- INT_EULERS_ZERO(ahrs_ice.ltp_to_imu_euler)
- INT_RATES_ZERO(ahrs_ice.imu_rate);
-
- INT_RATES_ZERO(ahrs_ice.gyro_bias);
- ahrs_ice.reinj_1 = FACE_REINJ_1;
-
- ahrs_ice.mag_offset = AHRS_MAG_OFFSET;
-}
-
-bool ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
- struct Int32Vect3 *lp_mag)
-{
-
- get_phi_theta_measurement_fom_accel(&ahrs_ice.hi_res_euler.phi,
- &ahrs_ice.hi_res_euler.theta, lp_accel);
- get_psi_measurement_from_mag(&ahrs_ice.hi_res_euler.psi,
- ahrs_ice.hi_res_euler.phi / F_UPDATE,
- ahrs_ice.hi_res_euler.theta / F_UPDATE, lp_mag);
-
- EULERS_COPY(ahrs_ice.measure, ahrs_ice.hi_res_euler);
- EULERS_COPY(ahrs_ice.measurement, ahrs_ice.hi_res_euler);
-
- /* Compute LTP to IMU eulers */
- EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE);
-
- RATES_COPY(ahrs_ice.gyro_bias, *lp_gyro);
-
- ahrs_ice.status = AHRS_ICE_RUNNING;
- ahrs_ice.is_aligned = true;
-
- return true;
-}
-
-//#define USE_NOISE_CUT 1
-//#define USE_NOISE_FILTER 1
-#define NOISE_FILTER_GAIN 50
-
-#if USE_NOISE_CUT
-#include "led.h"
-static inline bool cut_rates(struct Int32Rates i1, struct Int32Rates i2, int32_t threshold)
-{
- struct Int32Rates diff;
- RATES_DIFF(diff, i1, i2);
- if (diff.p < -threshold || diff.p > threshold ||
- diff.q < -threshold || diff.q > threshold ||
- diff.r < -threshold || diff.r > threshold) {
- return true;
- } else {
- return false;
- }
-}
-#define RATE_CUT_THRESHOLD RATE_BFP_OF_REAL(1)
-
-static inline bool cut_accel(struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold)
-{
- struct Int32Vect3 diff;
- VECT3_DIFF(diff, i1, i2);
- if (diff.x < -threshold || diff.x > threshold ||
- diff.y < -threshold || diff.y > threshold ||
- diff.z < -threshold || diff.z > threshold) {
- LED_ON(4);
- return true;
- } else {
- LED_OFF(4);
- return false;
- }
-}
-#define ACCEL_CUT_THRESHOLD ACCEL_BFP_OF_REAL(20)
-
-#endif
-
-/*
- *
- * fc = 1/(2*pi*tau)
- *
- * alpha = dt / ( tau + dt )
- *
- *
- * y(i) = alpha x(i) + (1-alpha) y(i-1)
- * or
- * y(i) = y(i-1) + alpha * (x(i) - y(i-1))
- *
- *
- */
-
-void ahrs_ice_propagate(struct Int32Rates *gyro)
-{
-
- /* unbias gyro */
- struct Int32Rates uf_rate;
- RATES_DIFF(uf_rate, *gyro, ahrs_ice.gyro_bias);
-#if USE_NOISE_CUT
- static struct Int32Rates last_uf_rate = { 0, 0, 0 };
- if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
-#endif
- /* low pass rate */
-#if USE_NOISE_FILTER
- RATES_SUM_SCALED(ahrs_ice.imu_rate, ahrs_ice.imu_rate, uf_rate, NOISE_FILTER_GAIN);
- RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, NOISE_FILTER_GAIN + 1);
-#else
- RATES_ADD(ahrs_ice.imu_rate, uf_rate);
- RATES_SDIV(ahrs_ice.imu_rate, ahrs_ice.imu_rate, 2);
-#endif
-#if USE_NOISE_CUT
- }
- RATES_COPY(last_uf_rate, uf_rate);
-#endif
-
- /* integrate eulers */
- struct Int32Eulers euler_dot;
- int32_eulers_dot_of_rates(&euler_dot, &ahrs_ice.ltp_to_imu_euler, &ahrs_ice.imu_rate);
- EULERS_ADD(ahrs_ice.hi_res_euler, euler_dot);
-
- /* low pass measurement */
- EULERS_ADD(ahrs_ice.measure, ahrs_ice.measurement);
- EULERS_SDIV(ahrs_ice.measure, ahrs_ice.measure, 2);
-
- /* compute residual */
- EULERS_DIFF(ahrs_ice.residual, ahrs_ice.measure, ahrs_ice.hi_res_euler);
- INTEG_EULER_NORMALIZE(ahrs_ice.residual.psi);
-
- struct Int32Eulers correction;
- /* compute a correction */
- EULERS_SDIV(correction, ahrs_ice.residual, ahrs_ice.reinj_1);
- /* correct estimation */
- EULERS_ADD(ahrs_ice.hi_res_euler, correction);
- INTEG_EULER_NORMALIZE(ahrs_ice.hi_res_euler.psi);
-
-
- /* Compute LTP to IMU eulers */
- EULERS_SDIV(ahrs_ice.ltp_to_imu_euler, ahrs_ice.hi_res_euler, F_UPDATE);
-}
-
-void ahrs_ice_update_accel(struct Int32Vect3 *accel)
-{
-
-#if USE_NOISE_CUT || USE_NOISE_FILTER
- static struct Int32Vect3 last_accel = { 0, 0, 0 };
-#endif
-#if USE_NOISE_CUT
- if (!cut_accel(*accel, last_accel, ACCEL_CUT_THRESHOLD)) {
-#endif
-#if USE_NOISE_FILTER
- VECT3_SUM_SCALED(*accel, *accel, last_accel, NOISE_FILTER_GAIN);
- VECT3_SDIV(*accel, *accel, NOISE_FILTER_GAIN + 1);
-#endif
- get_phi_theta_measurement_fom_accel(&ahrs_ice.measurement.phi, &ahrs_ice.measurement.theta, accel);
-#if USE_NOISE_CUT
- }
- VECT3_COPY(last_accel, *accel);
-#endif
-
-}
-
-
-void ahrs_ice_update_mag(struct Int32Vect3 *mag)
-{
-
- get_psi_measurement_from_mag(&ahrs_ice.measurement.psi, ahrs_ice.ltp_to_imu_euler.phi, ahrs_ice.ltp_to_imu_euler.theta,
- mag);
-
-}
-
-/* measures phi and theta assuming no dynamic acceleration ?!! */
-__attribute__((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t *phi_meas,
- int32_t *theta_meas, struct Int32Vect3 *accel)
-{
-
- *phi_meas = int32_atan2(-accel->y, -accel->z);
- int32_t cphi;
- PPRZ_ITRIG_COS(cphi, *phi_meas);
- int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC);
- *theta_meas = int32_atan2(-cphi_ax, -accel->z);
- *phi_meas *= F_UPDATE;
- *theta_meas *= F_UPDATE;
-
-}
-
-/* measure psi by projecting magnetic vector in local tangeant plan */
-__attribute__((always_inline)) static inline void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est,
- int32_t theta_est, struct Int32Vect3 *mag)
-{
-
- int32_t sphi;
- PPRZ_ITRIG_SIN(sphi, phi_est);
- int32_t cphi;
- PPRZ_ITRIG_COS(cphi, phi_est);
- int32_t stheta;
- PPRZ_ITRIG_SIN(stheta, theta_est);
- int32_t ctheta;
- PPRZ_ITRIG_COS(ctheta, theta_est);
-
- int32_t sphi_stheta = (sphi * stheta) >> INT32_TRIG_FRAC;
- int32_t cphi_stheta = (cphi * stheta) >> INT32_TRIG_FRAC;
- //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
- //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
-
- const int32_t mn = ctheta * mag->x + sphi_stheta * mag->y + cphi_stheta * mag->z;
- const int32_t me = 0 * mag->x + cphi * mag->y - sphi * mag->z;
- //const int32_t md =
- // -stheta * mag->x +
- // sphi_ctheta * mag->y +
- // cphi_ctheta * mag->z;
- float m_psi = -atan2(me, mn);
- *psi_meas = ((m_psi - ahrs_ice.mag_offset) * (float)(1 << (INT32_ANGLE_FRAC)) * F_UPDATE);
-
-}
-
-void ahrs_ice_set_body_to_imu(struct OrientationReps *body_to_imu)
-{
- ahrs_ice_set_body_to_imu_quat(orientationGetQuat_f(body_to_imu));
-}
-
-void ahrs_ice_set_body_to_imu_quat(struct FloatQuat *q_b2i)
-{
- orientationSetQuat_f(&ahrs_ice.body_to_imu, q_b2i);
-
- if (!ahrs_ice.is_aligned) {
- /* Set ltp_to_imu so that body is zero */
- ahrs_ice.ltp_to_imu_euler = *orientationGetEulers_i(&ahrs_ice.body_to_imu);
- }
-}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
deleted file mode 100644
index 8408f6cfbe..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- * Copyright (C) 2008-2010 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-/**
- * @file subsystems/ahrs/ahrs_int_cmpl_euler.h
- *
- * Complementary filter in euler representation (fixed-point).
- *
- */
-
-
-#ifndef AHRS_INT_CMPL_EULER_H
-#define AHRS_INT_CMPL_EULER_H
-
-#include "subsystems/ahrs.h"
-#include "std.h"
-#include "math/pprz_algebra_int.h"
-#include "math/pprz_orientation_conversion.h"
-
-enum AhrsICEStatus {
- AHRS_ICE_UNINIT,
- AHRS_ICE_RUNNING
-};
-
-struct AhrsIntCmplEuler {
- struct Int32Rates gyro_bias;
- struct Int32Rates imu_rate;
- struct Int32Eulers hi_res_euler;
- struct Int32Eulers measure;
- struct Int32Eulers residual;
- struct Int32Eulers measurement;
- struct Int32Eulers ltp_to_imu_euler;
- int32_t reinj_1;
- float mag_offset;
-
- struct OrientationReps body_to_imu;
-
- enum AhrsICEStatus status;
- bool is_aligned;
-};
-
-extern struct AhrsIntCmplEuler ahrs_ice;
-
-extern void ahrs_ice_init(void);
-extern void ahrs_ice_set_body_to_imu(struct OrientationReps *body_to_imu);
-extern void ahrs_ice_set_body_to_imu_quat(struct FloatQuat *q_b2i);
-extern bool ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
- struct Int32Vect3 *lp_mag);
-extern void ahrs_ice_propagate(struct Int32Rates *gyro);
-extern void ahrs_ice_update_accel(struct Int32Vect3 *accel);
-extern void ahrs_ice_update_mag(struct Int32Vect3 *mag);
-
-#endif /* AHRS_INT_CMPL_EULER_H */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
deleted file mode 100644
index 635110397c..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
+++ /dev/null
@@ -1,217 +0,0 @@
-/*
- * Copyright (C) 2015 Felix Ruess
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Paparazzi; see the file COPYING. If not, see
- * .
- */
-
-/**
- * @file subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.c
- *
- * Paparazzi specific wrapper to run floating point complementary filter.
- */
-
-#include "subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h"
-#include "subsystems/ahrs.h"
-#include "subsystems/abi.h"
-#include "state.h"
-
-#ifndef AHRS_ICE_OUTPUT_ENABLED
-#define AHRS_ICE_OUTPUT_ENABLED TRUE
-#endif
-PRINT_CONFIG_VAR(AHRS_ICE_OUTPUT_ENABLED)
-
-/** if TRUE with push the estimation results to the state interface */
-static bool ahrs_ice_output_enabled;
-static uint32_t ahrs_ice_last_stamp;
-static uint8_t ahrs_ice_id = AHRS_COMP_ID_ICE;
-
-static void set_body_state_from_euler(void);
-
-#if PERIODIC_TELEMETRY
-#include "subsystems/datalink/telemetry.h"
-#include "mcu_periph/sys_time.h"
-#include "state.h"
-
-static void send_filter(struct transport_tx *trans, struct link_device *dev)
-{
- pprz_msg_send_FILTER(trans, dev, AC_ID,
- &ahrs_ice.ltp_to_imu_euler.phi,
- &ahrs_ice.ltp_to_imu_euler.theta,
- &ahrs_ice.ltp_to_imu_euler.psi,
- &ahrs_ice.measure.phi,
- &ahrs_ice.measure.theta,
- &ahrs_ice.measure.psi,
- &ahrs_ice.hi_res_euler.phi,
- &ahrs_ice.hi_res_euler.theta,
- &ahrs_ice.hi_res_euler.psi,
- &ahrs_ice.residual.phi,
- &ahrs_ice.residual.theta,
- &ahrs_ice.residual.psi,
- &ahrs_ice.gyro_bias.p,
- &ahrs_ice.gyro_bias.q,
- &ahrs_ice.gyro_bias.r,
- &ahrs_ice_id);
-}
-
-static void send_euler(struct transport_tx *trans, struct link_device *dev)
-{
- struct Int32Eulers *eulers = stateGetNedToBodyEulers_i();
- pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
- &ahrs_ice.ltp_to_imu_euler.phi,
- &ahrs_ice.ltp_to_imu_euler.theta,
- &ahrs_ice.ltp_to_imu_euler.psi,
- &(eulers->phi),
- &(eulers->theta),
- &(eulers->psi),
- &ahrs_ice_id);
-}
-
-static void send_bias(struct transport_tx *trans, struct link_device *dev)
-{
- pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
- &ahrs_ice.gyro_bias.p, &ahrs_ice.gyro_bias.q,
- &ahrs_ice.gyro_bias.r, &ahrs_ice_id);
-}
-
-static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
-{
- uint8_t mde = 3;
- uint16_t val = 0;
- if (!ahrs_ice.is_aligned) { mde = 2; }
- uint32_t t_diff = get_sys_time_usec() - ahrs_ice_last_stamp;
- /* set lost if no new gyro measurements for 50ms */
- if (t_diff > 50000) { mde = 5; }
- pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_ice_id, &mde, &val);
-}
-#endif
-
-/** ABI binding for IMU data.
- * Used for gyro, accel ABI messages.
- */
-#ifndef AHRS_ICE_IMU_ID
-#define AHRS_ICE_IMU_ID ABI_BROADCAST
-#endif
-PRINT_CONFIG_VAR(AHRS_ICE_IMU_ID)
-/** ABI binding for magnetometer data.
- * Used for IMU_MAG_INT32 ABI messages.
- */
-#ifndef AHRS_ICE_MAG_ID
-#define AHRS_ICE_MAG_ID AHRS_ICE_IMU_ID
-#endif
-PRINT_CONFIG_VAR(AHRS_ICE_MAG_ID)
-static abi_event gyro_ev;
-static abi_event accel_ev;
-static abi_event mag_ev;
-static abi_event aligner_ev;
-static abi_event body_to_imu_ev;
-
-
-static void gyro_cb(uint8_t sender_id __attribute__((unused)),
- uint32_t stamp, struct Int32Rates *gyro)
-{
- ahrs_ice_last_stamp = stamp;
- if (ahrs_ice.is_aligned) {
- ahrs_ice_propagate(gyro);
- set_body_state_from_euler();
- }
-}
-
-static void accel_cb(uint8_t sender_id __attribute__((unused)),
- uint32_t stamp __attribute__((unused)),
- struct Int32Vect3 *accel)
-{
- if (ahrs_ice.is_aligned) {
- ahrs_ice_update_accel(accel);
- }
-}
-
-static void mag_cb(uint8_t sender_id __attribute__((unused)),
- uint32_t stamp __attribute__((unused)),
- struct Int32Vect3 *mag)
-{
- if (ahrs_ice.is_aligned) {
- ahrs_ice_update_mag(mag);
- }
-}
-
-static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
- uint32_t stamp __attribute__((unused)),
- struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
- struct Int32Vect3 *lp_mag)
-{
- if (!ahrs_ice.is_aligned) {
- if (ahrs_ice_align(lp_gyro, lp_accel, lp_mag)) {
- set_body_state_from_euler();
- }
- }
-}
-
-static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
- struct FloatQuat *q_b2i_f)
-{
- ahrs_ice_set_body_to_imu_quat(q_b2i_f);
-}
-
-static bool ahrs_ice_enable_output(bool enable)
-{
- ahrs_ice_output_enabled = enable;
- return ahrs_ice_output_enabled;
-}
-
-/* Rotate angles and rates from imu to body frame and set state */
-static void set_body_state_from_euler(void)
-{
- if (ahrs_ice_output_enabled) {
- struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_ice.body_to_imu);
- struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat;
- /* Compute LTP to IMU rotation matrix */
- int32_rmat_of_eulers(<p_to_imu_rmat, &ahrs_ice.ltp_to_imu_euler);
- /* Compute LTP to BODY rotation matrix */
- int32_rmat_comp_inv(<p_to_body_rmat, <p_to_imu_rmat, body_to_imu_rmat);
- /* Set state */
- stateSetNedToBodyRMat_i(<p_to_body_rmat);
-
- struct Int32Rates body_rate;
- /* compute body rates */
- int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_ice.imu_rate);
- /* Set state */
- stateSetBodyRates_i(&body_rate);
- }
-}
-
-void ahrs_ice_register(void)
-{
- ahrs_ice_output_enabled = AHRS_ICE_OUTPUT_ENABLED;
- ahrs_ice_init();
- ahrs_register_impl(ahrs_ice_enable_output);
-
- /*
- * Subscribe to scaled IMU measurements and attach callbacks
- */
- AbiBindMsgIMU_GYRO_INT32(AHRS_ICE_IMU_ID, &gyro_ev, gyro_cb);
- AbiBindMsgIMU_ACCEL_INT32(AHRS_ICE_IMU_ID, &accel_ev, accel_cb);
- AbiBindMsgIMU_MAG_INT32(AHRS_ICE_MAG_ID, &mag_ev, mag_cb);
- AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
- AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
-
-#if PERIODIC_TELEMETRY
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FILTER, send_filter);
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_euler);
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_GYRO_BIAS_INT, send_bias);
- register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
-#endif
-}
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h
deleted file mode 100644
index 119be53543..0000000000
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * Copyright (C) 2015 Felix Ruess
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Paparazzi; see the file COPYING. If not, see
- * .
- */
-
-/**
- * @file subsystems/ahrs/ahrs_int_cmpl_euler_wrapper.h
- *
- * Paparazzi specific wrapper to run floating point DCM filter.
- */
-
-#ifndef AHRS_INT_CMPL_EULER_WRAPPER_H
-#define AHRS_INT_CMPL_EULER_WRAPPER_H
-
-#include "subsystems/ahrs/ahrs_int_cmpl_euler.h"
-
-#ifndef PRIMARY_AHRS
-#define PRIMARY_AHRS ahrs_ice
-#endif
-
-extern void ahrs_ice_register(void);
-
-#endif /* AHRS_INT_CMPL_EULER_WRAPPER_H */
diff --git a/sw/airborne/subsystems/sensors/infrared.c b/sw/airborne/subsystems/sensors/infrared.c
deleted file mode 100644
index 3ae8c20d82..0000000000
--- a/sw/airborne/subsystems/sensors/infrared.c
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * Copyright (C) 2003-2010 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file subsystems/sensors/infrared.c
- * \brief common infrared
- */
-
-#include "subsystems/sensors/infrared.h"
-#include "generated/airframe.h"
-
-struct Infrared infrared;
-
-/** \brief Initialisation of \a ir structure
- */
-void infrared_struct_init(void)
-{
- infrared.roll_neutral = IR_ROLL_NEUTRAL_DEFAULT;
- infrared.pitch_neutral = IR_PITCH_NEUTRAL_DEFAULT;
-
- infrared.correction_left = IR_CORRECTION_LEFT;
- infrared.correction_right = IR_CORRECTION_RIGHT;
- infrared.correction_up = IR_CORRECTION_UP;
- infrared.correction_down = IR_CORRECTION_DOWN;
-
- infrared.lateral_correction = IR_LATERAL_CORRECTION;
- infrared.longitudinal_correction = IR_LONGITUDINAL_CORRECTION;
- infrared.vertical_correction = IR_VERTICAL_CORRECTION;
-
-}
-
-
diff --git a/sw/airborne/subsystems/sensors/infrared.h b/sw/airborne/subsystems/sensors/infrared.h
deleted file mode 100644
index b728c1fb6a..0000000000
--- a/sw/airborne/subsystems/sensors/infrared.h
+++ /dev/null
@@ -1,166 +0,0 @@
-/*
- * Copyright (C) 2003-2010 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#ifndef SUBSYSTEMS_SENSORS_INFRARED_H
-#define SUBSYSTEMS_SENSORS_INFRARED_H
-
-#include "std.h"
-#include "generated/airframe.h"
-
-/*
- * Sensor installation
- */
-
-#ifndef IR_IR1_SIGN
-#define IR_IR1_SIGN 1
-#endif /* IR_IR1_SIGN */
-
-#ifndef IR_IR2_SIGN
-#define IR_IR2_SIGN 1
-#endif /* IR_IR2_SIGN */
-
-#ifndef IR_TOP_SIGN
-#define IR_TOP_SIGN 1
-#endif /* IR_TOP_SIGN */
-
-#if defined IR_HORIZ_SENSOR_ALIGNED
-/* IR1 on the lateral axis, IR2 on the longitudal axis */
-#define IR_RollOfIrs(_ir1, _ir2) (_ir1)
-#define IR_PitchOfIrs(_ir1, _ir2) (_ir2)
-#elif IR_HORIZ_SENSOR_TILTED
-/* IR1 rear-left -- front-right, IR2 rear-right -- front-left
- IR1_SIGN and IR2_SIGN give positive values when it's warm on the right side
-*/
-#define IR_RollOfIrs(_ir1, _ir2) (_ir1 + _ir2)
-#define IR_PitchOfIrs(_ir1, _ir2) (-(_ir1) + _ir2)
-#else
-#ifndef SITL
-#error "You have to define either HORIZ_SENSOR_ALIGNED or HORIZ_SENSOR_TILTED in the IR section"
-#endif
-#endif
-/* Vertical sensor, TOP_SIGN gives positice values when it's warm on the bottom */
-#ifndef IR_TopOfIr
-#define IR_TopOfIr(_ir) (_ir)
-#endif
-
-/*
- * Default correction values
- */
-
-#ifndef IR_LATERAL_CORRECTION
-#define IR_LATERAL_CORRECTION 1.
-#endif
-
-#ifndef IR_LONGITUDINAL_CORRECTION
-#define IR_LONGITUDINAL_CORRECTION 1.
-#endif
-
-#ifndef IR_VERTICAL_CORRECTION
-#define IR_VERTICAL_CORRECTION 1.
-#endif
-
-#ifndef IR_CORRECTION_LEFT
-#define IR_CORRECTION_LEFT 1.
-#endif
-
-#ifndef IR_CORRECTION_RIGHT
-#define IR_CORRECTION_RIGHT 1.
-#endif
-
-#ifndef IR_CORRECTION_UP
-#define IR_CORRECTION_UP 1.
-#endif
-
-#ifndef IR_CORRECTION_DOWN
-#define IR_CORRECTION_DOWN 1.
-#endif
-
-
-/*
- * Default neutral values
- */
-#ifndef IR_ROLL_NEUTRAL_DEFAULT
-#define IR_ROLL_NEUTRAL_DEFAULT 0.0
-#endif
-
-#ifndef IR_PITCH_NEUTRAL_DEFAULT
-#define IR_PITCH_NEUTRAL_DEFAULT 0.0
-#endif
-
-struct Infrared_raw {
- /* the 3 channels of the sensor
- */
- int16_t ir1;
- int16_t ir2;
- int16_t ir3;
-};
-
-/** Infrared structure */
-struct Infrared {
- /* raw infrared values
- */
- struct Infrared_raw value;
- /* neutrals in radians
- */
- float roll_neutral;
- float pitch_neutral;
- float pitch_vneutral;
- /* roll, pitch, top unscaled reading
- */
- int16_t roll;
- int16_t pitch;
- int16_t top;
- /* coefficients used to compensate
- for sensors sensitivity
- */
- float lateral_correction;
- float longitudinal_correction;
- float vertical_correction;
- /* coefficients used to compensate
- for masking
- */
- float correction_left;
- float correction_right;
- float correction_up;
- float correction_down;
-};
-
-extern struct Infrared infrared;
-
-#define UpdateIRValue(_v) { \
- infrared.value.ir1 = (IR_IR1_SIGN)*_v.ir1; \
- infrared.value.ir2 = (IR_IR2_SIGN)*_v.ir2; \
- infrared.value.ir3 = (IR_TOP_SIGN)*_v.ir3; \
- infrared.roll = infrared.lateral_correction * IR_RollOfIrs(infrared.value.ir1, infrared.value.ir2); \
- infrared.pitch = infrared.longitudinal_correction * IR_PitchOfIrs(infrared.value.ir1, infrared.value.ir2); \
- infrared.top = infrared.vertical_correction * IR_TopOfIr(infrared.value.ir3); \
- }
-
-// initialization of the infrared structure
-void infrared_struct_init(void);
-
-// implementation dependent functions
-void infrared_init(void);
-void infrared_update(void);
-void infrared_event(void);
-
-#endif /* SUBSYSTEMS_SENSORS_INFRARED_H */
diff --git a/sw/airborne/subsystems/sensors/infrared_adc.c b/sw/airborne/subsystems/sensors/infrared_adc.c
deleted file mode 100644
index 153a2ad982..0000000000
--- a/sw/airborne/subsystems/sensors/infrared_adc.c
+++ /dev/null
@@ -1,103 +0,0 @@
-/*
- * Copyright (C) 2003-2010 The Paparazzi Team
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-/** \file subsystems/sensors/infrared_adc.c
- * \brief Regroup all functions link to ADC \a ir
- */
-
-
-
-
-#include
-
-#include "subsystems/sensors/infrared_adc.h"
-#include "mcu_periph/adc.h"
-
-#include BOARD_CONFIG
-#include "generated/airframe.h"
-
-// TODO Specific sim implementation
-#if ! (defined SITL || defined HITL)
-static struct adc_buf buf_ir1;
-static struct adc_buf buf_ir2;
-
-#ifdef ADC_CHANNEL_IR_TOP
-static struct adc_buf buf_ir3;
-#endif
-#endif
-
-#ifndef ADC_CHANNEL_IR_NB_SAMPLES
-#define ADC_CHANNEL_IR_NB_SAMPLES DEFAULT_AV_NB_SAMPLE
-#endif
-
-struct Infrared_raw ir_adc;
-
-// Standard infrared implementation
-void infrared_init(void)
-{
- infrared_adc_init();
-}
-
-void infrared_update(void)
-{
- infrared_adc_update();
-}
-
-/* No event with adc ir */
-void infrared_event(void) {}
-
-/** \brief Initialisation of \a ir */
-/** Initialize \a adc_buf_channel
- */
-void infrared_adc_init(void)
-{
-#if ! (defined SITL || defined HITL)
- adc_buf_channel(ADC_CHANNEL_IR1, &buf_ir1, ADC_CHANNEL_IR_NB_SAMPLES);
- adc_buf_channel(ADC_CHANNEL_IR2, &buf_ir2, ADC_CHANNEL_IR_NB_SAMPLES);
-#ifdef ADC_CHANNEL_IR_TOP
- adc_buf_channel(ADC_CHANNEL_IR_TOP, &buf_ir3, ADC_CHANNEL_IR_NB_SAMPLES);
-#endif
-#endif
-
- infrared_struct_init();
-
-#if ! (defined ADC_CHANNEL_IR_TOP || defined HITL || defined SITL)
- ir_adc.ir3 = IR_DEFAULT_CONTRAST;
-#endif
-}
-
-
-/** \brief Update \a ir_roll and ir_pitch from ADCs or from simulator
- * message in HITL and SITL modes
- */
-void infrared_adc_update(void)
-{
-#if ! (defined SITL || defined HITL)
- ir_adc.ir1 = (int32_t)(buf_ir1.sum / buf_ir1.av_nb_sample) - IR_ADC_IR1_NEUTRAL;
- ir_adc.ir2 = (int32_t)(buf_ir2.sum / buf_ir2.av_nb_sample) - IR_ADC_IR2_NEUTRAL;
-#ifdef ADC_CHANNEL_IR_TOP
- ir_adc.ir3 = (int32_t)(buf_ir3.sum / buf_ir3.av_nb_sample) - IR_ADC_TOP_NEUTRAL;
-#endif // IR_TOP
- UpdateIRValue(ir_adc);
-#endif /* !SITL && !HITL */
-}
-
diff --git a/sw/airborne/subsystems/sensors/infrared_adc.h b/sw/airborne/subsystems/sensors/infrared_adc.h
deleted file mode 100644
index 74dd729daa..0000000000
--- a/sw/airborne/subsystems/sensors/infrared_adc.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/*
- * Copyright (C) 2010 ENAC
- *
- * 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.
- *
- */
-
-/*
- * Driver for adc infrared sensor
- */
-
-#ifndef SUBSYSTEMS_SENSORS_INFRARED_ADC_H
-#define SUBSYSTEMS_SENSORS_INFRARED_ADC_H
-
-#include "std.h"
-#include "subsystems/sensors/infrared.h"
-
-extern struct Infrared_raw ir_adc;
-
-extern void infrared_adc_init(void);
-extern void infrared_adc_update(void);
-
-#endif // SUBSYSTEMS_SENSORS_INFRARED_ADC_H
-
diff --git a/sw/airborne/subsystems/sensors/infrared_i2c.c b/sw/airborne/subsystems/sensors/infrared_i2c.c
deleted file mode 100644
index ff6b29c5ef..0000000000
--- a/sw/airborne/subsystems/sensors/infrared_i2c.c
+++ /dev/null
@@ -1,218 +0,0 @@
-/*
- * Copyright (C) 2010 ENAC
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#include "sensors/infrared_i2c.h"
-
-// IR I2C definitions
-#define IR_HOR_I2C_ADDR (0x6C << 1)
-#define IR_VER_I2C_ADDR (0x68 << 1)
-#define IR_SAMPLE_RATE_SELECT (0 << 2)
-#define IR_HOR_OC_BIT (0 << 4)
-#define IR_VER_OC_BIT (1 << 4)
-#define IR_HOR_I2C_SELECT_IR1 (0 << 5)
-#define IR_HOR_I2C_SELECT_IR2 (1 << 5)
-#define IR_START_CONV (1 << 7)
-
-
-#ifndef IR_I2C_IR1_NEUTRAL
-#define IR_I2C_IR1_NEUTRAL 0
-#endif
-
-#ifndef IR_I2C_IR2_NEUTRAL
-#define IR_I2C_IR2_NEUTRAL 0
-#endif
-
-#ifndef IR_I2C_TOP_NEUTRAL
-#define IR_I2C_TOP_NEUTRAL 0
-#endif
-
-struct Infrared_raw ir_i2c;
-bool ir_i2c_data_hor_available, ir_i2c_data_ver_available;
-uint8_t ir_i2c_conf_word;
-bool ir_i2c_conf_hor_done, ir_i2c_conf_ver_done;
-
-// Local variables
-#define IR_I2C_IDLE 0
-#define IR_I2C_READ_IR1 1
-#define IR_I2C_IR2_SELECTED 2
-#define IR_I2C_READ_IR2 3
-#define IR_I2C_IR1_SELECTED 4
-#define IR_I2C_CONFIGURE_HOR 5
-
-static uint8_t ir_i2c_hor_status;
-
-#define NO_CONF_WORD 0xff
-#define ValidConfWord(_x) (_x < 0x4)
-
-// I2C structure
-struct i2c_transaction irh_trans, irv_trans;
-
-// Standard infrared implementation
-void infrared_init(void)
-{
- infrared_i2c_init();
-}
-
-void infrared_update(void)
-{
- infrared_i2c_update();
-}
-
-void infrared_event(void)
-{
- infrared_i2cEvent();
-}
-
-/** Initialisation
- */
-void infrared_i2c_init(void)
-{
- ir_i2c_data_hor_available = false;
- ir_i2c_data_ver_available = false;
- ir_i2c_hor_status = IR_I2C_IDLE;
- ir_i2c_conf_word = IR_I2C_DEFAULT_CONF;
- ir_i2c_conf_hor_done = false;
- ir_i2c_conf_ver_done = false;
- irh_trans.status = I2CTransDone;
- irv_trans.status = I2CTransDone;
-
- infrared_struct_init();
-}
-
-void infrared_i2c_update(void)
-{
-#if ! (defined SITL || defined HITL)
- // IR horizontal
- if (irh_trans.status == I2CTransDone && ir_i2c_hor_status == IR_I2C_IDLE) {
- if (ValidConfWord(ir_i2c_conf_word) && !ir_i2c_conf_hor_done) {
- irh_trans.buf[0] = ir_i2c_conf_word | IR_HOR_OC_BIT | IR_START_CONV ;
- i2c_transmit(&i2c0, &irh_trans, IR_HOR_I2C_ADDR, 1);
- ir_i2c_hor_status = IR_I2C_CONFIGURE_HOR;
- } else {
- // Read next values
- i2c_receive(&i2c0, &irh_trans, IR_HOR_I2C_ADDR, 3);
- ir_i2c_data_hor_available = false;
- ir_i2c_hor_status = IR_I2C_READ_IR1;
- }
- }
- // IR vertical
- if (irv_trans.status == I2CTransDone) {
- if (ValidConfWord(ir_i2c_conf_word) && !ir_i2c_conf_ver_done) {
- irv_trans.buf[0] = ir_i2c_conf_word | IR_VER_OC_BIT;
- i2c_transmit(&i2c0, &irv_trans, IR_VER_I2C_ADDR, 1);
- } else {
- // Read next values
- i2c_receive(&i2c0, &irv_trans, IR_VER_I2C_ADDR, 2);
- ir_i2c_data_ver_available = false;
- }
- }
-#endif /* SITL || HITL */
-}
-
-#define FilterIR(_ir_prev, _ir_next) (((1<
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-
-#include BOARD_CONFIG
-
-#include "mcu.h"
-#include "mcu_periph/sys_time.h"
-#include "led.h"
-
-#include "mcu_periph/i2c.h"
-
-static inline void main_init(void);
-static inline void main_periodic_task(void);
-static inline void main_event_task(void);
-
-static struct i2c_transaction trans;
-
-int main(void)
-{
- main_init();
-
- while (1) {
- if (sys_time_check_and_ack_timer(0)) {
- main_periodic_task();
- }
- main_event_task();
- }
-
- return 0;
-}
-
-static inline void main_init(void)
-{
- mcu_init();
- sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
-}
-
-
-
-static inline void main_periodic_task(void)
-{
-
- trans.type = I2CTransTx;
- trans.slave_addr = 0x02;
- trans.len_w = 4;
- trans.buf[0] = 100;
- trans.buf[1] = 100;
- trans.buf[2] = 100;
- trans.buf[3] = 1;
- i2c_submit(&i2c1, &trans);
-
- LED_PERIODIC();
-
-}
-
-
-
-static inline void main_event_task(void)
-{
-
-}
diff --git a/sw/airborne/test/test_esc_mkk_simple.c b/sw/airborne/test/test_esc_mkk_simple.c
deleted file mode 100644
index 08d277902c..0000000000
--- a/sw/airborne/test/test_esc_mkk_simple.c
+++ /dev/null
@@ -1,75 +0,0 @@
-/*
- * Copyright (C) 2009 Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING. If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-
-#include "mcu.h"
-#include "mcu_periph/sys_time.h"
-#include "led.h"
-
-#include "mcu_periph/i2c.h"
-
-static inline void main_init(void);
-static inline void main_periodic_task(void);
-static inline void main_event_task(void);
-
-static struct i2c_transaction trans;
-
-int main(void)
-{
- main_init();
-
- while (1) {
- if (sys_time_check_and_ack_timer(0)) {
- main_periodic_task();
- }
- main_event_task();
- }
-
- return 0;
-}
-
-static inline void main_init(void)
-{
- mcu_init();
- sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL);
-}
-
-
-
-static inline void main_periodic_task(void)
-{
-
- trans.type = I2CTransTx;
- trans.buf[0] = 0x04;
- trans.len_w = 1;
- trans.slave_addr = 0x58;
- i2c_submit(&ACTUATORS_MKK_DEV, &trans);
-
- LED_PERIODIC();
-
-}
-
-
-
-static inline void main_event_task(void)
-{
-
-}
diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml
index 7ef8bcf7b9..bcfafd08c8 100644
--- a/sw/simulator/sim.ml
+++ b/sw/simulator/sim.ml
@@ -39,7 +39,7 @@ let georef_of_xml = fun xml ->
(* Frequencies for perdiodic tasks are expressed in s *)
-let ir_period = 1./.20.
+let airspeed_period = 1./.20.
let fm_period = 1./.25.
let fg_period = 1./.25.
let ahrs_period = 1./.20.
@@ -61,8 +61,8 @@ sig
val commands : pprz_t array -> unit
(** Called once at init *)
- val infrared_and_airspeed : float -> float -> float -> float -> unit
- (** [infrared ir_left ir_front ir_top air_speed] Called on timer *)
+ val airspeed : float -> unit
+ (** [air_speed] Called on timer *)
val attitude_and_rates : float -> float -> float -> float -> float -> float ->unit
(** [ahrs phi theta psi p q r] Called on timer *)
@@ -155,8 +155,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
let wind_x = ref 0.
and wind_y = ref 0.
and wind_z = ref 0. in
- let infrared_contrast = ref 266.
- and time_scale = object val mutable v = 1. method value = v method set_value x = v <- x end
+ let time_scale = object val mutable v = 1. method value = v method set_value x = v <- x end
and gps_availability = ref 1 in
let world_update = fun _ vs ->
@@ -164,7 +163,6 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
wind_x := PprzLink.float_assoc "wind_east" vs;
wind_y := PprzLink.float_assoc "wind_north" vs;
wind_z := PprzLink.float_assoc "wind_up" vs;
- infrared_contrast := PprzLink.float_assoc "ir_contrast" vs;
time_scale#set_value (PprzLink.float_assoc "time_scale" vs)
in
@@ -203,16 +201,8 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
| None -> 0. in
FM.state_update !state FM.nominal_airspeed (!wind_x, !wind_y, !wind_z) agl fm_period
- and ir_task = fun () ->
- let phi, theta, _ = FlightModel.get_attitude !state in
-
- let phi_sensor = phi +. FM.roll_neutral_default
- and theta_sensor = theta +. FM.pitch_neutral_default in
-
- let ir_left = sin phi_sensor *. !infrared_contrast
- and ir_front = sin theta_sensor *. !infrared_contrast
- and ir_top = cos phi_sensor *. cos theta_sensor *. !infrared_contrast in
- Aircraft.infrared_and_airspeed ir_left ir_front ir_top (FlightModel.get_air_speed !state)
+ and airspeed_task = fun () ->
+ Aircraft.airspeed (FlightModel.get_air_speed !state)
and gps_task = fun () ->
let (x,y,z) = FlightModel.get_xyz !state in
@@ -265,7 +255,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
let boot = fun () ->
Aircraft.boot (time_scale:>value);
Simlib.timer ~scale:time_scale fm_period fm_task;
- Simlib.timer ~scale:time_scale ir_period ir_task;
+ Simlib.timer ~scale:time_scale airspeed_period airspeed_task;
Simlib.timer ~scale:time_scale gps_period gps_task;
Simlib.timer ~scale:time_scale ahrs_period ahrs_task;
diff --git a/sw/simulator/sim.mli b/sw/simulator/sim.mli
index 3e65b7c3d9..5393572cbc 100644
--- a/sw/simulator/sim.mli
+++ b/sw/simulator/sim.mli
@@ -9,7 +9,7 @@ module type AIRCRAFT =
val init : int -> GPack.box -> unit
val boot : Simlib.value -> unit
val commands : Simlib.pprz_t array -> unit
- val infrared_and_airspeed : float -> float -> float -> float -> unit
+ val airspeed : float -> unit
val attitude_and_rates : float -> float -> float -> float -> float -> float -> unit
val gps : Gps.state -> unit
end
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index 02dd73f21a..5259aba9f2 100644
--- a/sw/simulator/sitl.ml
+++ b/sw/simulator/sitl.ml
@@ -206,10 +206,9 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
(* Functions called by the simulator *)
let commands = fun s -> rcommands := s
- external set_ir_and_airspeed : int -> int -> int -> float -> unit = "set_ir_and_airspeed"
- let infrared_and_airspeed = fun ir_left ir_front ir_top air_speed ->
- (** ADC neutral is not taken into account in the soft sim (c.f. sim_ir.c)*)
- set_ir_and_airspeed (truncate ir_left) (truncate ir_front) (truncate ir_top) air_speed
+ external set_airspeed : float -> unit = "set_airspeed"
+ let airspeed = fun air_speed ->
+ set_airspeed air_speed
external provide_attitude : float -> float -> float -> unit = "provide_attitude"
external provide_rates : float -> float -> float -> unit = "provide_rates"