[ins] Add EKF2

paparazzi · Mar 5, 2019 · dc1e460 · dc1e460
1 parent 3a6e074
commit dc1e460
Show file tree

Hide file tree

Showing 17 changed files with 677 additions and 62 deletions.
diff --git a/.gitmodules b/.gitmodules
@@ -44,3 +44,9 @@
 [submodule "sw/ext/eigen"]
 	path = sw/ext/eigen
 	url = https://github.com/eigenteam/eigen-git-mirror.git
+[submodule "sw/ext/ecl"]
+	path = sw/ext/ecl
+	url = https://github.com/PX4/ecl.git
+[submodule "sw/ext/matrix"]
+	path = sw/ext/matrix
+	url = https://github.com/PX4/Matrix.git
diff --git a/conf/airframes/examples/bebop2_indi.xml b/conf/airframes/examples/bebop2_indi.xml
@@ -4,7 +4,7 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="bebop2">
-      <define name="STABILIZATION_INDI_G2_R" value="0.20"/>
+      <define name="STABILIZATION_INDI_G2_R" value="0.2784"/>
     </target>
 
     <target name="nps" board="pc">
@@ -19,16 +19,16 @@
     <module name="radio_control" type="datalink"/>
     <module name="motor_mixing"/>
     <module name="actuators" type="bebop"/>
-    <module name="imu" type="bebop"/>
+    <module name="imu" type="bebop">
+      <define name="BEBOP_LOWPASS_FILTER" value="MPU60X0_DLPF_42HZ"/>
+      <define name="BEBOP_SMPLRT_DIV" value="0"/>
+      <define name="BEBOP_GYRO_RANGE" value="MPU60X0_GYRO_RANGE_2000"/>
+      <define name="BEBOP_ACCEL_RANGE" value="MPU60X0_ACCEL_RANGE_16G"/>
+    </module>
     <module name="gps" type="ublox"/>
     <module name="stabilization" type="indi_simple"/>
-    <module name="ahrs" type="int_cmpl_quat">
-      <configure name="USE_MAGNETOMETER" value="TRUE"/>
-      <define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
-    </module>
-    <module name="ins" type="extended"/>
+    <module name="ins" type="ekf2"/>
 
-    <module name="geo_mag"/>
     <module name="air_data"/>
     <module name="send_imu_mag_current"/>
     <module name="gps" type="ubx_ucenter"/>
@@ -75,31 +75,24 @@
 
   <section name="IMU" prefix="IMU_">
     <!-- Magneto calibration -->
-    <define name="MAG_X_NEUTRAL" value="18"/>
-    <define name="MAG_Y_NEUTRAL" value="46"/>
-    <define name="MAG_Z_NEUTRAL" value="202"/>
-    <define name="MAG_X_SENS" value="8.32441255621" integer="16"/>
-    <define name="MAG_Y_SENS" value="8.25720085664" integer="16"/>
-    <define name="MAG_Z_SENS" value="8.60009819293" integer="16"/>
-  </section>
-
-  <!-- local magnetic field -->
-  <!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
-  <section name="AHRS" prefix="AHRS_">
-    <!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
-    <!-- Toulouse -->
-    <!--define name="H_X" value="0.513081"/>
-    <define name="H_Y" value="-0.00242783"/>
-    <define name="H_Z" value="0.858336"/-->
-    <!-- Delft -->
-    <define name="H_X" value="0.3892503"/>
-    <define name="H_Y" value="0.0017972"/>
-    <define name="H_Z" value="0.9211303"/>
+    <define name="MAG_X_NEUTRAL" value="286"/>
+    <define name="MAG_Y_NEUTRAL" value="-156"/>
+    <define name="MAG_Z_NEUTRAL" value="-275"/>
+    <define name="MAG_X_SENS" value="7.074334106912319" integer="16"/>
+    <define name="MAG_Y_SENS" value="7.069181442728008" integer="16"/>
+    <define name="MAG_Z_SENS" value="7.411815536475566" integer="16"/>
+
+    <!--define name="ACCEL_X_NEUTRAL" value="11"/>
+    <define name="ACCEL_Y_NEUTRAL" value="-43"/>
+    <define name="ACCEL_Z_NEUTRAL" value="-124"/>
+    <define name="ACCEL_X_SENS" value="2.4523434144387464" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="2.428429001527193" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="2.423519834802844" integer="16"/-->
   </section>
 
   <section name="INS" prefix="INS_">
     <define name="SONAR_MAX_RANGE" value="2.2"/>
-    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
+    <!--define name="SONAR_UPDATE_ON_AGL" value="TRUE"/-->
   </section>
 
 
@@ -133,9 +126,9 @@
 
   <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
     <!-- control effectiveness -->
-    <define name="G1_P" value="0.05"/>
-    <define name="G1_Q" value="0.025"/>
-    <define name="G1_R" value="0.0022"/>
+    <define name="G1_P" value="0.0390"/>
+    <define name="G1_Q" value="0.0473"/>
+    <define name="G1_R" value="0.00122"/>
 
     <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
     <define name="FILTER_ROLL_RATE" value="FALSE"/><!-- Set to TRUE when flying without locked dampers -->

diff --git a/conf/airframes/tudelft/splash3.xml b/conf/airframes/tudelft/splash3.xml
@@ -42,12 +42,12 @@
     <module name="gps"           type="ubx_ucenter"/>
     <module name="stabilization" type="int_quat"/>
     <module name="stabilization" type="rate"/>
-    <module name="ahrs"          type="float_cmpl_quat">
-      <!--define name="AHRS_ICQ_MAG_ID" value="MAG_LIS3MDL_SENDER_ID" /-->
+    <!--module name="ahrs"          type="float_cmpl_quat">
+      <!-define name="AHRS_ICQ_MAG_ID" value="MAG_LIS3MDL_SENDER_ID" /->
       <configure name="USE_MAGNETOMETER" value="TRUE"/>
     </module>
-    <module name="ins"           type="hff"/>
-    <!--module name="ins"           type="ekf2"/-->
+    <module name="ins"           type="hff"/-->
+    <module name="ins"           type="ekf2"/>
     <!--module name="mag_lis3mdl">
       <define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
       <configure name="MAG_LIS3MDL_I2C_DEV" value="i2c1"/>
@@ -115,24 +115,17 @@
     <define name="ACCEL_Y_SENS" value="4.85140885386" integer="16"/>
     <define name="ACCEL_Z_SENS" value="4.89977338537" integer="16"/-->
 
-    <!--define name="MAG_X_CURRENT_COEF" value="9.037571651280377"/>
-    <define name="MAG_Y_CURRENT_COEF" value="-4.490670308897883"/>
-    <define name="MAG_Z_CURRENT_COEF" value="30.7029351308597"/-->
+    <define name= "MAG_X_CURRENT_COEF" value="-1.8609825961051705"/>
+    <define name= "MAG_Y_CURRENT_COEF" value="-1.6568817208655613"/>
+    <define name= "MAG_Z_CURRENT_COEF" value="16.329866025311297"/>
 
     <!-- replace this with your own calibration -->
-    <!--define name="MAG_X_NEUTRAL" value="2197"/>
-    <define name="MAG_Y_NEUTRAL" value="1460"/>
-    <define name="MAG_Z_NEUTRAL" value="288"/>
-    <define name="MAG_X_SENS" value="0.621654140011012" integer="16"/>
-    <define name="MAG_Y_SENS" value="0.5993861893464758" integer="16"/>
-    <define name="MAG_Z_SENS" value="0.6238423840038542" integer="16"/-->
-    <define name="MAG_X_NEUTRAL" value="26"/>
-    <define name="MAG_Y_NEUTRAL" value="32"/>
-    <define name="MAG_Z_NEUTRAL" value="143"/>
-    <define name="MAG_X_SENS" value="7.891481623127505" integer="16"/>
-    <define name="MAG_Y_SENS" value="8.144089259820777" integer="16"/>
-    <define name="MAG_Z_SENS" value="8.186555447435524" integer="16"/>
-
+    <define name="MAG_X_NEUTRAL" value="-48"/>
+    <define name="MAG_Y_NEUTRAL" value="69"/>
+    <define name="MAG_Z_NEUTRAL" value="155"/>
+    <define name="MAG_X_SENS" value="7.646781508055661" integer="16"/>
+    <define name="MAG_Y_SENS" value="7.71865134635356" integer="16"/>
+    <define name="MAG_Z_SENS" value="7.691679165490501" integer="16"/>
 
     <define name="BODY_TO_IMU_PHI"   value="0" unit="deg"/>
     <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
@@ -212,26 +205,26 @@
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
-    <define name="HOVER_KP"    value="150"/>
-    <define name="HOVER_KD"    value="80"/>
-    <define name="HOVER_KI"    value="20"/>
+    <define name="HOVER_KP"    value="250"/>
+    <define name="HOVER_KD"    value="90"/>
+    <define name="HOVER_KI"    value="5"/>
     <define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
     <define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
   </section>
 
   <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
     <define name="MAX_BANK" value="35" unit="deg"/>
     <define name="USE_SPEED_REF" value="TRUE"/>
-    <define name="PGAIN" value="50"/>
+    <define name="PGAIN" value="60"/>
     <define name="DGAIN" value="100"/>
-    <define name="AGAIN" value="70"/>
+    <define name="AGAIN" value="0"/>
     <define name="IGAIN" value="20"/>
   </section>
 
   <section name="AUTOPILOT">
     <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
     <define name="MODE_AUTO1"  value="AP_MODE_ATTITUDE_Z_HOLD"/>
-    <define name="MODE_AUTO2"  value="AP_MODE_NAV"/>
+    <define name="MODE_AUTO2"  value="AP_MODE_HOVER_Z_HOLD"/>
   </section>
 
   <section name="BAT">

diff --git a/conf/conf_example.xml b/conf/conf_example.xml
@@ -161,7 +161,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
-   settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml [modules/geo_mag.xml] modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
+   settings_modules="modules/gps_ubx_ucenter.xml modules/air_data.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
    gui_color="red"
   />
   <aircraft

diff --git a/conf/modules/ins_ekf2.xml b/conf/modules/ins_ekf2.xml
@@ -0,0 +1,45 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ekf2" dir="ins">
+  <doc>
+    <description>
+      simple INS and AHRS using EKF2 from PX4
+    </description>
+  </doc>
+  <header>
+    <file name="ins_ekf2.h" dir="subsystems/ins"/>
+  </header>
+  <init fun="ins_ekf2_init()"/>
+  <periodic fun="ins_ekf2_update()" autorun="TRUE"/>
+  <makefile target="ap|nps">
+    <define name="INS_TYPE_H" value="subsystems/ins/ins_ekf2.h" type="string"/>
+    <file name="ins.c" dir="subsystems"/>
+    <file name="ins_ekf2.cpp" dir="subsystems/ins"/>
+
+    <include name="$(PAPARAZZI_SRC)/sw/ext/ecl/"/>
+    <define name="__PAPARAZZI" value="true"/>
+    <define name="ECL_STANDALONE" value="true"/>
+    <define name="USE_MAGNETOMETER" value="true"/>
+
+    <file name="mathlib.cpp" dir="ecl/mathlib"/>
+    <file name="geo.cpp" dir="ecl/geo"/>
+    <file name="geo_mag_declination.cpp" dir="ecl/geo_lookup"/>
+    <file name="airspeed_fusion.cpp" dir="ecl/EKF"/>
+    <file name="control.cpp" dir="ecl/EKF"/>
+    <file name="covariance.cpp" dir="ecl/EKF"/>
+    <file name="drag_fusion.cpp" dir="ecl/EKF"/>
+    <file name="ekf.cpp" dir="ecl/EKF"/>
+    <file name="ekf_helper.cpp" dir="ecl/EKF"/>
+    <file name="estimator_interface.cpp" dir="ecl/EKF"/>
+    <file name="gps_checks.cpp" dir="ecl/EKF"/>
+    <file name="mag_fusion.cpp" dir="ecl/EKF"/>
+    <file name="optflow_fusion.cpp" dir="ecl/EKF"/>
+    <file name="sideslip_fusion.cpp" dir="ecl/EKF"/>
+    <file name="terrain_estimator.cpp" dir="ecl/EKF"/>
+    <file name="vel_pos_fusion.cpp" dir="ecl/EKF"/>
+    <file name="gps_yaw_fusion.cpp" dir="ecl/EKF"/>
+    <raw>
+      ap.CXXFLAGS += -I../ext/matrix -I../ext/ecl -D_USE_MATH_DEFINES -Wno-deprecated-declarations -Wno-enum-compare -Wno-missing-field-initializers -Wno-unused-parameter
+    </raw>
+  </makefile>
+</module>
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
@@ -34,6 +34,7 @@
       <message name="DRAGSPEED"                period="0.02"/>
       <message name="LOGGER_STATUS"            period="5.1"/>
       <message name="LIDAR"                    period="1.2"/>
+      <message name="INS_EKF2"                 period=".25"/>
     </mode>
 
     <mode name="ppm">

diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
@@ -339,7 +339,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/lidar_tfmini.xml modules/ahrs_float_cmpl_quat.xml modules/stabilization_rate.xml 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"
+   settings_modules="modules/air_data.xml modules/lidar_tfmini.xml modules/stabilization_rate.xml 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="blue"
   />
   <aircraft

diff --git a/sw/airborne/autopilot.h b/sw/airborne/autopilot.h
@@ -32,6 +32,10 @@
 #ifndef AUTOPILOT_H
 #define AUTOPILOT_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include "std.h"
 #include "paparazzi.h"
 #include "generated/airframe.h"
@@ -196,5 +200,9 @@ extern void autopilot_send_version(void);
  */
 extern void autopilot_send_mode(void);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif /* AUTOPILOT_H */
 
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
@@ -27,6 +27,9 @@
 #ifndef GUIDANCE_H_H
 #define GUIDANCE_H_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 #include "math/pprz_algebra_int.h"
 #include "math/pprz_algebra_float.h"
@@ -191,4 +194,8 @@ static inline void guidance_h_SetTau(float tau)
   gh_set_tau(tau);
 }
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif /* GUIDANCE_H_H */
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
@@ -27,6 +27,10 @@
 #ifndef STABILIZATION_ATTITUDE_H
 #define STABILIZATION_ATTITUDE_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include "firmwares/rotorcraft/stabilization.h"
 #include "math/pprz_algebra_int.h"
 #include STABILIZATION_ATTITUDE_TYPE_H
@@ -39,5 +43,8 @@ extern void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy);
 extern void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
 extern void stabilization_attitude_run(bool in_flight);
 
+#ifdef __cplusplus
+}
+#endif
 
 #endif /* STABILIZATION_ATTITUDE_H */
diff --git a/sw/airborne/subsystems/datalink/telemetry_common.h b/sw/airborne/subsystems/datalink/telemetry_common.h
@@ -29,6 +29,10 @@
 #ifndef TELEMETRY_COMMON_H
 #define TELEMETRY_COMMON_H
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 #include <inttypes.h>
 #include "std.h"
 #include "pprzlink/pprzlink_device.h"
@@ -83,5 +87,9 @@ static inline int8_t register_periodic_telemetry(struct periodic_telemetry *_pt
 extern void periodic_telemetry_err_report(uint8_t _process, uint8_t _mode, uint8_t _id);
 #endif
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif /* TELEMETRY_COMMON_H */