X-Git-Url: http://rtime.felk.cvut.cz/gitweb/fpga/lx-cpu1/lx-rocon.git/blobdiff_plain/3fc92d46ab6381eaa3cc936d066eae73f8c5a7b7..622f5a3f8ab61cf4e625b1667b96ed3f1b58be1a:/sw/app/rocon/appl_pxmc.c diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c index 77f16de..1f9287f 100644 --- a/sw/app/rocon/appl_pxmc.c +++ b/sw/app/rocon/appl_pxmc.c @@ -33,6 +33,9 @@ #include "appl_defs.h" #include "appl_fpga.h" #include "pxmcc_types.h" +#include "pxmcc_interface.h" + +#define PXMC_AXIS_MODE_BLDC_PXMCC (PXMC_AXIS_MODE_BLDC + 1) int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc, unsigned long index_irc, int diff2err); @@ -55,7 +58,9 @@ int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc, #define LX_MASTER_DATA_OFFS 8 -unsigned pxmc_rocon_pwm_magnitude = 2500; +#define PXMC_LXPWR_PWM_CYCLE 2500 + +unsigned pxmc_rocon_pwm_magnitude = PXMC_LXPWR_PWM_CYCLE; long pxmc_rocon_irc_offset[PXML_MAIN_CNT]; unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT]; @@ -707,6 +712,187 @@ pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs) return 0; } +/*******************************************************************/ +/* PXMCC - PXMC coprocessor support and communication */ + +int +pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs) +{ + if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) || + (mcs->pxms_flg & PXMS_PRA_m)) { + short ptindx; + short ptirc = mcs->pxms_ptirc; + short divisor = mcs->pxms_ptper * 6; + unsigned char hal_pos; + + hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bldc_hal_rd(mcs)]; + + if (hal_pos == 0xff) { + if (mcs->pxms_ene) + pxmc_rocon_process_hal_error(mcs); + } else { + if (mcs->pxms_halerc) + mcs->pxms_halerc--; + + ptindx = (hal_pos * ptirc + divisor / 2) / divisor; + + if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m)) { + int set_ptofs_fl = 0; + + if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1) { + short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;; + + if ((ptindx > ptindx_prev + ptirc / 2) || + (ptindx_prev > ptindx + ptirc / 2)) { + ptindx = (ptindx_prev + ptindx - ptirc) / 2; + + if (ptindx < 0) + ptindx += ptirc; + } else { + ptindx = (ptindx_prev + ptindx) / 2; + } + + set_ptofs_fl = 1; + + pxmc_set_flag(mcs, PXMS_PTI_b); + pxmc_clear_flag(mcs, PXMS_PRA_b); + } else { + if (!(mcs->pxms_flg & PXMS_PTI_m)) + set_ptofs_fl = 1; + } + if (set_ptofs_fl) { + volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs); + uint32_t ptofs; + uint32_t irc; + + mcs->pxms_ptindx = ptindx; + mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx; + + irc = fpga_irc[mcs->pxms_inp_info]->count; + ptofs = (int16_t)(mcs->pxms_ptofs - irc) + irc; + mcc_axis->ptofs = ptofs; + } + } else { + /* if phase table position to mask is know do fine phase table alignment */ + if (mcs->pxms_cfg & PXMS_CFG_I2PT_m) { + int res; + + res = pxmc_inp_rocon_ptofs_from_index_poll(mcs, 0); + if (res < 0) { + pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG); + } else if (res) { + pxmc_set_flag(mcs, PXMS_PTI_b); + pxmc_set_flag(mcs, PXMS_PHA_b); + } + } + } + mcs->pxms_hal = hal_pos; + } + } + + { + /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/ + /* FIXME - check winding current against limit */ + /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */ + } + + { + int ene, pwm_d, pwm_q; + + ene = mcs->pxms_ene; + pwm_d = 0; + pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15; + + pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q); + + if (mcs->pxms_flg & PXMS_ERR_m) + pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0); + } + + return 0; +} + + +int pxmcc_axis_setup(pxmc_state_t *mcs, int mode) +{ + volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data(); + volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs); + uint32_t ptofs; + uint32_t irc; + uint32_t ptirc; + uint32_t ptreci; + uint32_t pwmtx_info; + uint32_t pwmtx_info_dummy = 27; + uint64_t ull; + int i; + int phcnt = 0; + int pwm_chan; + + if (mcc_axis == NULL) + return -1; + + mcc_axis->ccflg = 0; + mcc_axis->mode = PXMCC_MODE_IDLE; + + mcc_data->common.pwm_cycle = PXMC_LXPWR_PWM_CYCLE; + + irc = fpga_irc[mcs->pxms_inp_info]->count; + ptofs = (int16_t)(mcs->pxms_ptofs - irc) + irc; + + ptirc = mcs->pxms_ptirc; + ull = (1ULL << 32) * mcs->pxms_ptper; + ptreci = (ull + ptirc / 2) / ptirc; + + mcc_axis->ptirc = ptirc; + mcc_axis->ptreci = ptreci; + mcc_axis->ptofs = ptofs; + + switch (mode) { + case PXMCC_MODE_IDLE: + phcnt = 0; + break; + case PXMCC_MODE_MODE_BLDC: + phcnt = 3; + break; + case PXMCC_MODE_STEPPER_WITH_IRC: + phcnt = 4; + break; + } + + mcc_axis->inp_info = mcs->pxms_inp_info; + mcc_axis->out_info = mcs->pxms_out_info; + + pwm_chan = mcs->pxms_out_info; + + pwmtx_info = (pwmtx_info_dummy << 0) | (pwmtx_info_dummy << 8) | + (pwmtx_info_dummy << 16) | (pwmtx_info_dummy << 24); + + for (i = phcnt; --i >= 0; ) { + volatile uint32_t *pwm_reg; + volatile uint32_t *pwm_reg_base = fpga_lx_master_transmitter_base; + + pwmtx_info <<= 8; + + pwm_reg = pxmc_rocon_pwm_chan2reg(pwm_chan + i); + if (pwm_reg == &pxmc_rocon_pwm_dummy_reg) { + pwmtx_info |= pwmtx_info_dummy; + } else { + pwmtx_info |= pwm_reg - pwm_reg_base; + } + } + + mcc_axis->pwmtx_info = pwmtx_info; + + mcc_axis->mode = mode; + + mcc_axis->ccflg = 0; + mcc_axis->pwm_dq = 0; + + return 0; +} + +/*******************************************************************/ + volatile void *pxmc_rocon_rx_data_hist_buff; volatile void *pxmc_rocon_rx_data_hist_buff_end; int pxmc_rocon_rx_data_hist_mode; @@ -753,7 +939,7 @@ IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr) } else if (pxmc_rocon_rx_data_hist_mode == 1) { int i; uint32_t *pbuf = (uint32_t *)pxmc_rocon_rx_data_hist_buff; - pxmcc_data_t *mcc_data = (pxmcc_data_t *)fpga_tumbl_dmem; + pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data(); pxmcc_axis_data_t *mcc_axis = mcc_data->axis + 0; uint32_t *ptumbl = (uint32_t *)mcc_axis; @@ -841,7 +1027,7 @@ pxmc_rocon_pwm_master_init(void) *fpga_lx_master_reset = 1; *fpga_lx_master_transmitter_reg = 0; - *fpga_lx_master_transmitter_cycle = 2500; /* 50 MHz -> 20 kHz */ + *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */ *fpga_lx_master_receiver_done_div = receiver_done_div << 8; for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++) @@ -869,7 +1055,7 @@ pxmc_rocon_pwm_master_init(void) fpga_lx_master_transmitter_base[grp_out++] = 0x0000; *fpga_lx_master_reset = 0; - *fpga_lx_master_transmitter_cycle = 2500; /* 50 MHz -> 20 kHz */ + *fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */ *fpga_lx_master_receiver_done_div = receiver_done_div << 8; return 0; @@ -1595,6 +1781,8 @@ int pxmc_axis_rdmode(pxmc_state_t *mcs) return PXMC_AXIS_MODE_DC; if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out) return PXMC_AXIS_MODE_BLDC; + if (mcs->pxms_do_out == pxmc_pxmcc_pwm3ph_out) + return PXMC_AXIS_MODE_BLDC_PXMCC; return -1; } @@ -1626,6 +1814,7 @@ pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode) mcs->pxms_ptscale_shift=15; break; case PXMC_AXIS_MODE_BLDC: + case PXMC_AXIS_MODE_BLDC_PXMCC: /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */ #ifndef PXMC_WITH_PT_ZIC res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0); @@ -1688,6 +1877,12 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode) case PXMC_AXIS_MODE_BLDC: mcs->pxms_do_out = pxmc_rocon_pwm3ph_out; break; + case PXMC_AXIS_MODE_BLDC_PXMCC: + if (pxmcc_axis_setup(mcs, PXMCC_MODE_MODE_BLDC) < 0) + return -1; + pxmcc_axis_enable(mcs, 1); + mcs->pxms_do_out = pxmc_pxmcc_pwm3ph_out; + break; default: return -1; }