From 3d47f5c6fc1ee3ac3d3b31830ef015b05903f7be Mon Sep 17 00:00:00 2001 From: Pavel Pisa Date: Mon, 5 Jan 2015 22:57:21 +0100 Subject: [PATCH] RoCoN and TUMBL firmware for control of stepper motor with IRC sensor. Signed-off-by: Pavel Pisa --- hw/lx-rocon_firmware/firmware.c | 33 +++++++------- hw/lx-rocon_firmware/pxmcc_types.h | 2 +- sw/app/rocon/appl_pxmc.c | 73 +++++++++++++++++++++++------- sw/app/rocon/appl_tests.c | 12 +++-- sw/app/rocon/pxmcc_interface.h | 2 + 5 files changed, 83 insertions(+), 39 deletions(-) diff --git a/hw/lx-rocon_firmware/firmware.c b/hw/lx-rocon_firmware/firmware.c index 86a6076..0d8bfec 100644 --- a/hw/lx-rocon_firmware/firmware.c +++ b/hw/lx-rocon_firmware/firmware.c @@ -87,6 +87,7 @@ void main(void) 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 { @@ -162,7 +163,7 @@ void main(void) 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_MODE_BLDC) { + if (pxmcc->mode == PXMCC_MODE_BLDC) { pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16); #ifndef SUPPRESS_CONDITIONALS @@ -279,14 +280,14 @@ void main(void) *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); @@ -303,7 +304,7 @@ void main(void) *uptr = pwm4 | 0x4000; } } else { - if (pxmcc->mode == PXMCC_MODE_MODE_BLDC) { + 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; @@ -392,7 +393,7 @@ void main(void) #endif /*COMPUTE_PHASE_SECTOR*/ out_info = pxmcc->out_info; - if (pxmcc->mode == PXMCC_MODE_MODE_BLDC) { + if (pxmcc->mode == PXMCC_MODE_BLDC) { pwm1 = pxmcc->pwm_prew[0]; pwm2 = pxmcc->pwm_prew[1]; @@ -534,14 +535,14 @@ void main(void) 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; diff --git a/hw/lx-rocon_firmware/pxmcc_types.h b/hw/lx-rocon_firmware/pxmcc_types.h index 2e7856a..c09a18d 100644 --- a/hw/lx-rocon_firmware/pxmcc_types.h +++ b/hw/lx-rocon_firmware/pxmcc_types.h @@ -25,7 +25,7 @@ #define PXMCC_CURADC_CHANNELS 16 #define PXMCC_MODE_IDLE 2 -#define PXMCC_MODE_MODE_BLDC 0 +#define PXMCC_MODE_BLDC 0 #define PXMCC_MODE_STEPPER_WITH_IRC 1 typedef struct pxmcc_common_data_t { diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c index 1f9287f..f49f5e1 100644 --- a/sw/app/rocon/appl_pxmc.c +++ b/sw/app/rocon/appl_pxmc.c @@ -35,7 +35,8 @@ #include "pxmcc_types.h" #include "pxmcc_interface.h" -#define PXMC_AXIS_MODE_BLDC_PXMCC (PXMC_AXIS_MODE_BLDC + 1) +#define PXMC_AXIS_MODE_BLDC_PXMCC (PXMC_AXIS_MODE_BLDC + 1) +#define PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC (PXMC_AXIS_MODE_BLDC + 2) int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc, unsigned long index_irc, int diff2err); @@ -715,6 +716,21 @@ pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs) /*******************************************************************/ /* PXMCC - PXMC coprocessor support and communication */ +void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs) +{ + volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs); + int inp_chan=mcs->pxms_inp_info; + uint32_t ptofs; + uint32_t irc; + + if (mcc_axis != NULL) { + ptofs = mcs->pxms_ptofs - pxmc_rocon_irc_offset[inp_chan]; + irc = fpga_irc[inp_chan]->count; + ptofs = (int16_t)(ptofs - irc) + irc; + mcc_axis->ptofs = ptofs; + } +} + int pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs) { @@ -761,16 +777,10 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs) 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; + pxmcc_pxmc_ptofs2mcc(mcs); } } else { /* if phase table position to mask is know do fine phase table alignment */ @@ -781,6 +791,7 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs) if (res < 0) { pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG); } else if (res) { + pxmcc_pxmc_ptofs2mcc(mcs); pxmc_set_flag(mcs, PXMS_PTI_b); pxmc_set_flag(mcs, PXMS_PHA_b); } @@ -812,13 +823,29 @@ pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs) return 0; } +int +pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs) +{ + { + 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_pwm2ph_wr(mcs, 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; @@ -836,22 +863,20 @@ int pxmcc_axis_setup(pxmc_state_t *mcs, int mode) 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; + + pxmcc_pxmc_ptofs2mcc(mcs); switch (mode) { case PXMCC_MODE_IDLE: phcnt = 0; break; - case PXMCC_MODE_MODE_BLDC: + case PXMCC_MODE_BLDC: phcnt = 3; break; case PXMCC_MODE_STEPPER_WITH_IRC: @@ -1801,6 +1826,7 @@ pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode) switch (mode) { /*case PXMC_AXIS_MODE_STEPPER:*/ case PXMC_AXIS_MODE_STEPPER_WITH_IRC: + case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC: res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0); break; /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/ @@ -1844,6 +1870,7 @@ int pxmc_axis_mode(pxmc_state_t *mcs, int mode) { int res; + int prev_mode; pxmc_set_const_out(mcs, 0); pxmc_clear_flag(mcs, PXMS_ENI_b); @@ -1854,13 +1881,19 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode) pxmc_clear_flag(mcs, PXMS_PTI_b); + prev_mode = pxmc_axis_rdmode(mcs); + if (mode == PXMC_AXIS_MODE_NOCHANGE) - mode = pxmc_axis_rdmode(mcs); + mode = prev_mode; if (mode < 0) return -1; if (!mode) mode = PXMC_AXIS_MODE_DC; + if ((prev_mode == PXMC_AXIS_MODE_BLDC_PXMCC) || + (prev_mode == PXMCC_MODE_STEPPER_WITH_IRC)) + pxmcc_axis_setup(mcs, PXMCC_MODE_IDLE); + res = pxmc_axis_pt4mode(mcs, mode); if (res < 0) return -1; @@ -1878,11 +1911,17 @@ pxmc_axis_mode(pxmc_state_t *mcs, int mode) 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) + if (pxmcc_axis_setup(mcs, PXMCC_MODE_BLDC) < 0) return -1; pxmcc_axis_enable(mcs, 1); mcs->pxms_do_out = pxmc_pxmcc_pwm3ph_out; break; + case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC: + if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER_WITH_IRC) < 0) + return -1; + pxmcc_axis_enable(mcs, 1); + mcs->pxms_do_out = pxmc_pxmcc_pwm2ph_out; + break; default: return -1; } diff --git a/sw/app/rocon/appl_tests.c b/sw/app/rocon/appl_tests.c index a8d977a..481be41 100644 --- a/sw/app/rocon/appl_tests.c +++ b/sw/app/rocon/appl_tests.c @@ -419,26 +419,28 @@ int cmd_do_testtumblefw(cmd_io_t *cmd_io, const struct cmd_des *des, char *param return -CMDERR_BADPAR; - pxmcc_axis_setup(mcs, PXMCC_MODE_MODE_BLDC); + if (0) { + pxmcc_axis_setup(mcs, PXMCC_MODE_MODE_BLDC); + } pxmc_clear_flags(mcs,PXMS_ENO_m|PXMS_ENG_m|PXMS_ENR_m|PXMS_BSY_m); pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q); pxmcc_axis_enable(mcs, 1); - if (1) { + if (0) { mcc_data->axis[1].inp_info = 1; mcc_data->axis[1].out_info = 3; mcc_data->axis[1].pwmtx_info = (12 << 0) | (13 << 8) | (14 << 16); - mcc_data->axis[1].mode = PXMCC_MODE_MODE_BLDC; + mcc_data->axis[1].mode = PXMCC_MODE_BLDC; mcc_data->axis[1].ccflg = 1; mcc_data->axis[2].inp_info = 2; mcc_data->axis[2].out_info = 6; mcc_data->axis[2].pwmtx_info = (15 << 0) | (16 << 8) | (18 << 16); - mcc_data->axis[2].mode = PXMCC_MODE_MODE_BLDC; + mcc_data->axis[2].mode = PXMCC_MODE_BLDC; mcc_data->axis[2].ccflg = 1; mcc_data->axis[3].inp_info = 3; mcc_data->axis[3].out_info = 9; mcc_data->axis[3].pwmtx_info = (19 << 0) | (20 << 8) | (21 << 16); - mcc_data->axis[3].mode = PXMCC_MODE_MODE_BLDC; + mcc_data->axis[3].mode = PXMCC_MODE_BLDC; mcc_data->axis[3].ccflg = 1; } diff --git a/sw/app/rocon/pxmcc_interface.h b/sw/app/rocon/pxmcc_interface.h index be0163d..e55ade4 100644 --- a/sw/app/rocon/pxmcc_interface.h +++ b/sw/app/rocon/pxmcc_interface.h @@ -52,6 +52,8 @@ void pxmcc_axis_enable(pxmc_state_t *mcs, int enable) mcc_axis->ccflg = enable? 1: 0; } +void pxmcc_pxmc_ptofs2mcc(pxmc_state_t *mcs); + int pxmcc_axis_setup(pxmc_state_t *mcs, int mode); #endif /*_PXMCC_INTERFACE_H_*/ -- 2.39.2