Line data    Source code

       1             : #include "AP_Baro.h"
       2             : #include <AP_AHRS/AP_AHRS.h>
       3             : 
       4             : #if HAL_BARO_WIND_COMP_ENABLED
       5             : 
       6             : // table of compensation coefficient parameters for one baro
       7             : const AP_Param::GroupInfo AP_Baro::WindCoeff::var_info[] = {
       8             : 
       9             :     // @Param: ENABLE
      10             :     // @DisplayName: Wind coefficient enable
      11             :     // @Description: This enables the use of wind coefficients for barometer compensation
      12             :     // @Values: 0:Disabled, 1:Enabled
      13             :     // @User: Advanced
      14             :     AP_GROUPINFO_FLAGS("ENABLE", 1, WindCoeff, enable, 0, AP_PARAM_FLAG_ENABLE),
      15             : 
      16             :     // @Param: FWD
      17             :     // @DisplayName: Pressure error coefficient in positive X direction (forward)
      18             :     // @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis. If the baro height estimate rises during forwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
      19             :     // @Range: -1.0 1.0
      20             :     // @Increment: 0.05
      21             :     // @User: Advanced
      22             :     AP_GROUPINFO("FWD", 2, WindCoeff, xp, 0.0),
      23             : 
      24             :     // @Param: BCK
      25             :     // @DisplayName: Pressure error coefficient in negative X direction (backwards)
      26             :     // @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis. If the baro height estimate rises during backwards flight, then this will be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
      27             :     // @Range: -1.0 1.0
      28             :     // @Increment: 0.05
      29             :     // @User: Advanced
      30             :     AP_GROUPINFO("BCK", 3, WindCoeff, xn, 0.0),
      31             : 
      32             :     // @Param: RGT
      33             :     // @DisplayName: Pressure error coefficient in positive Y direction (right)
      34             :     // @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the right, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
      35             :     // @Range: -1.0 1.0
      36             :     // @Increment: 0.05
      37             :     // @User: Advanced
      38             :     AP_GROUPINFO("RGT", 4, WindCoeff, yp, 0.0),
      39             : 
      40             :     // @Param: LFT
      41             :     // @DisplayName: Pressure error coefficient in negative Y direction (left)
      42             :     // @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Y body axis. If the baro height estimate rises during sideways flight to the left, then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
      43             :     // @Range: -1.0 1.0
      44             :     // @Increment: 0.05
      45             :     // @User: Advanced
      46             :     AP_GROUPINFO("LFT", 5, WindCoeff, yn, 0.0),
      47             : 
      48             :     // @Param: UP
      49             :     // @DisplayName: Pressure error coefficient in positive Z direction (up)
      50             :     // @Description: This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the Z body axis. If the baro height estimate rises above truth height during climbing flight (or forward flight with a high forwards lean angle), then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
      51             :     // @Range: -1.0 1.0
      52             :     // @Increment: 0.05
      53             :     // @User: Advanced
      54             :     AP_GROUPINFO("UP", 6, WindCoeff, zp, 0.0),
      55             : 
      56             :     // @Param: DN
      57             :     // @DisplayName: Pressure error coefficient in negative Z direction (down)
      58             :     // @Description: This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the Z body axis. If the baro height estimate rises above truth height during descending flight (or forward flight with a high backwards lean angle, eg braking manoeuvre), then this should be a negative number. Multirotors can use this feature only if using EKF3 and if the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters have been tuned.
      59             :     // @Range: -1.0 1.0
      60             :     // @Increment: 0.05
      61             :     // @User: Advanced
      62             :     AP_GROUPINFO("DN", 7, WindCoeff, zn, 0.0),
      63             : 
      64             :     AP_GROUPEND
      65             : };
      66             : 
      67             : 
      68             : /*
      69             :   return pressure correction for wind based on GND_WCOEF parameters
      70             :  */
      71     2973723 : float AP_Baro::wind_pressure_correction(uint8_t instance)
      72             : {
      73     2973723 :     const WindCoeff &wcoef = sensors[instance].wind_coeff;
      74             : 
      75     2973723 :     if (!wcoef.enable) {
      76     2972137 :         return 0;
      77             :     }
      78             : 
      79        1586 :     auto &ahrs = AP::ahrs();
      80             : 
      81             :     // correct for static pressure position errors
      82        1586 :     Vector3f airspeed_vec_bf;
      83        1586 :     if (!ahrs.airspeed_vector_TAS(airspeed_vec_bf)) {
      84         469 :         return 0;
      85             :     }
      86             : 
      87        1117 :     float error = 0.0;
      88             : 
      89        1117 :     const float kp = 0.5 * SSL_AIR_DENSITY * ahrs.get_air_density_ratio();
      90        1117 :     const float sqxp = sq(airspeed_vec_bf.x) * kp;
      91        1117 :     const float sqyp = sq(airspeed_vec_bf.y) * kp;
      92        1117 :     const float sqzp = sq(airspeed_vec_bf.z) * kp;
      93             : 
      94        1117 :     if (is_positive(airspeed_vec_bf.x)) {
      95         625 :         sensors[instance].dynamic_pressure.x = sqxp;
      96         625 :         error += wcoef.xp * sqxp;
      97             :     } else {
      98         492 :         sensors[instance].dynamic_pressure.x = -sqxp;
      99         492 :         error += wcoef.xn * sqxp;
     100             :     }
     101        1117 :     if (is_positive(airspeed_vec_bf.y)) {
     102         722 :         sensors[instance].dynamic_pressure.y = sqyp;
     103         722 :         error += wcoef.yp * sqyp;
     104             :     } else {
     105         395 :         sensors[instance].dynamic_pressure.y = -sqyp;
     106         395 :         error += wcoef.yn * sqyp;
     107             :     }
     108        1117 :     if (is_positive(airspeed_vec_bf.z)) {
     109         129 :         sensors[instance].dynamic_pressure.z = sqzp;
     110         129 :         error += wcoef.zp * sqzp;
     111             :     } else {
     112         988 :         sensors[instance].dynamic_pressure.z = -sqzp;
     113         988 :         error += wcoef.zn * sqzp;
     114             :     }
     115             : 
     116        1117 :     return error;
     117             : }
     118             : #endif // HAL_BARO_WIND_COMP_ENABLED