From 793f20977c6118cf24cc742c99ad6209400f39f5 Mon Sep 17 00:00:00 2001 From: Pavel Pisa Date: Fri, 12 Dec 2014 22:19:05 +0100 Subject: [PATCH] Variant of 3-phase DQ firmware which replaces branches by arithmetic. Signed-off-by: Pavel Pisa --- hw/lx-rocon_firmware/firmware.c | 66 +++++++++++++++++++++++++++++---- 1 file changed, 59 insertions(+), 7 deletions(-) diff --git a/hw/lx-rocon_firmware/firmware.c b/hw/lx-rocon_firmware/firmware.c index 171b2a2..f6ffdb1 100644 --- a/hw/lx-rocon_firmware/firmware.c +++ b/hw/lx-rocon_firmware/firmware.c @@ -15,6 +15,8 @@ *******************************************************************/ +#define SUPPRESS_CONDITIONALS 1 +#undef COMPUTE_PHASE_SECTOR #include #include "tumbl_addr.h" @@ -32,6 +34,8 @@ typedef struct pxmcc_axis_data_t { uint32_t ccflg; int32_t pwm_d; int32_t pwm_q; + int32_t cur_d; + int32_t cur_q; uint32_t ptindx; /* index into phase table / irc in the cycle */ uint32_t ptirc; /* IRC count per phase table */ uint32_t ptreci; /* Reciprocal value of ptirc * 63356 */ @@ -39,9 +43,6 @@ typedef struct pxmcc_axis_data_t { int32_t ptsin; int32_t ptcos; uint32_t ptphs; - int32_t pwm_alp; - int32_t pwm_bet; - int32_t pwm_bet2; uint32_t act_idle; uint32_t min_idle; @@ -96,7 +97,7 @@ void main(void) pxmcc->ccflg = 0; pxmcc->ptindx = 0; - pxmcc->ptofs = *FPGA_IRC0; + /*pxmcc->ptofs = *FPGA_IRC0;*/ pxmcc->ptirc = 1000; pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */ pxmcc->min_idle = 0; @@ -144,16 +145,19 @@ void main(void) int32_t pwm_alp; int32_t pwm_bet; int32_t pwm_bet_div_2_k3; - uint32_t phs; uint32_t pwm1; uint32_t pwm2; uint32_t pwm3; + #if defined(COMPUTE_PHASE_SECTOR) || !defined(SUPPRESS_CONDITIONALS) + uint32_t phs; + #endif /*COMPUTE_PHASE_SECTOR*/ - pwm_alp = pxmcc->pwm_d * pxmcc->ptcos + pxmcc->pwm_q * pxmcc->ptsin; - pwm_bet = pxmcc->pwm_d * pxmcc->ptsin - pxmcc->pwm_q * pxmcc->ptcos; + pwm_alp = pxmcc->pwm_d * pxmcc->ptcos - pxmcc->pwm_q * pxmcc->ptsin; + pwm_bet = pxmcc->pwm_d * pxmcc->ptsin + pxmcc->pwm_q * pxmcc->ptcos; 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 */ @@ -200,8 +204,56 @@ void main(void) /* 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; + alp_m_bet_d2k3 = (alp_sgn ^ pwm_alp) - (bet_sgn ^ pwm_bet_div_2_k3); + alp_m_bet_d2k3 = alp_m_bet_d2k3 >> 31; + #ifdef COMPUTE_PHASE_SECTOR + uint32_t bet_sgn_cpl = ~bet_sgn; + #endif /*COMPUTE_PHASE_SECTOR*/ + + 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*/ + + asm volatile("": : : "memory"); *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000; *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000; -- 2.39.2