/*******************************************************************
Motion and Robotic System (MARS) aplication components.
- appl_pxmc.c - position controller subsystem core generic
- and LP_MPW1 hardware specific support
+ appl_pxmc.c - position controller RoCoN hardware support
Copyright (C) 2001-2013 by Pavel Pisa - originator
pisa@cmp.felk.cvut.cz
#include "appl_defs.h"
#include "appl_fpga.h"
+#include "appl_pxmc.h"
#include "pxmcc_types.h"
#include "pxmcc_interface.h"
long pxmc_rocon_irc_offset[PXML_MAIN_CNT];
unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT];
+unsigned pxmc_rocon_lxpwr_chips = 0;
+
+static inline
+pxmc_rocon_state_t *pxmc_state2rocon_state(pxmc_state_t *mcs)
+{
+ pxmc_rocon_state_t *mcsrc;
+ #ifdef UL_CONTAINEROF
+ mcsrc = UL_CONTAINEROF(mcs, pxmc_rocon_state_t, base);
+ #else /*UL_CONTAINEROF*/
+ mcsrc = (pxmc_rocon_state_t*)((char*)mcs - __builtin_offsetof(pxmc_rocon_state_t, base));
+ #endif /*UL_CONTAINEROF*/
+ return mcsrc;
+}
+
+uint32_t pxmc_rocon_vin_adc_last;
+int pxmc_rocon_vin_act;
+int pxmc_rocon_vin_ofs = 20978;
+int pxmc_rocon_vin_mul = 32905;
+int pxmc_rocon_vin_shr = 14;
+
+static inline
+void pxmc_rocon_vin_compute(void)
+{
+ volatile uint32_t *vin_adc_reg;
+ uint32_t vin_adc;
+ int vin_act;
+
+ vin_adc_reg = fpga_lx_master_receiver_base;
+ vin_adc_reg += LX_MASTER_DATA_OFFS + 1 + 8 * 2;
+
+ vin_adc = *vin_adc_reg;
+
+ vin_act = (int16_t)(vin_adc - pxmc_rocon_vin_adc_last);
+ pxmc_rocon_vin_adc_last = vin_adc;
+
+ vin_act = (pxmc_rocon_vin_ofs - vin_act) * pxmc_rocon_vin_mul;
+ vin_act >>= pxmc_rocon_vin_shr;
+
+ 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;
+}
+
+uint32_t pxmc_rocon_rx_err_cnt_last;
+uint32_t pxmc_rocon_rx_err_level;
+uint32_t pxmc_rocon_mcc_rx_done_sqn_last;
+uint32_t pxmc_rocon_mcc_stuck;
+
+static inline
+void pxmc_rocon_rx_error_check(void)
+{
+ uint32_t cnt;
+ uint32_t mcc_sqn;
+ pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
+
+ cnt = mcc_data->common.rx_err_cnt;
+ pxmc_rocon_rx_err_level = cnt - pxmc_rocon_rx_err_cnt_last;
+ pxmc_rocon_rx_err_cnt_last = cnt;
+
+ mcc_sqn = mcc_data->common.rx_done_sqn;
+ pxmc_rocon_mcc_stuck = mcc_sqn == pxmc_rocon_mcc_rx_done_sqn_last? 1: 0;
+ pxmc_rocon_mcc_rx_done_sqn_last = mcc_sqn;
+}
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,
return pwm_reg;
}
+int pxmc_rocon_pwm_direct_wr(unsigned chan, unsigned pwm, int en)
+{
+ volatile uint32_t *pwm_reg;
+ pwm_reg = pxmc_rocon_pwm_chan2reg(chan);
+
+ if (pwm_reg == &pxmc_rocon_pwm_dummy_reg)
+ return -1;
+
+ *pwm_reg = pwm | (en? 0x4000: 0x8000);
+
+ return 0;
+}
+
/**
* pxmc_rocon_pwm3ph_wr - Output of the 3-phase PWM to the hardware
* @mcs: Motion controller state information
}
}
+static inline
+void pxmcc_axis_get_cur_dq_filt_raw(pxmc_state_t *mcs,
+ int32_t *p_cur_d_raw, int32_t *p_cur_q_raw)
+{
+ volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
+ pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
+ uint32_t cur_d_cum = mcc_axis->cur_d_cum;
+ uint32_t cur_q_cum = mcc_axis->cur_q_cum;
+ int32_t cur_d;
+ int32_t cur_q;
+
+ cur_d = cur_d_cum - mcsrc->cur_d_cum_prev;
+ mcsrc->cur_d_cum_prev = cur_d_cum;
+ cur_q = cur_q_cum - mcsrc->cur_q_cum_prev;
+ mcsrc->cur_q_cum_prev = cur_q_cum;
+
+ *p_cur_d_raw = cur_d;
+ *p_cur_q_raw = cur_q;
+}
+
+static inline
+void pxmcc_axis_cur_dq_raw2filt(int *p_cur, int32_t cur_raw)
+{
+ int cur_div;
+ int32_t cur;
+ cur_div = cur_raw & 0x1f;
+ cur = cur_raw / cur_div;
+ cur += (1 << 11) | 0x20;
+ *p_cur = cur >> 12;
+}
+
+
+void pxmcc_axis_get_cur_dq_filt(pxmc_state_t *mcs, int *p_cur_d, int *p_cur_q)
+{
+ int32_t cur_d_raw;
+ int32_t cur_q_raw;
+
+ pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
+
+ pxmcc_axis_cur_dq_raw2filt(p_cur_d, cur_d_raw);
+ pxmcc_axis_cur_dq_raw2filt(p_cur_q, cur_q_raw);
+}
+
+static inline
+void pxmcc_cur_ctrl_pi(int *p_pwm, int32_t *p_cur_err_sum,
+ int cur_err, short cur_ctrl_p, short cur_ctrl_i, int max_pwm)
+{
+ int pwm;
+ int32_t cur_err_sum = *p_cur_err_sum;
+
+ pwm = (cur_err * cur_ctrl_p) >> 8;
+
+ if (cur_ctrl_i)
+ cur_err_sum += cur_err * cur_ctrl_i;
+ else
+ cur_err_sum = 0;
+
+ pwm += cur_err_sum >> 16;
+
+ if (pwm > max_pwm) {
+ cur_err_sum -= (pwm - max_pwm) << 16;
+ pwm = max_pwm;
+ } else if (-pwm > max_pwm) {
+ cur_err_sum -= (pwm + max_pwm) << 16;
+ pwm = 0 - max_pwm;
+ }
+ *p_cur_err_sum = cur_err_sum;
+ *p_pwm = pwm;
+}
+
int
pxmc_pxmcc_pwm3ph_out(pxmc_state_t *mcs)
{
+ int32_t cur_d_raw;
+ int32_t cur_q_raw;
+ pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
+
if (!(mcs->pxms_flg & PXMS_PTI_m) || !(mcs->pxms_flg & PXMS_PHA_m) ||
(mcs->pxms_flg & PXMS_PRA_m)) {
short ptindx;
}
}
+ pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
+
{
/*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
/* FIXME - check winding current against limit */
pwm_d = 0;
pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15;
+ if (mcs->pxms_flg & (PXMS_ENR_m | PXMS_ENO_m)) {
+ int cur_d;
+ int cur_d_req, cur_d_err;
+ int max_pwm = (pxmc_rocon_pwm_magnitude * mcs->pxms_me) >> 15;
+
+ cur_d_req = 0;
+
+ pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw);
+
+ cur_d_err = cur_d_req - cur_d;
+
+ pxmcc_cur_ctrl_pi(&pwm_d, &mcsrc->cur_d_err_sum, cur_d_err,
+ mcsrc->cur_d_p, mcsrc->cur_d_i, max_pwm);
+
+ if (pxmc_rocon_rx_err_level >= 2)
+ pxmc_set_errno(mcs, PXMS_E_WINDCURADC);
+ else if (pxmc_rocon_mcc_stuck)
+ pxmc_set_errno(mcs, PXMS_E_MCC_FAULT);
+ }
+
pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
if (mcs->pxms_flg & PXMS_ERR_m)
int
pxmc_pxmcc_pwm2ph_out(pxmc_state_t *mcs)
{
+ pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
+ int32_t cur_d_raw;
+ int32_t cur_q_raw;
+
+ pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
+
if(!(mcs->pxms_flg&PXMS_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) ||
(mcs->pxms_flg&PXMS_PRA_m)) {
ene = mcs->pxms_ene;
pwm_d = 0;
+
+ if (mcs->pxms_flg & PXMS_PHA_m &&
+ (mcs->pxms_flg & (PXMS_ENR_m | PXMS_ENO_m))) {
+ int cur_d;
+ int cur_d_req, cur_d_err;
+ int max_pwm = (pxmc_rocon_pwm_magnitude * mcs->pxms_me) >> 15;
+
+ cur_d_req = 0;
+
+ pxmcc_axis_cur_dq_raw2filt(&cur_d, cur_d_raw);
+
+ cur_d_err = cur_d_req - cur_d;
+
+ pxmcc_cur_ctrl_pi(&pwm_d, &mcsrc->cur_d_err_sum, cur_d_err,
+ mcsrc->cur_d_p, mcsrc->cur_d_i, max_pwm);
+
+ if (pxmc_rocon_rx_err_level >= 2)
+ pxmc_set_errno(mcs, PXMS_E_WINDCURADC);
+ else if (pxmc_rocon_mcc_stuck)
+ pxmc_set_errno(mcs, PXMS_E_MCC_FAULT);
+ }
+
pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15;
pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
}
/**
- * 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;
}
int
pxmc_pxmcc_nofb2ph_out(pxmc_state_t *mcs)
{
+ pxmc_rocon_state_t *mcsrc = pxmc_state2rocon_state(mcs);
volatile pxmcc_axis_data_t *mcc_axis = pxmc_rocon_mcs2pxmcc(mcs);
- {
- int ene, pwm_d, pwm_q;
+ int32_t cur_d_raw;
+ int32_t cur_q_raw;
- ene = mcs->pxms_ene;
- pwm_d = 0;
- pwm_q = (pxmc_rocon_pwm_magnitude * ene) >> 15;
+ pxmcc_axis_get_cur_dq_filt_raw(mcs, &cur_d_raw, &cur_q_raw);
+
+ if ((mcs->pxms_flg & PXMS_ERR_m) ||
+ !(mcs->pxms_flg & (PXMS_ENO_m | PXMS_ENR_m))) {
+ 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 pwm_d, pwm_q;
+ int cur_d, cur_q;
+ int cur_d_req, cur_d_err;
+ 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);
+
+ if (!mcs->pxms_ene)
+ cur_d_req = 0;
+ else
+ cur_d_req = mcsrc->cur_hold;
+
+ cur_d_err = cur_d_req - cur_d;
+
+ pxmcc_cur_ctrl_pi(&pwm_d, &mcsrc->cur_d_err_sum, cur_d_err,
+ mcsrc->cur_d_p, mcsrc->cur_d_i, max_pwm);
+
+ /* pwm_d = (pxmc_rocon_pwm_magnitude * ene) >> 15; */
+
+ cur_q_req = 0;
+
+ cur_q_err = cur_q_req - cur_q;
+
+ pxmcc_cur_ctrl_pi(&pwm_q, &mcsrc->cur_q_err_sum, cur_q_err,
+ mcsrc->cur_q_p, mcsrc->cur_q_i, max_pwm);
pxmcc_axis_pwm_dq_out(mcs, pwm_d, pwm_q);
- if (mcs->pxms_flg & PXMS_ERR_m) {
- pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
- } else {
- int32_t stpinc;
- 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;
- stpinc = mcs->pxms_rs;
- mcc_axis->steps_inc = stpinc;
+ 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_ptirc << PXMC_SUBDIV(mcs)); */
- /* pxms_ptscale_mult; pxms_ptscale_shift; */
- }
+ mcsrc->steps_pos_prev = mcs->pxms_rp;
+
+ mcc_axis->steps_sqn_next = pxmc_rocon_rx_done_sqn +
+ pxmc_rocon_rx_done_sqn_inc - 1;
+
+ if (pxmc_rocon_rx_err_level >= 2)
+ pxmc_set_errno(mcs, PXMS_E_WINDCURADC);
+ else if (pxmc_rocon_mcc_stuck)
+ pxmc_set_errno(mcs, PXMS_E_MCC_FAULT);
}
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;
}
+int pxmcc_curadc_zero(int wait)
+{
+ int chan;
+ unsigned try = wait? 200: 0;
+ volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
+ volatile pxmcc_curadc_data_t *curadc;
+
+ for (chan = 0; chan < PXMCC_CURADC_CHANNELS; chan++)
+ pxmc_rocon_pwm_direct_wr(chan, 0, 0);
+
+ do {
+ if (mcc_data->common.fwversion == PXMCC_FWVERSION)
+ break;
+ if (!try--)
+ return -1;
+ } while(1);
+
+ if (wait) {
+ if (pxmc_rocon_wait_rx_done() < 0)
+ return -1;
+
+ if (pxmc_rocon_wait_rx_done() < 0)
+ return -1;
+ }
+
+ for (chan = 0; chan < PXMCC_CURADC_CHANNELS; chan++) {
+ curadc = mcc_data->curadc + chan;
+ curadc->siroladc_offs += curadc->cur_val;
+ }
+
+ return 0;
+}
+
/*******************************************************************/
volatile void *pxmc_rocon_rx_data_hist_buff;
hal_gpio_set_value(T2MAT1_PIN, 0);
hal_gpio_set_value(T2MAT0_PIN, 0);
+ pxmc_rocon_rx_done_sqn_compute();
+ pxmc_rocon_vin_compute();
+ pxmc_rocon_rx_error_check();
+
if (pxmc_rocon_rx_data_hist_buff >= pxmc_rocon_rx_data_hist_buff_end)
pxmc_rocon_rx_data_hist_buff = NULL;
*(pbuf++) = *(ptumbl++);
pxmc_rocon_rx_data_hist_buff = pbuf;
+ } else if (!((pxmc_rocon_rx_data_hist_mode & 0xf8) ^ 0x10)) {
+ uint32_t *pbuf = (uint32_t *)pxmc_rocon_rx_data_hist_buff;
+ volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
+ volatile pxmcc_curadc_data_t *curadc;
+ pxmc_rocon_state_t *mcsrc = NULL;
+ int chan = pxmc_rocon_rx_data_hist_mode & 7;
+ if (chan < pxmc_main_list.pxml_cnt)
+ mcsrc = pxmc_state2rocon_state(pxmc_main_list.pxml_arr[chan]);
+ if (mcsrc) {
+ *(pbuf++) = pxmc_rocon_vin_act;
+ chan = mcsrc->base.pxms_inp_info;
+ *(pbuf++) = fpga_irc[chan]->count + pxmc_rocon_irc_offset[chan];;
+ *(pbuf++) = mcsrc->base.pxms_rp >> PXMC_SUBDIV(&mcsrc->base);
+ *(pbuf++) = mcsrc->base.pxms_ene;
+ *(pbuf++) = mcsrc->base.pxms_foi;
+ chan = mcsrc->base.pxms_out_info;
+ curadc = mcc_data->curadc + chan;
+ *(pbuf++) = *pxmc_rocon_pwm_chan2reg(chan++);
+ *(pbuf++) = (curadc++)->cur_val;
+ *(pbuf++) = *pxmc_rocon_pwm_chan2reg(chan++);
+ *(pbuf++) = (curadc++)->cur_val;
+ *(pbuf++) = *pxmc_rocon_pwm_chan2reg(chan++);
+ *(pbuf++) = (curadc++)->cur_val;
+ *(pbuf++) = *pxmc_rocon_pwm_chan2reg(chan++);
+ *(pbuf++) = (curadc++)->cur_val;
+ pxmc_rocon_rx_data_hist_buff = pbuf;
+ }
}
}
}
int
-pxmc_rocon_pwm_master_init(void)
+pxmc_rocon_pwm_master_setup(unsigned lxpwr_chips)
{
int i;
int grp_in = 0;
int grp_out = 0;
unsigned word_slot;
unsigned receiver_done_div = 1;
+ unsigned lxpwr_chips_max = 2;
#ifdef LXPWR_WITH_SIROLADC
unsigned lxpwr_header = 1;
unsigned lxpwr_words = 1 + 8 * 2 + 2;
- unsigned lxpwr_chips = 2;
unsigned lxpwr_chip_pwm_cnt = 8;
#else /*LXPWR_WITH_SIROLADC*/
unsigned lxpwr_header = 0;
unsigned lxpwr_words = 8;
- unsigned lxpwr_chips = 2;
unsigned lxpwr_chip_pwm_cnt = 8;
#endif /*LXPWR_WITH_SIROLADC*/
*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++)
+ if (lxpwr_chips > lxpwr_chips_max)
+ return -1;
+
+ for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips_max; i++)
fpga_lx_master_receiver_base[i] = 0;
+ if (lxpwr_chips >= 2) {
+ word_slot = LX_MASTER_DATA_OFFS + lxpwr_words;
+ fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
+ }
+
word_slot = LX_MASTER_DATA_OFFS;
fpga_lx_master_receiver_base[grp_in++] = (word_slot << 8) | lxpwr_words;
+
fpga_lx_master_receiver_base[grp_in++] = 0x0000;
- for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
+ for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips_max; i++)
fpga_lx_master_transmitter_base[i] = 0;
word_slot = LX_MASTER_DATA_OFFS + lxpwr_header + lxpwr_chip_pwm_cnt;
return 0;
}
+int
+pxmc_rocon_wait_rx_done(void)
+{
+ uint32_t sqn_last;
+ uint32_t sqn_act;
+ uint32_t timeout = 10000;
+
+ sqn_last = *fpga_lx_master_receiver_done_div;
+ sqn_last = sqn_last & 0x1f;
+
+ do {
+ sqn_act = *fpga_lx_master_receiver_done_div;
+ sqn_act = sqn_act & 0x1f;
+ if (sqn_act != sqn_last)
+ return 0;
+ } while(timeout--);
+
+ return -1;
+}
+
+int
+pxmc_rocon_pwm_master_init(void)
+{
+ int res;
+ volatile uint32_t *lxpwr_header_ptr;
+ unsigned lxpwr_words = 1 + 8 * 2 + 2;
+
+ pxmc_rocon_lxpwr_chips = 0;
+
+ res = pxmc_rocon_pwm_master_setup(2);
+ if (res < 0)
+ return 0;
+
+ if (pxmc_rocon_wait_rx_done() < 0)
+ return -1;
+ if (pxmc_rocon_wait_rx_done() < 0)
+ return -1;
+
+ lxpwr_header_ptr = fpga_lx_master_receiver_base;
+ lxpwr_header_ptr += LX_MASTER_DATA_OFFS;
+
+ if (lxpwr_header_ptr[0] == 0xb100 + lxpwr_words - 1) {
+ if (lxpwr_header_ptr[lxpwr_words] == 0xb100 + lxpwr_words - 1) {
+ pxmc_rocon_lxpwr_chips = 2;
+ return 2;
+ }
+ return -1;
+ }
+
+ if (lxpwr_header_ptr[lxpwr_words] != 0xb100 + lxpwr_words - 1) {
+ return -1;
+ }
+
+ res = pxmc_rocon_pwm_master_setup(1);
+ if (res < 0)
+ return 0;
+
+ if (pxmc_rocon_wait_rx_done() < 0)
+ return -1;
+ if (pxmc_rocon_wait_rx_done() < 0)
+ return -1;
+
+ if (lxpwr_header_ptr[0] != 0xb100 + lxpwr_words - 1)
+ return -1;
+
+ pxmc_rocon_lxpwr_chips = 1;
+
+ return 1;
+}
+
int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
unsigned long index_irc, int diff2err)
{
return pxmc_rocon_hh_gi;
}
-pxmc_state_t mcs0 =
+pxmc_rocon_state_t mcs0 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
+},
+ .cur_d_p = 150,
+ .cur_d_i = 6000,
+ .cur_q_p = 150,
+ .cur_q_i = 6000,
+ .cur_hold = 200,
};
-pxmc_state_t mcs1 =
+pxmc_rocon_state_t mcs1 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs2 =
+pxmc_rocon_state_t mcs2 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs3 =
+pxmc_rocon_state_t mcs3 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs4 =
+pxmc_rocon_state_t mcs4 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs5 =
+pxmc_rocon_state_t mcs5 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs6 =
+pxmc_rocon_state_t mcs6 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
+}};
-pxmc_state_t mcs7 =
+pxmc_rocon_state_t mcs7 =
{
+.base = {
pxms_flg:
PXMS_ENI_m,
pxms_do_inp:
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
-};
-
+}};
pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
-{&mcs0, &mcs1, &mcs2, &mcs3, &mcs4, &mcs5, &mcs6, &mcs7};
+{&mcs0.base, &mcs1.base, &mcs2.base, &mcs3.base,
+ &mcs4.base, &mcs5.base, &mcs6.base, &mcs7.base};
pxmc_state_list_t pxmc_main_list =
return res;
}
+int pxmc_axis_out_chans4mode(int mode)
+{
+ switch (mode) {
+ case PXMC_AXIS_MODE_DC:
+ return 2;
+ case PXMC_AXIS_MODE_BLDC:
+ case PXMC_AXIS_MODE_BLDC_PXMCC:
+ return 3;
+ case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
+ case PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC:
+ case PXMC_AXIS_MODE_STEPPER_PXMCC:
+ return 4;
+ }
+ return -1;
+}
+
int pxmc_axis_rdmode(pxmc_state_t *mcs)
{
if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out)
return -1;
}
-
int
pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
{
int res;
int prev_mode;
- pxmc_set_const_out(mcs, 0);
+ pxmc_axis_release(mcs);
pxmc_clear_flag(mcs, PXMS_ENI_b);
pxmc_clear_flags(mcs,PXMS_ENO_m);
/* Clear possible stall index flags from hardware */
mode = PXMC_AXIS_MODE_DC;
if ((prev_mode == PXMC_AXIS_MODE_BLDC_PXMCC) ||
- (prev_mode == PXMCC_MODE_STEPPER_WITH_IRC))
+ (prev_mode == PXMC_AXIS_MODE_STEPPER_WITH_IRC_PXMCC) ||
+ (prev_mode == PXMC_AXIS_MODE_STEPPER_PXMCC))
pxmcc_axis_setup(mcs, PXMCC_MODE_IDLE);
res = pxmc_axis_pt4mode(mcs, mode);
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;
}
+pxmc_call_t *const pxmc_reg_type_table[] = {
+ pxmc_pid_con,
+ pxmc_pid_con,
+ pxmc_pidnl_con
+};
+
+
+int pxmc_get_reg_type(pxmc_state_t *mcs)
+{
+ int reg_type;
+ int max_type = sizeof(pxmc_reg_type_table) / sizeof(*pxmc_reg_type_table);
+
+ for (reg_type = 1; reg_type < max_type; reg_type++)
+ if (mcs->pxms_do_con == pxmc_reg_type_table[reg_type])
+ return reg_type;
+ return 0;
+}
+
+int pxmc_set_reg_type(pxmc_state_t *mcs, int reg_type)
+{
+ int max_type = sizeof(pxmc_reg_type_table) / sizeof(*pxmc_reg_type_table);
+
+ if ((reg_type < 0) || (reg_type >= max_type))
+ return -1;
+ if (mcs->pxms_flg & PXMS_ENR_m)
+ return -1;
+
+ mcs->pxms_do_con = pxmc_reg_type_table[reg_type];
+ return 0;
+}
+
int pxmc_clear_power_stop(void)
{
return 0;
if (pbusy_bits != NULL)
*pbusy_bits = busy_bits;
- if (error_bits != NULL)
+ if (perror_bits != NULL)
*perror_bits = error_bits;
return flg;
pxmc_for_each_mcs(var, mcs)
{
- pxmc_set_const_out(mcs,0);
+ pxmc_axis_release(mcs);
}
pxmc_main_list.pxml_cnt = 0;
int res;
int i;
- pxmc_state_t *mcs = &mcs0;
+ pxmc_main_list.pxml_cnt = 0;
+ pxmc_dbg_hist = NULL;
+ #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
+ disable_irq(ROCON_RX_IRQn);
+ #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
+ __memory_barrier();
+
+ pxmc_state_t *mcs = &mcs0.base;
lpc_qei_state_t *qst = &lpc_qei_state;
*fpga_irc_reset = 1;
/*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
//pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
- pxmc_rocon_pwm_master_init();
+ res = pxmc_rocon_pwm_master_init();
+ if (res < 0)
+ return -1;
+
#ifdef PXMC_ROCON_TIMED_BY_RX_DONE
pxmc_rocon_rx_done_isr_setup(pxmc_rocon_rx_done_isr);
#endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
- pxmc_main_list.pxml_cnt = 0;
- pxmc_dbg_hist = NULL;
__memory_barrier();
pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
projects / fpga / lx-cpu1 / lx-rocon.git / blobdiff