Q定点数C库

/**
 * @file fl_q.h
 * @copyright SPDX-License-Identifier: MIT
 * @brief Shared signed Q arithmetic, C99, version 1.
 * @details Qm.f includes the sign bit in m; m + f = bits.
 *          Raw value r represents r / 2^f. Storage: 16 or 32 bits;
 *          fractional bits: 2..bits-1. int32_t carries either format.
 *          Round to nearest, ties away from zero; saturate at storage limits.
 *          No global format macros. Different formats may coexist.
 */
#ifndef FL_Q_H
#define FL_Q_H

#include <stdint.h>

/** @brief Signed storage width and fractional bit count. */
typedef struct
{
    uint8_t bits; /**< Storage width: 16 or 32. */
    uint8_t frac; /**< Fractional bits: 2..bits-1. */
} fl_q_format_t;

/**
 * @brief Test a format before using arithmetic functions.
 * @param[in] q Format to validate.
 * @return Nonzero for a supported format; zero otherwise.
 * @note All other functions require a valid format and raw operands in range.
 */
static inline int fl_q_valid(fl_q_format_t q)
{
    return (q.bits == 16u || q.bits == 32u) && q.frac >= 2u && q.frac < q.bits;
}

/**
 * @brief Saturate a wide raw result to a format's signed storage limits.
 * @param[in] value Wide raw result.
 * @param[in] q Valid destination format.
 * @return Saturated raw value.
 */
static inline int32_t fl_q_sat(int64_t value, fl_q_format_t q)
{
    const int64_t limit = INT64_C(1) << (q.bits - 1u);
    if (value >= limit)
    {
        return (int32_t)(limit - 1);
    }
    if (value < -limit)
    {
        return (int32_t)-limit;
    }
    return (int32_t)value;
}

/**
 * @brief Divide a wide integer, rounding nearest with ties away from zero.
 * @param[in] value Signed numerator, including INT64_MIN.
 * @param[in] divisor Positive denominator.
 * @return Rounded integer quotient.
 */
static inline int64_t fl_q_round_div(int64_t value, int64_t divisor)
{
    int64_t result = value / divisor;
    const int64_t remainder = value % divisor;
    const int64_t threshold = divisor / 2 + divisor % 2;
    if (remainder >= threshold)
    {
        ++result;
    }
    else if (remainder <= -threshold)
    {
        --result;
    }
    return result;
}

/**
 * @brief Multiply raw values; retain a wide result in the original Q units.
 * @param[in] a First raw operand.
 * @param[in] b Second raw operand in the same format.
 * @param[in] q Valid operand format.
 * @return Rounded product without storage saturation.
 * @note With frac >= 2, five such products fit an int64_t accumulator.
 */
static inline int64_t fl_q_mul_wide(int32_t a, int32_t b, fl_q_format_t q)
{
    return fl_q_round_div((int64_t)a * b, INT64_C(1) << q.frac);
}

/**
 * @brief Add two raw values with saturation.
 * @param[in] a First raw operand.
 * @param[in] b Second raw operand.
 * @param[in] q Valid operand format.
 * @return Saturated sum.
 */
static inline int32_t fl_q_add(int32_t a, int32_t b, fl_q_format_t q)
{
    return fl_q_sat((int64_t)a + b, q);
}

/**
 * @brief Subtract two raw values with saturation.
 * @param[in] a Raw minuend.
 * @param[in] b Raw subtrahend.
 * @param[in] q Valid operand format.
 * @return Saturated difference.
 */
static inline int32_t fl_q_sub(int32_t a, int32_t b, fl_q_format_t q)
{
    return fl_q_sat((int64_t)a - b, q);
}

/**
 * @brief Multiply two raw values with rounding and saturation.
 * @param[in] a First raw operand.
 * @param[in] b Second raw operand.
 * @param[in] q Valid operand format.
 * @return Saturated product in the same Q format.
 */
static inline int32_t fl_q_mul(int32_t a, int32_t b, fl_q_format_t q)
{
    return fl_q_sat(fl_q_mul_wide(a, b, q), q);
}

/**
 * @brief Divide two raw values with rounding and saturation.
 * @param[in] a Raw numerator.
 * @param[in] b Nonzero raw denominator in the same format.
 * @param[in] q Valid operand format.
 * @return Saturated quotient in the same Q format.
 */
static inline int32_t fl_q_div(int32_t a, int32_t b, fl_q_format_t q)
{
    int64_t numerator = (int64_t)a * (INT64_C(1) << q.frac);
    int64_t denominator = b;
    if (denominator < 0)
    {
        numerator = -numerator;
        denominator = -denominator;
    }
    return fl_q_sat(fl_q_round_div(numerator, denominator), q);
}

/**
 * @brief Convert an application value to raw Q, rounding and saturating.
 * @param[in] value Application value; NaN maps to zero, infinities saturate.
 * @param[in] q Valid destination format.
 * @return Raw Q value; the filter's sample loop needs no floating arithmetic.
 */
static inline int32_t fl_q_from_double(double value, fl_q_format_t q)
{
    const double scaled = value * (double)(INT64_C(1) << q.frac);
    const int64_t limit = INT64_C(1) << (q.bits - 1u);
    if (scaled != scaled)
    {
        return 0;
    }
    if (scaled >= (double)(limit - 1))
    {
        return (int32_t)(limit - 1);
    }
    if (scaled <= (double)-limit)
    {
        return (int32_t)-limit;
    }
    return fl_q_sat((int64_t)(scaled < 0 ? scaled - 0.5 : scaled + 0.5), q);
}

/**
 * @brief Convert a raw Q value to application units.
 * @param[in] raw Raw Q value.
 * @param[in] q Valid source format.
 * @return Value divided by 2^frac.
 */
static inline double fl_q_to_double(int32_t raw, fl_q_format_t q)
{
    return (double)raw / (double)(INT64_C(1) << q.frac);
}

#endif /* FL_Q_H */

/**
 * @file q_demo.c
 * @brief Multiply two application values using Q4.28 arithmetic, C99.
 * @copyright SPDX-License-Identifier: MIT
 */

#include "fl_q.h"
#include <inttypes.h>
#include <stdio.h>

/**
 * @brief Convert, multiply and print a raw Q value and its application value.
 * @return Zero after writing the result to standard output.
 */
int main(void)
{
    const fl_q_format_t q = {32u, 28u};
    const int32_t input = fl_q_from_double(0.5, q);
    const int32_t gain = fl_q_from_double(0.25, q);
    const int32_t output = fl_q_mul(input, gain, q);

    printf("%" PRId32 " %.17g\n", output, fl_q_to_double(output, q));
    return 0;
}

/**
 * @file filter_demo.c
 * @brief Run a step through a tool-exported Q filter, C99.
 * @copyright SPDX-License-Identifier: MIT
 * @details Place fl_filter.h and fl_q.h in the same include directory.
 */

#include "fl_filter.h"
#include <stdio.h>

/**
 * @brief Print sample index and filtered application value for a unit step.
 * @return Zero after processing 16 samples at the exported sampling rate.
 */
int main(void)
{
    Filter state;
    filter_reset(&state, 0);

    for (unsigned i = 0; i < 16u; ++i)
    {
        const double input = i < 4u ? 0.0 : 1.0;
        const FilterSample x = (FilterSample)fl_q_from_double(input, filter_q);
        const FilterSample y = filter_step(&state, x);

        printf("%u %.17g\n", i, fl_q_to_double(y, filter_q));
    }
    return 0;
}