software/firmware/src/hall_line.c

1/*!2    \file    hall_line.c3    \brief   直线霍尔阵列的基线、零点和对数高斯定位4*/56#include "hall_line.h"78#define HALL_LINE_COUNT             39#define HALL_LINE_BASE_DEFAULT      204810#define HALL_LINE_CAL_MIN           16U11#define HALL_LINE_CAL_MAX           4096U12#define HALL_LINE_LN2_Q16           454261314/*! log2(1 + i/32) 的 Q16，最后一项是 65536，所以用 32 位 */15static const uint32_t s_log2_q16[33] = {16    0U, 2909U, 5732U, 8473U, 11136U, 13727U, 16248U, 18704U,17    21098U, 23433U, 25711U, 27936U, 30109U, 32234U, 34312U, 36346U,18    38336U, 40286U, 42196U, 44068U, 45904U, 47705U, 49472U, 51207U,19    52911U, 54584U, 56229U, 57845U, 59434U, 60997U, 62534U, 64047U,20    65536U21};2223static const int32_t s_pos_um[HALL_LINE_COUNT] = {24    -HALL_LINE_PITCH_UM, 0, HALL_LINE_PITCH_UM25};2627static int16_t s_baseline[HALL_LINE_COUNT] = {28    HALL_LINE_BASE_DEFAULT, HALL_LINE_BASE_DEFAULT, HALL_LINE_BASE_DEFAULT29};30static uint16_t s_sigma_um = HALL_LINE_SIGMA_DEFAULT;31static int16_t s_zero_cmm;32static uint8_t s_valid;33static uint8_t s_cal_run;34static uint8_t s_out;35static int32_t s_cal_sum[HALL_LINE_COUNT];36static uint16_t s_cal_count;3738/*!39    \brief      自然对数用的 log2，Q1640    \param[in]  value: 正数41    \retval     log2(value) * 6553642*/43static int32_t hall_line_log2_q16(uint32_t value)44{45    int32_t exp = 0;46    uint32_t index;47    uint32_t frac;48    uint32_t base;49    uint32_t span;5051    if(0U == value) {52        return 0;53    }54    while(value > 65535U) {55        value >>= 1;56        exp++;57    }58    while(value < 32768U) {59        value <<= 1;60        exp--;61    }62    index = (value - 32768U) >> 10;63    frac = (value - 32768U) & 1023U;64    base = s_log2_q16[index];65    span = s_log2_q16[index + 1U] - base;66    return ((15 + exp) << 16) + (int32_t)base + (int32_t)((span * frac) >> 10);67}6869/*!70    \brief      用峰和相邻一路估计磁铁位置71    \param[in]  raw: 三路原码72    \param[out] sample: 位置写在 angle_cdeg，单位 0.01 mm73    \retval     none74*/75static void hall_line_solve(const uint16_t raw[HALL_LINE_COUNT], hall_sample_t *sample)76{77    int32_t field[HALL_LINE_COUNT];78    int32_t peak_abs = -1;79    int32_t amp;80    int32_t neighbor = 0;81    int32_t pos_um;82    int32_t best = -1;83    uint32_t i;84    uint32_t peak = HALL_LINE_MID;8586    s_out = 0U;87    for(i = 0U; i < HALL_LINE_COUNT; i++) {88        int32_t mag;8990        field[i] = (int32_t)raw[i] - (int32_t)s_baseline[i];91        sample->raw[i] = raw[i];92        mag = (field[i] < 0) ? -field[i] : field[i];93        if(mag > peak_abs) {94            peak_abs = mag;95            peak = i;96        }97    }98    if(field[peak] < 0) {99        for(i = 0U; i < HALL_LINE_COUNT; i++) {100            field[i] = -field[i];101        }102    }103    amp = field[peak];104    sample->amplitude = amp;105    if(amp <= 0) {106        sample->angle_cdeg = -(int32_t)s_zero_cmm;107        return;108    }109110    if((peak > 0U) && (field[peak - 1U] > neighbor)) {111        best = (int32_t)peak - 1;112        neighbor = field[peak - 1U];113    }114    if((peak + 1U) < HALL_LINE_COUNT) {115        if(field[peak + 1U] > neighbor) {116            best = (int32_t)peak + 1;117            neighbor = field[peak + 1U];118        }119    }120121    if((best < 0) || ((neighbor * 8) < amp)) {122        pos_um = s_pos_um[peak];123        if(HALL_LINE_MID != peak) {124            s_out = 1U;125        }126    } else {127        int32_t lns = hall_line_log2_q16((uint32_t)amp) - hall_line_log2_q16((uint32_t)neighbor);128        int32_t ln_q16 = (int32_t)(((int64_t)lns * HALL_LINE_LN2_Q16) / 65536);129        uint32_t scale = ((uint32_t)s_sigma_um * (uint32_t)s_sigma_um) / (uint32_t)HALL_LINE_PITCH_UM;130        int32_t delta = (int32_t)(((int64_t)scale * (int64_t)ln_q16) / 65536);131        int32_t mid = (s_pos_um[peak] + s_pos_um[best]) / 2;132        int32_t direction = (s_pos_um[peak] > s_pos_um[best]) ? 1 : -1;133134        pos_um = mid + (direction * delta);135        /*! 三路几乎一样时没有梯度，比值会落到两颗传感器的中点。 */136        if(neighbor >= amp) {137            uint32_t third = 3U - peak - (uint32_t)best;138139            if(field[third] >= neighbor) {140                pos_um = 0;141            }142        }143    }144145    if(pos_um > HALL_LINE_SPAN_UM) {146        pos_um = HALL_LINE_SPAN_UM;147        s_out = 1U;148    } else if(pos_um < -HALL_LINE_SPAN_UM) {149        pos_um = -HALL_LINE_SPAN_UM;150        s_out = 1U;151    }152    sample->angle_cdeg = (pos_um / 10) - (int32_t)s_zero_cmm;153}154155void hall_line_apply(const int16_t baseline[3], uint8_t valid,156                     uint16_t sigma_um, int16_t zero_cmm)157{158    uint32_t i;159160    s_cal_run = 0U;161    s_cal_count = 0U;162    s_zero_cmm = zero_cmm;163    if((sigma_um < HALL_LINE_SIGMA_MIN) || (sigma_um > HALL_LINE_SIGMA_MAX)) {164        sigma_um = HALL_LINE_SIGMA_DEFAULT;165    }166    s_sigma_um = sigma_um;167    if(0U != valid) {168        s_valid = 1U;169        for(i = 0U; i < HALL_LINE_COUNT; i++) {170            s_baseline[i] = baseline[i];171        }172    } else {173        s_valid = 0U;174        for(i = 0U; i < HALL_LINE_COUNT; i++) {175            s_baseline[i] = HALL_LINE_BASE_DEFAULT;176        }177    }178}179180void hall_line_get(int16_t baseline[3], uint8_t *valid,181                   uint16_t *sigma_um, int16_t *zero_cmm)182{183    uint32_t i;184185    *valid = s_valid;186    *sigma_um = s_sigma_um;187    *zero_cmm = s_zero_cmm;188    for(i = 0U; i < HALL_LINE_COUNT; i++) {189        baseline[i] = (0U != s_valid) ? s_baseline[i] : 0;190    }191}192193void hall_line_sigma_set(uint16_t sigma_um)194{195    if((sigma_um < HALL_LINE_SIGMA_MIN) || (sigma_um > HALL_LINE_SIGMA_MAX)) {196        return;197    }198    s_sigma_um = sigma_um;199}200201void hall_line_cal_begin(void)202{203    uint32_t i;204205    for(i = 0U; i < HALL_LINE_COUNT; i++) {206        s_cal_sum[i] = 0;207    }208    s_cal_count = 0U;209    s_cal_run = 1U;210}211212uint8_t hall_line_cal_finish(void)213{214    uint32_t i;215216    if(0U == s_cal_run) {217        return HALL_CAL_IDLE;218    }219    if(s_cal_count < HALL_LINE_CAL_MIN) {220        return HALL_CAL_WEAK;221    }222    for(i = 0U; i < HALL_LINE_COUNT; i++) {223        s_baseline[i] = (int16_t)(s_cal_sum[i] / (int32_t)s_cal_count);224    }225    s_valid = 1U;226    s_cal_run = 0U;227    return HALL_CAL_OK;228}229230void hall_line_cal_clear(void)231{232    uint32_t i;233234    s_cal_run = 0U;235    s_cal_count = 0U;236    s_valid = 0U;237    for(i = 0U; i < HALL_LINE_COUNT; i++) {238        s_baseline[i] = HALL_LINE_BASE_DEFAULT;239        s_cal_sum[i] = 0;240    }241}242243void hall_line_zero_set(int16_t zero_cmm)244{245    s_zero_cmm = zero_cmm;246}247248int16_t hall_line_zero_get(void)249{250    return s_zero_cmm;251}252253void hall_line_zero_capture(void)254{255    hall_sample_t sample;256    int32_t raw_cmm;257258    hall_line_read(&sample);259    raw_cmm = sample.angle_cdeg + (int32_t)s_zero_cmm;260    if(raw_cmm > 32767) {261        raw_cmm = 32767;262    }263    if(raw_cmm < -32768) {264        raw_cmm = -32768;265    }266    s_zero_cmm = (int16_t)raw_cmm;267}268269void hall_line_read(hall_sample_t *sample)270{271    uint16_t raw[HALL_LINE_COUNT];272    uint32_t i;273274    hall_encoder_copy_raw(raw);275    if((0U != s_cal_run) && (s_cal_count < HALL_LINE_CAL_MAX)) {276        for(i = 0U; i < HALL_LINE_COUNT; i++) {277            s_cal_sum[i] += (int32_t)raw[i];278        }279        s_cal_count++;280    }281    hall_line_solve(raw, sample);282}283284uint8_t hall_line_flags(const hall_sample_t *sample)285{286    uint8_t flags = 0U;287288    if(sample->amplitude < HALL_AMP_WEAK) {289        flags |= HALL_FLAG_WEAK;290    }291    if(0U != s_valid) {292        flags |= HALL_FLAG_CAL_VALID;293    }294    if(0U != s_cal_run) {295        flags |= HALL_FLAG_CAL_RUN;296    }297    if(0U != s_out) {298        flags |= HALL_FLAG_OUT;299    }300    return flags;301}