*******************************************************************/
#define SUPPRESS_CONDITIONALS 1
-#undef COMPUTE_PHASE_SECTOR
-
-#define PXMCC_CURADC_CHANNELS 3
+#undef COMPUTE_PHASE_SECTOR
#include <stdint.h>
#include "pxmcc_types.h"
#endif
while (1) {
- uint32_t irc = *FPGA_IRC0;
- uint32_t ofs = pxmcc->ptofs;
- uint32_t per = pxmcc->ptirc;
- int32_t pti;
- uint32_t pta;
- uint32_t dummy;
-
- pti = irc - ofs;
- if ((uint32_t)pti >= per) {
- if (pti < 0) {
- ofs -= per;
- } else {
- ofs += per;
- }
- pti = irc - ofs;
- pxmcc->ptofs = ofs;
- }
- pxmcc->ptindx = pti;
-
- pta = pti * pxmcc->ptreci;
-
- *FPGA_FNCAPPROX_SIN = pta;
-
- dummy = *FPGA_FNCAPPROX_SIN;
- dummy = *FPGA_FNCAPPROX_SIN;
- dummy = *FPGA_FNCAPPROX_SIN;
- pxmcc->ptsin = *FPGA_FNCAPPROX_SIN;
- dummy = *FPGA_FNCAPPROX_COS;
- pxmcc->ptcos = *FPGA_FNCAPPROX_COS;
-
- if (pxmcc->ccflg) {
- int32_t pwm_alp;
- int32_t pwm_bet;
- int32_t pwm_bet_div_2_k3;
- uint32_t pwm1;
- uint32_t pwm2;
- uint32_t pwm3;
- int32_t pwm_d;
- int32_t pwm_q;
- #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
- uint32_t phs;
- #endif /*COMPUTE_PHASE_SECTOR*/
-
- pwm_d = (volatile uint32_t)pxmcc->pwm_dq;
- pwm_q = (pwm_d << 16) >> 16;
- pwm_d >>= 16;
-
- pwm_alp = pwm_d * pxmcc->ptcos - pwm_q * pxmcc->ptsin;
- pwm_bet = pwm_d * pxmcc->ptsin + pwm_q * pxmcc->ptcos;
-
- pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
+ pxmcc = pxmcc_data.axis;
+ do {
+ uint32_t irc;
+ uint32_t ofs = pxmcc->ptofs;
+ uint32_t per = pxmcc->ptirc;
+ int32_t pti;
+ uint32_t pta;
+ uint32_t pwmtx_info;
+ volatile uint32_t *uptr;
+
+ irc = *(FPGA_IRC0 + (FPGA_IRC1 - FPGA_IRC0) * pxmcc->inp_info);
- #ifndef SUPPRESS_CONDITIONALS
- if (pwm_bet > 0)
- if (pwm_alp > 0)
- /* pwm_bet > 2 * k3 * pwm_alp */
- if (pwm_bet_div_2_k3 > pwm_alp)
- phs = 1;
- else
- phs = 0;
- else
- /* -pwm_bet > 2 * k3 * pwm_alp */
- if (pwm_bet_div_2_k3 < -pwm_alp)
- phs = 2;
- else
- phs = 1;
- else
- if (pwm_alp > 0)
- /* pwm_bet > -2 * k3 * pwm_alp */
- if (pwm_bet_div_2_k3 > -pwm_alp)
- phs = 5;
- else
- phs = 4;
- else
- /* pwm_bet > 2 * k3 * u_alp */
- if (pwm_bet_div_2_k3 > pwm_alp)
- phs = 3;
- else
- phs = 4;
-
- if (phs <= 1) {
- /* pwm1 = pwm_alp + 1.0/(2.0*k3) * pwm_bet */
- pwm1 = (pwm_alp + pwm_bet_div_2_k3) >> 16;
- /* pwm2 = 1/k3 * pwm_bet */
- pwm2 = pwm_bet_div_2_k3 >> 15;
- pwm3 = 0;
- } else if (phs <= 3) {
- pwm1 = 0;
- /* pwm2 = 1.0/(2.0*k3) * pwm_bet - pwm_alp */
- pwm2 = (pwm_bet_div_2_k3 - pwm_alp) >> 16;
- /* pwm3 = -1.0/(2.0*k3) * pwm_bet - pwm_alp */
- pwm3 = (-pwm_bet_div_2_k3 - pwm_alp) >>16;
- } else {
- /* pwm1 = pwm_alp - 1.0/(2.0*k3) * pwm_bet */
- pwm1 = (pwm_alp - pwm_bet_div_2_k3) >>16;
- pwm2 = 0;
- /* pwm3 = -1/k3 * pwm_bet */
- pwm3 = -pwm_bet_div_2_k3 >> 15;
+ pti = irc - ofs;
+ if ((uint32_t)pti >= per) {
+ if (pti < 0) {
+ ofs -= per;
+ } else {
+ ofs += per;
+ }
+ pti = irc - ofs;
+ pxmcc->ptofs = ofs;
}
- #else /*SUPPRESS_CONDITIONALS*/
- {
- int32_t alp_m_bet_d2k3;
- uint32_t state23_msk;
- uint32_t pwm23_shift;
- uint32_t bet_sgn = pwm_bet >> 31;
- uint32_t alp_sgn = pwm_alp >> 31;
- #ifdef COMPUTE_PHASE_SECTOR
- uint32_t bet_sgn_cpl = ~bet_sgn;
- #endif /*COMPUTE_PHASE_SECTOR*/
-
- alp_m_bet_d2k3 = (alp_sgn ^ pwm_alp) - (bet_sgn ^ (pwm_bet_div_2_k3 + bet_sgn));
- alp_m_bet_d2k3 = alp_m_bet_d2k3 >> 31;
-
- state23_msk = alp_sgn & ~alp_m_bet_d2k3;
-
- /*
- * bet alp amb s23
- * 0 0 0 0 -> 0 (000)
- * 0 0 -1 0 -> 1 (001)
- * -1 0 -1 0 -> 1 (001)
- * -1 0 0 -1 -> 2 (010)
- * -1 -1 0 -1 -> 3 (011)
- * -1 -1 -1 0 -> 4 (100)
- * 0 -1 -1 0 -> 4 (100)
- * 0 -1 0 0 -> 5 (101)
- */
-
- #ifdef COMPUTE_PHASE_SECTOR
- phs = (bet_sgn & 5) + (bet_sgn_cpl ^ ((alp_m_bet_d2k3 + 2 * state23_msk) + bet_sgn_cpl));
+ pxmcc->ptindx = pti;
+
+ pta = pti * pxmcc->ptreci;
+
+ *FPGA_FNCAPPROX_SIN = pta;
+ asm volatile("nop\n": : : "memory");
+ asm volatile("nop\n": : : "memory");
+ asm volatile("nop\n": : : "memory");
+ pxmcc->ptsin = *FPGA_FNCAPPROX_SIN;
+ asm volatile("nop\n": : : "memory");
+ pxmcc->ptcos = *FPGA_FNCAPPROX_COS;
+
+ if (pxmcc->ccflg) {
+ int32_t pwm_alp;
+ int32_t pwm_bet;
+ int32_t pwm_bet_div_2_k3;
+ uint32_t pwm1;
+ uint32_t pwm2;
+ uint32_t pwm3;
+ uint32_t pwm4;
+ int32_t pwm_d;
+ int32_t pwm_q;
+ #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
+ uint32_t phs;
#endif /*COMPUTE_PHASE_SECTOR*/
- pwm1 = pwm_alp & state23_msk;
- pwm2 = pwm_bet_div_2_k3 - pwm1;
- pwm3 = -pwm_bet_div_2_k3 - pwm1;
- pwm2 &= (~bet_sgn | state23_msk);
- pwm3 &= (bet_sgn | state23_msk);
- pwm1 = pwm_alp + pwm2 + pwm3;
- pwm1 &= ~state23_msk;
- pwm1 >>= 16;
- pwm23_shift = 15 - state23_msk;
- pwm2 >>= pwm23_shift;
- pwm3 >>= pwm23_shift;
+ pwm_d = (volatile uint32_t)pxmcc->pwm_dq;
+ pwm_q = (pwm_d << 16) >> 16;
+ pwm_d >>= 16;
+
+ pwm_alp = pwm_d * pxmcc->ptcos - pwm_q * pxmcc->ptsin;
+ pwm_bet = pwm_d * pxmcc->ptsin + pwm_q * pxmcc->ptcos;
+
+ if (!pxmcc->mode) {
+ pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
+
+ #ifndef SUPPRESS_CONDITIONALS
+ if (pwm_bet > 0)
+ if (pwm_alp > 0)
+ /* pwm_bet > 2 * k3 * pwm_alp */
+ if (pwm_bet_div_2_k3 > pwm_alp)
+ phs = 1;
+ else
+ phs = 0;
+ else
+ /* -pwm_bet > 2 * k3 * pwm_alp */
+ if (pwm_bet_div_2_k3 < -pwm_alp)
+ phs = 2;
+ else
+ phs = 1;
+ else
+ if (pwm_alp > 0)
+ /* pwm_bet > -2 * k3 * pwm_alp */
+ if (pwm_bet_div_2_k3 > -pwm_alp)
+ phs = 5;
+ else
+ phs = 4;
+ else
+ /* pwm_bet > 2 * k3 * u_alp */
+ if (pwm_bet_div_2_k3 > pwm_alp)
+ phs = 3;
+ else
+ phs = 4;
+
+ if (phs <= 1) {
+ /* pwm1 = pwm_alp + 1.0/(2.0*k3) * pwm_bet */
+ pwm1 = (pwm_alp + pwm_bet_div_2_k3) >> 16;
+ /* pwm2 = 1/k3 * pwm_bet */
+ pwm2 = pwm_bet_div_2_k3 >> 15;
+ pwm3 = 0;
+ } else if (phs <= 3) {
+ pwm1 = 0;
+ /* pwm2 = 1.0/(2.0*k3) * pwm_bet - pwm_alp */
+ pwm2 = (pwm_bet_div_2_k3 - pwm_alp) >> 16;
+ /* pwm3 = -1.0/(2.0*k3) * pwm_bet - pwm_alp */
+ pwm3 = (-pwm_bet_div_2_k3 - pwm_alp) >>16;
+ } else {
+ /* pwm1 = pwm_alp - 1.0/(2.0*k3) * pwm_bet */
+ pwm1 = (pwm_alp - pwm_bet_div_2_k3) >>16;
+ pwm2 = 0;
+ /* pwm3 = -1/k3 * pwm_bet */
+ pwm3 = -pwm_bet_div_2_k3 >> 15;
+ }
+ #else /*SUPPRESS_CONDITIONALS*/
+ {
+ int32_t alp_m_bet_d2k3;
+ uint32_t state23_msk;
+ uint32_t pwm23_shift;
+ uint32_t bet_sgn = pwm_bet >> 31;
+ uint32_t alp_sgn = pwm_alp >> 31;
+ #ifdef COMPUTE_PHASE_SECTOR
+ uint32_t bet_sgn_cpl = ~bet_sgn;
+ #endif /*COMPUTE_PHASE_SECTOR*/
+
+ alp_m_bet_d2k3 = (alp_sgn ^ pwm_alp) - (bet_sgn ^ (pwm_bet_div_2_k3 + bet_sgn));
+ alp_m_bet_d2k3 = alp_m_bet_d2k3 >> 31;
+
+ state23_msk = alp_sgn & ~alp_m_bet_d2k3;
+
+ /*
+ * bet alp amb s23
+ * 0 0 0 0 -> 0 (000)
+ * 0 0 -1 0 -> 1 (001)
+ * -1 0 -1 0 -> 1 (001)
+ * -1 0 0 -1 -> 2 (010)
+ * -1 -1 0 -1 -> 3 (011)
+ * -1 -1 -1 0 -> 4 (100)
+ * 0 -1 -1 0 -> 4 (100)
+ * 0 -1 0 0 -> 5 (101)
+ */
+
+ #ifdef COMPUTE_PHASE_SECTOR
+ phs = (bet_sgn & 5) + (bet_sgn_cpl ^ ((alp_m_bet_d2k3 + 2 * state23_msk) + bet_sgn_cpl));
+ #endif /*COMPUTE_PHASE_SECTOR*/
+
+ pwm1 = pwm_alp & state23_msk;
+ pwm2 = pwm_bet_div_2_k3 - pwm1;
+ pwm3 = -pwm_bet_div_2_k3 - pwm1;
+ pwm2 &= (~bet_sgn | state23_msk);
+ pwm3 &= (bet_sgn | state23_msk);
+ pwm1 = pwm_alp + pwm2 + pwm3;
+ pwm1 &= ~state23_msk;
+ pwm1 >>= 16;
+ pwm23_shift = 15 - state23_msk;
+ pwm2 >>= pwm23_shift;
+ pwm3 >>= pwm23_shift;
+ }
+ #endif /*SUPPRESS_CONDITIONALS*/
+
+ #ifdef COMPUTE_PHASE_SECTOR
+ pxmcc->ptphs = phs;
+ #endif /*COMPUTE_PHASE_SECTOR*/
+
+ #if 0
+ *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000;
+ *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000;
+ *FPGA_LX_MASTER_TX_PWM2 = pwm1 | 0x4000;
+ #else
+ pwmtx_info = pxmcc->pwmtx_info;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 0) & 0xff);
+ pxmcc->pwm_prew[1] = *uptr & 0x3fff;
+ *uptr = pwm2 | 0x4000;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 8) & 0xff);
+ pxmcc->pwm_prew[2] = *uptr & 0x3fff;
+ *uptr = pwm3 | 0x4000;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
+ pxmcc->pwm_prew[0] = *uptr & 0x3fff;
+ *uptr = pwm1 | 0x4000;
+ #endif
+ } else {
+ uint32_t bet_sgn = pwm_bet >> 31;
+ uint32_t alp_sgn = pwm_alp >> 31;
+ pwm1 = pwm2 = pwm_bet | 0x4000;
+ pwm3 = pwm4 = pwm_alp | 0x4000;
+ pwm1 &= ~bet_sgn;
+ pwm2 &= bet_sgn;
+ pwm3 &= ~alp_sgn;
+ pwm4 &= alp_sgn;
+
+ pwmtx_info = pxmcc->pwmtx_info;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 0) & 0xff);
+ pxmcc->pwm_prew[0] = *uptr & 0x3fff;
+ *uptr = pwm1 | 0x4000;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 8) & 0xff);
+ pxmcc->pwm_prew[1] = *uptr & 0x3fff;
+ *uptr = pwm2 | 0x4000;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
+ pxmcc->pwm_prew[2] = *uptr & 0x3fff;
+ *uptr = pwm3 | 0x4000;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 24) & 0xff);
+ pxmcc->pwm_prew[3] = *uptr & 0x3fff;
+ *uptr = pwm4 | 0x4000;
+ }
+ } else {
+ if (!pxmcc->mode) {
+ pwmtx_info = pxmcc->pwmtx_info;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 0) & 0xff);
+ pxmcc->pwm_prew[1] = *uptr & 0x3fff;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 8) & 0xff);
+ pxmcc->pwm_prew[2] = *uptr & 0x3fff;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
+ pxmcc->pwm_prew[0] = *uptr & 0x3fff;
+ } else {
+ pwmtx_info = pxmcc->pwmtx_info;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 0) & 0xff);
+ pxmcc->pwm_prew[0] = *uptr & 0x3fff;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 8) & 0xff);
+ pxmcc->pwm_prew[1] = *uptr & 0x3fff;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 16) & 0xff);
+ pxmcc->pwm_prew[2] = *uptr & 0x3fff;
+ uptr = FPGA_LX_MASTER_TX + ((pwmtx_info >> 24) & 0xff);
+ pxmcc->pwm_prew[3] = *uptr & 0x3fff;
+ }
}
- #endif /*SUPPRESS_CONDITIONALS*/
-
- #ifdef COMPUTE_PHASE_SECTOR
- pxmcc->ptphs = phs;
- #endif /*COMPUTE_PHASE_SECTOR*/
+ pxmcc++;
- asm volatile("": : : "memory");
-
- *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000;
- *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000;
- *FPGA_LX_MASTER_TX_PWM2 = pwm1 | 0x4000;
- }
+ } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
asm volatile("": : : "memory");
pxmcc_data.common.rx_done_sqn = last_rx_done_sqn;
asm volatile("": : : "memory");
}
+
{
int i;
pxmcc_curadc_data_t *curadc = pxmcc_data.curadc;
siroladc += 3;
}
}
- {
+
+ pxmcc = pxmcc_data.axis;
+ do {
int32_t cur_alp;
int32_t cur_bet;
int32_t cur_d;
int32_t cur_q;
- uint32_t pwm[3];
uint32_t pwm1;
uint32_t pwm2;
uint32_t pwm3;
int32_t cur3;
uint32_t curmult_idx;
uint32_t pwm_reci;
+ uint32_t out_info;
#if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS)
uint32_t phs;
#ifdef COMPUTE_PHASE_SECTOR
#endif /*COMPUTE_PHASE_SECTOR*/
#endif /*COMPUTE_PHASE_SECTOR*/
- pwm[1] = *FPGA_LX_MASTER_TX_PWM0 & 0x3fff;
- pwm[2] = *FPGA_LX_MASTER_TX_PWM1 & 0x3fff;
- pwm[0] = *FPGA_LX_MASTER_TX_PWM2 & 0x3fff;
-
- cur[1] = pxmcc_data.curadc[0].cur_val;
- cur[2] = pxmcc_data.curadc[1].cur_val;
- cur[0] = pxmcc_data.curadc[2].cur_val;
-
- pwm1 = pwm[0];
- pwm2 = pwm[1];
- pwm3 = pwm[2];
-
- #ifndef SUPPRESS_CONDITIONALS
- #ifndef COMPUTE_PHASE_SECTOR
- if (pwm1 > pwm2)
- if (pwm2 > pwm3)
- phs = 0;
- else if (pwm1 > pwm3)
- phs = 5;
- else
- phs = 4;
- else
- if (pwm2 < pwm3)
- phs = 3;
- else if (pwm1 < pwm3)
- phs = 2;
+ out_info = pxmcc->out_info;
+ if (!pxmcc->mode) {
+ cur[1] = pxmcc_data.curadc[out_info + 0].cur_val;
+ cur[2] = pxmcc_data.curadc[out_info + 1].cur_val;
+ cur[0] = pxmcc_data.curadc[out_info + 2].cur_val;
+
+ pwm1 = pxmcc->pwm_prew[0];
+ pwm2 = pxmcc->pwm_prew[1];
+ pwm3 = pxmcc->pwm_prew[2];
+
+ #ifndef SUPPRESS_CONDITIONALS
+ #ifndef COMPUTE_PHASE_SECTOR
+ if (pwm1 > pwm2)
+ if (pwm2 > pwm3)
+ phs = 0;
+ else if (pwm1 > pwm3)
+ phs = 5;
+ else
+ phs = 4;
else
- phs = 1;
- #endif /*COMPUTE_PHASE_SECTOR*/
-
- curmult_idx = (0x00102102 >> (4 * phs)) & 3;
- pwm_reci = pxmcc_data.common.pwm_cycle - pwm[curmult_idx];
- pwm_reci = (pxmcc_data.common.pwm_cycle << 16) / pwm_reci;
- cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
-
- cur1 = cur[0];
- cur2 = cur[1];
- cur3 = cur[2];
-
- if ((phs == 5) || (phs == 0))
- cur_alp = -(cur2 + cur3);
- else
- cur_alp = cur1;
-
- if ((phs == 5) || (phs == 0))
- cur_bet = cur2 - cur3;
- else if ((phs == 3) || (phs == 4))
- cur_bet = 2 * cur2 + cur1;
- else /* 1 2 */
- cur_bet = -(2 * cur3 + cur1);
- #else /*SUPPRESS_CONDITIONALS*/
- {
- /*
- * u1>u2 u2>u3 u1>u3 cm
- * 0 1 1 -> 1 (1, 0, 2) 2
- * 0 1 0 -> 2 (1, 2, 0) 0
- * 0 0 0 -> 3 (2, 1, 0) 1
- * 1 0 0 -> 4 (2, 0, 1) 2
- * 1 0 1 -> 5 (0, 2, 1) 0
- * 1 1 1 -> 0 (0, 1, 2) 1
- */
-
- uint32_t u1gtu2 = (int32_t)(pwm2 - pwm1) >> 31;
- uint32_t u1gtu3 = (int32_t)(pwm3 - pwm1) >> 31;
- uint32_t u2gtu3 = (int32_t)(pwm3 - pwm2) >> 31;
- uint32_t state50_msk = u1gtu2 & u1gtu3;
- uint32_t pwm_reci_bits;
-
- curmult_idx = (((u1gtu3 ^ u1gtu2) | 1) ^ u2gtu3 ^ u1gtu2) & 3;
- pwm_reci = pxmcc_data.common.pwm_cycle - pwm[curmult_idx];
- #if 0
- pwm_reci_bits = __builtin_clzl(pwm_reci);
- #else
- asm("clz %0,%1\n":"=r"(pwm_reci_bits):"r"(pwm_reci));
- #endif
- pwm_reci <<= pwm_reci_bits;
- *FPGA_FNCAPPROX_RECI = pwm_reci;
- dummy = *FPGA_FNCAPPROX_RECI;
- dummy = *FPGA_FNCAPPROX_RECI;
- dummy = *FPGA_FNCAPPROX_RECI;
- pwm_reci = *FPGA_FNCAPPROX_RECI;
- pwm_reci >>= 16;
- pwm_reci *= pxmcc_data.common.pwm_cycle;
- pwm_reci >>= 30 - pwm_reci_bits;
- cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
+ if (pwm2 < pwm3)
+ phs = 3;
+ else if (pwm1 < pwm3)
+ phs = 2;
+ else
+ phs = 1;
+ #endif /*COMPUTE_PHASE_SECTOR*/
+
+ curmult_idx = (0x00102102 >> (4 * phs)) & 3;
+ pwm_reci = pxmcc_data.common.pwm_cycle - pxmcc->pwm_prew[curmult_idx];
+ pwm_reci = (pxmcc_data.common.pwm_cycle << 16) / pwm_reci;
+ cur[curmult_idx] = (int32_t)(pwm_reci *
+ pxmcc_data.curadc[curmult_idx].cur_val) >> 16;
cur1 = cur[0];
cur2 = cur[1];
cur3 = cur[2];
- cur_alp = -(cur2 + cur3); /* 5 0 */
- cur_alp &= state50_msk; /* 1 2 3 4 */
- cur_alp |= cur1 & ~state50_msk;
-
- cur_bet = (-2 * cur3 - cur1) & u2gtu3; /* 1 2 */
- cur_bet |= (2 * cur2 + cur1) & ~u2gtu3; /* 3 4 */
- cur_bet &= ~state50_msk;
- cur_bet |= (cur2 - cur3) & state50_msk; /* 5 0 */
+ if ((phs == 5) || (phs == 0))
+ cur_alp = -(cur2 + cur3);
+ else
+ cur_alp = cur1;
+
+ if ((phs == 5) || (phs == 0))
+ cur_bet = cur2 - cur3;
+ else if ((phs == 3) || (phs == 4))
+ cur_bet = 2 * cur2 + cur1;
+ else /* 1 2 */
+ cur_bet = -(2 * cur3 + cur1);
+ #else /*SUPPRESS_CONDITIONALS*/
+ {
+ /*
+ * u1>u2 u2>u3 u1>u3 cm
+ * 0 1 1 -> 1 (1, 0, 2) 2
+ * 0 1 0 -> 2 (1, 2, 0) 0
+ * 0 0 0 -> 3 (2, 1, 0) 1
+ * 1 0 0 -> 4 (2, 0, 1) 2
+ * 1 0 1 -> 5 (0, 2, 1) 0
+ * 1 1 1 -> 0 (0, 1, 2) 1
+ */
+
+ uint32_t u1gtu2 = (int32_t)(pwm2 - pwm1) >> 31;
+ uint32_t u1gtu3 = (int32_t)(pwm3 - pwm1) >> 31;
+ uint32_t u2gtu3 = (int32_t)(pwm3 - pwm2) >> 31;
+ uint32_t state50_msk = u1gtu2 & u1gtu3;
+ uint32_t pwm_reci_bits;
+
+ curmult_idx = (((u1gtu3 ^ u1gtu2) | 1) ^ u2gtu3 ^ u1gtu2) & 3;
+ pwm_reci = pxmcc_data.common.pwm_cycle - pxmcc->pwm_prew[curmult_idx];
+ #if 0
+ pwm_reci_bits = __builtin_clzl(pwm_reci);
+ #else
+ asm("clz %0,%1\n":"=r"(pwm_reci_bits):"r"(pwm_reci));
+ #endif
+ pwm_reci <<= pwm_reci_bits;
+ *FPGA_FNCAPPROX_RECI = pwm_reci;
+ asm volatile("nop\n": : : "memory");
+ asm volatile("nop\n": : : "memory");
+ asm volatile("nop\n": : : "memory");
+ pwm_reci = *FPGA_FNCAPPROX_RECI;
+ pwm_reci >>= 16;
+ pwm_reci *= pxmcc_data.common.pwm_cycle;
+ pwm_reci >>= 30 - pwm_reci_bits;
+ cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
+
+ cur1 = cur[0];
+ cur2 = cur[1];
+ cur3 = cur[2];
+
+ cur_alp = -(cur2 + cur3); /* 5 0 */
+ cur_alp &= state50_msk; /* 1 2 3 4 */
+ cur_alp |= cur1 & ~state50_msk;
+
+ cur_bet = (-2 * cur3 - cur1) & u2gtu3; /* 1 2 */
+ cur_bet |= (2 * cur2 + cur1) & ~u2gtu3; /* 3 4 */
+ cur_bet &= ~state50_msk;
+ cur_bet |= (cur2 - cur3) & state50_msk; /* 5 0 */
+ }
+ #endif /*SUPPRESS_CONDITIONALS*/
+
+ cur_alp *= 3;
+ cur_alp >>= 1;
+
+ cur_bet *= CONST16_K3;
+ cur_bet >>= 16;
+ } else {
+ int32_t bet_pwm = pxmcc->pwm_prew[1];
+ uint32_t bet_sgn = (bet_pwm - 1) >> 31;
+ int32_t alp_pwm = pxmcc->pwm_prew[3];
+ uint32_t alp_sgn = (alp_pwm - 1) >> 31;
+ cur_bet = (pxmcc_data.curadc[out_info + 0].cur_val & ~bet_sgn) -
+ (pxmcc_data.curadc[out_info + 1].cur_val & bet_sgn);
+ cur_alp = (pxmcc_data.curadc[out_info + 2].cur_val & ~alp_sgn) -
+ (pxmcc_data.curadc[out_info + 3].cur_val & alp_sgn);
}
- #endif /*SUPPRESS_CONDITIONALS*/
-
- cur_alp *= 3;
- cur_alp >>= 1;
-
- cur_bet *= CONST16_K3;
- cur_bet >>= 16;
cur_d = cur_alp * pxmcc->ptcos + cur_bet * pxmcc->ptsin;
cur_q = -cur_alp * pxmcc->ptsin + cur_bet * pxmcc->ptcos;
pxmcc->cur_q_cum = ((pxmcc->cur_q_cum + (cur_q >> 4)) & ~0x3f) |
(last_rx_done_sqn & 0x1f);
- }
+ pxmcc++;
+ } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
}
}
projects / fpga / lx-cpu1 / lx-rocon.git / commitdiff