1/*!2 \file hall_encoder.c3 \brief 读取 U1/U2/U4，并把 120° 三相信号合成磁铁角度45 GD32E230 只有一个 ADC。三路放入规则组扫描，DMA 通道 0 循环写入。6 传感器是 SC4703，零磁场输出约为 VDD/2，所以零点用三路平均值去掉，7 不依赖固定的 2048。8*/910#include "hall_encoder.h"1112#include "gd32e23x.h"13#include "systick.h"1415#define HALL_ADC_COUNT 3U16#define HALL_SQRT3_NUM 88717#define HALL_SQRT3_SHIFT 91819static volatile uint16_t s_adc_dma[HALL_ADC_COUNT];20static hall_cal_t s_cal = {21 {0, 0, 0},22 {HALL_CAL_GAIN_ONE, HALL_CAL_GAIN_ONE, HALL_CAL_GAIN_ONE},23 0U24};25static uint16_t s_zero_cdeg;26static int32_t s_angle_raw;27static uint8_t s_cal_run;28static uint16_t s_cal_min[HALL_ADC_COUNT];29static uint16_t s_cal_max[HALL_ADC_COUNT];3031/*!32 \brief 把未使用的引脚设成模拟输入，降低悬空输入的漏电33 \param[in] none34 \param[out] none35 \retval none36*/37static void hall_unused_pins_analog(void)38{39 rcu_periph_clock_enable(RCU_GPIOA);40 rcu_periph_clock_enable(RCU_GPIOB);41 rcu_periph_clock_enable(RCU_GPIOF);4243 gpio_mode_set(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE,44 GPIO_PIN_2 | GPIO_PIN_3 | GPIO_PIN_5 | GPIO_PIN_6 |45 GPIO_PIN_7 | GPIO_PIN_8 | GPIO_PIN_15);46 gpio_mode_set(GPIOB, GPIO_MODE_ANALOG, GPIO_PUPD_NONE,47 GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_3 | GPIO_PIN_6 | GPIO_PIN_7);48 gpio_mode_set(GPIOF, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO_PIN_0 | GPIO_PIN_1);49}5051/*!52 \brief PA0、PA1、PA4 扫描转换，结果由 DMA 循环刷新53 \param[in] none54 \param[out] none55 \retval none56*/57static void hall_adc_dma_init(void)58{59 dma_parameter_struct dma;6061 rcu_periph_clock_enable(RCU_GPIOA);62 rcu_periph_clock_enable(RCU_ADC);63 rcu_periph_clock_enable(RCU_DMA);64 /*!65 SystemInit 会关掉 IRC28M。ADC 时钟源选到它之前必须重新打开，66 否则校准停在 CLB 标志上，主循环里的 RTT 日志不会发出去。67 */68 rcu_osci_on(RCU_IRC28M);69 if(SUCCESS == rcu_osci_stab_wait(RCU_IRC28M)) {70 rcu_adc_clock_config(RCU_ADCCK_IRC28M_DIV2);71 } else {72 rcu_adc_clock_config(RCU_ADCCK_APB2_DIV4);73 }7475 gpio_mode_set(GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE,76 GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_4);7778 dma_deinit(DMA_CH0);79 dma_struct_para_init(&dma);80 dma.periph_addr = (uint32_t)(&ADC_RDATA);81 dma.periph_width = DMA_PERIPHERAL_WIDTH_16BIT;82 dma.memory_addr = (uint32_t)s_adc_dma;83 dma.memory_width = DMA_MEMORY_WIDTH_16BIT;84 dma.number = HALL_ADC_COUNT;85 dma.priority = DMA_PRIORITY_HIGH;86 dma.periph_inc = DMA_PERIPH_INCREASE_DISABLE;87 dma.memory_inc = DMA_MEMORY_INCREASE_ENABLE;88 dma.direction = DMA_PERIPHERAL_TO_MEMORY;89 dma_init(DMA_CH0, &dma);90 dma_circulation_enable(DMA_CH0);91 dma_channel_enable(DMA_CH0);9293 adc_deinit();94 adc_special_function_config(ADC_SCAN_MODE, ENABLE);95 adc_special_function_config(ADC_CONTINUOUS_MODE, ENABLE);96 adc_data_alignment_config(ADC_DATAALIGN_RIGHT);97 adc_channel_length_config(ADC_REGULAR_CHANNEL, HALL_ADC_COUNT);98 adc_regular_channel_config(0U, ADC_CHANNEL_0, ADC_SAMPLETIME_55POINT5);99 adc_regular_channel_config(1U, ADC_CHANNEL_1, ADC_SAMPLETIME_55POINT5);100 adc_regular_channel_config(2U, ADC_CHANNEL_4, ADC_SAMPLETIME_55POINT5);101 adc_external_trigger_source_config(ADC_REGULAR_CHANNEL, ADC_EXTTRIG_REGULAR_NONE);102 adc_external_trigger_config(ADC_REGULAR_CHANNEL, ENABLE);103 /*!104 每路连续转换 64 次再右移 6 位，结果仍是 12 位满量程。105 SC4703 输出噪声大约 9 mV，单次采样会晃十几个码；64 次平均后大约降到 1～2 个码。106 */107 adc_oversample_mode_config(ADC_OVERSAMPLING_ALL_CONVERT,108 ADC_OVERSAMPLING_SHIFT_6B,109 ADC_OVERSAMPLING_RATIO_MUL64);110 adc_oversample_mode_enable();111 adc_dma_mode_enable();112113 adc_enable();114 delay_1ms(1U);115 adc_calibration_enable();116 adc_software_trigger_enable(ADC_REGULAR_CHANNEL);117}118119/*!120 \brief 初始化霍尔采样121 \param[in] none122 \param[out] none123 \retval none124*/125void hall_encoder_init(void)126{127 hall_unused_pins_analog();128 hall_adc_dma_init();129}130131/*!132 \brief 整数平方根133 \param[in] value: 被开方数134 \retval floor(sqrt(value))135*/136static uint32_t hall_isqrt(uint32_t value)137{138 uint32_t op = value;139 uint32_t res = 0U;140 uint32_t one = 1UL << 30;141142 while(one > op) {143 one >>= 2;144 }145 while(0U != one) {146 if(op >= (res + one)) {147 op -= res + one;148 res = (res >> 1) + one;149 } else {150 res >>= 1;151 }152 one >>= 2;153 }154 return res;155}156157/*!158 \brief atan2 近似，返回 -18000..18000，单位 0.01°159 \param[in] y: 正弦分量160 \param[in] x: 余弦分量161 \retval 角度，0.01°162*/163static int32_t hall_atan2_cdeg(int32_t y, int32_t x)164{165 uint32_t ax;166 uint32_t ay;167 uint32_t a;168 uint32_t b;169 uint32_t z;170 uint32_t acute;171 int32_t angle;172173 if((0 == x) && (0 == y)) {174 return 0;175 }176177 ax = (x < 0) ? (uint32_t)(-x) : (uint32_t)x;178 ay = (y < 0) ? (uint32_t)(-y) : (uint32_t)y;179 if(ax >= ay) {180 a = ay;181 b = ax;182 } else {183 a = ax;184 b = ay;185 }186 if(0U == b) {187 return 0;188 }189190 z = (uint32_t)(((uint64_t)a << 16) / b);191 if(z > 65536U) {192 z = 65536U;193 }194 acute = (z * 4500U) >> 16;195 acute += (uint32_t)(((uint64_t)z * (65536U - z) * 1564U) >> 32);196 if(ay > ax) {197 acute = 9000U - acute;198 }199200 angle = (int32_t)acute;201 if(x < 0) {202 angle = 18000 - angle;203 }204 if(y < 0) {205 angle = -angle;206 }207 return angle;208}209210/*!211 \brief 由三路原码计算角度和幅度212 \param[in] raw: U1、U2、U4213 \param[out] sample: 原码、0..35999 的角度、单路峰值214 \retval none215*/216static void hall_solve(const uint16_t raw[3], hall_sample_t *sample)217{218 int32_t centered[3];219 int32_t mean;220 int32_t s0;221 int32_t s1;222 int32_t s2;223 int32_t alpha;224 int32_t beta;225 int32_t angle;226 uint64_t mag2;227 uint32_t i;228229 for(i = 0U; i < HALL_ADC_COUNT; i++) {230 sample->raw[i] = raw[i];231 if(0U != s_cal.valid) {232 centered[i] = (int32_t)raw[i] - (int32_t)s_cal.offset[i];233 centered[i] = (int32_t)(((int64_t)centered[i] * (int32_t)s_cal.gain_q12[i]) >> 12);234 } else {235 centered[i] = (int32_t)raw[i];236 }237 }238 mean = (centered[0] + centered[1] + centered[2]) / 3;239 centered[0] -= mean;240 centered[1] -= mean;241 centered[2] -= mean;242243 s0 = centered[HALL_PHASE_0_DEG];244 s1 = centered[HALL_PHASE_120_DEG];245 s2 = centered[HALL_PHASE_240_DEG];246247 /* alpha = 3*A*cos(θ)，beta = 3*A*sin(θ)。√3 ≈ 887/512。 */248 alpha = (2 * s0) - s1 - s2;249 beta = (int32_t)(((int64_t)(s1 - s2) * HALL_SQRT3_NUM) >> HALL_SQRT3_SHIFT);250 beta *= HALL_ANGLE_SIGN;251252 angle = hall_atan2_cdeg(beta, alpha);253 if(angle < 0) {254 angle += 36000;255 }256 s_angle_raw = angle;257 if(angle < (int32_t)s_zero_cdeg) {258 angle += 36000;259 }260 angle -= (int32_t)s_zero_cdeg;261 sample->angle_cdeg = angle;262263 mag2 = ((int64_t)alpha * alpha) + ((int64_t)beta * beta);264 if(mag2 > 0xFFFFFFFFULL) {265 mag2 = 0xFFFFFFFFULL;266 }267 /* 矢量长度是 3*A，除以 3 得到单路峰值。 */268 sample->amplitude = (int32_t)(hall_isqrt((uint32_t)mag2) / 3U);269}270271/*!272 \brief 读取 DMA 缓冲区并解算273 \param[out] sample: 本次结果274 \retval none275*/276/*!277 \brief 校准过程中记下三路原码的最小和最大278 \param[in] raw: U1、U2、U4279 \param[out] none280 \retval none281*/282static void hall_cal_note(const uint16_t raw[3])283{284 uint32_t i;285286 if(0U == s_cal_run) {287 return;288 }289 for(i = 0U; i < HALL_ADC_COUNT; i++) {290 if(raw[i] < s_cal_min[i]) {291 s_cal_min[i] = raw[i];292 }293 if(raw[i] > s_cal_max[i]) {294 s_cal_max[i] = raw[i];295 }296 }297}298299void hall_encoder_copy_raw(uint16_t raw[3])300{301 raw[0] = s_adc_dma[0];302 raw[1] = s_adc_dma[1];303 raw[2] = s_adc_dma[2];304}305306void hall_encoder_read(hall_sample_t *sample)307{308 uint16_t raw[HALL_ADC_COUNT];309310 hall_encoder_copy_raw(raw);311 hall_cal_note(raw);312 hall_solve(raw, sample);313}314315uint8_t hall_encoder_flags(const hall_sample_t *sample)316{317 uint8_t flags = 0U;318319 if(sample->amplitude < HALL_AMP_WEAK) {320 flags |= HALL_FLAG_WEAK;321 }322 if(0U != s_cal.valid) {323 flags |= HALL_FLAG_CAL_VALID;324 }325 if(0U != s_cal_run) {326 flags |= HALL_FLAG_CAL_RUN;327 }328 return flags;329}330331void hall_encoder_cal_apply(const hall_cal_t *cal)332{333 uint32_t i;334335 if(0U == cal->valid) {336 hall_encoder_cal_clear();337 return;338 }339 s_cal.valid = 1U;340 for(i = 0U; i < HALL_ADC_COUNT; i++) {341 s_cal.offset[i] = cal->offset[i];342 if(0U == cal->gain_q12[i]) {343 s_cal.gain_q12[i] = HALL_CAL_GAIN_ONE;344 } else {345 s_cal.gain_q12[i] = cal->gain_q12[i];346 }347 }348}349350void hall_encoder_cal_get(hall_cal_t *cal)351{352 *cal = s_cal;353}354355void hall_encoder_cal_begin(void)356{357 uint32_t i;358359 for(i = 0U; i < HALL_ADC_COUNT; i++) {360 s_cal_min[i] = 4095U;361 s_cal_max[i] = 0U;362 }363 s_cal_run = 1U;364}365366uint8_t hall_encoder_cal_finish(void)367{368 uint32_t i;369 uint16_t span[HALL_ADC_COUNT];370 int16_t offset[HALL_ADC_COUNT];371 uint16_t gain_q12[HALL_ADC_COUNT];372 uint32_t gain;373374 if(0U == s_cal_run) {375 return HALL_CAL_IDLE;376 }377 for(i = 0U; i < HALL_ADC_COUNT; i++) {378 if(s_cal_max[i] <= s_cal_min[i]) {379 return HALL_CAL_WEAK;380 }381 span[i] = (uint16_t)(s_cal_max[i] - s_cal_min[i]);382 if(span[i] < HALL_CAL_SPAN_MIN) {383 return HALL_CAL_WEAK;384 }385 offset[i] = (int16_t)(((uint32_t)s_cal_min[i] + (uint32_t)s_cal_max[i]) / 2U);386 gain = ((uint32_t)span[0] << 12) / (uint32_t)span[i];387 if((0U == gain) || (gain > 65535U)) {388 return HALL_CAL_WEAK;389 }390 gain_q12[i] = (uint16_t)gain;391 }392 for(i = 0U; i < HALL_ADC_COUNT; i++) {393 s_cal.offset[i] = offset[i];394 s_cal.gain_q12[i] = gain_q12[i];395 }396 s_cal.valid = 1U;397 s_cal_run = 0U;398 return HALL_CAL_OK;399}400401void hall_encoder_cal_clear(void)402{403 uint32_t i;404405 s_cal_run = 0U;406 s_cal.valid = 0U;407 for(i = 0U; i < HALL_ADC_COUNT; i++) {408 s_cal.offset[i] = 0;409 s_cal.gain_q12[i] = HALL_CAL_GAIN_ONE;410 s_cal_min[i] = 4095U;411 s_cal_max[i] = 0U;412 }413}414415void hall_encoder_zero_set(uint16_t zero_cdeg)416{417 if(zero_cdeg < 36000U) {418 s_zero_cdeg = zero_cdeg;419 }420}421422uint16_t hall_encoder_zero_get(void)423{424 return s_zero_cdeg;425}426427void hall_encoder_zero_capture(void)428{429 hall_sample_t sample;430431 hall_encoder_read(&sample);432 s_zero_cdeg = (uint16_t)s_angle_raw;433}
元驭机器人Hall 传感器教学下载原文件