Advertisement
7134956

Untitled

Nov 9th, 2020
696
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 1.78 KB | None | 0 0
  1.     if(ctl.state & (CAP | RUN | RDY)) {
  2.         static OBS_SELECT_TypeDef obs_t_prev = OBS_NULL;
  3.         static float *obs_p_prev = &McKit.GND;//Указатель предыдущего наблюдателя
  4.         static float *obs_p_curr = &McKit.GND;//Указатель текущего наблюдателя
  5.         static float yph_offset = 0.0f;     //Аккумулятор поправки
  6.  
  7.         ctl.yph = *obs_p_prev;  //Свежий угол предыдущего наблюдателя
  8.  
  9.         if(obs_t_prev != ctl.obs) { //Если выбран новый тип наблюдателя, взять новый указатель на угол
  10.             obs_t_prev = ctl.obs;
  11.             obs_p_prev = obs_p_curr;
  12.             switch(ctl.obs) {
  13.                 case OBS_NULL:      // 4 Работа по датчикам Холла
  14.                     obs_p_curr = &McKit.GND;
  15.                     break;
  16.                 case OBS_HALL:      // 4 Работа по датчикам Холла
  17.                     obs_p_curr = &hall.th;
  18.                     break;
  19.                 case OBS_NL_ORIG:   // 5 Нелинейный оригинальный наблюдатель
  20.                     obs_p_curr = &nlobs_orig.ph;
  21.                     break;
  22.                 case OBS_BEMF:      // 3 Наблюдатель обратной ЭДС
  23.                     obs_p_curr = &BEMF_est.ph;
  24.                     break;
  25.             }
  26.  
  27.         } else if(obs_p_prev != obs_p_curr) { //Теперь будем сводить угол
  28.             float yph_step = M_PI * TIMESTEP / 0.01f; //Максимальное время сведения 0.01 Секунды.
  29.             yph_step *= SIGN(utils_angle_difference_rad(*obs_p_curr, ctl.yph));
  30.             yph_offset += yph_step; //Растим поправку
  31.             ctl.yph += yph_offset;  //Добавляем поправку
  32.             utils_norm_angle_rad(&ctl.yph);
  33.  
  34.             if(fabsf(utils_angle_difference_rad(ctl.yph, *obs_p_curr)) < yph_step) { //Углы свелись
  35.                 obs_p_prev = obs_p_curr;
  36.                 yph_offset = 0.0f;
  37.             }
  38.         }
  39.     }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement