pxmc_rocon_vin_act = vin_act;
}
+unsigned int pxmc_rocon_rx_done_sqn;
+unsigned int pxmc_rocon_rx_done_sqn_inc;
+unsigned int pxmc_rocon_rx_done_sqn_misscnt;
+unsigned int pxmc_rocon_rx_done_sqn_missoffs;
+
+static inline
+void pxmc_rocon_rx_done_sqn_compute(void)
+{
+ uint32_t sqn_act;
+ uint32_t sqn_offs;
+ unsigned int sqn_expect = pxmc_rocon_rx_done_sqn + pxmc_rocon_rx_done_sqn_inc;
+
+ sqn_act = *fpga_lx_master_receiver_done_div;
+ sqn_offs = (sqn_act - sqn_expect) & 0x1f;
+ if (sqn_offs) {
+ if (pxmc_rocon_rx_done_sqn_missoffs != sqn_offs) {
+ pxmc_rocon_rx_done_sqn_misscnt = 1;
+ } else {
+ pxmc_rocon_rx_done_sqn_misscnt++;
+ if (pxmc_rocon_rx_done_sqn_misscnt >= 10)
+ sqn_expect += 1 - ((sqn_offs >> 3) & 2);
+ }
+ } else {
+ pxmc_rocon_rx_done_sqn_misscnt = 0;
+ }
+ pxmc_rocon_rx_done_sqn = sqn_expect;
+ pxmc_rocon_rx_done_sqn_missoffs = sqn_offs;
+}
+
const uint8_t onesin10bits[1024]={
0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,
1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
}
/**
- * pxmc_pxmcc_nofb_inp - Dummy input for direct stepper motor control
+ * pxmc_pxmcc_nofb2ph_inp - Dummy input for direct stepper motor control
* @mcs: Motion controller state information
*/
int
-pxmc_pxmcc_nofb_inp(pxmc_state_t *mcs)
+pxmc_pxmcc_nofb2ph_inp(pxmc_state_t *mcs)
{
+ volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+ uint32_t steps_pos_act = mcc_axis->steps_pos;
+ mcs->pxms_as = (int32_t)(steps_pos_act - mcs->pxms_ap);
+ mcs->pxms_ap += mcs->pxms_as;
return 0;
}
pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
pxmcc_axis_pwm_dq_out(mcs, 0, 0);
mcc_axis->steps_inc = 0;
+ mcc_axis->steps_inc_next = 0;
mcsrc->cur_d_err_sum = 0;
mcsrc->cur_q_err_sum = 0;
} else {
int cur_q_req, cur_q_err;
int max_pwm = mcs->pxms_me;
int32_t stpinc;
+ int32_t stpdiv;
pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw);
pxmcc_axis_cur_dq_raw2filt(&cur_q, cur_q_raw);
pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
- mcs->pxms_ap=mcs->pxms_rp;
- mcs->pxms_as=mcs->pxms_rs;
- mcc_axis->steps_lim = mcc_axis->steps_cnt + 6;
+ stpinc = mcs->pxms_rp - mcsrc->steps_pos_prev;
+
+ stpdiv = pxmc_rocon_rx_done_sqn_inc;
+ mcc_axis->steps_inc_next = (stpinc + stpdiv / 2) / stpdiv;
+ mcc_axis->steps_pos_next = mcsrc->steps_pos_prev;
- stpinc = mcs->pxms_rs;
- mcc_axis->steps_inc = stpinc;
+ mcsrc->steps_pos_prev = mcs->pxms_rp;
- /* stpinc /= (mcs->pxms_ptirc << PXMC_SUBDIV(mcs)); */
- /* pxms_ptscale_mult; pxms_ptscale_shift; */
+ mcc_axis->steps_sqn_next = pxmc_rocon_rx_done_sqn +
+ pxmc_rocon_rx_done_sqn_inc - 1;
}
return 0;
{
volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+ pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
uint32_t ptirc;
uint32_t ptreci;
uint32_t inp_info;
mcc_data->common.pwm_cycle = PXMC_LXPWR_PWM_CYCLE;
ptirc = mcs->pxms_ptirc;
+ if (mode == PXMCC_MODE_STEPPER)
+ ptirc <<= PXMC_SUBDIV(mcs);
+
ull = (1ULL << 32) * mcs->pxms_ptper;
ptreci = (ull + ptirc / 2) / ptirc;
break;
case PXMCC_MODE_STEPPER:
phcnt = 4;
- mcc_axis->ptreci = 1;
inp_info = (char*)&mcc_axis->steps_pos - (char*)mcc_data;
break;
}
mcc_axis->ccflg = 0;
mcc_axis->pwm_dq = 0;
- mcc_axis->steps_lim = mcc_axis->steps_cnt;
- mcc_axis->steps_inc = 0;
- mcc_axis->steps_pos = 0;
- if (mode != PXMCC_MODE_STEPPER) {
+ if (mode == PXMCC_MODE_STEPPER) {
+ mcsrc->steps_pos_prev = mcs->pxms_rp = mcs->pxms_ap;
+ mcs->pxms_rs = mcs->pxms_as = 0;
+ mcc_axis->steps_inc_next = 0;
+ mcc_axis->steps_pos_next = mcsrc->steps_pos_prev;
+ mcc_axis->steps_inc = 0;
+ mcc_axis->steps_pos = mcsrc->steps_pos_prev;
+ mcc_axis->ptirc = mcs->pxms_ptirc << PXMC_SUBDIV(mcs);
+ } else {
pxmcc_pxmc_ptofs2mcc(mcs, 1);
}
return 0;
hal_gpio_set_value(T2MAT1_PIN, 0);
hal_gpio_set_value(T2MAT0_PIN, 0);
+ pxmc_rocon_rx_done_sqn_compute();
pxmc_rocon_vin_compute();
if (pxmc_rocon_rx_data_hist_buff >= pxmc_rocon_rx_data_hist_buff_end)
*fpga_lx_master_transmitter_reg = 0;
*fpga_lx_master_transmitter_cycle = PXMC_LXPWR_PWM_CYCLE; /* 50 MHz -> 20 kHz */
*fpga_lx_master_receiver_done_div = receiver_done_div << 8;
+ pxmc_rocon_rx_done_sqn_inc = receiver_done_div;
for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
fpga_lx_master_receiver_base[i] = 0;
if (res < 0)
return -1;
- if (mcs->pxms_do_inp == pxmc_pxmcc_nofb_inp)
+ if (mcs->pxms_do_inp == pxmc_pxmcc_nofb2ph_inp)
mcs->pxms_do_inp = pxmc_inp_rocon_inp;
if (mcs->pxms_do_con == pxmc_pxmcc_nofb_con)
mcs->pxms_do_con = pxmc_pid_con;
if (pxmcc_axis_setup(mcs, PXMCC_MODE_STEPPER) < 0)
return -1;
pxmcc_axis_enable(mcs, 1);
- mcs->pxms_do_inp = pxmc_pxmcc_nofb_inp;
+ mcs->pxms_do_inp = pxmc_pxmcc_nofb2ph_inp;
mcs->pxms_do_con = pxmc_pxmcc_nofb_con;
mcs->pxms_do_out = pxmc_pxmcc_nofb2ph_out;
break;