Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- if(ctl.state & (CAP | RUN | RDY)) {
- static OBS_SELECT_TypeDef obs_t_prev = OBS_NULL;
- static float *obs_p_prev = &McKit.GND;//Указатель предыдущего наблюдателя
- static float *obs_p_curr = &McKit.GND;//Указатель текущего наблюдателя
- static float yph_offset = 0.0f; //Аккумулятор поправки
- ctl.yph = *obs_p_prev; //Свежий угол предыдущего наблюдателя
- if(obs_t_prev != ctl.obs) { //Если выбран новый тип наблюдателя, взять новый указатель на угол
- obs_t_prev = ctl.obs;
- obs_p_prev = obs_p_curr;
- switch(ctl.obs) {
- case OBS_NULL: // 4 Работа по датчикам Холла
- obs_p_curr = &McKit.GND;
- break;
- case OBS_HALL: // 4 Работа по датчикам Холла
- obs_p_curr = &hall.th;
- break;
- case OBS_NL_ORIG: // 5 Нелинейный оригинальный наблюдатель
- obs_p_curr = &nlobs_orig.ph;
- break;
- case OBS_BEMF: // 3 Наблюдатель обратной ЭДС
- obs_p_curr = &BEMF_est.ph;
- break;
- }
- } else if(obs_p_prev != obs_p_curr) { //Теперь будем сводить угол
- float yph_step = M_PI * TIMESTEP / 0.01f; //Максимальное время сведения 0.01 Секунды.
- yph_step *= SIGN(utils_angle_difference_rad(*obs_p_curr, ctl.yph));
- yph_offset += yph_step; //Растим поправку
- ctl.yph += yph_offset; //Добавляем поправку
- utils_norm_angle_rad(&ctl.yph);
- if(fabsf(utils_angle_difference_rad(ctl.yph, *obs_p_curr)) < yph_step) { //Углы свелись
- obs_p_prev = obs_p_curr;
- yph_offset = 0.0f;
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement