[wind] add a vertical component to the wind vector

- use new WIND_INFO message
- update state interface

conf/airframes/ENAC/fixed-wing/apogee.xml

@@ -45,11 +45,11 @@
     <!-- <module name="mcp355x"> -->
     <!--   <define name="USE_SPI1"/> -->
     <!-- </module> -->
-    <!--module name="extra_dl">
+    <module name="extra_dl">
       <configure name="EXTRA_DL_PORT" value="UART6"/>
       <configure name="EXTRA_DL_BAUD" value="B57600"/>
     </module>
-    <module name="meteo_france_DAQ"/-->
+    <module name="meteo_france_DAQ"/>
   </firmware>
 
   <firmware name="test_chibios">

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/firmwares/fixedwing/nav.c

@@ -264,12 +264,12 @@ bool nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide)
    waypoints, using glide airspeed, glide vertical speed and wind */
 static inline bool compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
 {
-  struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
+  struct FloatVect2 wind = stateGetHorizontalWindspeed_f();
   float td_af_x = WaypointX(_af) - WaypointX(_td);
   float td_af_y = WaypointY(_af) - WaypointY(_td);
   float td_af = sqrtf(td_af_x * td_af_x + td_af_y * td_af_y);
   float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(
-                   wind->x * wind->x + wind->y * wind->y));
+                   wind.x * wind.x + wind.y * wind.y));
   WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
   WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
   WaypointAlt(_tod) = WaypointAlt(_af);

sw/airborne/modules/ahrs/ahrs_infrared.c

@@ -114,8 +114,8 @@ void ahrs_infrared_update_gps(struct GpsState *gps_s)
 
   // Heading estimator from wind-information, usually computed with -DWIND_INFO
   // wind_north and wind_east initialized to 0, so still correct if not updated
-  float w_vn = cosf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->x;
-  float w_ve = sinf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f()->y;
+  float w_vn = cosf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f().x;
+  float w_ve = sinf(course_f) * hspeed_mod_f - stateGetHorizontalWindspeed_f().y;
   heading = atan2f(w_ve, w_vn);
   if (heading < 0.) {
     heading += 2 * M_PI;

sw/airborne/modules/enose/anemotaxis.c

@@ -20,8 +20,8 @@ static void last_plume_was_here(void)
 
 bool nav_anemotaxis_downwind(uint8_t c, float radius)
 {
-  struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
-  float wind_dir = atan2(wind->x, wind->y);
+  struct FloatVect2 wind = stateGetHorizontalWindspeed_f();
+  float wind_dir = atan2(wind.x, wind.y);
   waypoints[c].x = waypoints[WP_HOME].x + radius * cos(wind_dir);
   waypoints[c].y = waypoints[WP_HOME].y + radius * sin(wind_dir);
   return false;
@@ -31,8 +31,8 @@ bool nav_anemotaxis_init(uint8_t c)
 {
   status = UTURN;
   sign = 1;
-  struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
-  float wind_dir = atan2(wind->x, wind->y);
+  struct FloatVect2 wind = stateGetHorizontalWindspeed_f();
+  float wind_dir = atan2(wind.x, wind.y);
   waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS * cos(wind_dir + M_PI);
   waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS * sin(wind_dir + M_PI);
   last_plume_was_here();
@@ -48,8 +48,8 @@ bool nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume)
     //    DownlinkSendWp(plume);
   }
 
-  struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
-  float wind_dir = atan2(wind->x, wind->y) + M_PI;
+  struct FloatVect2 wind = stateGetHorizontalWindspeed_f();
+  float wind_dir = atan2(wind.x, wind.y) + M_PI;
 
   /** Not null even if wind_east=wind_north=0 */
   float upwind_x = cos(wind_dir);

sw/airborne/modules/meteo/meteo_france_DAQ.c

@@ -66,6 +66,7 @@ void init_mf_daq(void)
 
 void mf_daq_send_state(void)
 {
+  struct FloatVect2 wind = stateGetHorizontalWindspeed_f();
   // Send aircraft state to DAQ board
   DOWNLINK_SEND_MF_DAQ_STATE(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
                              &autopilot_flight_time,
@@ -84,8 +85,7 @@ void mf_daq_send_state(void)
                              &stateGetPositionLla_f()->lat,
                              &stateGetPositionLla_f()->lon,
                              &stateGetPositionLla_f()->alt,
-                             &stateGetHorizontalWindspeed_f()->y,
-                             &stateGetHorizontalWindspeed_f()->x);
+                             &wind.y, &wind.x);
 }
 
 void mf_daq_send_report(void)
@@ -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,
@@ -123,6 +123,7 @@ void parse_mf_daq_msg(void)
     float *buf = (float*)(dl_buffer+3);
     memcpy(mf_daq.values, buf, mf_daq.nb * sizeof(float));
     // Log on SD card
+    struct FloatVect2 wind = stateGetHorizontalWindspeed_f();
     if (log_started) {
       DOWNLINK_SEND_PAYLOAD_FLOAT(pprzlog_tp, chibios_sdlog, mf_daq.nb, mf_daq.values);
       DOWNLINK_SEND_MF_DAQ_STATE(pprzlog_tp, chibios_sdlog,
@@ -142,8 +143,7 @@ void parse_mf_daq_msg(void)
                                  &stateGetPositionLla_f()->lat,
                                  &stateGetPositionLla_f()->lon,
                                  &stateGetPositionLla_f()->alt,
-                                 &stateGetHorizontalWindspeed_f()->y,
-                                 &stateGetHorizontalWindspeed_f()->x);
+                                 &wind.y, &wind.x);
     }
   }
 }

sw/airborne/modules/nav/nav_drop.c

@@ -81,8 +81,8 @@ static void integrate(uint8_t wp_target)
   int i = 0;
   while (nav_drop_z > 0. && i < MAX_STEPS) {
     /* relative wind experienced by the ball (wind in NED frame) */
-    float airx = -nav_drop_vx + stateGetHorizontalWindspeed_f()->y;
-    float airy = -nav_drop_vy + stateGetHorizontalWindspeed_f()->x;
+    float airx = -nav_drop_vx + stateGetHorizontalWindspeed_f().y;
+    float airy = -nav_drop_vy + stateGetHorizontalWindspeed_f().x;
     float airz = -nav_drop_vz;
 
     /* alpha / m * air */
@@ -161,12 +161,12 @@ unit_t nav_drop_compute_approach(uint8_t wp_target, uint8_t wp_start, uint8_t wp
 
   // wind in NED frame
   if (stateIsAirspeedValid()) {
-    nav_drop_vx = x1 * stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->y;
-    nav_drop_vy = y_1 * stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->x;
+    nav_drop_vx = x1 * stateGetAirspeed_f() + stateGetHorizontalWindspeed_f().y;
+    nav_drop_vy = y_1 * stateGetAirspeed_f() + stateGetHorizontalWindspeed_f().x;
   } else {
     // use approximate airspeed, initially set to AIRSPEED_AT_RELEASE
-    nav_drop_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y;
-    nav_drop_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f()->x;
+    nav_drop_vx = x1 * airspeed + stateGetHorizontalWindspeed_f().y;
+    nav_drop_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f().x;
   }
   nav_drop_vz = 0.;
 

sw/airborne/modules/nav/nav_gls.c

@@ -97,10 +97,10 @@ static inline bool gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8
   WaypointAlt(_tod) = WaypointAlt(_af);
 
   // calculate ground speed on final (target_speed - head wind)
-  struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
-  float wind_norm = sqrtf(wind->x * wind->x + wind->y * wind->y);
-  float wind_on_final = wind_norm * (((td_af_x * wind->y) / (td_af * wind_norm)) +
-                                     ((td_af_y * wind->x) / (td_af * wind_norm)));
+  struct FloatVect2 wind = stateGetHorizontalWindspeed_f();
+  float wind_norm = sqrtf(wind.x * wind.x + wind.y * wind.y);
+  float wind_on_final = wind_norm * (((td_af_x * wind.y) / (td_af * wind_norm)) +
+                                     ((td_af_y * wind.x) / (td_af * wind_norm)));
   Bound(wind_on_final, -MAX_WIND_ON_FINAL, MAX_WIND_ON_FINAL);
   gs_on_final = target_speed - wind_on_final;
 
@@ -137,8 +137,8 @@ bool gls_start(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
 
   init = true;
 
-  //struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
-  //float wind_additional = sqrtf(wind->x*wind->x + wind->y*wind->y); // should be gusts only!
+  //struct FloatVect2 wind = stateGetHorizontalWindspeed_f();
+  //float wind_additional = sqrtf(wind.x*wind.x + wind.y*wind.y); // should be gusts only!
   //Bound(wind_additional, 0, 0.5);
   //target_speed = STALL_AIRSPEED * 1.3 + wind_additional; FIXME
   target_speed =  APP_TARGET_SPEED; //  ok for now!

sw/airborne/modules/nav/nav_smooth.c

@@ -194,8 +194,8 @@ bool snav_on_time(float nominal_radius)
   /* Recompute ground speeds every 10 s */
   if (ground_speed_timer == 0) {
     ground_speed_timer = 40; /* every 10s, called at 40Hz */
-    compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y,
-                         stateGetHorizontalWindspeed_f()->x); // Wind in NED frame
+    compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f().y,
+                         stateGetHorizontalWindspeed_f().x); // Wind in NED frame
   }
   ground_speed_timer--;
 

sw/airborne/modules/nav/nav_survey_disc.c

@@ -58,8 +58,8 @@ bool nav_survey_disc_setup(float grid)
 
 bool nav_survey_disc_run(uint8_t center_wp, float radius)
 {
-  struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
-  float wind_dir = atan2(wind->x, wind->y) + M_PI;
+  struct FloatVect2 wind = stateGetHorizontalWindspeed_f();
+  float wind_dir = atan2(wind.x, wind.y) + M_PI;
 
   /** Not null even if wind_east=wind_north=0 */
   struct FloatVect2 upwind;

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.x = SPEED_BFP_OF_REAL(state.windspeed_f.x);
+    state.windspeed_i.y = SPEED_BFP_OF_REAL(state.windspeed_f.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.z = SPEED_BFP_OF_REAL(state.windspeed_f.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.x = SPEED_FLOAT_OF_BFP(state.windspeed_i.x);
+    state.windspeed_f.x = SPEED_FLOAT_OF_BFP(state.windspeed_i.y);
+  }
+  /* set bit to indicate this representation is computed */
+  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.z = SPEED_FLOAT_OF_BFP(state.windspeed_i.z);
   }
   /* set bit to indicate this representation is computed */
-  SetBit(state.rate_status, WINDSPEED_F);
+  SetBit(state.wind_air_status, DOWNWIND_F);
 }
 
 void stateCalcAirspeed_f(void)