Use altitude waypoints for EKF2

paparazzi · Mar 25, 2024 · 3bedf95 · 3bedf95
1 parent 757c637
commit 3bedf95
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 5 deletions.
diff --git a/conf/modules/ins_ekf2.xml b/conf/modules/ins_ekf2.xml
@@ -25,7 +25,7 @@
     <define name="INS_EKF2_FLOW_POS_X" value="0" description="Flow sensor X offset from CoG position [m]"/>
     <define name="INS_EKF2_FLOW_POS_Y" value="0" description="Flow sensor Y offset from CoG position [m]"/>
     <define name="INS_EKF2_FLOW_POS_Z" value="0" description="Flow sensor Z offset from CoG position [m]"/>
-    
+
     <define name="INS_EKF2_SONAR_MIN_RANGE" value="0.05" description="AGL sensor minimum range [m]"/>
     <define name="INS_EKF2_SONAR_MAX_RANGE" value="3" description="AGL sensor maximum range [m]"/>
     <define name="INS_EKF2_RANGE_MAIN_AGL" value="1" description="If enabled uses radar sensor as primary AGL source, if possible"/>
@@ -80,6 +80,7 @@
     <define name="__PAPARAZZI" value="TRUE"/>
     <define name="ECL_STANDALONE" value="TRUE"/>
     <define name="IMU_INTEGRATION" value="TRUE"/> <!-- Needed to enable IMU integration -->
+    <define name="USE_ALT_LLA_WAYPOINTS" value="TRUE"/>
 
     <!-- Compile needed ecl files -->
     <file name="geo.cpp" dir="ecl/geo"/>
@@ -103,6 +104,6 @@
     <file name="EKFGSF_yaw.cpp" dir="ecl/EKF"/>
     <file name="sensor_range_finder.cpp" dir="ecl/EKF"/>
     <file name="utils.cpp" dir="ecl/EKF"/>
-    
+
   </makefile>
 </module>
diff --git a/sw/airborne/modules/nav/waypoints.c b/sw/airborne/modules/nav/waypoints.c
@@ -118,6 +118,14 @@ float waypoint_get_alt(uint8_t wp_id)
   return 0.f;
 }
 
+float waypoint_get_lla_alt(uint8_t wp_id)
+{
+  if (wp_id < nb_waypoint) {
+    return waypoints[wp_id].lla.alt/1000.f - state.ned_origin_i.lla.alt/1000.f;
+  }
+  return 0.f;
+}
+
 float waypoint_get_lat_deg(uint8_t wp_id)
 {
   if (wp_id < nb_waypoint) {
@@ -335,9 +343,9 @@ void waypoint_localize(uint8_t wp_id)
     struct EnuCoor_i enu;
     enu_of_lla_point_i(&enu, &state.ned_origin_i, &waypoints[wp_id].lla);
     // convert ENU pos from cm to BFP with INT32_POS_FRAC
-    enu.x = POS_BFP_OF_REAL(enu.x) / 100;
-    enu.y = POS_BFP_OF_REAL(enu.y) / 100;
-    enu.z = POS_BFP_OF_REAL(enu.z) / 100;
+    enu.x = POS_BFP_OF_REAL((int64_t) enu.x) / 100;
+    enu.y = POS_BFP_OF_REAL((int64_t) enu.y) / 100;
+    enu.z = POS_BFP_OF_REAL((int64_t) enu.z) / 100;
     waypoints[wp_id].enu_i = enu;
     SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
     ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);

diff --git a/sw/airborne/modules/nav/waypoints.h b/sw/airborne/modules/nav/waypoints.h
@@ -53,7 +53,11 @@ extern struct Waypoint waypoints[];
 /* aliases for backwards compatibilty */
 #define WaypointX(_wp)    waypoint_get_x(_wp)
 #define WaypointY(_wp)    waypoint_get_y(_wp)
+#if USE_ALT_LLA_WAYPOINTS
+#define WaypointAlt(_wp)  waypoint_get_lla_alt(_wp)
+#else
 #define WaypointAlt(_wp)  waypoint_get_alt(_wp)
+#endif
 #define Height(_h) (_h)
 
 extern void waypoints_init(void);
@@ -72,6 +76,7 @@ extern float waypoint_get_x(uint8_t wp_id);
 extern float waypoint_get_y(uint8_t wp_id);
 /** Get altitude of waypoint in meters (above reference) */
 extern float waypoint_get_alt(uint8_t wp_id);
+extern float waypoint_get_lla_alt(uint8_t wp_id);
 /** Get latitude of waypoint in deg */
 extern float waypoint_get_lat_deg(uint8_t wp_id);
 /** Get latitude of waypoint in rad */