diff --git a/conf/modules/ins_ekf2.xml b/conf/modules/ins_ekf2.xml
index 1520534b55e..5e061d81a80 100644
--- a/conf/modules/ins_ekf2.xml
+++ b/conf/modules/ins_ekf2.xml
@@ -25,7 +25,7 @@
-
+
@@ -80,6 +80,7 @@
+
@@ -103,6 +104,6 @@
-
+
diff --git a/sw/airborne/modules/nav/waypoints.c b/sw/airborne/modules/nav/waypoints.c
index cbc9994f2e8..56be84296e2 100644
--- 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
index 75e46b275e7..c1e854bbd3f 100644
--- 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 */