[messages] MOVE_WP alt in mm

conf/messages.xml

@@ -2223,7 +2223,7 @@
   <field name="ac_id" type="uint8"/>
   <field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
   <field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
-  <field name="alt" type="int32" unit="cm" alt_unit="m">Height above Mean Sea Level (geoid)</field>
+  <field name="alt" type="int32" unit="mm" alt_unit="m">Height above Mean Sea Level (geoid)</field>
  </message>
 
  <message name="WIND_INFO" id="3" link="forwarded">

sw/airborne/arch/sim/jsbsim_transport.c

@@ -7,6 +7,7 @@
 #include <stdlib.h>
 
 #define MOfCm(_x) (((float)(_x))/100.)
+#define MOfMM(_x) (((float)(_x))/1000.)
 
 void parse_dl_ping(char *argv[] __attribute__((unused)))
 {
@@ -52,7 +53,7 @@ void parse_dl_block(char *argv[])
 void parse_dl_move_wp(char *argv[])
 {
   uint8_t wp_id = atoi(argv[1]);
-  float a = MOfCm(atoi(argv[5]));
+  float a = MOfMM(atoi(argv[5]));
 
   /* Computes from (lat, long) in the referenced UTM zone */
   struct LlaCoor_f lla;

sw/airborne/firmwares/fixedwing/datalink.c

@@ -75,6 +75,7 @@ uint8_t joystick_block;
 #endif
 
 #define MOfCm(_x) (((float)(_x))/100.)
+#define MOfMM(_x) (((float)(_x))/1000.)
 
 #define SenderIdOfMsg(x) (x[0])
 #define IdOfMsg(x) (x[1])
@@ -121,7 +122,7 @@ void dl_parse_msg(void)
 #ifdef NAV
       if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) {
         uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
-        float a = MOfCm(DL_MOVE_WP_alt(dl_buffer));
+        float a = MOfMM(DL_MOVE_WP_alt(dl_buffer));
 
         /* Computes from (lat, long) in the referenced UTM zone */
         struct LlaCoor_f lla;

sw/airborne/firmwares/rotorcraft/datalink.c

@@ -103,10 +103,10 @@ void dl_parse_msg(void)
         struct LlaCoor_i lla;
         lla.lat = DL_MOVE_WP_lat(dl_buffer);
         lla.lon = DL_MOVE_WP_lon(dl_buffer);
-        /* WP_alt from message is alt above MSL in cm
+        /* WP_alt from message is alt above MSL in mm
          * lla.alt is above ellipsoid in mm
          */
-        lla.alt = DL_MOVE_WP_alt(dl_buffer) * 10 - state.ned_origin_i.hmsl +
+        lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl +
                   state.ned_origin_i.lla.alt;
         nav_move_waypoint_lla(wp_id, &lla);
       }

sw/ground_segment/tmtc/server.ml

@@ -683,8 +683,12 @@ let ivy_server = fun http ->
   ignore (Ground_Pprz.message_answerer my_id "AIRCRAFTS" send_aircrafts_msg);
   ignore (Ground_Pprz.message_answerer my_id "CONFIG" (send_config http))
 
+(** Convert to cm, with rounding *)
+let cm_of_m = fun f -> Pprz.Int (truncate ((100. *. f) +. 0.5))
+
+(** Convert to mm, with rounding *)
+let mm_of_m = fun f -> Pprz.Int (truncate ((1000. *. f) +. 0.5))
 
-let cm_of_m = fun f -> Pprz.Int (truncate ((100. *. f) +. 0.5)) (* Convert to cm, with rounding *)
 let dl_id = "ground_dl" (* Hack, should be [my_id] *)
 
 (** Got a ground.MOVE_WAYPOINT and send a datalink.MOVE_WP *)
@@ -696,7 +700,7 @@ let move_wp = fun logging _sender vs ->
              "ac_id", Pprz.String ac_id;
              "lat", deg7 "lat";
              "lon", deg7 "long";
-             "alt", cm_of_m (Pprz.float_assoc "alt" vs) ] in
+             "alt", mm_of_m (Pprz.float_assoc "alt" vs) ] in
   Dl_Pprz.message_send dl_id "MOVE_WP" vs;
   log logging ac_id "MOVE_WP" vs
 

sw/simulator/nps/nps_ivy_fixedwing.c

@@ -28,6 +28,7 @@ void nps_ivy_init(char* ivy_bus) {
 #include "subsystems/datalink/downlink.h"
 
 #define MOfCm(_x) (((float)(_x))/100.)
+#define MOfMM(_x) (((float)(_x))/1000.)
 
 void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
                    void *user_data __attribute__ ((unused)),
@@ -35,7 +36,7 @@ void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
 
   if (atoi(argv[2]) == AC_ID) {
     uint8_t wp_id = atoi(argv[1]);
-    float a = MOfCm(atoi(argv[5]));
+    float a = MOfMM(atoi(argv[5]));
 
     /* Computes from (lat, long) in the referenced UTM zone */
     struct LlaCoor_f lla;

sw/simulator/nps/nps_ivy_rotorcraft.c

@@ -47,11 +47,12 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
     struct LlaCoor_i lla;
     lla.lat = atoi(argv[3]);
     lla.lon = atoi(argv[4]);
-    /* WP_alt from message is alt above MSL in cm
+    /* WP_alt from message is alt above MSL in mm
      * lla.alt is above ellipsoid in mm
      */
-    lla.alt = atoi(argv[5]) *10 - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
+    lla.alt = atoi(argv[5]) - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
     nav_move_waypoint_lla(wp_id, &lla);
-    printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, waypoints[wp_id].x, waypoints[wp_id].y, waypoints[wp_id].z);
+    printf("move wp id=%d x=% .4f, y=% .4f, z=% .4f\n", wp_id,
+           WaypointX(wp_id), WaypointY(wp_id), WaypointAlt(wp_id));
   }
 }