}
return (void*)0;
}
-
+/*
+ * \brief
+ * Multiplication of 11 bit
+ */
+inline int16_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;
+}
inline
int sin_commutator(int duty){
#define DEGREE_60 715827883
#define DEGREE_300 3579139413
uint32_t j,pos;
int32_t sin;
- uint16_t pwm;
pos=rps.index_dist*4294967;
if (duty>=0){ /*clockwise rotation*/
/* 1st phase */
sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm1=pwm;
+ rps.pwm1=mult_cap(sin, duty);
/* 2nd phase */
sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm2=pwm;
+ rps.pwm2=mult_cap(sin, duty);;
/* 3rd phase */
sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm3=pwm;
+ rps.pwm3=mult_cap(sin, duty);;
}else{
duty=-duty;
/* 1st phase */
sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm1=pwm;
+ rps.pwm1=mult_cap(sin, duty);
/* 2nd phase */
sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm2=pwm;
+ rps.pwm2=mult_cap(sin, duty);
/* 3rd phase */
sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm3=pwm;
+ rps.pwm3=mult_cap(sin, duty);
}
return 0;
}
* signalu indexu
*/
void comIndDist(){
- uint16_t pos_12 = 0x0FFF & data.pozice_raw;
- if (data.index_position<2048){
- if (pos_12<data.index_position){
+ uint16_t pos = 0x0FFF & data.pozice_raw;
+ uint16_t dist;
+ uint16_t index = data.index_position;
+
+ if (index<1999){ /*index e<0,1998> */
+ if (pos<index){ /*pozice e<0,index-1> */
/*proti smeru h.r. od indexu*/
- rps.index_dist=pos_12+2000-data.index_position;
- }else if (pos_12<=data.index_position+2048){
+ dist=pos+2000-index;
+ }else if (pos<=index+1999){ /*pozice e<index,index+1999> */
/*po smeru h.r. od indexu*/
- rps.index_dist=pos_12-data.index_position;
- }else{
+ dist=pos-index;
+ }else if (pos<index+2096){ /*pozice e<index+2000,index+2095> */
+ goto index_lost;
+ }else{ /*pozice e<index+2096,4095> */
/*proti smeru h.r. od indexu - podtecena pozice*/
- rps.index_dist=pos_12-data.index_position-2096;
+ dist=pos-index-2096;
}
- }else{
- if (pos_12<data.index_position-2048){
+ }else if (index<=2096){ /*index e<1999,2096>*/
+ if (pos<index-1999){ /*pozice e<0,index-2000> */
+ goto index_lost;
+ }else if (pos<index){ /*pozice e<index-1999,index-1> */
+ /*proti smeru h.r. od indexu*/
+ dist=pos+2000-index;
+ }else if (pos<=index+1999){ /*pozice e<index,index+1999> */
+ /*po smeru h.r. od indexu*/
+ dist=pos-index;
+ }else { /*pozice e<index+2000,4095> */
+ 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*/
- rps.index_dist=4096+pos_12-data.index_position;
- }else if (pos_12<data.index_position){
+ dist=4096+pos-index;
+ }else if (pos<index-1999){ /*pozice e<index-2096,index-2000> */
+ goto index_lost;
+ }else if (pos<index){ /*pozice e<index-1999,index-1> */
/*proti smeru h.r. od indexu*/
- rps.index_dist=pos_12+2000-data.index_position;
- }else{
+ dist=pos+2000-index;
+ }else{ /*pozice e<index,4095> */
/*po smeru h.r. od indexu*/
- rps.index_dist=pos_12-data.index_position;
+ dist=pos-index;
}
-
}
+
+ rps.index_dist = dist;
+ return;
+
+ index_lost:
+ rps.index_ok=0;
+ return;
}
/*
* \brief