C++ LowPassFilter2p::reset方法代码示例

//.........这里部分代码省略.........
        break;

    case 1:
    /* fallthrough */
    case 2:
    /* fallthrough */
    case 3:
        perf_count(_comms_errors);
        perf_end(_sample_perf);
        return -EAGAIN;
    }

    int16_t dp_raw = 0, dT_raw = 0;
    dp_raw = (val[0] << 8) + val[1];
    /* mask the used bits */
    dp_raw = 0x3FFF & dp_raw;
    dT_raw = (val[2] << 8) + val[3];
    dT_raw = (0xFFE0 & dT_raw) >> 5;
    float temperature = ((200.0f * dT_raw) / 2047) - 50;

    // Calculate differential pressure. As its centered around 8000
    // and can go positive or negative
    const float P_min = -1.0f;
    const float P_max = 1.0f;
    const float PSI_to_Pa = 6894.757f;
    /*
      this equation is an inversion of the equation in the
      pressure transfer function figure on page 4 of the datasheet

      We negate the result so that positive differential pressures
      are generated when the bottom port is used as the static
      port on the pitot and top port is used as the dynamic port
     */
    float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
    float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;

    // correct for 5V rail voltage if possible
    voltage_correction(diff_press_pa_raw, temperature);

    // the raw value still should be compensated for the known offset
    diff_press_pa_raw -= _diff_pres_offset;

    float diff_press_pa = fabsf(diff_press_pa_raw);

    /*
      note that we return both the absolute value with offset
      applied and a raw value without the offset applied. This
      makes it possible for higher level code to detect if the
      user has the tubes connected backwards, and also makes it
      possible to correctly use offsets calculated by a higher
      level airspeed driver.

      With the above calculation the MS4525 sensor will produce a
      positive number when the top port is used as a dynamic port
      and bottom port is used as the static port

      Also note that the _diff_pres_offset is applied before the
      fabsf() not afterwards. It needs to be done this way to
      prevent a bias at low speeds, but this also means that when
      setting a offset you must set it based on the raw value, not
      the offset value
     */

    struct differential_pressure_s report;

    /* track maximum differential pressure measured (so we can work out top speed). */
    if (diff_press_pa > _max_differential_pressure_pa) {
        _max_differential_pressure_pa = diff_press_pa;
    }

    report.timestamp = hrt_absolute_time();
    report.error_count = perf_event_count(_comms_errors);
    report.temperature = temperature;
    report.differential_pressure_pa = diff_press_pa;
    report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);

    /* the dynamics of the filter can make it overshoot into the negative range */
    if (report.differential_pressure_filtered_pa < 0.0f) {
        report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
    }

    report.differential_pressure_raw_pa = diff_press_pa_raw;
    report.max_differential_pressure_pa = _max_differential_pressure_pa;

    if (_airspeed_pub > 0 && !(_pub_blocked)) {
        /* publish it */
        orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
    }

    new_report(report);

    /* notify anyone waiting for data */
    poll_notify(POLLIN);

    ret = OK;

    perf_end(_sample_perf);

    return ret;
}