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
/* 1 / (2 * k3) * 65536 */
#define RECI16_2_K3 37837
+#define offsbychar(_ptr, _offs) \
+ ((typeof(&(_ptr)[0]))((char*)(_ptr) + (_offs)))
+
pxmcc_data_t pxmcc_data;
void init_defvals(void)
uint32_t last_rx_done_sqn = 0;
pxmcc_axis_data_t *pxmcc;
- pxmcc_data.common.fwversion = PXMCC_FWVERSION;
pxmcc_data.common.pwm_cycle = 2500;
pxmcc_data.common.min_idle = 0x7fff;
- pxmcc = pxmcc_data.axis;
- pxmcc->ccflg = 0;
- pxmcc->ptindx = 0;
- /*pxmcc->ptofs = *FPGA_IRC0;*/
- pxmcc->ptirc = 1000;
- pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */
- pxmcc->pwm_dq = 0;
+ 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();
uint32_t pwmtx_info;
volatile uint32_t *uptr;
- irc = *(FPGA_IRC0 + (FPGA_IRC1 - FPGA_IRC0) * pxmcc->inp_info);
+ irc = *(uint32_t*)pxmcc->inp_info;
pti = irc - ofs;
- if ((uint32_t)pti >= per) {
+ /*
+ * 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 {
pta = pti * pxmcc->ptreci;
*FPGA_FNCAPPROX_SIN = pta;
- asm volatile("nop\n": : : "memory");
- asm volatile("nop\n": : : "memory");
- asm volatile("nop\n": : : "memory");
+ asm volatile("nop\n");
+ asm volatile("nop\n");
+ asm volatile("nop\n");
pxmcc->ptsin = *FPGA_FNCAPPROX_SIN;
- asm volatile("nop\n": : : "memory");
+ asm volatile("nop\n");
pxmcc->ptcos = *FPGA_FNCAPPROX_COS;
if (pxmcc->ccflg) {
pwm_alp = pwm_d * pxmcc->ptcos - pwm_q * pxmcc->ptsin;
pwm_bet = pwm_d * pxmcc->ptsin + pwm_q * pxmcc->ptcos;
- if (!pxmcc->mode) {
+ if (pxmcc->mode == PXMCC_MODE_BLDC) {
pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16);
#ifndef SUPPRESS_CONDITIONALS
*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;
+ 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);
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) {
+ 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;
int i;
pxmcc_curadc_data_t *curadc = pxmcc_data.curadc;
volatile uint32_t *siroladc = FPGA_LX_MASTER_RX_ADC0;
- uint16_t val;
+ uint32_t val;
for (i = 0; i < PXMCC_CURADC_CHANNELS; ) {
val = *siroladc;
- curadc->cur_val = val - curadc->siroladc_last
- - curadc->siroladc_offs;
+ curadc->cur_val = (uint16_t)(val - curadc->siroladc_last) -
+ curadc->siroladc_offs;
curadc->siroladc_last = val;
- curadc->reserved1=0x11aa;
i++;
curadc += 1;
uint32_t pwm1;
uint32_t pwm2;
uint32_t pwm3;
- int32_t cur[3];
int32_t cur1;
int32_t cur2;
int32_t cur3;
+ int32_t *pcurmult;
uint32_t curmult_idx;
uint32_t pwm_reci;
uint32_t out_info;
#endif /*COMPUTE_PHASE_SECTOR*/
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;
+ if (pxmcc->mode == PXMCC_MODE_BLDC) {
pwm1 = pxmcc->pwm_prew[0];
pwm2 = pxmcc->pwm_prew[1];
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;
- cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
- cur1 = cur[0];
- cur2 = cur[1];
- cur3 = cur[2];
+ /*
+ * 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);
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
#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");
+ 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;
- cur[curmult_idx] = (int32_t)(pwm_reci * cur[curmult_idx]) >> 16;
- cur1 = cur[0];
- cur2 = cur[1];
- cur3 = cur[2];
+ /*
+ * 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_bet *= CONST16_K3;
cur_bet >>= 16;
} else {
- int32_t bet_pwm = pxmcc->pwm_prew[1];
+ int32_t bet_pwm = pxmcc->pwm_prew[2];
uint32_t bet_sgn = (bet_pwm - 1) >> 31;
- int32_t alp_pwm = pxmcc->pwm_prew[3];
+ int32_t alp_pwm = pxmcc->pwm_prew[0];
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);
+ 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;
projects / fpga / lx-cpu1 / lx-rocon.git / blobdiff