3 * Computatations of position, index, speed.
8 #include "pmsm_state.h"
13 * Substact initial position.
15 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
16 data->pozice=data->pozice_raw-offset->pozice_raw;
22 * Multiplication of 11 bit
23 * Zaporne vysledky prvede na nulu.
25 uint16_t mult_cap(int32_t s,int d){
29 /* multiplicate as if maximum sinus value was unity */
30 res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j));
38 * Computation of distance to index.
40 * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
41 * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
43 * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
46 void comIndDist(struct rpi_state* this){
47 uint16_t pos = 0x0FFF & this->spi_dat->pozice_raw;
49 uint16_t index = this->spi_dat->index_position;
51 if (index<1999){ /*index e<0,1998> */
52 if (pos<index){ /*pozice e<0,index-1> */
53 /*proti smeru h.r. od indexu*/
55 }else if (pos<=index+1999){ /*pozice e<index,index+1999> */
56 /*po smeru h.r. od indexu*/
58 }else if (pos<index+2096){ /*pozice e<index+2000,index+2095> */
60 }else{ /*pozice e<index+2096,4095> */
61 /*proti smeru h.r. od indexu - podtecena pozice*/
64 }else if (index<=2096){ /*index e<1999,2096>*/
65 if (pos<index-1999){ /*pozice e<0,index-2000> */
67 }else if (pos<index){ /*pozice e<index-1999,index-1> */
68 /*proti smeru h.r. od indexu*/
70 }else if (pos<=index+1999){ /*pozice e<index,index+1999> */
71 /*po smeru h.r. od indexu*/
73 }else { /*pozice e<index+2000,4095> */
76 }else{ /*index e<2097,4095> */
77 if (pos<=index-2097){ /*pozice e<0,index-2097> */
78 /*po smeru h.r. od indexu - pretecena pozice*/
80 }else if (pos<index-1999){ /*pozice e<index-2096,index-2000> */
82 }else if (pos<index){ /*pozice e<index-1999,index-1> */
83 /*proti smeru h.r. od indexu*/
85 }else{ /*pozice e<index,4095> */
86 /*po smeru h.r. od indexu*/
91 this->index_dist = dist;
104 void compSpeed(struct rpi_state* this){
106 spd=this->spi_dat->pozice-this->old_pos[this->tf_count%OLD_POS_NUM];
107 this->speed=(int32_t)spd;