|
|
@@ -1,488 +1,257 @@
|
|
|
-#include <string.h>
|
|
|
-#include "bsp/bsp_driver.h"
|
|
|
-#include "os/os_task.h"
|
|
|
-#include "libs/utils.h"
|
|
|
-#include "libs/logger.h"
|
|
|
-#include "math/fast_math.h"
|
|
|
-#include "foc/motor/hall.h"
|
|
|
-#include "app/nv_storage.h"
|
|
|
-#include "libs/time_measure.h"
|
|
|
-#include "app/nv_storage.h"
|
|
|
-#include "libs/logger.h"
|
|
|
-
|
|
|
-//#define USE_DETECTED_ANGLE 1
|
|
|
-
|
|
|
-#define HALL_READ_TIMES 9
|
|
|
-
|
|
|
-static u32 _hall_detect_task(void *args);
|
|
|
-static void _hall_init_el_angle(void);
|
|
|
-
|
|
|
-
|
|
|
-//#define HALL_PLACE_OFFSET (360-25)//(230) //(345) //315
|
|
|
-#define HALL_PLACE_OFFSET (230)
|
|
|
-
|
|
|
-/*
|
|
|
-4,5,1,3,2,6,4
|
|
|
-*/
|
|
|
-
|
|
|
-hall_t _sensor_hander;
|
|
|
-
|
|
|
-measure_time_t g_meas_hall = {.exec_max_time = 6,};
|
|
|
-
|
|
|
-//#define read_hall(h,t) {h = get_hall_stat(HALL_READ_TIMES); t = _hall_table[h];}
|
|
|
-#define us_2_s(tick) ((float)tick / 1000000.0f) //s32q14
|
|
|
-
|
|
|
-static void __inline _hall_put_sample(u32 ticks, float angle) {
|
|
|
- hall_sample_t *s = &_sensor_hander.samples;
|
|
|
- s->ticks_sum -= s->ticks[s->index];
|
|
|
- s->angles_sum -= s->angles[s->index];
|
|
|
- s->ticks[s->index] = ticks;
|
|
|
- s->angles[s->index] = angle;
|
|
|
- s->ticks_sum += s->ticks[s->index];
|
|
|
- s->angles_sum += s->angles[s->index];
|
|
|
- s->index += 1;
|
|
|
- if (s->index >= SAMPLE_MAX_COUNT) {
|
|
|
- s->full = true;
|
|
|
- s->index = 0;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-static u32 __inline__ _hall_get_angle_ticks(void) {
|
|
|
- hall_sample_t *s = &_sensor_hander.samples;
|
|
|
- if (!s->full) {
|
|
|
- return s->ticks[s->index-1];
|
|
|
- }else {
|
|
|
- return s->ticks_sum/SAMPLE_MAX_COUNT;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-static float __inline _hall_angle_speed(void){
|
|
|
- hall_sample_t *s = &_sensor_hander.samples;
|
|
|
- if (s->ticks_sum == 0) {
|
|
|
- return 0.0f;
|
|
|
- }
|
|
|
-
|
|
|
- if (!s->full) {
|
|
|
- return s->angles[s->index - 1] / us_2_s(s->ticks[s->index-1]);
|
|
|
- }else {
|
|
|
- return s->angles_sum / us_2_s(s->ticks_sum);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-void hall_debug_log(void) {
|
|
|
- sys_debug("angle dir %d\n", _sensor_hander.direction);
|
|
|
-}
|
|
|
-
|
|
|
-/*
|
|
|
-static bool __inline _hall_data_empty(void) {
|
|
|
- hall_sample_t *s = &_sensor_hander.samples;
|
|
|
- if ((!s->full) && (s->index == 0)){
|
|
|
- return true;
|
|
|
- }
|
|
|
- return false;
|
|
|
-}
|
|
|
-*/
|
|
|
-
|
|
|
-static void hall_sensor_default(s8 direction) {
|
|
|
- memset(&_sensor_hander, 0, sizeof(_sensor_hander));
|
|
|
- _sensor_hander.phase_offset = HALL_PLACE_OFFSET;//mc_config_get()->hall_offset;
|
|
|
- for (int i = 0; i < 8; i++) {
|
|
|
- _sensor_hander.angle_table[i] = (nv_get_motor_params()->hall_table[i]);
|
|
|
- }
|
|
|
- _sensor_hander.manual_angle = 0x3FF;
|
|
|
- _sensor_hander.direction = direction;
|
|
|
- _sensor_hander.running_dir = _sensor_hander.direction;
|
|
|
- _hall_init_el_angle();
|
|
|
-}
|
|
|
-
|
|
|
-void hall_sensor_init(void) {
|
|
|
- hall_sensor_default(POSITIVE);
|
|
|
- mc_hall_init();
|
|
|
- shark_task_create(_hall_detect_task, NULL);
|
|
|
-}
|
|
|
-
|
|
|
-void hall_set_direction(s8 direction) {
|
|
|
- hall_sensor_default(direction);
|
|
|
-}
|
|
|
-
|
|
|
-void hall_sensor_clear(void) {
|
|
|
- hall_set_direction(POSITIVE);
|
|
|
-}
|
|
|
-
|
|
|
-static u32 _hall_detect_task(void *args) {
|
|
|
- if (_sensor_hander.el_speed != 0) {
|
|
|
- u32 ticks_now = task_ticks_abs();
|
|
|
- if (ticks_now > _sensor_hander.hall_ticks) {
|
|
|
- if (task_delta_ticks(ticks_now) > 2000*1000) {
|
|
|
- hall_sensor_clear();
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- return 0;
|
|
|
-}
|
|
|
-
|
|
|
-#if 1
|
|
|
-int hall_e_count = 0;
|
|
|
-float hall_sensor_get_theta(bool detect_err){
|
|
|
- hall_e_count++;
|
|
|
- float angle_add = _sensor_hander.delta_angle_ts;
|
|
|
- if (_sensor_hander.comp_count > 0) {
|
|
|
- _sensor_hander.comp_count--;
|
|
|
- angle_add += _sensor_hander.angle_comp_ts;
|
|
|
- }
|
|
|
- _sensor_hander.estimate_delta_angle += angle_add;
|
|
|
-
|
|
|
- float el_angle = _sensor_hander.estimate_delta_angle;
|
|
|
- if (el_angle > _sensor_hander.next_delta_angle) {
|
|
|
- el_angle = _sensor_hander.next_delta_angle;
|
|
|
- }
|
|
|
- if (_sensor_hander.direction == POSITIVE) {
|
|
|
- el_angle = _sensor_hander.estimate_el_angle + el_angle;
|
|
|
- }else {
|
|
|
- el_angle = _sensor_hander.estimate_el_angle - el_angle;
|
|
|
- }
|
|
|
- norm_angle_deg(el_angle);
|
|
|
- if (hall_e_count%5 == 0) {
|
|
|
- plot_2data16((s16)el_angle, _sensor_hander.hall_stat*10);
|
|
|
- }
|
|
|
- return (el_angle);
|
|
|
-}
|
|
|
-#else
|
|
|
-static int test_loop = 0;
|
|
|
-float hall_sensor_get_theta(void){
|
|
|
- float angle_add = _sensor_hander.delta_angle_ts;
|
|
|
- float comp_angle = 0;
|
|
|
- if (_sensor_hander.comp_count > 0) {
|
|
|
- _sensor_hander.comp_count--;
|
|
|
- comp_angle = _sensor_hander.angle_comp_ts;
|
|
|
- }
|
|
|
-
|
|
|
- if (_sensor_hander.direction == POSITIVE) {
|
|
|
- _sensor_hander.estimate_delta_angle += angle_add;
|
|
|
- _sensor_hander.estimate_el_angle += (angle_add + comp_angle);
|
|
|
- }else {
|
|
|
- _sensor_hander.estimate_delta_angle -= angle_add;
|
|
|
- _sensor_hander.estimate_el_angle -= (angle_add + comp_angle);
|
|
|
- }
|
|
|
- norm_angle_deg(_sensor_hander.estimate_el_angle);
|
|
|
- test_loop ++;
|
|
|
- if (test_loop % 20 == 0) {
|
|
|
- //plot_3data16(_sensor_hander.estimate_el_angle, _sensor_hander.angle_comp_ts * 1000, _sensor_hander.comp_count);
|
|
|
- }
|
|
|
- return ( _sensor_hander.estimate_el_angle);
|
|
|
-}
|
|
|
-
|
|
|
-#endif
|
|
|
-
|
|
|
-float hall_sensor_get_speed(void) {
|
|
|
- return (_sensor_hander.rpm) * _sensor_hander.running_dir;
|
|
|
-}
|
|
|
-
|
|
|
-int hall_offset_increase(int inc) {
|
|
|
- inc = inc << 19;
|
|
|
- if (_sensor_hander.phase_offset + inc >= PHASE_360_DEGREE) {
|
|
|
- _sensor_hander.phase_offset = _sensor_hander.phase_offset + inc - PHASE_360_DEGREE;
|
|
|
- }else {
|
|
|
- _sensor_hander.phase_offset += inc;
|
|
|
- }
|
|
|
- return _sensor_hander.phase_offset;
|
|
|
-}
|
|
|
-
|
|
|
-s32 *hall_get_table(void) {
|
|
|
- return _sensor_hander.angle_table;
|
|
|
-}
|
|
|
-
|
|
|
-static float sin_hall[8];
|
|
|
-static float cos_hall[8];
|
|
|
-static int hall_iterations[8];
|
|
|
-void hall_detect_angle(s16 angle) {
|
|
|
- _sensor_hander.manual_angle = (angle);
|
|
|
- int hall = get_hall_stat(HALL_READ_TIMES);
|
|
|
- float s, c;
|
|
|
- normal_sincosf(degree_2_pi(angle), &s, &c);
|
|
|
- sin_hall[hall] += s;
|
|
|
- cos_hall[hall] += c;
|
|
|
- hall_iterations[hall]++;
|
|
|
-}
|
|
|
-
|
|
|
-bool hall_detect_angle_finish(void) {
|
|
|
- int fails = 0;
|
|
|
- for(int i = 0;i < 8;i++) {
|
|
|
- if (hall_iterations[i] > 30) {
|
|
|
- float ang = pi_2_degree(atan2f(sin_hall[i], cos_hall[i]));
|
|
|
- fast_norm_angle(&ang);
|
|
|
- _sensor_hander.angle_table[i] = (ang);
|
|
|
- } else {
|
|
|
- _sensor_hander.angle_table[i] = (0x3FF);
|
|
|
- fails++;
|
|
|
- }
|
|
|
- }
|
|
|
- if (fails == 2) {
|
|
|
- nv_save_hall_table(_sensor_hander.angle_table);
|
|
|
- }
|
|
|
- memset(sin_hall, 0, sizeof(sin_hall));
|
|
|
- memset(cos_hall, 0, sizeof(cos_hall));
|
|
|
- memset(hall_iterations, 0, sizeof(hall_iterations));
|
|
|
- return fails == 2;
|
|
|
-}
|
|
|
-
|
|
|
-void hall_detect_offset(s16 angle) {
|
|
|
- _sensor_hander.manual_angle = (angle);
|
|
|
-}
|
|
|
-
|
|
|
-bool hall_detect_offset_finish(void) {
|
|
|
- int fails = 0;
|
|
|
- for(int i = 0;i < 8;i++) {
|
|
|
- if (hall_iterations[i] > 20) {
|
|
|
- _sensor_hander.angle_table[i] = _sensor_hander.angle_table[i] / hall_iterations[i];
|
|
|
- }
|
|
|
- }
|
|
|
- return (fails == 2);
|
|
|
-}
|
|
|
-static void _hall_init_el_angle(void) {
|
|
|
- _sensor_hander.hall_stat = get_hall_stat(HALL_READ_TIMES);
|
|
|
-#ifdef USE_DETECTED_ANGLE
|
|
|
- if (_sensor_hander.hall_stat == 0 || _sensor_hander.hall_stat == 7) {
|
|
|
- _sensor_hander.sensor_error ++;
|
|
|
- return;
|
|
|
- }
|
|
|
- _sensor_hander.measured_el_angle = _sensor_hander.phase_offset + _sensor_hander.angle_table[_sensor_hander.hall_stat];
|
|
|
-#else
|
|
|
- s32 sector_center = PHASE_60_DEGREE/2;
|
|
|
- switch ( _sensor_hander.hall_stat )
|
|
|
- {
|
|
|
- case STATE_5:
|
|
|
- _sensor_hander.measured_el_angle = _sensor_hander.phase_offset + sector_center;
|
|
|
- break;
|
|
|
- case STATE_1:
|
|
|
- _sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_60_DEGREE + sector_center;
|
|
|
- break;
|
|
|
- case STATE_3:
|
|
|
- _sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_120_DEGREE + sector_center;
|
|
|
- break;
|
|
|
- case STATE_2:
|
|
|
- _sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_180_DEGREE + sector_center;
|
|
|
- break;
|
|
|
- case STATE_6:
|
|
|
- _sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_240_DEGREE + sector_center;
|
|
|
- break;
|
|
|
- case STATE_4:
|
|
|
- _sensor_hander.measured_el_angle = _sensor_hander.phase_offset + PHASE_300_DEGREE + sector_center;
|
|
|
- break;
|
|
|
- default:
|
|
|
- /* Bad hall sensor configutarion so update the speed reliability */
|
|
|
- _sensor_hander.sensor_error ++;
|
|
|
- return;
|
|
|
- }
|
|
|
-#endif
|
|
|
- _sensor_hander.sensor_error = 0;
|
|
|
- /* Initialize the measured angle */
|
|
|
- _sensor_hander.measured_el_angle += PHASE_60_DEGREE;
|
|
|
- norm_angle_deg(_sensor_hander.measured_el_angle);
|
|
|
- _sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
|
|
|
- _sensor_hander.hall_ticks = task_ticks_abs();
|
|
|
-}
|
|
|
-
|
|
|
-static __inline__ float _get_angle(u8 state, float added) {
|
|
|
-#ifdef USE_DETECTED_ANGLE
|
|
|
- return _sensor_hander.phase_offset + _sensor_hander.angle_table[state];
|
|
|
-#else
|
|
|
- return _sensor_hander.phase_offset + added;
|
|
|
-#endif
|
|
|
-}
|
|
|
-/* 4,5,1,3,2,6,4 */
|
|
|
-static float _hall_position(u8 state_now, u8 state_prev) {
|
|
|
- float theta_now = 0xFFFFFFFF;
|
|
|
- switch (state_now) {
|
|
|
- case STATE_1:
|
|
|
- if (state_prev == STATE_5) {
|
|
|
- _sensor_hander.running_dir = POSITIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
|
|
|
- }else if (state_prev == STATE_3) {
|
|
|
- _sensor_hander.running_dir = NEGATIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
|
|
|
- }
|
|
|
- break;
|
|
|
- case STATE_2:
|
|
|
- if (state_prev == STATE_3) {
|
|
|
- _sensor_hander.running_dir = POSITIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
|
|
|
- }else if (state_prev == STATE_6) {
|
|
|
- _sensor_hander.running_dir = NEGATIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
|
|
|
- }
|
|
|
- break;
|
|
|
- case STATE_3:
|
|
|
- if (state_prev == STATE_1) {
|
|
|
- _sensor_hander.running_dir = POSITIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_120_DEGREE);//_sensor_hander.phase_offset + PHASE_120_DEGREE;
|
|
|
- }else if (state_prev == STATE_2) {
|
|
|
- _sensor_hander.running_dir = NEGATIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_180_DEGREE);//_sensor_hander.phase_offset + PHASE_180_DEGREE;
|
|
|
- }
|
|
|
- break;
|
|
|
- case STATE_4:
|
|
|
- if (state_prev == STATE_6) {
|
|
|
- _sensor_hander.running_dir = POSITIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
|
|
|
- }else if (state_prev == STATE_5) {
|
|
|
- _sensor_hander.running_dir = NEGATIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
|
|
|
- }
|
|
|
- break;
|
|
|
- case STATE_5:
|
|
|
- if (state_prev == STATE_4) {
|
|
|
- _sensor_hander.running_dir = POSITIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_0_DEGREE);//_sensor_hander.phase_offset + PHASE_0_DEGREE;
|
|
|
- }else if (state_prev == STATE_1) {
|
|
|
- _sensor_hander.running_dir = NEGATIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_60_DEGREE);//_sensor_hander.phase_offset + PHASE_60_DEGREE;
|
|
|
- }
|
|
|
- break;
|
|
|
- case STATE_6:
|
|
|
- if (state_prev == STATE_2) {
|
|
|
- _sensor_hander.running_dir = POSITIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_240_DEGREE);//_sensor_hander.phase_offset + PHASE_240_DEGREE;
|
|
|
- }else if (state_prev == STATE_4) {
|
|
|
- _sensor_hander.running_dir = NEGATIVE;
|
|
|
- theta_now = _get_angle(state_now, PHASE_300_DEGREE);//_sensor_hander.phase_offset + PHASE_300_DEGREE;
|
|
|
- }
|
|
|
- break;
|
|
|
- default:
|
|
|
- _sensor_hander.sensor_error ++;
|
|
|
- return 0xFFFFFFFF;
|
|
|
- }
|
|
|
- if (theta_now != 0xFFFFFFFF) {
|
|
|
- norm_angle_deg(theta_now);
|
|
|
- }
|
|
|
- return theta_now;
|
|
|
-}
|
|
|
-
|
|
|
-#ifdef USE_DETECTED_ANGLE
|
|
|
-static __inline u8 _next_hall(u8 hall_now) {
|
|
|
- switch (hall_now) {
|
|
|
- case STATE_1:
|
|
|
- if (_sensor_hander.direction == POSITIVE) {
|
|
|
- return STATE_3;
|
|
|
- }else {
|
|
|
- return STATE_5;
|
|
|
- }
|
|
|
- case STATE_2:
|
|
|
- if (_sensor_hander.direction == POSITIVE) {
|
|
|
- return STATE_6;
|
|
|
- }else {
|
|
|
- return STATE_3;
|
|
|
- }
|
|
|
- case STATE_3:
|
|
|
- if (_sensor_hander.direction == POSITIVE) {
|
|
|
- return STATE_2;
|
|
|
- }else {
|
|
|
- return STATE_1;
|
|
|
- }
|
|
|
- case STATE_4:
|
|
|
- if (_sensor_hander.direction == POSITIVE) {
|
|
|
- return STATE_5;
|
|
|
- }else {
|
|
|
- return STATE_6;
|
|
|
- }
|
|
|
- case STATE_5:
|
|
|
- if (_sensor_hander.direction == POSITIVE) {
|
|
|
- return STATE_1;
|
|
|
- }else {
|
|
|
- return STATE_4;
|
|
|
- }
|
|
|
- case STATE_6:
|
|
|
- if (_sensor_hander.direction == POSITIVE) {
|
|
|
- return STATE_4;
|
|
|
- }else {
|
|
|
- return STATE_2;
|
|
|
- }
|
|
|
- default: //not reached here
|
|
|
- return STATE_1;
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-static __inline__ s32 _get_delta_angle(u8 now, u8 next) {
|
|
|
- s32 delta_angle = _sensor_hander.angle_table[next] - _sensor_hander.angle_table[now];
|
|
|
- if (_sensor_hander.direction == POSITIVE) {
|
|
|
- if (delta_angle < 0) { //process cross 360 degree
|
|
|
- delta_angle += PHASE_360_DEGREE;
|
|
|
- }
|
|
|
- }else if (_sensor_hander.direction == NEGATIVE) {
|
|
|
- if (delta_angle > 0) { //process cross 360 degree
|
|
|
- delta_angle -= PHASE_360_DEGREE;
|
|
|
- }
|
|
|
- delta_angle = -delta_angle;
|
|
|
- }
|
|
|
- return delta_angle;
|
|
|
-}
|
|
|
-#endif
|
|
|
-
|
|
|
-void HALL_IRQHandler(void) {
|
|
|
- time_measure_start(&g_meas_hall);
|
|
|
- u8 hall_stat_now = get_hall_stat(HALL_READ_TIMES);
|
|
|
- u8 hall_stat_prev = _sensor_hander.hall_stat;
|
|
|
- u32 hall_ticks_now = task_ticks_abs();
|
|
|
-
|
|
|
- /*获取当前转子角度*/
|
|
|
- float theta_now = _hall_position(hall_stat_now, hall_stat_prev);
|
|
|
- if (theta_now == 0xFFFFFFFF) {
|
|
|
- return;
|
|
|
- }
|
|
|
- //plot_2data16(hall_stat_now*60, _sensor_hander.manual_angle);
|
|
|
- //获取两次中断的时间间隔，估计速度
|
|
|
- u32 delta_us = task_delta_ticks(hall_ticks_now);
|
|
|
- if (delta_us == 0) {
|
|
|
- return;
|
|
|
- }
|
|
|
- _sensor_hander.hall_ticks = hall_ticks_now;
|
|
|
- //获取两次中断之间转子转过的角度,获取预期的下次hall状态变换转过的角度
|
|
|
-#ifdef USE_DETECTED_ANGLE
|
|
|
- float delta_angle = _get_delta_angle(hall_stat_prev, hall_stat_now);
|
|
|
- float next_delta_angle = _get_delta_angle(hall_stat_now, _next_hall(hall_stat_now));
|
|
|
-#else
|
|
|
- float delta_angle = PHASE_60_DEGREE;
|
|
|
- float next_delta_angle = delta_angle;
|
|
|
-#endif
|
|
|
- float delta_time = us_2_s(delta_us);
|
|
|
- float prev_imme_el_speed = _sensor_hander.immediately_el_speed + 1;
|
|
|
- _sensor_hander.immediately_el_speed = delta_angle/delta_time; //s32q5
|
|
|
- float delta_el_speed = abs(_sensor_hander.immediately_el_speed - prev_imme_el_speed);
|
|
|
- if (delta_el_speed*100/prev_imme_el_speed >= 40) { //即时速度增加40%,认为不稳定，需要使用即时速度估计转子位置
|
|
|
- _sensor_hander.trns_detect = true;
|
|
|
- }else {
|
|
|
- _sensor_hander.trns_detect = false;
|
|
|
- }
|
|
|
- _hall_put_sample(delta_us, delta_angle);
|
|
|
- u32 mask = cpu_enter_critical();
|
|
|
- if (_sensor_hander.samples.full) {
|
|
|
- //float estimate_delta_angle = _sensor_hander.next_delta_angle - _sensor_hander.estimate_delta_angle;
|
|
|
- //plot_2data16(estimate_delta_angle>>19, (estimate_delta_angle/((s32)(delta_us/FOC_CTRL_US)))>>10);//, _sensor_hander.estimate_delta_angle>>19);
|
|
|
- /*通过上次预估的转子位置，对当前的预估速度进行补偿*/
|
|
|
- delta_us = _hall_get_angle_ticks();
|
|
|
- //_sensor_hander.comp_count = 50;
|
|
|
- //_sensor_hander.angle_comp_ts = estimate_delta_angle/(float)_sensor_hander.comp_count;
|
|
|
- _sensor_hander.estimate_el_angle = theta_now;
|
|
|
- }else {
|
|
|
- _sensor_hander.comp_count = 0;
|
|
|
- _sensor_hander.angle_comp_ts = 0;
|
|
|
- _sensor_hander.estimate_el_angle = theta_now;
|
|
|
- }
|
|
|
- _sensor_hander.estimate_delta_angle = 0;
|
|
|
- _sensor_hander.delta_angle_ts = (next_delta_angle/(us_2_s(delta_us)/FOC_CTRL_US));
|
|
|
- _sensor_hander.next_delta_angle = next_delta_angle;
|
|
|
- _sensor_hander.measured_el_angle = theta_now;
|
|
|
-
|
|
|
- cpu_exit_critical(mask);
|
|
|
- _sensor_hander.hall_stat = hall_stat_now;
|
|
|
- _sensor_hander.hall_ticks = hall_ticks_now;
|
|
|
- _sensor_hander.el_speed = _hall_angle_speed(); //s32q5
|
|
|
- _sensor_hander.rpm = (_sensor_hander.el_speed / 360 * 60) * nv_get_motor_params()->poles; //s32q5
|
|
|
- //plot_3data16(_sensor_hander.rpm >> 5, _sensor_hander.el_speed>>5, delta_us);
|
|
|
- time_measure_end(&g_meas_hall);
|
|
|
-
|
|
|
- //plot_3data16(delta_us/10, hall_stat_prev * 10, _sensor_hander.hall_stat * 10);
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
+#include <string.h>
|
|
|
+#include "bsp/bsp_driver.h"
|
|
|
+#include "os/os_task.h"
|
|
|
+#include "libs/utils.h"
|
|
|
+#include "libs/logger.h"
|
|
|
+#include "math/fast_math.h"
|
|
|
+#include "foc/motor/hall.h"
|
|
|
+#include "foc/mc_config.h"
|
|
|
+#include "libs/time_measure.h"
|
|
|
+#include "app/nv_storage.h"
|
|
|
+#include "libs/logger.h"
|
|
|
+
|
|
|
+//#define USE_DETECTED_ANGLE 1
|
|
|
+
|
|
|
+#define HALL_READ_TIMES 9
|
|
|
+#define SMOOTH_COUNT 10.0F
|
|
|
+
|
|
|
+/*
|
|
|
+4,5,1,3,2,6,4
|
|
|
+*/
|
|
|
+static s8 hall_2_pos[] = {7,5,1,0,3,4,2,7};
|
|
|
+static hall_t g_hall;
|
|
|
+
|
|
|
+measure_time_t g_meas_hall = {.exec_max_time = 6,};
|
|
|
+
|
|
|
+//#define read_hall(h,t) {h = get_hall_stat(HALL_READ_TIMES); t = _hall_table[h];}
|
|
|
+#define us_2_s(tick) ((float)tick / 1000000.0f) //s32q14
|
|
|
+#define HALL_TIMEOUT_US (1*1000000L)
|
|
|
+
|
|
|
+
|
|
|
+static u8 __INLINE hall_read_state(void) {
|
|
|
+ u8 hall_a = 0, hall_b = 0, hall_c = 0;
|
|
|
+ for (int i = 0; i < HALL_READ_TIMES; i++) {
|
|
|
+ if (gpio_hall_a_value()) {
|
|
|
+ hall_a++;
|
|
|
+ }
|
|
|
+ if (gpio_hall_b_value()) {
|
|
|
+ hall_b++;
|
|
|
+ }
|
|
|
+ if (gpio_hall_c_value()) {
|
|
|
+ hall_c++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ u8 state = 0;
|
|
|
+ if (hall_a > (HALL_READ_TIMES/2 + 1)) {
|
|
|
+ state = 1<<2;
|
|
|
+ }
|
|
|
+ if (hall_b > (HALL_READ_TIMES/2 + 1)) {
|
|
|
+ state = state | (1<<1);
|
|
|
+ }
|
|
|
+ if (hall_c > (HALL_READ_TIMES/2 + 1)) {
|
|
|
+ state = state | 1;
|
|
|
+ }
|
|
|
+ return state;
|
|
|
+}
|
|
|
+
|
|
|
+static void hall_init_low_pos(void) {
|
|
|
+ u8 state = hall_read_state();
|
|
|
+ s16 pos = hall_2_pos[state];
|
|
|
+ if (pos == 7) {
|
|
|
+ g_hall.errors ++;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ g_hall.state = state;
|
|
|
+ g_hall.prev_dir = g_hall.dir = POSITIVE;
|
|
|
+ g_hall.low_res_pos = pos;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+static void __INLINE hall_put_sample(u32 ticks, float angle) {
|
|
|
+ hsample_t *s = &g_hall.samples;
|
|
|
+ s->ticks_sum -= s->ticks[s->index];
|
|
|
+ s->angles_sum -= s->angles[s->index];
|
|
|
+ s->ticks[s->index] = ticks;
|
|
|
+ s->angles[s->index] = angle;
|
|
|
+ s->ticks_sum += s->ticks[s->index];
|
|
|
+ s->angles_sum += s->angles[s->index];
|
|
|
+ s->index += 1;
|
|
|
+ if (s->index >= SAMPLE_MAX_COUNT) {
|
|
|
+ s->full = true;
|
|
|
+ s->index = 0;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+static float __INLINE hall_elec_angle_vel(void){
|
|
|
+ hsample_t *s = &g_hall.samples;
|
|
|
+ if (s->ticks_sum == 0) {
|
|
|
+ return 0.0f;
|
|
|
+ }
|
|
|
+ return s->angles_sum / us_2_s(s->ticks_sum);
|
|
|
+}
|
|
|
+
|
|
|
+void hall_debug_log(void) {
|
|
|
+ sys_debug("angle dir %d, stat %d, lowres %d, err %d\n", g_hall.dir, g_hall.state, g_hall.low_res_pos, g_hall.errors);
|
|
|
+}
|
|
|
+
|
|
|
+static u32 hall_timeout_task(void *args) {
|
|
|
+ hall_t *phall = (hall_t *)args;
|
|
|
+ if (phall->velocity_raw != 0) {
|
|
|
+ if (time_delta_us(phall->edge_ticks, NULL) >= HALL_TIMEOUT_US) {
|
|
|
+ phall->velocity_raw = phall->velocity_filted = 0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+void hall_init(void) {
|
|
|
+ g_hall.phase_offset = mc_conf()->m.encoder_offset;
|
|
|
+ g_hall.mot_poles = mc_conf()->m.poles;
|
|
|
+ g_hall.b_trns_det = false;
|
|
|
+ g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
|
|
|
+ g_hall.samples.ticks_sum = 0;
|
|
|
+ g_hall.position = 0;
|
|
|
+ for (int i = 0; i < SAMPLE_MAX_COUNT; i++) {
|
|
|
+ g_hall.samples.ticks[i] = 120*1000000*1;
|
|
|
+ g_hall.samples.angles[i] = 0;
|
|
|
+ g_hall.samples.ticks_sum += g_hall.samples.ticks[i];
|
|
|
+ }
|
|
|
+ if (!g_hall.inited) {
|
|
|
+ g_hall.inited = true;
|
|
|
+ gpio_hall_init();
|
|
|
+ shark_task_create(hall_timeout_task, &g_hall);
|
|
|
+ }
|
|
|
+ hall_init_low_pos();
|
|
|
+}
|
|
|
+
|
|
|
+static float get_angle_diff(float a1, float a2) {
|
|
|
+ float diff = a1 - a2;
|
|
|
+ float abs_diff = ABS(diff);
|
|
|
+ if (abs_diff >= PHASE_180_DEGREE) {
|
|
|
+ return (PHASE_360_DEGREE - abs_diff);
|
|
|
+ }else {
|
|
|
+ return diff;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+static bool hall_update_low_pos(void) {
|
|
|
+ u8 state = hall_read_state();
|
|
|
+ s16 pos = hall_2_pos[state];
|
|
|
+ if (pos == 7) {
|
|
|
+ g_hall.errors ++;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ g_hall.state = state;
|
|
|
+ s16 delta_pos = pos - g_hall.low_res_pos;
|
|
|
+ g_hall.low_res_pos = pos;
|
|
|
+ s8 prev_dir = g_hall.dir;
|
|
|
+ if (delta_pos == 1 || delta_pos == -5) {
|
|
|
+ g_hall.dir = POSITIVE;
|
|
|
+ }else{
|
|
|
+ g_hall.dir = NEGATIVE;
|
|
|
+ }
|
|
|
+ if (delta_pos != 0) {
|
|
|
+ g_hall.edge_ticks = task_ticks_abs();
|
|
|
+ g_hall.prev_dir = prev_dir;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+hall_t *hall_get(void) {
|
|
|
+ return &g_hall;
|
|
|
+}
|
|
|
+
|
|
|
+float hall_get_elec_angle(void) {
|
|
|
+ float angle = g_hall.elec_angle + g_hall.phase_offset;
|
|
|
+ norm_angle_deg(angle);
|
|
|
+ return angle;
|
|
|
+}
|
|
|
+
|
|
|
+float hall_update_elec_angle(void) {
|
|
|
+ float delta_ticks = (float)time_delta_us(g_hall.edge_ticks, NULL);//上次hall变换到目前的时间
|
|
|
+ float low_res = g_hall.low_res_pos;
|
|
|
+ if (g_hall.dir == NEGATIVE) {
|
|
|
+ low_res += 1.0f;
|
|
|
+ }
|
|
|
+ float delta_pos = g_hall.elec_angle_vel / PHASE_60_DEGREE * us_2_s(delta_ticks) * g_hall.dir;//上次hall变换到目前走过的角度(对60度的比值，小于1)，通过速度插值
|
|
|
+ float high_res_pos = delta_pos + low_res;
|
|
|
+ if (high_res_pos < 0) {
|
|
|
+ high_res_pos = 0;
|
|
|
+ }
|
|
|
+ float elec_angle = high_res_pos * PHASE_60_DEGREE;
|
|
|
+ float delta_angle = delta_pos * PHASE_60_DEGREE;
|
|
|
+ float elec_smooth_angle;
|
|
|
+ if (g_hall.angle_smooth_cnt < (SMOOTH_COUNT + 1)) {
|
|
|
+ elec_smooth_angle = g_hall.delta_angle_edge + g_hall.angle_smooth_step * g_hall.angle_smooth_cnt + delta_angle;
|
|
|
+ g_hall.angle_smooth_cnt++;
|
|
|
+ if (g_hall.angle_smooth_step >= 0) {
|
|
|
+ elec_smooth_angle = min(elec_smooth_angle, elec_angle);
|
|
|
+ }else {
|
|
|
+ elec_smooth_angle = MAX(elec_smooth_angle, elec_angle);
|
|
|
+ }
|
|
|
+ }else {
|
|
|
+ elec_smooth_angle = elec_angle;
|
|
|
+ }
|
|
|
+ norm_angle_deg(elec_smooth_angle);
|
|
|
+ g_hall.elec_angle = elec_smooth_angle;
|
|
|
+ g_hall.position += g_hall.elec_angle_vel * FOC_CTRL_US / g_hall.mot_poles;
|
|
|
+ return hall_get_elec_angle();
|
|
|
+}
|
|
|
+
|
|
|
+float hall_get_velocity(void) {
|
|
|
+ return g_hall.velocity_filted;
|
|
|
+}
|
|
|
+
|
|
|
+float hall_get_position(void) {
|
|
|
+ return g_hall.position;
|
|
|
+}
|
|
|
+
|
|
|
+static void hall_calc_mot_velocity(u32 prev_ticks) {
|
|
|
+ u32 delta_cnt = time_delta_us(prev_ticks, NULL);
|
|
|
+ hall_put_sample(PHASE_60_DEGREE, delta_cnt);
|
|
|
+ float elec_vel;
|
|
|
+ if (g_hall.b_trns_det) {
|
|
|
+ elec_vel = PHASE_60_DEGREE/(us_2_s(delta_cnt));
|
|
|
+ LowPass_Filter(g_hall.elec_angle_vel, elec_vel, 0.8f);
|
|
|
+ }else {
|
|
|
+ g_hall.elec_angle_vel = hall_elec_angle_vel();
|
|
|
+ }
|
|
|
+ float velocity_raw = g_hall.elec_angle_vel/PHASE_360_DEGREE/g_hall.mot_poles * 60.0f * g_hall.dir;
|
|
|
+ float del_abs = ABS(velocity_raw - g_hall.velocity_raw);
|
|
|
+ if (del_abs > 140) {
|
|
|
+ g_hall.b_trns_det = true;
|
|
|
+ }else if (del_abs < 100) {
|
|
|
+ g_hall.b_trns_det = false;
|
|
|
+ }
|
|
|
+ g_hall.velocity_raw = velocity_raw;
|
|
|
+ LowPass_Filter(g_hall.velocity_filted, velocity_raw, 0.5f);
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+float hall_offset_detect(float *off) {
|
|
|
+ return 0.0f;
|
|
|
+}
|
|
|
+
|
|
|
+void HALL_IRQHandler(void) {
|
|
|
+ u32 prev_ticks = g_hall.edge_ticks;
|
|
|
+ if (!hall_update_low_pos()) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ g_hall.elec_angle_edge = g_hall.elec_angle;
|
|
|
+ float low_res = g_hall.low_res_pos;
|
|
|
+ if (g_hall.dir == NEGATIVE) {
|
|
|
+ low_res += 1.0f;
|
|
|
+ }
|
|
|
+ g_hall.delta_angle_edge = get_angle_diff(low_res * PHASE_60_DEGREE, g_hall.elec_angle_edge);
|
|
|
+ if (ABS(g_hall.delta_angle_edge) >= 2.0f) {
|
|
|
+ g_hall.angle_smooth_step = g_hall.delta_angle_edge/SMOOTH_COUNT;
|
|
|
+ g_hall.angle_smooth_cnt = 1;
|
|
|
+ }else {
|
|
|
+ g_hall.angle_smooth_step = 0;
|
|
|
+ g_hall.angle_smooth_cnt = SMOOTH_COUNT + 1;
|
|
|
+ }
|
|
|
+ hall_calc_mot_velocity(prev_ticks);
|
|
|
+}
|
|
|
+
|
|
|
+
|
kevin