diff --git a/conf/messages.xml b/conf/messages.xml
index a3dfee9468..0d00f86e74 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -91,8 +91,8 @@
-
-
+
+
@@ -1435,6 +1435,7 @@
+
diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h
index d7263d7431..f7f6aad44e 100644
--- a/sw/airborne/firmwares/fixedwing/nav.h
+++ b/sw/airborne/firmwares/fixedwing/nav.h
@@ -204,7 +204,9 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
#define SEND_NAVIGATION(_trans, _dev) { \
uint8_t _circle_count = NavCircleCount(); \
struct EnuCoor_f* pos = stateGetPositionEnu_f(); \
- DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count); \
+ float dist_wp = sqrtf(dist2_to_wp); \
+ float dist_home = sqrtf(dist2_to_home); \
+ DOWNLINK_SEND_NAVIGATION(_trans, _dev, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist_wp, &dist_home, &_circle_count, &nav_oval_count); \
}
#define DownlinkSendWp(_trans, _dev, i) { \
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index 37109e4cbf..a1b58a7eda 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -105,8 +105,10 @@ static inline void nav_set_altitude( void );
#include "subsystems/datalink/telemetry.h"
static void send_nav_status(void) {
+ float dist_home = sqrtf(dist2_to_home);
DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(DefaultChannel, DefaultDevice,
&block_time, &stage_time,
+ &dist_home,
&nav_block, &nav_stage,
&horizontal_mode);
if (horizontal_mode == HORIZONTAL_MODE_ROUTE) {
diff --git a/sw/ground_segment/cockpit/compass.ml b/sw/ground_segment/cockpit/compass.ml
index a83b68c790..92761bb026 100644
--- a/sw/ground_segment/cockpit/compass.ml
+++ b/sw/ground_segment/cockpit/compass.ml
@@ -134,7 +134,7 @@ let _ =
let course = ref None in (* deg *)
let desired_course = ref 0. in (* deg *)
let get_navigation = fun _ values ->
- let distance = sqrt (Pprz.float_assoc "dist2_wp" values) in
+ let distance = (try sqrt (Pprz.float_assoc "dist2_wp" values) with _ -> Pprz.float_assoc "dist_wp" values) in
draw da !desired_course !course distance in
ignore (Tm_Pprz.message_bind "NAVIGATION" get_navigation);
let get_gps = fun _ values ->
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index 446f3d9609..263257f08a 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -187,7 +187,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
| "NAVIGATION" ->
a.cur_block <- ivalue "cur_block";
a.cur_stage <- ivalue "cur_stage";
- a.dist_to_wp <- sqrt (fvalue "dist2_wp")
+ a.dist_to_wp <- (try sqrt (fvalue "dist2_wp") with _ -> fvalue "dist_wp");
| "BAT" ->
a.throttle <- fvalue "throttle" /. 9600. *. 100.;
a.kill_mode <- ivalue "kill_auto_throttle" <> 0;
diff --git a/sw/ground_segment/tmtc/ivy_serial_bridge.c b/sw/ground_segment/tmtc/ivy_serial_bridge.c
index 27486b9f82..ccc100dfc8 100644
--- a/sw/ground_segment/tmtc/ivy_serial_bridge.c
+++ b/sw/ground_segment/tmtc/ivy_serial_bridge.c
@@ -122,8 +122,8 @@ static void on_Navigation(IvyClientPtr app, void *user_data, int argc, char *arg
-
-
+
+
@@ -261,8 +261,8 @@ void send_ivy(void)
-
-
+
+
diff --git a/sw/in_progress/satcom/tcp2ivy.c b/sw/in_progress/satcom/tcp2ivy.c
index f8f1635e04..d26bfdaba2 100644
--- a/sw/in_progress/satcom/tcp2ivy.c
+++ b/sw/in_progress/satcom/tcp2ivy.c
@@ -328,8 +328,8 @@ printf("gps_utm_zone %d\n", gps_utm_zone);
-
-
+
+
@@ -340,8 +340,8 @@ printf("gps_utm_zone %d\n", gps_utm_zone);
0, // cur_stage
0, // pos_x
0, // pos_y
- 0, // dist2_wp
- 0, // dist2_home
+ 0, // dist_wp
+ 0, // dist_home
0, // circle_count
0); // oval_count