AP_Baro: add logging of dynamic pressure

libraries/AP_Baro/AP_Baro.h

@@ -4,6 +4,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Param/AP_Param.h>
+#include <AP_Math/AP_Math.h>
 #include <Filter/DerivativeFilter.h>
 #include <AP_MSP/msp.h>
 #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
@@ -73,6 +74,10 @@ public:
     // pressure in Pascal. Divide by 100 for millibars or hectopascals
     float get_pressure(void) const { return get_pressure(_primary); }
     float get_pressure(uint8_t instance) const { return sensors[instance].pressure; }
+#if HAL_BARO_WIND_COMP_ENABLED
+    // dynamic pressure in Pascal. Divide by 100 for millibars or hectopascals
+    const Vector3f& get_dynamic_pressure(uint8_t instance) const { return sensors[instance].dynamic_pressure; }
+#endif
 
     // temperature in degrees C
     float get_temperature(void) const { return get_temperature(_primary); }
@@ -270,6 +275,7 @@ private:
         AP_Int32 bus_id;
 #if HAL_BARO_WIND_COMP_ENABLED
         WindCoeff wind_coeff;
+        Vector3f dynamic_pressure;      // calculated dynamic pressure
 #endif
     } sensors[BARO_MAX_INSTANCES];
 

libraries/AP_Baro/AP_Baro_Logging.cpp

@@ -15,9 +15,20 @@ void AP_Baro::Write_Baro_instance(uint64_t time_us, uint8_t baro_instance)
         sample_time_ms: get_last_update(baro_instance),
         drift_offset  : get_baro_drift_offset(),
         ground_temp   : get_ground_temperature(),
-        healthy       : (uint8_t)healthy(baro_instance)
+        healthy       : (uint8_t)healthy(baro_instance),
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
+#if HAL_BARO_WIND_COMP_ENABLED
+    const struct log_BARD pkt2{
+        LOG_PACKET_HEADER_INIT(LOG_BARD_MSG),
+        time_us       : time_us,
+        instance      : baro_instance,
+        dyn_pressure_x: get_dynamic_pressure(baro_instance).x,
+        dyn_pressure_y: get_dynamic_pressure(baro_instance).y,
+        dyn_pressure_z: get_dynamic_pressure(baro_instance).z,
+    };
+    AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
+#endif
 }
 
 // Write a BARO packet

libraries/AP_Baro/AP_Baro_Wind.cpp

@@ -83,26 +83,34 @@ float AP_Baro::wind_pressure_correction(uint8_t instance)
     }
 
     float error = 0.0;
-    const float sqx = sq(airspeed_vec_bf.x);
-    const float sqy = sq(airspeed_vec_bf.y);
-    const float sqz = sq(airspeed_vec_bf.z);
+
+    const float kp = 0.5 * SSL_AIR_DENSITY * get_air_density_ratio();
+    const float sqxp = sq(airspeed_vec_bf.x) * kp;
+    const float sqyp = sq(airspeed_vec_bf.y) * kp;
+    const float sqzp = sq(airspeed_vec_bf.z) * kp;
 
     if (is_positive(airspeed_vec_bf.x)) {
-        error += wcoef.xp * sqx;
+        sensors[instance].dynamic_pressure.x = sqxp;
+        error += wcoef.xp * sqxp;
     } else {
-        error += wcoef.xn * sqx;
+        sensors[instance].dynamic_pressure.x = -sqxp;
+        error += wcoef.xn * sqxp;
     }
     if (is_positive(airspeed_vec_bf.y)) {
-        error += wcoef.yp * sqy;
+        sensors[instance].dynamic_pressure.y = sqyp;
+        error += wcoef.yp * sqyp;
     } else {
-        error += wcoef.yn * sqy;
+        sensors[instance].dynamic_pressure.y = -sqyp;
+        error += wcoef.yn * sqyp;
     }
     if (is_positive(airspeed_vec_bf.z)) {
-        error += wcoef.zp * sqz;
+        sensors[instance].dynamic_pressure.z = sqzp;
+        error += wcoef.zp * sqzp;
     } else {
-        error += wcoef.zn * sqz;
+        sensors[instance].dynamic_pressure.z = -sqzp;
+        error += wcoef.zn * sqzp;
     }
 
-    return error * 0.5 * SSL_AIR_DENSITY * get_air_density_ratio();
+    return error;
 }
 #endif // HAL_BARO_WIND_COMP_ENABLED

libraries/AP_Baro/LogStructure.h

@@ -3,7 +3,8 @@
 #include <AP_Logger/LogStructure.h>
 
 #define LOG_IDS_FROM_BARO \
-    LOG_BARO_MSG
+    LOG_BARO_MSG, \
+    LOG_BARD_MSG
 
 // @LoggerMessage: BARO
 // @Description: Gathered Barometer data
@@ -31,6 +32,22 @@ struct PACKED log_BARO {
     uint8_t healthy;
 };
 
+// @LoggerMessage: BARD
+// @Description: Barometer dynamic data
+// @Field: TimeUS: Time since system startup
+// @Field: I: barometer sensor instance number
+// @Field: DynPrX: calculated dynamic pressure in the bodyframe X-axis
+// @Field: DynPrY: calculated dynamic pressure in the bodyframe Y-axis
+// @Field: DynPrZ: calculated dynamic pressure in the bodyframe Z-axis
+struct PACKED log_BARD {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t instance;
+    float dyn_pressure_x;
+    float dyn_pressure_y;
+    float dyn_pressure_z;
+};
+
 #define LOG_STRUCTURE_FROM_BARO                                         \
     { LOG_BARO_MSG, sizeof(log_BARO),                                   \
             "BARO",                                                     \
@@ -39,4 +56,12 @@ struct PACKED log_BARO {
             "s"       "#"  "m"    "P"      "O"     "n"    "s"    "m"       "O"        "-", \
             "F"       "-"  "0"    "0"      "B"     "0"    "C"    "?"       "0"        "-", \
             true                                                        \
+            },                                                          \
+    { LOG_BARD_MSG, sizeof(log_BARD),                                   \
+            "BARD",                                                     \
+            "Q"       "B"  "fff", \
+            "TimeUS," "I," "DynPrX,DynPrY,DynPrZ", \
+            "s"       "#"  "PPP", \
+            "F"       "-"  "000", \
+            true                                                        \
             },