-/* Firmware file for lx-rocon tumbl coprocessor */
+/*******************************************************************
+ Components for embedded applications builded for
+ laboratory and medical instruments firmware
+
+ firmware.c - multi axis motion controller coprocessor
+ firmware for FPGA tumble CPU of lx-rocon system
+
+ (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz
+ (C) 2002-2014 by PiKRON Ltd. http://www.pikron.com
+
+ This file can be used and copied according to next
+ license alternatives
+ - GPL - GNU Public License
+ - other license provided by project originators
+
+ *******************************************************************/
+
+#define SUPPRESS_CONDITIONALS 1
+#undef COMPUTE_PHASE_SECTOR
#include <stdint.h>
+#include "pxmcc_types.h"
+#include "tumbl_addr.h"
-typedef struct
-{
- int16_t p;
- int16_t i;
- int32_t irc_period;
- int16_t req_current;
- int16_t acc_dev;
- int16_t max_acc;
-} axis_settings;
-
-typedef struct
-{
- /* Input */
- int16_t current;
- int32_t irc;
- int32_t irc_last;
- int32_t irc_norm;
+/* k3 = math.sqrt(3) / 2 */
+
+#define CONST16_K3 56756
- /* Output */
- uint16_t pwm[3];
-} axis_io;
+/* 1 / k3 * 65536 */
-axis_settings a_settings;
+#define RECI16_K3 75674
-axis_io a_io;
-int32_t irc_a_reg;
-int16_t current_a_reg;
-uint32_t count;
-int16_t phase_table[1][1];
+/* 1 / (2 * k3) * 65536 */
+#define RECI16_2_K3 37837
-void init_defvals()
+#define offsbychar(_ptr, _offs) \
+ ((typeof(&(_ptr)[0]))((char*)(_ptr) + (_offs)))
+
+pxmcc_data_t pxmcc_data;
+
+void init_defvals(void)
{
- a_settings.p = 20;
- a_settings.i = 2;
- a_settings.irc_period = 7000;
- a_settings.req_current = 500;
- a_settings.acc_dev = 0;
}
-void read_input(axis_io* io)
+#if 0
+
+uint32_t sin_lat[7];
+
+void find_sin_lat(void)
{
- io->irc_last = io->irc;
- io->irc = *((int32_t*)0x8000);
+ int i;
+ register uint32_t a0, a1, a2, a3, a4, a5;
+
+ *FPGA_FNCAPPROX_SIN = 0;
+
+ for (i = 0; i < 20; i++)
+ asm volatile("": : : "memory");
+
+ *FPGA_FNCAPPROX_SIN = 0x40000000;
+ a0 = *FPGA_FNCAPPROX_SIN;
+ a1 = *FPGA_FNCAPPROX_SIN;
+ a2 = *FPGA_FNCAPPROX_SIN;
+ a3 = *FPGA_FNCAPPROX_SIN;
+ a4 = *FPGA_FNCAPPROX_SIN;
+ a5 = *FPGA_FNCAPPROX_SIN;
+ asm volatile("": : : "memory");
+
+ sin_lat[0] = a0;
+ sin_lat[1] = a1;
+ sin_lat[2] = a2;
+ sin_lat[3] = a3;
+ sin_lat[4] = a4;
+ sin_lat[5] = a5;
+ sin_lat[6] = 0x4321;
+ sin_lat[0] = 0x1234;
}
-void update_axis(axis_settings *settings, axis_io* io)
+#endif
+
+void main(void)
{
- int i;
- int16_t dev;
- int32_t mag, irc_diff;
+ uint32_t last_rx_done_sqn = 0;
+ pxmcc_axis_data_t *pxmcc;
- dev = settings->req_current - io->current;
- settings->acc_dev += dev;
+ pxmcc_data.common.pwm_cycle = 2500;
+ pxmcc_data.common.min_idle = 0x7fff;
+ pxmcc_data.common.irc_base = (uint32_t)FPGA_IRC0;
+ pxmcc_data.curadc[0].siroladc_offs = 0x0c17;
+ pxmcc_data.curadc[1].siroladc_offs = 0x0c66;
+ pxmcc_data.curadc[2].siroladc_offs = 0x0c66;
+ pxmcc_data.curadc[3].siroladc_offs = 0x0c66;
- if (settings->acc_dev > settings->max_acc)
- settings->acc_dev = settings->max_acc;
- else if (-(settings->acc_dev) <= -(settings->max_acc))
- settings->acc_dev = -(settings->max_acc);
+ pxmcc = pxmcc_data.axis;
+ do {
+ pxmcc->ccflg = 0;
+ pxmcc->mode = PXMCC_MODE_IDLE;
+ pxmcc->ptirc = 1000;
+ pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
+ pxmcc->pwm_dq = 0;
+ pxmcc++;
+ } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
- mag = settings->acc_dev * settings->i + dev * settings->p;
+ asm volatile("": : : "memory");
+ pxmcc_data.common.fwversion = PXMCC_FWVERSION;
+ asm volatile("": : : "memory");
- irc_diff = io->irc - io->irc_last;
- io->irc_last = io->irc;
- io->irc_norm += irc_diff;
+ #if 0
+ find_sin_lat();
+ #endif
- if (io->irc_norm > settings->irc_period)
- io->irc_norm -= settings->irc_period;
- else if (io->irc_norm < 0)
- io->irc_norm += settings->irc_period;
+ while (1) {
+ 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;
- for (i = 0; i < 3; i++)
- io->pwm[i] = (uint16_t)((mag * phase_table[/*i*/ 0][/*irc_norm*/ 0]) >> 16);
-}
+ irc = *(uint32_t*)pxmcc->inp_info;
-void main()
-{
- while (1)
- {
- read_input(&a_io);
- update_axis(&a_settings, &a_io);
- a_io.irc = irc_a_reg;
- a_io.current = current_a_reg;
- count++;
- }
+ pti = irc - ofs;
+ /*
+ * the '>=' is appropriate to keep pti in <0;per-1> range,
+ * but sine and cosine computations can work over multiple
+ * periods and value of per=0xffffffff allows to control/stop
+ * updates if only '>' is used.
+ */
+ 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;
+ asm volatile("nop\n");
+ asm volatile("nop\n");
+ asm volatile("nop\n");
+ pxmcc->ptsin = *FPGA_FNCAPPROX_SIN;
+ asm volatile("nop\n");
+ 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*/
+
+ 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 == PXMCC_MODE_BLDC) {
+ 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 alp_sgn = pwm_alp >> 31;
+ uint32_t bet_sgn = pwm_bet >> 31;
+ pwm_alp >>= 16;
+ pwm_bet >>= 16;
+ pwm1 = -pwm_alp & alp_sgn;
+ pwm2 = pwm_alp & ~alp_sgn;
+ pwm3 = -pwm_bet & bet_sgn;
+ pwm4 = pwm_bet & ~bet_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;
+ if (pxmcc->mode == PXMCC_MODE_STEPPER) {
+ if ((pxmcc->steps_sqn_next ^ last_rx_done_sqn) & 0x1f) {
+ pxmcc->steps_pos += pxmcc->steps_inc;
+ } else {
+ pxmcc->steps_pos = pxmcc->steps_pos_next;
+ pxmcc->steps_inc = pxmcc->steps_inc_next;
+ pxmcc->steps_inc_next = 0;
+ }
+ }
+ }
+ } else {
+ if (pxmcc->mode == PXMCC_MODE_BLDC) {
+ 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;
+ }
+ }
+ pxmcc++;
+
+ } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
+
+ asm volatile("": : : "memory");
+
+ {
+ uint32_t idlecnt = 0;
+ uint32_t sqn;
+ do {
+ sqn = *FPGA_LX_MASTER_RX_DDIV;
+ idlecnt++;
+ } while (sqn == last_rx_done_sqn);
+ pxmcc_data.common.act_idle = idlecnt;
+ if ((idlecnt < pxmcc_data.common.min_idle) &&
+ last_rx_done_sqn) {
+ pxmcc_data.common.min_idle = idlecnt;
+ }
+ last_rx_done_sqn = sqn;
+ pxmcc_data.common.rx_done_sqn = last_rx_done_sqn;
+ asm volatile("": : : "memory");
+ }
+
+ {
+ int i;
+ pxmcc_curadc_data_t *curadc = pxmcc_data.curadc;
+ volatile uint32_t *siroladc = FPGA_LX_MASTER_RX_ADC0;
+ uint32_t val;
+
+ for (i = 0; i < PXMCC_CURADC_CHANNELS; ) {
+ val = *siroladc;
+
+ curadc->cur_val = (uint16_t)(val - curadc->siroladc_last) -
+ curadc->siroladc_offs;
+ curadc->siroladc_last = val;
+
+ i++;
+ curadc += 1;
+ siroladc += 2;
+ /* additional 3 required for 7 -> 8 change */
+ if (!(i & 7))
+ 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 pwm1;
+ uint32_t pwm2;
+ uint32_t pwm3;
+ int32_t cur1;
+ int32_t cur2;
+ int32_t cur3;
+ int32_t *pcurmult;
+ 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
+ phs = pxmcc->ptphs;
+ #endif /*COMPUTE_PHASE_SECTOR*/
+ #endif /*COMPUTE_PHASE_SECTOR*/
+
+ out_info = pxmcc->out_info;
+ if (pxmcc->mode == PXMCC_MODE_BLDC) {
+
+ 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
+ if (pwm2 < pwm3)
+ phs = 3;
+ else if (pwm1 < pwm3)
+ phs = 2;
+ else
+ phs = 1;
+ #endif /*COMPUTE_PHASE_SECTOR*/
+
+ curmult_idx = (0x00201201 >> (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;
+
+ /*
+ * Translate index from pwm1, pwm2, pwm3 order to
+ * to order of current sources 0->2 1->0 2->1
+ *
+ * This solution modifies directly value in pxmcc_curadc_data_t
+ * so it is destructive and has not to be applied twice,
+ * but it is much better optimized
+ */
+ curmult_idx = (0x102 >> (curmult_idx * 4)) & 3;
+ pcurmult = &pxmcc_data.curadc[out_info + curmult_idx].cur_val;
+ *pcurmult = (int32_t)(pwm_reci * (*pcurmult)) >> 16;
+
+ cur2 = pxmcc_data.curadc[out_info + 0].cur_val;
+ cur3 = pxmcc_data.curadc[out_info + 1].cur_val;
+ cur1 = pxmcc_data.curadc[out_info + 2].cur_val;
+
+ 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) 0
+ * 0 1 0 -> 2 (1, 2, 0) 2
+ * 0 0 0 -> 3 (2, 1, 0) 1
+ * 1 0 0 -> 4 (2, 0, 1) 0
+ * 1 0 1 -> 5 (0, 2, 1) 2
+ * 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;
+ uint32_t sz4idx = sizeof(*pxmcc->pwm_prew);
+ uint32_t sz4cur = sizeof(*pxmcc_data.curadc);
+ uint32_t curmult_idx2curadc;
+
+ #if 0
+ curmult_idx = (((u1gtu3 ^ u1gtu2) | 1) ^ u2gtu3 ^ u1gtu2) & 3;
+ pwm_reci = pxmcc_data.common.pwm_cycle - pxmcc->pwm_prew[curmult_idx];
+ #else
+ /* Variant where curmult_idx is directly computed as byte offset to uint32_t array */
+ curmult_idx = (((u1gtu3 ^ u1gtu2) | (1 * sz4idx)) ^ u2gtu3 ^ u1gtu2) & (3 * sz4idx);
+ pwm_reci = pxmcc_data.common.pwm_cycle - *offsbychar(pxmcc->pwm_prew, curmult_idx);
+ #endif
+
+ #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");
+ asm volatile("nop\n");
+ asm volatile("nop\n");
+ pwm_reci = *FPGA_FNCAPPROX_RECI;
+ pwm_reci >>= 16;
+ pwm_reci *= pxmcc_data.common.pwm_cycle;
+ pwm_reci >>= 30 - pwm_reci_bits;
+
+ /*
+ * Translate index from pwm1, pwm2, pwm3 order to
+ * to order of current sources 0->2 1->0 2->1
+ *
+ * This solution modifies directly value in pxmcc_curadc_data_t
+ * so it is destructive and has not to be applied twice,
+ * but it is much better optimized
+ */
+ #if 0
+ curmult_idx = (0x102 >> (curmult_idx * 4)) & 3;
+ pcurmult = &pxmcc_data.curadc[out_info + curmult_idx].cur_val;
+ #else
+ curmult_idx2curadc = ((2 * sz4cur) << (0 * sz4idx)) |
+ ((0 * sz4cur) << (1 * sz4idx)) |
+ ((1 * sz4cur) << (2 * sz4idx));
+ curmult_idx = (curmult_idx2curadc >> curmult_idx) & (sz4cur | (sz4cur << 1));
+ pcurmult = &pxmcc_data.curadc[out_info].cur_val;
+ pcurmult = offsbychar(pcurmult, curmult_idx);
+ #endif
+ *pcurmult = (int32_t)(pwm_reci * (*pcurmult)) >> 16;
+
+ cur2 = pxmcc_data.curadc[out_info + 0].cur_val;
+ cur3 = pxmcc_data.curadc[out_info + 1].cur_val;
+ cur1 = pxmcc_data.curadc[out_info + 2].cur_val;
+
+ 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[2];
+ uint32_t bet_sgn = (bet_pwm - 1) >> 31;
+ int32_t alp_pwm = pxmcc->pwm_prew[0];
+ uint32_t alp_sgn = (alp_pwm - 1) >> 31;
+ cur_bet = (pxmcc_data.curadc[out_info + 3].cur_val & ~bet_sgn) -
+ (pxmcc_data.curadc[out_info + 2].cur_val & bet_sgn);
+ cur_alp = (pxmcc_data.curadc[out_info + 1].cur_val & ~alp_sgn) -
+ (pxmcc_data.curadc[out_info + 0].cur_val & alp_sgn);
+ }
+
+ cur_d = cur_alp * pxmcc->ptcos + cur_bet * pxmcc->ptsin;
+ cur_q = -cur_alp * pxmcc->ptsin + cur_bet * pxmcc->ptcos;
+
+ pxmcc->cur_dq = (cur_d & 0xffff0000) | ((cur_q >> 16) & 0xffff);
+
+ pxmcc->cur_d_cum = ((pxmcc->cur_d_cum + (cur_d >> 4)) & ~0x3f) |
+ (last_rx_done_sqn & 0x1f);
+ 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 / blobdiff