[wind] add a vertical component to the wind vector (#1713)

* [wind] add a vertical component to the wind vector

- use new WIND_INFO message
- update state interface

* [state] use union and return pointers for wind speed

sw/airborne/firmwares/fixedwing/fixedwing_datalink.c

@@ -101,16 +101,26 @@ void firmware_parse_msg(void)
 #ifdef WIND_INFO
     case DL_WIND_INFO: {
       if (DL_WIND_INFO_ac_id(dl_buffer) != AC_ID) { break; }
-      struct FloatVect2 wind;
-      wind.x = DL_WIND_INFO_north(dl_buffer);
-      wind.y = DL_WIND_INFO_east(dl_buffer);
-      stateSetHorizontalWindspeed_f(&wind);
+      uint8_t flags = DL_WIND_INFO_flags(dl_buffer);
+      struct FloatVect2 wind = { 0.f, 0.f };
+      float upwind = 0.f;
+      if (bit_is_set(flags, 0)) {
+        wind.x = DL_WIND_INFO_north(dl_buffer);
+        wind.y = DL_WIND_INFO_east(dl_buffer);
+        stateSetHorizontalWindspeed_f(&wind);
+      }
+      if (bit_is_set(flags, 1)) {
+        upwind = DL_WIND_INFO_up(dl_buffer);
+        stateSetVerticalWindspeed_f(upwind);
+      }
 #if !USE_AIRSPEED
-      stateSetAirspeed_f(DL_WIND_INFO_airspeed(dl_buffer));
+      if (bit_is_set(flags, 2)) {
+        stateSetAirspeed_f(DL_WIND_INFO_airspeed(dl_buffer));
+      }
 #endif
 #ifdef WIND_INFO_RET
       float airspeed = stateGetAirspeed_f();
-      DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, &airspeed);
+      DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &flags, &wind.y, &wind.x, &upwind, &airspeed);
 #endif
     }
     break;

sw/airborne/modules/meteo/meteo_france_DAQ.c

@@ -105,7 +105,7 @@ void mf_daq_send_report(void)
     uint8_t foo = 0;
     int16_t climb = -gps.ned_vel.z;
     int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
-    struct UtmCoor_f utm = stateGetPositionEnu_f();
+    struct UtmCoor_f utm = *stateGetPositionUtm_f();
     int32_t east = utm.east * 100;
     int32_t north = utm.north * 100;
     DOWNLINK_SEND_GPS(pprzlog_tp, chibios_sdlog, &gps.fix,

sw/airborne/state.c

@@ -1124,11 +1124,24 @@ void stateCalcHorizontalWindspeed_i(void)
   }
 
   if (bit_is_set(state.wind_air_status, WINDSPEED_F)) {
-    state.h_windspeed_i.x = SPEED_BFP_OF_REAL(state.h_windspeed_f.x);
-    state.h_windspeed_i.y = SPEED_BFP_OF_REAL(state.h_windspeed_f.y);
+    state.windspeed_i.vect2.x = SPEED_BFP_OF_REAL(state.windspeed_f.vect2.x);
+    state.windspeed_i.vect2.y = SPEED_BFP_OF_REAL(state.windspeed_f.vect2.y);
   }
   /* set bit to indicate this representation is computed */
-  SetBit(state.rate_status, WINDSPEED_I);
+  SetBit(state.wind_air_status , WINDSPEED_I);
+}
+
+void stateCalcVerticalWindspeed_i(void)
+{
+  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
+    return;
+  }
+
+  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
+    state.windspeed_i.vect3.z = SPEED_BFP_OF_REAL(state.windspeed_f.vect3.z);
+  }
+  /* set bit to indicate this representation is computed */
+  SetBit(state.wind_air_status, DOWNWIND_I);
 }
 
 void stateCalcAirspeed_i(void)
@@ -1151,11 +1164,24 @@ void stateCalcHorizontalWindspeed_f(void)
   }
 
   if (bit_is_set(state.wind_air_status, WINDSPEED_I)) {
-    state.h_windspeed_f.x = SPEED_FLOAT_OF_BFP(state.h_windspeed_i.x);
-    state.h_windspeed_f.x = SPEED_FLOAT_OF_BFP(state.h_windspeed_i.y);
+    state.windspeed_f.vect2.x = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect2.x);
+    state.windspeed_f.vect2.x = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect2.y);
   }
   /* set bit to indicate this representation is computed */
-  SetBit(state.rate_status, WINDSPEED_F);
+  SetBit(state.wind_air_status, WINDSPEED_F);
+}
+
+void stateCalcVerticalWindspeed_f(void)
+{
+  if (bit_is_set(state.wind_air_status, DOWNWIND_F)) {
+    return;
+  }
+
+  if (bit_is_set(state.wind_air_status, DOWNWIND_I)) {
+    state.windspeed_f.vect3.z = SPEED_FLOAT_OF_BFP(state.windspeed_i.vect3.z);
+  }
+  /* set bit to indicate this representation is computed */
+  SetBit(state.wind_air_status, DOWNWIND_F);
 }
 
 void stateCalcAirspeed_f(void)

sw/airborne/state.h

@@ -118,11 +118,13 @@
  * @{
  */
 #define WINDSPEED_I 0
-#define AIRSPEED_I  1
-#define WINDSPEED_F 2
-#define AIRSPEED_F  3
-#define AOA_F       4
-#define SIDESLIP_F  5
+#define DOWNWIND_I  1
+#define AIRSPEED_I  2
+#define WINDSPEED_F 3
+#define DOWNWIND_F  4
+#define AIRSPEED_F  5
+#define AOA_F       6
+#define SIDESLIP_F  7
 /**@}*/
 
 
@@ -392,10 +394,13 @@ struct State {
   uint8_t wind_air_status;
 
   /**
-   * Horizontal windspeed in north/east.
+   * Horizontal windspeed in north/east/down.
    * Units: m/s in BFP with #INT32_SPEED_FRAC
    */
-  struct Int32Vect2 h_windspeed_i;
+  union {
+    struct Int32Vect3 vect3;
+    struct Int32Vect2 vect2;
+  } windspeed_i;
 
   /**
    * Norm of relative wind speed.
@@ -405,9 +410,12 @@ struct State {
 
   /**
    * Horizontal windspeed.
-   * Units: m/s with x=north, y=east
+   * Units: m/s with x=north, y=east, z=down
    */
-  struct FloatVect2 h_windspeed_f;
+  union {
+    struct FloatVect3 vect3;
+    struct FloatVect2 vect2;
+  } windspeed_f;
 
   /**
    * Norm of relative air speed.
@@ -1192,8 +1200,10 @@ static inline struct FloatRates *stateGetBodyRates_f(void)
 
 /************* declaration of transformation functions ************/
 extern void stateCalcHorizontalWindspeed_i(void);
+extern void stateCalcVerticalWindspeed_i(void);
 extern void stateCalcAirspeed_i(void);
 extern void stateCalcHorizontalWindspeed_f(void);
+extern void stateCalcVerticalWindspeed_f(void);
 extern void stateCalcAirspeed_f(void);
 
 
@@ -1205,6 +1215,12 @@ static inline bool stateIsWindspeedValid(void)
   return (state.wind_air_status &= ~((1 << WINDSPEED_I) | (1 << WINDSPEED_F)));
 }
 
+/// test if vertical wind speed is available.
+static inline bool stateIsVerticalWindspeedValid(void)
+{
+  return (state.wind_air_status &= ~((1 << DOWNWIND_I) | (1 << DOWNWIND_F)));
+}
+
 /// test if air speed is available.
 static inline bool stateIsAirspeedValid(void)
 {
@@ -1228,12 +1244,21 @@ static inline bool stateIsSideslipValid(void)
 /// Set horizontal windspeed (int).
 static inline void stateSetHorizontalWindspeed_i(struct Int32Vect2 *h_windspeed)
 {
-  VECT2_COPY(state.h_windspeed_i, *h_windspeed);
-  /* clear bits for all windspeed representations and only set the new one */
+  VECT2_COPY(state.windspeed_i.vect2, *h_windspeed);
+  /* clear bits for all horizontal windspeed representations and only set the new one */
   ClearBit(state.wind_air_status, WINDSPEED_F);
   SetBit(state.wind_air_status, WINDSPEED_I);
 }
 
+/// Set vertical windspeed (int).
+static inline void stateSetVerticalWindspeed_i(int32_t v_windspeed)
+{
+  state.windspeed_i.vect3.z = v_windspeed;
+  /* clear bits for all vertical windspeed representations and only set the new one */
+  ClearBit(state.wind_air_status, DOWNWIND_F);
+  SetBit(state.wind_air_status, DOWNWIND_I);
+}
+
 /// Set airspeed (int).
 static inline void stateSetAirspeed_i(int32_t airspeed)
 {
@@ -1246,12 +1271,21 @@ static inline void stateSetAirspeed_i(int32_t airspeed)
 /// Set horizontal windspeed (float).
 static inline void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
 {
-  VECT2_COPY(state.h_windspeed_f, *h_windspeed);
-  /* clear bits for all windspeed representations and only set the new one */
+  VECT2_COPY(state.windspeed_f.vect2, *h_windspeed);
+  /* clear bits for all horizontal windspeed representations and only set the new one */
   ClearBit(state.wind_air_status, WINDSPEED_I);
   SetBit(state.wind_air_status, WINDSPEED_F);
 }
 
+/// Set vertical windspeed (float).
+static inline void stateSetVerticalWindspeed_f(float v_windspeed)
+{
+  state.windspeed_f.vect3.z = v_windspeed;
+  /* clear bits for all vertical windspeed representations and only set the new one */
+  ClearBit(state.wind_air_status, DOWNWIND_I);
+  SetBit(state.wind_air_status, DOWNWIND_F);
+}
+
 /// Set airspeed (float).
 static inline void stateSetAirspeed_f(float airspeed)
 {
@@ -1287,7 +1321,28 @@ static inline struct Int32Vect2 *stateGetHorizontalWindspeed_i(void)
   if (!bit_is_set(state.wind_air_status, WINDSPEED_I)) {
     stateCalcHorizontalWindspeed_i();
   }
-  return &state.h_windspeed_i;
+  return &state.windspeed_i.vect2;
+}
+
+/// Get vertical windspeed (int).
+static inline float stateGetVerticalWindspeed_i(void)
+{
+  if (!bit_is_set(state.wind_air_status, DOWNWIND_I)) {
+    stateCalcVerticalWindspeed_i();
+  }
+  return state.windspeed_i.vect3.z;
+}
+
+/// Get windspeed (int).
+static inline struct Int32Vect3 *stateGetWindspeed_i(void)
+{
+  if (!bit_is_set(state.wind_air_status, WINDSPEED_I)) {
+    stateCalcHorizontalWindspeed_i();
+  }
+  if (!bit_is_set(state.wind_air_status, DOWNWIND_I)) {
+    stateCalcVerticalWindspeed_i();
+  }
+  return &state.windspeed_i.vect3;
 }
 
 /// Get airspeed (int).
@@ -1305,7 +1360,28 @@ static inline struct FloatVect2 *stateGetHorizontalWindspeed_f(void)
   if (!bit_is_set(state.wind_air_status, WINDSPEED_F)) {
     stateCalcHorizontalWindspeed_f();
   }
-  return &state.h_windspeed_f;
+  return &state.windspeed_f.vect2;
+}
+
+/// Get vertical windspeed (float).
+static inline float stateGetVerticalWindspeed_f(void)
+{
+  if (!bit_is_set(state.wind_air_status, DOWNWIND_F)) {
+    stateCalcVerticalWindspeed_f();
+  }
+  return state.windspeed_f.vect3.z;
+}
+
+/// Get windspeed (float).
+static inline struct FloatVect3 *stateGetWindspeed_f(void)
+{
+  if (!bit_is_set(state.wind_air_status, WINDSPEED_F)) {
+    stateCalcHorizontalWindspeed_f();
+  }
+  if (!bit_is_set(state.wind_air_status, DOWNWIND_F)) {
+    stateCalcVerticalWindspeed_f();
+  }
+  return &state.windspeed_f.vect3;
 }
 
 /// Get airspeed (float).

sw/ground_segment/cockpit/live.ml

@@ -731,7 +731,8 @@ let create_ac = fun ?(confirm_kill=true) alert (geomap:G.widget) (acs_notebook:G
         and wind_north = sprintf "%.1f" (-. sin a *. w)
         and airspeed = sprintf "%.1f" ac.airspeed in
 
-        let msg_items = ["WIND_INFO"; ac_id; "42"; wind_east; wind_north;airspeed] in
+        (* only horizontal wind and airspeed are updated, so bitmask is 0b0000101 = 5 *)
+        let msg_items = ["WIND_INFO"; ac_id; "5"; wind_east; wind_north; "0.0"; airspeed] in
         let value = Compat.bytes_concat ";" msg_items in
         let vs = ["ac_id", PprzLink.String ac_id; "message", PprzLink.String value] in
         Ground_Pprz.message_send "dl" "RAW_DATALINK" vs;