Components for embedded applications builded for
laboratory and medical instruments firmware
- firmware.c - multi axis motion controller comprocesor
+ 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
*******************************************************************/
+#define SUPPRESS_CONDITIONALS 1
+#undef COMPUTE_PHASE_SECTOR
#include <stdint.h>
+#include "pxmcc_types.h"
#include "tumbl_addr.h"
/* k3 = math.sqrt(3) / 2 */
+#define CONST16_K3 56756
+
/* 1 / k3 * 65536 */
#define RECI16_K3 75674
/* 1 / (2 * k3) * 65536 */
#define RECI16_2_K3 37837
-typedef struct pxmcc_axis_data_t {
- uint32_t ccflg;
- int32_t pwm_d;
- int32_t pwm_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 */
- uint32_t ptofs; /* offset between table and IRC counter */
- 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;
- uint32_t rx_done_sqn;
-} pxmcc_axis_data_t;
-
-pxmcc_axis_data_t pxmcc_axis[1];
+#define offsbychar(_ptr, _offs) \
+ ((typeof(&(_ptr)[0]))((char*)(_ptr) + (_offs)))
+
+pxmcc_data_t pxmcc_data;
void init_defvals(void)
{
void main(void)
{
uint32_t last_rx_done_sqn = 0;
- pxmcc_axis_data_t *pxmcc = pxmcc_axis;
-
- pxmcc->ccflg = 0;
- pxmcc->ptindx = 0;
- pxmcc->ptofs = *FPGA_IRC0;
- pxmcc->ptirc = 1000;
- pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
- pxmcc->min_idle = 0;
- pxmcc->pwm_d = 0;
- pxmcc->pwm_q = 0;
+ pxmcc_axis_data_t *pxmcc;
+
+ 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;
+
+ 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);
asm volatile("": : : "memory");
+ pxmcc_data.common.fwversion = PXMCC_FWVERSION;
+ asm volatile("": : : "memory");
#if 0
find_sin_lat();
#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;
+ 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 = *(uint32_t*)pxmcc->inp_info;
+
+ 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 {
- ofs += per;
+ 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;
+ }
}
- pti = irc - ofs;
- pxmcc->ptofs = ofs;
- }
- pxmcc->ptindx = pti;
+ pxmcc++;
- pta = pti * pxmcc->ptreci;
+ } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
- *FPGA_FNCAPPROX_SIN = pta;
+ asm volatile("": : : "memory");
- 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;
+ {
+ 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");
+ }
- if (pxmcc->ccflg) {
- int32_t pwm_alp;
- int32_t pwm_bet;
- int32_t pwm_bet_div_2_k3;
- uint32_t phs;
+ {
+ int i;
+ pxmcc_curadc_data_t *curadc = pxmcc_data.curadc;
+ volatile uint32_t *siroladc = FPGA_LX_MASTER_RX_ADC0;
+ uint32_t val;
+
+ if (!(last_rx_done_sqn & 0x8000)) {
+ 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;
+ }
+ } else {
+ pxmcc_curadc_data_t *curadc = pxmcc_data.curadc;
+
+ for (i = 0; i < PXMCC_CURADC_CHANNELS; ) {
+ curadc->siroladc_last += curadc->cur_val;
+ i++;
+ curadc += 1;
+ }
+ pxmcc_data.common.rx_err_cnt++;
+ }
+ }
+
+ 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;
-
- 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);
-
- 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
+ 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
- /* -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)
+ else if (pwm1 > pwm3)
phs = 5;
else
phs = 4;
else
- /* pwm_bet > 2 * k3 * u_alp */
- if (pwm_bet_div_2_k3 > pwm_alp)
+ if (pwm2 < pwm3)
phs = 3;
+ else if (pwm1 < pwm3)
+ phs = 2;
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;
+ 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 {
- /* 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;
+ 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);
}
- pxmcc->ptphs = phs;
+ cur_d = cur_alp * pxmcc->ptcos + cur_bet * pxmcc->ptsin;
+ cur_q = -cur_alp * pxmcc->ptsin + cur_bet * pxmcc->ptcos;
- *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000;
- *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000;
- *FPGA_LX_MASTER_TX_PWM2 = pwm1 | 0x4000;
- }
+ pxmcc->cur_dq = (cur_d & 0xffff0000) | ((cur_q >> 16) & 0xffff);
- asm volatile("": : : "memory");
+ 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);
- {
- uint32_t idlecnt = 0;
- uint32_t sqn;
- do {
- sqn = *FPGA_LX_MASTER_RX_DDIV;
- idlecnt++;
- } while (sqn == last_rx_done_sqn);
- pxmcc->act_idle = idlecnt;
- if (((idlecnt < pxmcc->min_idle) ||
- (pxmcc->min_idle == 0)) &&
- last_rx_done_sqn) {
- pxmcc->min_idle = idlecnt;
- }
- last_rx_done_sqn = sqn;
- pxmcc->rx_done_sqn = last_rx_done_sqn;
- asm volatile("": : : "memory");
- }
+ pxmcc++;
+ } while(pxmcc != pxmcc_data.axis + PXMCC_AXIS_COUNT);
}
}
projects / fpga / lx-cpu1 / lx-rocon.git / blobdiff