[rotorcraft] add dist_wp

conf/messages.xml

@@ -1436,6 +1436,7 @@
     <field name="block_time" type="uint16" unit="s"/>
     <field name="stage_time" type="uint16" unit="s"/>
     <field name="dist_home" type="float" format="%.1f" unit="m"/>
+    <field name="dist_wp" type="float" format="%.1f" unit="m"/>
     <field name="cur_block" type="uint8"/>
     <field name="cur_stage" type="uint8"/>
     <field name="horizontal_mode" type="uint8"/>

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -69,6 +69,8 @@ float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE;
 float dist2_to_home;
 bool_t too_far_from_home;
 
+float dist2_to_wp;
+
 uint8_t horizontal_mode;
 struct EnuCoor_i nav_segment_start, nav_segment_end;
 struct EnuCoor_i nav_circle_center;
@@ -106,9 +108,10 @@ static inline void nav_set_altitude( void );
 
 static void send_nav_status(void) {
   float dist_home = sqrtf(dist2_to_home);
+  float dist_wp = sqrtf(dist2_to_wp);
   DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(DefaultChannel, DefaultDevice,
       &block_time, &stage_time,
-      &dist_home,
+      &dist_home, &dist_wp,
       &nav_block, &nav_stage,
       &horizontal_mode);
   if (horizontal_mode == HORIZONTAL_MODE_ROUTE) {
@@ -169,6 +172,7 @@ void nav_init(void) {
 
   too_far_from_home = FALSE;
   dist2_to_home = 0;
+  dist2_to_wp = 0;
 
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_NAV_STATUS", send_nav_status);
@@ -214,6 +218,7 @@ void nav_run(void) {
 void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) {
   if (radius == 0) {
     VECT2_COPY(navigation_target, *wp_center);
+    dist2_to_wp = get_dist2_to_point(wp_center);
   }
   else {
     struct Int32Vect2 pos_diff;
@@ -279,6 +284,8 @@ void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end) {
   nav_segment_start = *wp_start;
   nav_segment_end = *wp_end;
   horizontal_mode = HORIZONTAL_MODE_ROUTE;
+
+  dist2_to_wp = get_dist2_to_point(wp_end);
 }
 
 bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int16_t approaching_time) {
@@ -389,6 +396,8 @@ void nav_init_stage( void ) {
 void nav_periodic_task(void) {
   RunOnceEvery(16, { stage_time++;  block_time++; });
 
+  dist2_to_wp = 0;
+
   /* from flight_plan.h */
   auto_nav();
 
@@ -454,19 +463,31 @@ void nav_home(void) {
   nav_altitude = waypoints[WP_HOME].z;
   nav_flight_altitude = nav_altitude;
 
+  dist2_to_wp = dist2_to_home;
+
   /* run carrot loop */
   nav_run();
 }
 
+/** Returns squared horizontal distance to given point */
+float get_dist2_to_point(struct EnuCoor_i *p) {
+  struct EnuCoor_f* pos = stateGetPositionEnu_f();
+  struct FloatVect2 pos_diff;
+  pos_diff.x = POS_FLOAT_OF_BFP(p->x) - pos->x;
+  pos_diff.y = POS_FLOAT_OF_BFP(p->y) - pos->y;
+  return pos_diff.x * pos_diff.x + pos_diff.y * pos_diff.y;
+}
+
+/** Returns squared horizontal distance to given waypoint */
+float get_dist2_to_waypoint(uint8_t wp_id) {
+  return get_dist2_to_point(&waypoints[wp_id]);
+}
+
 /** Computes squared distance to the HOME waypoint potentially sets
  * #too_far_from_home
  */
 void compute_dist2_to_home(void) {
-  struct EnuCoor_i* pos = stateGetPositionEnu_i();
-  struct Int32Vect2 home_d;
-  VECT2_DIFF(home_d, waypoints[WP_HOME], *pos);
-  INT32_VECT2_RSHIFT(home_d, home_d, INT32_POS_FRAC);
-  dist2_to_home = (float)(home_d.x * home_d.x + home_d.y * home_d.y);
+  dist2_to_home = get_dist2_to_waypoint(WP_HOME);
   too_far_from_home = dist2_to_home > max_dist2_from_home;
 }
 

sw/airborne/firmwares/rotorcraft/navigation.h

@@ -74,6 +74,10 @@ extern float dist2_to_home;      ///< squared distance to home waypoint
 extern bool_t too_far_from_home;
 extern float failsafe_mode_dist2; ///< maximum squared distance to home wp before going to failsafe mode
 
+extern float dist2_to_wp;       ///< squared distance to next waypoint
+
+extern float get_dist2_to_waypoint(uint8_t wp_id);
+extern float get_dist2_to_point(struct EnuCoor_i *p);
 extern void compute_dist2_to_home(void);
 extern void nav_home(void);
 
@@ -121,6 +125,7 @@ extern bool_t nav_set_heading_current(void);
 #define NavGotoWaypoint(_wp) { \
   horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
   INT32_VECT3_COPY( navigation_target, waypoints[_wp]); \
+  dist2_to_wp = get_dist2_to_waypoint(_wp); \
 }
 
 /*********** Navigation on a circle **************************************/

sw/ground_segment/cockpit/live.ml

@@ -1167,7 +1167,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph ->
     (* Estimated Time Arrival to next waypoint *)
     let d = Pprz.float_assoc "dist_to_wp" vs in
     let label =
-      if d = 0. || ac.speed = 0. then
+      if d < 0.5 || ac.speed < 0.5 then
         "N/A"
       else
         sprintf "%.0fs" (d /. ac.speed) in

sw/ground_segment/tmtc/rotorcraft_server.ml

@@ -205,7 +205,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
       a.cur_block <- ivalue "cur_block";
       a.cur_stage <- ivalue "cur_stage";
       a.horizontal_mode <- check_index (ivalue "horizontal_mode") horiz_modes "AP_HORIZ";
-  (*a.dist_to_wp <- sqrt (fvalue "dist2_wp")*)
+      a.dist_to_wp <- fvalue "dist_wp";
     | "WP_MOVED_ENU" ->
       begin
         match a.nav_ref with