/** * \brief * Computatations of position, index, speed. * */ #include "comp.h" #include "pmsm_state.h" #include "rp_spi.h" /** * \brief * Substact initial position. */ void substractOffset(struct rpi_in* data, struct rpi_in* offset){ data->pozice=data->pozice_raw-offset->pozice_raw; return; } /* * \brief * Multiplication of 11 bit * Zaporne vysledky prvede na nulu. */ uint16_t mult_cap(int32_t s,int d){ int j; int res=0; for(j=0;j!=11;j++){ /* multiplicate as if maximum sinus value was unity */ res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j)); } return res; } /** * \brief * Computation of distance to index. * * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu - * to je 3999 bodu * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate * signalu indexu */ void comIndDist(struct rpi_state* this){ uint16_t pos = 0x0FFF & this->spi_dat->pozice_raw; uint16_t dist; uint16_t index = this->spi_dat->index_position; if (index<1999){ /*index e<0,1998> */ if (pos */ /*proti smeru h.r. od indexu*/ dist=pos+2000-index; }else if (pos<=index+1999){ /*pozice e */ /*po smeru h.r. od indexu*/ dist=pos-index; }else if (pos */ goto index_lost; }else{ /*pozice e */ /*proti smeru h.r. od indexu - podtecena pozice*/ dist=pos-index-2096; } }else if (index<=2096){ /*index e<1999,2096>*/ if (pos */ goto index_lost; }else if (pos */ /*proti smeru h.r. od indexu*/ dist=pos+2000-index; }else if (pos<=index+1999){ /*pozice e */ /*po smeru h.r. od indexu*/ dist=pos-index; }else { /*pozice e */ goto index_lost; } }else{ /*index e<2097,4095> */ if (pos<=index-2097){ /*pozice e<0,index-2097> */ /*po smeru h.r. od indexu - pretecena pozice*/ dist=4096+pos-index; }else if (pos */ goto index_lost; }else if (pos */ /*proti smeru h.r. od indexu*/ dist=pos+2000-index; }else{ /*pozice e */ /*po smeru h.r. od indexu*/ dist=pos-index; } } this->index_dist = dist; return; index_lost: setIndexLost(this); return; } /* * \brief * Computate speed. */ void compSpeed(struct rpi_state* this){ signed long int spd; spd=this->spi_dat->pozice-this->old_pos[this->tf_count%OLD_POS_NUM]; this->speed=(int32_t)spd; }