Copyright (C) 2001-2013 by Pavel Pisa - originator
pisa@cmp.felk.cvut.cz
(C) 2001-2013 by PiKRON Ltd. - originator
- http://www.pikron.com
+ http://www.pikron.com
This file can be used and copied according to next
license alternatives
#include <pxmc_lpc_qei.h>
#include <pxmc_internal.h>
#include <pxmc_inp_common.h>
+#include <pxmc_gen_info.h>
#include <hal_gpio.h>
#include <hal_machperiph.h>
#include <stdlib.h>
+#include <string.h>
+#include <LPC17xx.h>
+#include <lpcTIM.h>
#include "appl_defs.h"
+#include "appl_fpga.h"
-#define PXML_MAIN_CNT 4
+int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
+ unsigned long index_irc, int diff2err);
+
+#ifndef pxmc_fast_tick_time
+#define pxmc_fast_tick_time() (LPC_TIM0->TC)
+#endif
+
+#define PXML_MAIN_CNT 8
#define PXMC_WITH_PT_ZIC 1
#define PXMC_PT_ZIC_MASK 0x8000
#define HAL_ERR_SENSITIVITY 20
#define HAL_ERR_MAX_COUNT 5
-unsigned pxmc_lpcpwm1_magnitude;
-
-int pxmc_hh_gi(pxmc_state_t *mcs);
+#define LXPWR_WITH_SIROLADC 1
+
+#define LX_MASTER_DATA_OFFS 8
+
+unsigned pxmc_rocon_pwm_magnitude = 2500;
+
+long pxmc_rocon_irc_offset[PXML_MAIN_CNT];
+unsigned pxmc_rocon_mark_filt[PXML_MAIN_CNT];
+
+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,
+ 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,
+ 2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+ 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,
+ 2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+ 2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+ 3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+ 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,
+ 2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+ 2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+ 3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+ 2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+ 3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+ 3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+ 4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
+ 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,
+ 2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+ 2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+ 3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+ 2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+ 3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+ 3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+ 4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
+ 2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
+ 3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+ 3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+ 4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
+ 3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,
+ 4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
+ 4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8,5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,
+ 5,6,6,7,6,7,7,8,6,7,7,8,7,8,8,9,6,7,7,8,7,8,8,9,7,8,8,9,8,9,9,10
+};
int
pxmc_inp_rocon_inp(struct pxmc_state *mcs)
{
- /* int chan=mcs->pxms_inp_info */
+ int chan=mcs->pxms_inp_info;
long irc;
long pos;
- irc= 0; /* FIXME - read position from hardware */
- pos=irc<<PXMC_SUBDIV(mcs);
- mcs->pxms_as=pos-mcs->pxms_ap;
- mcs->pxms_ap=pos;
+
+ /* read position from hardware */
+ irc = fpga_irc[chan]->count;
+ irc += pxmc_rocon_irc_offset[chan];
+ pos = irc << PXMC_SUBDIV(mcs);
+ mcs->pxms_as = pos - mcs->pxms_ap;
+ mcs->pxms_ap = pos;
/* Running of the motor commutator */
- if(mcs->pxms_flg&PXMS_PTI_m)
- pxmc_irc_16bit_commindx(mcs,irc);
+ if (mcs->pxms_flg & PXMS_PTI_m)
+ pxmc_irc_16bit_commindx(mcs, irc);
return 0;
}
+int
+pxmc_inp_rocon_is_mark(pxmc_state_t *mcs)
+{
+ uint32_t irc_state;
+ int mark;
+ unsigned filt;
+ int chan=mcs->pxms_inp_info;
+
+ irc_state = *fpga_irc_state[chan];
+
+ mark = ((irc_state >> (ffs(FPGA_IRC_STATE_MARK_MASK) - 1)) ^
+ (mcs->pxms_cfg >> PXMS_CFG_HPS_b)) & 1;
+
+ filt = pxmc_rocon_mark_filt[chan];
+ filt = (filt << 1) | mark;
+ pxmc_rocon_mark_filt[chan] = filt;
+
+ return onesin10bits[filt & 0x03ff];
+}
+
+int
+pxmc_inp_rocon_is_index_edge(pxmc_state_t *mcs)
+{
+ uint32_t irc_state;
+ int index;
+ int chan=mcs->pxms_inp_info;
+
+ irc_state = *fpga_irc_state[chan];
+ *fpga_irc_state[chan] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
+
+ index = (irc_state >> (ffs(FPGA_IRC_STATE_INDEX_EVENT_MASK) - 1)) & 1;
+
+ return index;
+}
+
+int
+pxmc_inp_rocon_adjust_to_irc_change(struct pxmc_state *mcs, long irc_diff)
+{
+ long pos_diff = irc_diff << PXMC_SUBDIV(mcs);
+
+ mcs->pxms_ptofs += irc_diff;
+
+ mcs->pxms_ap += pos_diff;
+ mcs->pxms_rp += pos_diff;
+ return 0;
+}
+
+int
+pxmc_inp_rocon_irc_offset_from_index(struct pxmc_state *mcs)
+{
+ int chan=mcs->pxms_inp_info;
+ long irc_offset;
+ long irc_diff;
+
+ irc_offset = -fpga_irc[chan]->count_index;
+ irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
+ pxmc_rocon_irc_offset[chan] = irc_offset;
+ return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
+}
+
+int
+pxmc_inp_rocon_ap_zero(struct pxmc_state *mcs)
+{
+ int chan=mcs->pxms_inp_info;
+ long irc_offset;
+ long irc_diff;
+
+ irc_offset = -fpga_irc[chan]->count;
+ irc_diff = irc_offset - pxmc_rocon_irc_offset[chan];
+ pxmc_rocon_irc_offset[chan] = irc_offset;
+ return pxmc_inp_rocon_adjust_to_irc_change(mcs, irc_diff);
+}
+
int
pxmc_inp_rocon_ap2hw(struct pxmc_state *mcs)
{
- /* int chan=mcs->pxms_inp_info */
- /* Optional set of the actual position to the HW */
- /* We do not want to change real HW counter at any situation */
- /* It can be used only to set some software maintained offset */
+ int chan=mcs->pxms_inp_info;
+ long irc;
+ long pos_diff;
+
+ irc = fpga_irc[chan]->count;
+ pos_diff = mcs->pxms_ap - (irc << PXMC_SUBDIV(mcs));
+
+ irc = pos_diff >> PXMC_SUBDIV(mcs);
+
+ /* Adjust phase table alignemt to modified IRC readout */
+ mcs->pxms_ptofs += irc - pxmc_rocon_irc_offset[chan];
+
+ pxmc_rocon_irc_offset[chan] = irc;
return 0;
}
+int
+pxmc_inp_rocon_ptofs_from_index_poll(struct pxmc_state *mcs, int diff2err)
+{
+ int chan=mcs->pxms_inp_info;
+ long irc;
+ long index_irc;
+
+ if (!(*fpga_irc_state[chan] & FPGA_IRC_STATE_INDEX_EVENT_MASK))
+ return 0;
+
+ irc = fpga_irc[chan]->count + pxmc_rocon_irc_offset[chan];
+ index_irc = fpga_irc[chan]->count_index + pxmc_rocon_irc_offset[chan];
+
+ return pxmc_ptofs_from_index(mcs, irc, index_irc, diff2err);
+}
+
+uint32_t pxmc_rocon_receiver_dummy_reg;
+
+static inline volatile uint32_t *
+pxmc_rocon_receiver_chan2reg(unsigned chan)
+{
+ volatile uint32_t *rec_reg;
+
+ if (chan >= 16)
+ return &pxmc_rocon_receiver_dummy_reg;
+
+ rec_reg = fpga_lx_master_receiver_base;
+
+ #ifdef LXPWR_WITH_SIROLADC
+ rec_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 3) * 3 + chan * 2;
+ #else /*LXPWR_WITH_SIROLADC*/
+ rec_reg += LX_MASTER_DATA_OFFS + chan;
+ #endif /*LXPWR_WITH_SIROLADC*/
+
+ return rec_reg;
+}
+
inline unsigned
-pxmc_rocon_bdc_hal_rd(pxmc_state_t *mcs)
+pxmc_rocon_bldc_hal_rd(pxmc_state_t *mcs)
{
unsigned h = 0;
- /* FIXME */
+ volatile uint32_t *rec_reg_a, *rec_reg_b, *rec_reg_c;
+ int chan = mcs->pxms_out_info;
+ int hal_offs;
+
+ #ifdef LXPWR_WITH_SIROLADC
+ hal_offs = 1;
+ #else /*LXPWR_WITH_SIROLADC*/
+ hal_offs = 0;
+ #endif /*LXPWR_WITH_SIROLADC*/
+
+ rec_reg_a = pxmc_rocon_receiver_chan2reg(chan + 0);
+ rec_reg_b = pxmc_rocon_receiver_chan2reg(chan + 1);
+ rec_reg_c = pxmc_rocon_receiver_chan2reg(chan + 2);
+
+ h = (rec_reg_a[hal_offs] >> 14) & 1;
+ h |= (rec_reg_b[hal_offs] >> 13) & 2;
+ h |= (rec_reg_c[hal_offs] >> 12) & 4;
+
/* return 3 bits corresponding to the HAL senzor input */
return h;
}
#if 1
-const unsigned char pxmc_lpc_bdc_hal_pos_table[8]=
-{
- [0]=0xff,
- [7]=0xff,
- [1]=0,/*0*/
- [5]=1,/*1*/
- [4]=2,/*2*/
- [6]=3,/*3*/
- [2]=4,/*4*/
- [3]=5,/*5*/
+const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
+{
+ [0] = 0xff,
+ [7] = 0xff,
+ [1] = 0, /*0*/
+ [5] = 1, /*1*/
+ [4] = 2, /*2*/
+ [6] = 3, /*3*/
+ [2] = 4, /*4*/
+ [3] = 5, /*5*/
};
#else
-const unsigned char pxmc_lpc_bdc_hal_pos_table[8]=
-{
- [0]=0xff,
- [7]=0xff,
- [1]=0,/*0*/
- [5]=5,/*1*/
- [4]=4,/*2*/
- [6]=3,/*3*/
- [2]=2,/*4*/
- [3]=1,/*5*/
+const unsigned char pxmc_lpc_bdc_hal_pos_table[8] =
+{
+ [0] = 0xff,
+ [7] = 0xff,
+ [1] = 0, /*0*/
+ [5] = 5, /*1*/
+ [4] = 4, /*2*/
+ [6] = 3, /*3*/
+ [2] = 2, /*4*/
+ [3] = 1, /*5*/
};
#endif
+uint32_t pxmc_rocon_pwm_dummy_reg;
+
+static inline volatile uint32_t *
+pxmc_rocon_pwm_chan2reg(unsigned chan)
+{
+ volatile uint32_t *pwm_reg;
+
+ if (chan >= 16)
+ return &pxmc_rocon_pwm_dummy_reg;
+
+ pwm_reg = fpga_lx_master_transmitter_base;
+
+ #ifdef LXPWR_WITH_SIROLADC
+ pwm_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 3) + chan;
+ #else /*LXPWR_WITH_SIROLADC*/
+ pwm_reg += LX_MASTER_DATA_OFFS + chan;
+ #endif /*LXPWR_WITH_SIROLADC*/
+
+ return pwm_reg;
+}
+
/**
* pxmc_rocon_pwm3ph_wr - Output of the 3-phase PWM to the hardware
- * @mcs: Motion controller state information
+ * @mcs: Motion controller state information
*/
/*static*/ inline void
pxmc_rocon_pwm3ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2, short pwm3)
{
- unsigned long out_info=mcs->pxms_out_info;
+ volatile uint32_t *pwm_reg_a, *pwm_reg_b, *pwm_reg_c;
+ int chan = mcs->pxms_out_info;
- /* FIXME */
+ pwm_reg_c = pxmc_rocon_pwm_chan2reg(chan + 0);
+ pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
+ pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 2);
+
+ *pwm_reg_a = pwm1;
+ *pwm_reg_b = pwm2;
+ *pwm_reg_c = pwm3;
}
static inline void
pxmc_rocon_process_hal_error(struct pxmc_state *mcs)
{
- if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY*HAL_ERR_MAX_COUNT) {
+ if (mcs->pxms_halerc >= HAL_ERR_SENSITIVITY * HAL_ERR_MAX_COUNT)
+ {
pxmc_set_errno(mcs, PXMS_E_HAL);
mcs->pxms_ene = 0;
mcs->pxms_halerc--;
- } else
- mcs->pxms_halerc+=HAL_ERR_SENSITIVITY;
+ }
+ else
+ mcs->pxms_halerc += HAL_ERR_SENSITIVITY;
}
/**
- * pxmc_rocon_pwm_out - Phase output for brush-less 3-phase motor
- * @mcs: Motion controller state information
+ * pxmc_rocon_pwm3ph_out - Phase output for brush-less 3-phase motor
+ * @mcs: Motion controller state information
*/
int
-pxmc_rocon_pwm_out(pxmc_state_t *mcs)
+pxmc_rocon_pwm3ph_out(pxmc_state_t *mcs)
{
+ typeof(mcs->pxms_ptvang) ptvang;
+ int sync_mode = 0;
unsigned char hal_pos;
short pwm1;
short pwm2;
short ene;
int wind_current[4];
- if(!(mcs->pxms_flg&PXMS_PTI_m) || !(mcs->pxms_flg&PXMS_PHA_m) ||
- (mcs->pxms_flg&PXMS_PRA_m)){
+ 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;
+ short ptirc = mcs->pxms_ptirc;
+ short divisor = mcs->pxms_ptper * 6;
+
+ #if 0
+ pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
+ sync_mode = 1;
+ #elif 0
+ {
+ 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);
+ }
+ }
+ #else
- hal_pos=pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bdc_hal_rd(mcs)];
+ hal_pos = pxmc_lpc_bdc_hal_pos_table[pxmc_rocon_bldc_hal_rd(mcs)];
- if(hal_pos == 0xff) {
- if(mcs->pxms_ene)
+ if (hal_pos == 0xff)
+ {
+ if (mcs->pxms_ene)
pxmc_rocon_process_hal_error(mcs);
- } else {
- if(mcs->pxms_halerc)
+ }
+ 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)){
- 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;
- }
+ ptindx = (hal_pos * ptirc + divisor / 2) / divisor;
+
+ if (!(mcs->pxms_flg & PXMS_PTI_m) || (mcs->pxms_flg & PXMS_PRA_m))
+ {
+ if (((hal_pos != mcs->pxms_hal) && (mcs->pxms_hal != 0x40)) && 1)
+ {
+ short ptindx_prev = (mcs->pxms_hal * ptirc + divisor / 2) / divisor;;
- mcs->pxms_ptindx=ptindx;
+ if ((ptindx > ptindx_prev + ptirc / 2) ||
+ (ptindx_prev > ptindx + ptirc / 2))
+ {
+ ptindx = (ptindx_prev + ptindx - ptirc) / 2;
- mcs->pxms_ptofs=(mcs->pxms_ap>>PXMC_SUBDIV(mcs))+mcs->pxms_ptshift-ptindx;
+ if (ptindx < 0)
+ ptindx += ptirc;
+ }
+ else
+ {
+ ptindx = (ptindx_prev + ptindx) / 2;
+ }
- pxmc_set_flag(mcs,PXMS_PTI_b);
- pxmc_clear_flag(mcs,PXMS_PRA_b);
+ mcs->pxms_ptindx = ptindx;
- /* if phase table position to mask is know do fine phase table alignment */
- if(mcs->pxms_cfg & PXMS_CFG_I2PT_m)
- lpc_qei_setup_index_catch(&lpc_qei_state);
+ mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
- }else{
- if(!(mcs->pxms_flg&PXMS_PTI_m))
- mcs->pxms_ptindx=ptindx;
+ pxmc_set_flag(mcs, PXMS_PTI_b);
+ pxmc_clear_flag(mcs, PXMS_PRA_b);
+ }
+ else
+ {
+ if (!(mcs->pxms_flg & PXMS_PTI_m))
+ mcs->pxms_ptindx = ptindx;
+ }
+ } 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;
}
+ #endif
}
{
/* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
}
- ene=mcs->pxms_ene;
- if(ene){
- indx=mcs->pxms_ptindx;
- #if 1
+ if (!sync_mode) {
+ ptvang = mcs->pxms_ptvang;
+ ene = mcs->pxms_ene;
+ } else {
+ ptvang = 0;
+ ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
+ }
+
+ if (ene) {
+ indx = mcs->pxms_ptindx;
+#if 1
/* tuning of magnetic field/voltage advance angle */
- indx+=(mcs->pxms_s1*mcs->pxms_as)>>(PXMC_SUBDIV(mcs)+8);
- #endif
- if(ene<0){
+ indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
+#endif
+
+ if (ene < 0)
+ {
/* Generating direction of stator mag. field for backward torque */
- ene=-ene;
- if((indx-=mcs->pxms_ptvang)<0)
- indx+=mcs->pxms_ptirc;
- }else{
+ ene = -ene;
+
+ if ((indx -= ptvang) < 0)
+ indx += mcs->pxms_ptirc;
+ }
+ else
+ {
/* Generating direction of stator mag. field for forward torque */
- if((indx+=mcs->pxms_ptvang)>=mcs->pxms_ptirc)
- indx-=mcs->pxms_ptirc;
+ if ((indx += ptvang) >= mcs->pxms_ptirc)
+ indx -= mcs->pxms_ptirc;
}
- if(mcs->pxms_ptscale_mult)
- indx=((unsigned long)indx*mcs->pxms_ptscale_mult)>>mcs->pxms_ptscale_shift;
+ if (mcs->pxms_ptscale_mult)
+ indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
- pwm1=mcs->pxms_ptptr1[indx];
- pwm2=mcs->pxms_ptptr2[indx];
- pwm3=mcs->pxms_ptptr3[indx];
+ pwm1 = mcs->pxms_ptptr1[indx];
+ pwm2 = mcs->pxms_ptptr2[indx];
+ pwm3 = mcs->pxms_ptptr3[indx];
- #ifdef PXMC_WITH_PT_ZIC
- if(labs(mcs->pxms_as)>(10<<PXMC_SUBDIV(mcs))) {
- if(pwm1&PXMC_PT_ZIC_MASK){
- /*hal_gpio_set_value(PWM2_EN_PIN,0);*/
- } else {
- /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
- }
- if(pwm2&PXMC_PT_ZIC_MASK){
- /*hal_gpio_set_value(PWM4_EN_PIN,0);*/
- } else {
- /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
- }
- if(pwm3&PXMC_PT_ZIC_MASK){
- /*hal_gpio_set_value(PWM6_EN_PIN,0);*/
- } else {
- /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
+#ifdef PXMC_WITH_PT_ZIC
+ if (labs(mcs->pxms_as) < (10 << PXMC_SUBDIV(mcs)))
+ {
+ pwm1 &= ~PXMC_PT_ZIC_MASK;
+ pwm2 &= ~PXMC_PT_ZIC_MASK;
+ pwm3 &= ~PXMC_PT_ZIC_MASK;
+ }
+#endif /*PXMC_WITH_PT_ZIC*/
+
+ /* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
+ /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
+ {
+ unsigned long pwm_dc = pxmc_rocon_pwm_magnitude * (unsigned long)ene;
+ if (pwm1 & PXMC_PT_ZIC_MASK)
+ pwm1 = 0x8000;
+ else
+ pwm1 = (((unsigned long long)pwm1 * pwm_dc) >> (15 + 15)) | 0x4000;
+ if (pwm2 & PXMC_PT_ZIC_MASK)
+ pwm2 = 0x8000;
+ else
+ pwm2 = (((unsigned long long)pwm2 * pwm_dc) >> (15 + 15)) | 0x4000;
+ if (pwm3 & PXMC_PT_ZIC_MASK)
+ pwm3 = 0x8000;
+ else
+ pwm3 = (((unsigned long long)pwm3 * pwm_dc) >> (15 + 15)) | 0x4000;
+ }
+ pxmc_rocon_pwm3ph_wr(mcs, pwm1, pwm2, pwm3);
+ }
+ else
+ {
+ pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
+ }
+
+ return 0;
+}
+
+/**
+ * pxmc_rocon_pwm2ph_wr - Output of the 2-phase stepper motor PWM to the hardware
+ * @mcs: Motion controller state information
+ */
+/*static*/ inline void
+pxmc_rocon_pwm2ph_wr(pxmc_state_t *mcs, short pwm1, short pwm2)
+{
+ volatile uint32_t *pwm_reg_ap, *pwm_reg_an, *pwm_reg_bp, *pwm_reg_bn;
+ int chan = mcs->pxms_out_info;
+
+ pwm_reg_bn = pxmc_rocon_pwm_chan2reg(chan + 0);
+ pwm_reg_bp = pxmc_rocon_pwm_chan2reg(chan + 1);
+ pwm_reg_an = pxmc_rocon_pwm_chan2reg(chan + 2);
+ pwm_reg_ap = pxmc_rocon_pwm_chan2reg(chan + 3);
+
+ if (pwm2 >= 0) {
+ *pwm_reg_bp = pwm2 | 0x4000;
+ *pwm_reg_bn = 0;
+ } else {
+ *pwm_reg_bp = 0;
+ *pwm_reg_bn = -pwm2 | 0x4000;
+ }
+
+ if (pwm1 >= 0) {
+ *pwm_reg_ap = pwm1 | 0x4000;
+ *pwm_reg_an = 0;
+ } else {
+ *pwm_reg_ap = 0;
+ *pwm_reg_an = -pwm1 | 0x4000;
+ }
+}
+
+/**
+ * pxmc_rocon_pwm2ph_out - Phase output of the 2-phase stepper motor PWM
+ * @mcs: Motion controller state information
+ */
+int
+pxmc_rocon_pwm2ph_out(pxmc_state_t *mcs)
+{
+ typeof(mcs->pxms_ptvang) ptvang;
+ int sync_mode = 0;
+ short pwm1;
+ short pwm2;
+ int indx = 0;
+ short ene;
+ int wind_current[4];
+
+ 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;
+
+ #if 0
+ pxmc_irc_16bit_commindx(mcs, mcs->pxms_rp >> PXMC_SUBDIV(mcs));
+ sync_mode = 1;
+ #elif 1
+ {
+ 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);
}
+ }
+ #else
+
+ ptindx = mcs->pxms_ptindx;
+
+ if(0 && (!(mcs->pxms_flg&PXMS_PTI_m) || (mcs->pxms_flg&PXMS_PRA_m))) {
+
+ mcs->pxms_ptindx = ptindx;
+
+ mcs->pxms_ptofs = (mcs->pxms_ap >> PXMC_SUBDIV(mcs)) + mcs->pxms_ptshift - ptindx;
+
+ pxmc_set_flag(mcs, PXMS_PTI_b);
+ pxmc_clear_flag(mcs, PXMS_PRA_b);
+
+ /* if phase table position to mask is know do fine phase table alignment */
+ //if(mcs->pxms_cfg & PXMS_CFG_I2PT_m)
+ // lpc_qei_setup_index_catch(&lpc_qei_state);
+
+ } else {
+ if(!(mcs->pxms_flg&PXMS_PTI_m))
+ mcs->pxms_ptindx = ptindx;
+ }
+ #endif
+ }
+
+ {
+ /*wind_current[0]=(ADC->ADDR0 & 0xFFF0)>>4;*/
+ /* FIXME - check winding current against limit */
+ /* pxmc_set_errno(mcs, PXMS_E_WINDCURRENT); */
+ }
+
+ if (!sync_mode) {
+ ptvang = mcs->pxms_ptvang;
+ ene = mcs->pxms_ene;
+ } else {
+ ptvang = 0;
+ ene = mcs->pxms_flg & PXMS_BSY_m? mcs->pxms_me: 0;
+ }
+
+ if (ene) {
+ indx = mcs->pxms_ptindx;
+ #if 0
+ /* tuning of magnetic field/voltage advance angle */
+ indx += (mcs->pxms_s1 * mcs->pxms_as) >> (PXMC_SUBDIV(mcs) + 8);
+ #endif
+ if (ene<0){
+ /* Generating direction of stator mag. field for backward torque */
+ ene = -ene;
+ if ((indx -= ptvang)<0)
+ indx += mcs->pxms_ptirc;
}else{
- /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
- /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
- /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
+ /* Generating direction of stator mag. field for forward torque */
+ if ((indx += ptvang) >= mcs->pxms_ptirc)
+ indx -= mcs->pxms_ptirc;
}
- pwm1&=~PXMC_PT_ZIC_MASK;
- pwm2&=~PXMC_PT_ZIC_MASK;
- pwm3&=~PXMC_PT_ZIC_MASK;
- #endif /*PXMC_WITH_PT_ZIC*/
+
+ if (mcs->pxms_ptscale_mult)
+ indx = ((unsigned long)indx * mcs->pxms_ptscale_mult) >> mcs->pxms_ptscale_shift;
+
+ pwm1 = mcs->pxms_ptptr1[indx];
+ pwm2 = mcs->pxms_ptptr2[indx];
/* Default phase-table amplitude is 0x7fff, ene max is 0x7fff */
- /* Initialized CTM4 PWM period is 0x200 => divide by value about 2097024 */
+ /* Initialized PWM period is 0x200 => divide by value about 2097024 */
{
- unsigned long pwm_dc = pxmc_lpcpwm1_magnitude*(unsigned long)ene;
- pwm1=((unsigned long long)pwm1 * pwm_dc)>>(15+15);
- pwm2=((unsigned long long)pwm2 * pwm_dc)>>(15+15);
- pwm3=((unsigned long long)pwm3 * pwm_dc)>>(15+15);
+ unsigned long pwm_dc = pxmc_rocon_pwm_magnitude * (unsigned long)ene;
+ pwm1 = ((unsigned long long)pwm1 * pwm_dc) >> (15+15);
+ pwm2 = ((unsigned long long)pwm2 * pwm_dc) >> (15+15);
}
- pxmc_rocon_pwm3ph_wr(mcs,pwm1,pwm2,pwm3);
+ pxmc_rocon_pwm2ph_wr(mcs, pwm1, pwm2);
}else{
- /*pxmc_lpc_wind_current_over_cnt=0;*/
- pxmc_rocon_pwm3ph_wr(mcs,0,0,0);
- #ifdef PXMC_WITH_PT_ZIC
- /*hal_gpio_set_value(PWM2_EN_PIN,1);*/
- /*hal_gpio_set_value(PWM4_EN_PIN,1);*/
- /*hal_gpio_set_value(PWM6_EN_PIN,1);*/
- #endif /*PXMC_WITH_PT_ZIC*/
+ /*pxmc_lpc_wind_current_over_cnt = 0;*/
+ pxmc_rocon_pwm2ph_wr(mcs, 0, 0);
+ }
+
+ return 0;
+}
+
+/**
+ * pxmc_rocon_pwm_dc_out - DC motor CW and CCW PWM output
+ * @mcs: Motion controller state information
+ */
+int
+pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
+{
+ volatile uint32_t *pwm_reg_a, *pwm_reg_b;
+ int chan = mcs->pxms_out_info;
+ int ene = mcs->pxms_ene;
+
+ pwm_reg_a = pxmc_rocon_pwm_chan2reg(chan + 0);
+ pwm_reg_b = pxmc_rocon_pwm_chan2reg(chan + 1);
+
+ if (ene < 0) {
+ ene = -ene;
+ if (ene > 0x7fff)
+ ene = 0x7fff;
+ ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
+ *pwm_reg_a = 0;
+ *pwm_reg_b = ene | 0x4000;
+ } else {
+ if (ene > 0x7fff)
+ ene = 0x7fff;
+ ene = (ene * (pxmc_rocon_pwm_magnitude + 5)) >> 15;
+ *pwm_reg_b = 0;
+ *pwm_reg_a = ene | 0x4000;
}
return 0;
}
-/* PWM outputs placed on (PWM1), PWM2, PWM4, PWM6 */
+volatile void *pxmc_rocon_rx_data_hist_buff;
+volatile void *pxmc_rocon_rx_data_hist_buff_end;
+
+uint32_t pxmc_rocon_rx_last_irq;
+uint32_t pxmc_rocon_rx_cycle_time;
+uint32_t pxmc_rocon_rx_irq_latency;
+uint32_t pxmc_rocon_rx_irq_latency_max;
+
+IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr)
+{
+ uint32_t ir;
+
+ ir = ROCON_RX_TIM->IR & LPC_TIM_IR_ALL_m;
+ ROCON_RX_TIM->IR = ir;
+ if (ir & LPC_TIM_IR_CR1INT_m) {
+ uint32_t cr0, cr1;
+ cr0 = ROCON_RX_TIM->CR0;
+ cr1 = ROCON_RX_TIM->CR1;
+
+ pxmc_rocon_rx_cycle_time = cr1 - pxmc_rocon_rx_last_irq;
+ pxmc_rocon_rx_last_irq = cr1;
+
+ hal_gpio_set_value(T2MAT0_PIN, 1);
+ hal_gpio_set_value(T2MAT1_PIN, 0);
+ hal_gpio_set_value(T2MAT0_PIN, 0);
+
+ if (pxmc_rocon_rx_data_hist_buff >= pxmc_rocon_rx_data_hist_buff_end)
+ pxmc_rocon_rx_data_hist_buff = NULL;
+
+ if (pxmc_rocon_rx_data_hist_buff != NULL) {
+ int i;
+ volatile uint32_t *pwm_reg = fpga_lx_master_transmitter_base + 8;
+ volatile uint32_t *rec_reg = fpga_lx_master_receiver_base + 8;
+ uint16_t *pbuf = (uint16_t *)pxmc_rocon_rx_data_hist_buff;
+ for (i = 0; i < 8; i++) {
+ *(pbuf++) = *(rec_reg++);
+ }
+ for (i = 0; i < 8; i++) {
+ *(pbuf++) = *(pwm_reg++);
+ }
+ pxmc_rocon_rx_data_hist_buff = pbuf;
+ }
+
+ pxmc_rocon_rx_irq_latency = ROCON_RX_TIM->TC - cr1;
+ if (pxmc_rocon_rx_irq_latency > pxmc_rocon_rx_irq_latency_max)
+ pxmc_rocon_rx_irq_latency_max = pxmc_rocon_rx_irq_latency;
+
+ #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
+ pxmc_sfi_isr();
+ do_pxmc_coordmv();
+ #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
+ }
+
+ return IRQ_HANDLED;
+}
+
int
-pxmc_rocon_pwm_init(pxmc_state_t *mcs, int mode)
+pxmc_rocon_rx_done_isr_setup(irq_handler_t rx_done_isr_handler)
{
- /* hal_pin_conf(PWM1_EN_PIN); */
+ disable_irq(ROCON_RX_IRQn);
+
+ hal_pin_conf_set(T2MAT0_PIN, PORT_CONF_GPIO_OUT_LO);
+ hal_pin_conf_set(T2MAT1_PIN, PORT_CONF_GPIO_OUT_LO);
+ hal_pin_conf(T2CAP0_PIN);
+ hal_pin_conf(T2CAP1_PIN);
+
+ hal_gpio_direction_output(T2MAT0_PIN, 1);
+ hal_gpio_direction_output(T2MAT1_PIN, 0);
+ hal_gpio_set_value(T2MAT0_PIN, 0);
+
+ /* Enable CLKOUT pin function, source CCLK, divide by 1 */
+ LPC_SC->CLKOUTCFG = 0x0100;
+
+ request_irq(ROCON_RX_IRQn, rx_done_isr_handler, 0, NULL,NULL);
+
+ ROCON_RX_TIM->TCR = 0;
+ ROCON_RX_TIM->CTCR = 0;
+ ROCON_RX_TIM->PR = 0; /* Divide by 1 */
+
+ ROCON_RX_TIM->CCR = LPC_TIM_CCR_CAP0RE_m | LPC_TIM_CCR_CAP1FE_m |
+ LPC_TIM_CCR_CAP1I_m;
- /* hal_gpio_set_value(PWM1_EN_PIN,1); */
+ ROCON_RX_TIM->EMR = __val2mfld(LPC_TIM_EMR_EMC0_m, LPC_TIM_EMR_NOP) |
+ __val2mfld(LPC_TIM_EMR_EMC1_m, LPC_TIM_EMR_NOP);
+
+ ROCON_RX_TIM->MCR = 0; /* No IRQ on MRx */
+ ROCON_RX_TIM->TCR = LPC_TIM_TCR_CEN_m; /* Enable timer counting */
+ enable_irq(ROCON_RX_IRQn); /* Enable interrupt */
+
+ return 0;
+
+}
+
+int
+pxmc_rocon_pwm_master_init(void)
+{
+ int i;
+ int grp_in = 0;
+ int grp_out = 0;
+ unsigned word_slot;
+ unsigned receiver_done_div = 1;
+ #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*/
+
+ #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
+ receiver_done_div = 5;
+ #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
+
+ *fpga_lx_master_reset = 1;
+ *fpga_lx_master_transmitter_reg = 0;
+ *fpga_lx_master_transmitter_cycle = 2500; /* 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++)
+ fpga_lx_master_receiver_base[i] = 0;
+
+ 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++)
+ fpga_lx_master_transmitter_base[i] = 0;
+
+ word_slot = LX_MASTER_DATA_OFFS + lxpwr_header + lxpwr_chip_pwm_cnt;
+ fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
+ #ifdef LXPWR_WITH_SIROLADC
+ fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
+ #endif /*LXPWR_WITH_SIROLADC*/
+
+ word_slot = LX_MASTER_DATA_OFFS + 0;
+ fpga_lx_master_transmitter_base[grp_out++] = (word_slot << 8) | lxpwr_words;
+ #ifdef LXPWR_WITH_SIROLADC
+ fpga_lx_master_transmitter_base[word_slot] = 0xc100 | (lxpwr_words - 1);
+ #endif /*LXPWR_WITH_SIROLADC*/
+
+ 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_receiver_done_div = receiver_done_div << 8;
+
+ return 0;
+}
+
+int pxmc_ptofs_from_index(pxmc_state_t *mcs, unsigned long irc,
+ unsigned long index_irc, int diff2err)
+{
+ long ofsl;
+ short ofs;
+
+ ofs = ofsl = index_irc - mcs->pxms_ptmark;
+
+ if (diff2err) {
+ short diff;
+ diff = (unsigned short)ofs - (unsigned short)mcs->pxms_ptofs;
+ if (diff >= mcs->pxms_ptirc / 2)
+ diff -= mcs->pxms_ptirc;
+ if (diff <= -mcs->pxms_ptirc / 2)
+ diff += mcs->pxms_ptirc;
+ if (diff < 0)
+ diff = -diff;
+ if(diff >= mcs->pxms_ptirc / 6) {
+ return -1;
+ }
+ } else {
+ long diff;
+ diff = (unsigned long)ofsl - irc;
+ ofs = ofsl - (diff / mcs->pxms_ptirc) * mcs->pxms_ptirc;
+ }
+
+ mcs->pxms_ptofs = ofs;
+
+ return 1;
+}
+
+/**
+ * pxmc_dummy_con - Dummy controller for synchronous BLDC/PMSM/steper drive
+ * @mcs: Motion controller state information
+ */
+int
+pxmc_dummy_con(pxmc_state_t *mcs)
+{
+ return 0;
+}
+
+int pxmc_rocon_hh_gd10(pxmc_state_t *mcs);
+int pxmc_rocon_hh_gd20(pxmc_state_t *mcs);
+int pxmc_rocon_hh_gd30(pxmc_state_t *mcs);
+int pxmc_rocon_hh_gd40(pxmc_state_t *mcs);
+int pxmc_rocon_hh_gd50(pxmc_state_t *mcs);
+int pxmc_rocon_hh_gd90(pxmc_state_t *mcs);
+
+/* initialize for hard home */
+int
+pxmc_rocon_hh_gi(pxmc_state_t *mcs)
+{
+ pxmc_set_flag(mcs,PXMS_BSY_b);
+ #if 0 /* config and speed already set in pxmc_hh */
+ spd = mcs->pxms_ms;
+ spd >>= mcs->pxms_cfg & PXMS_CFG_HSPD_m;
+ if(!spd) spd = 1;
+ if(mcs->pxms_cfg & PXMS_CFG_HDIR_m)
+ spd = -spd;
+ mcs->pxms_gen_hsp = spd;
+ #endif
+ mcs->pxms_rsfg = 0;
+ mcs->pxms_gen_htim = 16;
+ mcs->pxms_do_gen = pxmc_rocon_hh_gd10;
+ return pxmc_rocon_hh_gd10(mcs);
+}
+
+/* fill initial mark filter and then decide */
+int
+pxmc_rocon_hh_gd10(pxmc_state_t *mcs)
+{
+ int mark;
+
+ if(mcs->pxms_flg & PXMS_ERR_m)
+ return pxmc_spdnext_gend(mcs);
+
+ pxmc_spd_gacc(&(mcs->pxms_rs), 0, mcs->pxms_ma);
+ mcs->pxms_rp += mcs->pxms_rs;
+
+ mark = pxmc_inp_rocon_is_mark(mcs);
+
+ if (mcs->pxms_gen_htim) {
+ mcs->pxms_gen_htim--;
+ return 0;
+ }
+
+ if (!(mcs->pxms_cfg & PXMS_CFG_HLS_m)) {
+ /* limit switch is not used */
+ pxmc_inp_rocon_is_index_edge(mcs);
+ mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
+ return 0;
+ }
+
+ if (mark >= 6) {
+ /* go out from switch */
+ mcs->pxms_do_gen = pxmc_rocon_hh_gd20;
+ } else {
+ /* switch is off -> look for it */
+ mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
+ }
+ return 0;
+}
+
+/* go out from switch */
+int
+pxmc_rocon_hh_gd20(pxmc_state_t *mcs)
+{
+ int mark;
+
+ if(mcs->pxms_flg & PXMS_ERR_m)
+ return pxmc_spdnext_gend(mcs);
+
+ mark = pxmc_inp_rocon_is_mark(mcs);
+
+ if (mark <= 1) {
+ /* switch is off -> look for it again */
+ mcs->pxms_do_gen = pxmc_rocon_hh_gd30;
+ }
+
+ pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
+ mcs->pxms_rp += mcs->pxms_rs;
+
+ return 0;
+}
+
+/* switch is off -> look for it */
+int
+pxmc_rocon_hh_gd30(pxmc_state_t *mcs)
+{
+ long spd;
+ int mark;
+
+ if (mcs->pxms_flg & PXMS_ERR_m)
+ return pxmc_spdnext_gend(mcs);
+
+ pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
+ mcs->pxms_rp += mcs->pxms_rs;
+
+ mark = pxmc_inp_rocon_is_mark(mcs);
+
+ if (mark >= 8){
+ spd = mcs->pxms_gen_hsp >> 2; /* slow down */
+ if (!spd)
+ spd = 1;
+ mcs->pxms_gen_hsp = spd;
+ mcs->pxms_do_gen = pxmc_rocon_hh_gd40;
+ }
+
+ return 0;
+}
+
+/* switch is on -> find edge */
+int
+pxmc_rocon_hh_gd40(pxmc_state_t *mcs)
+{
+ int mark;
+
+ if (mcs->pxms_flg & PXMS_ERR_m)
+ return pxmc_spdnext_gend(mcs);
+
+ mark = pxmc_inp_rocon_is_mark(mcs);
+
+ if (mark <= 1) {
+ if (mcs->pxms_cfg & PXMS_CFG_HRI_m) {
+ pxmc_inp_rocon_is_index_edge(mcs);
+ mcs->pxms_do_gen = pxmc_rocon_hh_gd50;
+ } else {
+ pxmc_inp_rocon_ap_zero(mcs);
+ mcs->pxms_do_gen = pxmc_stop_gi;
+ }
+ }
+
+ pxmc_spd_gacc(&(mcs->pxms_rs), -mcs->pxms_gen_hsp, mcs->pxms_ma);
+ mcs->pxms_rp += mcs->pxms_rs;
+
+ return 0;
+}
+
+/* calibrate on revolution index */
+int
+pxmc_rocon_hh_gd50(pxmc_state_t *mcs)
+{
+ if (mcs->pxms_flg & PXMS_ERR_m)
+ return pxmc_spdnext_gend(mcs);
+
+ pxmc_spd_gacc(&(mcs->pxms_rs), mcs->pxms_gen_hsp, mcs->pxms_ma);
+ mcs->pxms_rp += mcs->pxms_rs;
+
+ if (pxmc_inp_rocon_is_index_edge(mcs)){
+ pxmc_inp_rocon_irc_offset_from_index(mcs);
+ mcs->pxms_do_gen = pxmc_stop_gi;
+ }
return 0;
}
pxmc_call_t *pxmc_get_hh_gi_4axis(pxmc_state_t *mcs)
{
- /*return pxmc_hh_gi;*/
- return NULL;
-}
-
-pxmc_state_t mcs0={
- pxms_flg:PXMS_ENI_m,
- pxms_do_inp:pxmc_inp_rocon_inp,
- pxms_do_con:pxmc_pid_con,
- pxms_do_out:pxmc_rocon_pwm_out,
- pxms_do_deb:0,
- pxms_do_gen:0,
- pxms_do_ap2hw:pxmc_inp_rocon_ap2hw,
- pxms_ap:0, pxms_as:0,
- pxms_rp: 55*256, pxms_rs:0, pxms_subdiv:8,
- pxms_md:800<<8, pxms_ms:1000, pxms_ma:10,
- pxms_inp_info:0,
- pxms_out_info:0,
- pxms_ene:0, pxms_erc:0,
- pxms_p:80, pxms_i:30, pxms_d:200, pxms_s1:200, pxms_s2:0,
- pxms_me:0x7e00/*0x7fff*/,
- pxms_cfg:PXMS_CFG_SMTH_m|PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m|PXMS_CFG_I2PT_m*0|
- 0x1,
-
- pxms_ptper:1,
- pxms_ptirc:1000,
+ return pxmc_rocon_hh_gi;
+}
+
+pxmc_state_t mcs0 =
+{
+pxms_flg:
+ PXMS_ENI_m,
+pxms_do_inp:
+ pxmc_inp_rocon_inp,
+pxms_do_con:
+ pxmc_pid_con /*pxmc_dummy_con*/,
+pxms_do_out:
+ pxmc_rocon_pwm_dc_out /*pxmc_rocon_pwm3ph_out*/,
+ pxms_do_deb: 0,
+ pxms_do_gen: 0,
+pxms_do_ap2hw:
+ pxmc_inp_rocon_ap2hw,
+ pxms_ap: 0, pxms_as: 0,
+ pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
+ pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
+ pxms_inp_info: 0,
+ pxms_out_info: 0,
+ pxms_ene: 0, pxms_erc: 0,
+ pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
+ pxms_me: 0x7e00/*0x7fff*/,
+pxms_cfg:
+ PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
+ PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
+ PXMS_CFG_I2PT_m * 0 | 0x2,
+
+ pxms_ptper: 1,
+ pxms_ptirc: 1000,
+ pxms_ptmark: 1180,
+ /*pxms_ptamp: 0x7fff,*/
+
+ pxms_hal: 0x40,
+};
+
+pxmc_state_t mcs1 =
+{
+pxms_flg:
+ PXMS_ENI_m,
+pxms_do_inp:
+ pxmc_inp_rocon_inp,
+pxms_do_con:
+ pxmc_pid_con,
+pxms_do_out:
+ pxmc_rocon_pwm_dc_out,
+ pxms_do_deb: 0,
+ pxms_do_gen: 0,
+pxms_do_ap2hw:
+ pxmc_inp_rocon_ap2hw,
+ pxms_ap: 0, pxms_as: 0,
+ pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
+ pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
+ pxms_inp_info: 1,
+ pxms_out_info: 2,
+ pxms_ene: 0, pxms_erc: 0,
+ pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
+ pxms_me: 0x7e00/*0x7fff*/,
+pxms_cfg:
+ PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
+ PXMS_CFG_HRI_m * 0 | PXMS_CFG_HDIR_m * 0 |
+ PXMS_CFG_I2PT_m * 0 | 0x2,
+
+ pxms_ptper: 1,
+ pxms_ptirc: 1000,
+ /*pxms_ptamp: 0x7fff,*/
+
+ pxms_hal: 0x40,
+};
+
+pxmc_state_t mcs2 =
+{
+pxms_flg:
+ PXMS_ENI_m,
+pxms_do_inp:
+ pxmc_inp_rocon_inp,
+pxms_do_con:
+ pxmc_pid_con,
+pxms_do_out:
+ pxmc_rocon_pwm_dc_out,
+ pxms_do_deb: 0,
+ pxms_do_gen: 0,
+pxms_do_ap2hw:
+ pxmc_inp_rocon_ap2hw,
+ pxms_ap: 0, pxms_as: 0,
+ pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
+ pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
+ pxms_inp_info: 2,
+ pxms_out_info: 4,
+ pxms_ene: 0, pxms_erc: 0,
+ pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
+ pxms_me: 0x7e00/*0x7fff*/,
+pxms_cfg:
+ PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
+ PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
+ PXMS_CFG_HDIR_m | 0x2,
+
+ pxms_ptper: 1,
+ pxms_ptirc: 1000,
+ /*pxms_ptamp: 0x7fff,*/
+
+ pxms_hal: 0x40,
+};
+
+pxmc_state_t mcs3 =
+{
+pxms_flg:
+ PXMS_ENI_m,
+pxms_do_inp:
+ pxmc_inp_rocon_inp,
+pxms_do_con:
+ pxmc_pid_con,
+pxms_do_out:
+ pxmc_rocon_pwm_dc_out,
+ pxms_do_deb: 0,
+ pxms_do_gen: 0,
+pxms_do_ap2hw:
+ pxmc_inp_rocon_ap2hw,
+ pxms_ap: 0, pxms_as: 0,
+ pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
+ pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
+ pxms_inp_info: 3,
+ pxms_out_info: 6,
+ pxms_ene: 0, pxms_erc: 0,
+ pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
+ pxms_me: 0x7e00/*0x7fff*/,
+pxms_cfg:
+ PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
+ PXMS_CFG_I2PT_m * 0 | PXMS_CFG_HRI_m |
+ PXMS_CFG_HDIR_m * 0 | 0x2,
+
+ pxms_ptper: 1,
+ pxms_ptirc: 1000,
+ /*pxms_ptamp: 0x7fff,*/
+
+ pxms_hal: 0x40,
+};
+
+pxmc_state_t mcs4 =
+{
+pxms_flg:
+ PXMS_ENI_m,
+pxms_do_inp:
+ pxmc_inp_rocon_inp,
+pxms_do_con:
+ pxmc_pid_con,
+pxms_do_out:
+ pxmc_rocon_pwm_dc_out,
+ pxms_do_deb: 0,
+ pxms_do_gen: 0,
+pxms_do_ap2hw:
+ pxmc_inp_rocon_ap2hw,
+ pxms_ap: 0, pxms_as: 0,
+ pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
+ pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
+ pxms_inp_info: 4,
+ pxms_out_info: 8,
+ pxms_ene: 0, pxms_erc: 0,
+ pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
+ pxms_me: 0x7e00/*0x7fff*/,
+pxms_cfg:
+ PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m * 0 |
+ PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
+ PXMS_CFG_HDIR_m | 0x2,
+
+ pxms_ptper: 1,
+ pxms_ptirc: 1000,
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
};
-pxmc_state_t mcs1={
- pxms_flg:PXMS_ENI_m,
- pxms_do_inp:pxmc_inp_rocon_inp,
- pxms_do_con:pxmc_pid_con,
- pxms_do_out:pxmc_rocon_pwm_out,
- pxms_do_deb:0,
- pxms_do_gen:0,
- pxms_do_ap2hw:pxmc_inp_rocon_ap2hw,
- pxms_ap:0, pxms_as:0,
- pxms_rp: 55*256, pxms_rs:0, pxms_subdiv:8,
- pxms_md:800<<8, pxms_ms:1000, pxms_ma:10,
- pxms_inp_info:1,
- pxms_out_info:1,
- pxms_ene:0, pxms_erc:0,
- pxms_p:80, pxms_i:30, pxms_d:200, pxms_s1:200, pxms_s2:0,
- pxms_me:0x7e00/*0x7fff*/,
- pxms_cfg:PXMS_CFG_SMTH_m|PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m|PXMS_CFG_I2PT_m*0|
- 0x1,
-
- pxms_ptper:1,
- pxms_ptirc:1000,
+pxmc_state_t mcs5 =
+{
+pxms_flg:
+ PXMS_ENI_m,
+pxms_do_inp:
+ pxmc_inp_rocon_inp,
+pxms_do_con:
+ pxmc_pid_con,
+pxms_do_out:
+ pxmc_rocon_pwm_dc_out,
+ pxms_do_deb: 0,
+ pxms_do_gen: 0,
+pxms_do_ap2hw:
+ pxmc_inp_rocon_ap2hw,
+ pxms_ap: 0, pxms_as: 0,
+ pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
+ pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
+ pxms_inp_info: 5,
+ pxms_out_info: 10,
+ pxms_ene: 0, pxms_erc: 0,
+ pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
+ pxms_me: 0x7e00/*0x7fff*/,
+pxms_cfg:
+ PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_HPS_m |
+ PXMS_CFG_HRI_m | PXMS_CFG_I2PT_m * 0 |
+ PXMS_CFG_HDIR_m | 0x2,
+
+ pxms_ptper: 1,
+ pxms_ptirc: 1000,
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
};
-pxmc_state_t mcs2={
- pxms_flg:PXMS_ENI_m,
- pxms_do_inp:pxmc_inp_rocon_inp,
- pxms_do_con:pxmc_pid_con,
- pxms_do_out:pxmc_rocon_pwm_out,
- pxms_do_deb:0,
- pxms_do_gen:0,
- pxms_do_ap2hw:pxmc_inp_rocon_ap2hw,
- pxms_ap:0, pxms_as:0,
- pxms_rp: 55*256, pxms_rs:0, pxms_subdiv:8,
- pxms_md:800<<8, pxms_ms:1000, pxms_ma:10,
- pxms_inp_info:2,
- pxms_out_info:2,
- pxms_ene:0, pxms_erc:0,
- pxms_p:80, pxms_i:30, pxms_d:200, pxms_s1:200, pxms_s2:0,
- pxms_me:0x7e00/*0x7fff*/,
- pxms_cfg:PXMS_CFG_SMTH_m|PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m|PXMS_CFG_I2PT_m*0|
- 0x1,
-
- pxms_ptper:1,
- pxms_ptirc:1000,
+pxmc_state_t mcs6 =
+{
+pxms_flg:
+ PXMS_ENI_m,
+pxms_do_inp:
+ pxmc_inp_rocon_inp,
+pxms_do_con:
+ pxmc_pid_con,
+pxms_do_out:
+ pxmc_rocon_pwm_dc_out,
+ pxms_do_deb: 0,
+ pxms_do_gen: 0,
+pxms_do_ap2hw:
+ pxmc_inp_rocon_ap2hw,
+ pxms_ap: 0, pxms_as: 0,
+ pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
+ pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
+ pxms_inp_info: 6,
+ pxms_out_info: 12,
+ pxms_ene: 0, pxms_erc: 0,
+ pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
+ pxms_me: 0x7e00/*0x7fff*/,
+pxms_cfg:
+ PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
+ 0x1,
+
+ pxms_ptper: 1,
+ pxms_ptirc: 1000,
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
};
-pxmc_state_t mcs3={
- pxms_flg:PXMS_ENI_m,
- pxms_do_inp:pxmc_inp_rocon_inp,
- pxms_do_con:pxmc_pid_con,
- pxms_do_out:pxmc_rocon_pwm_out,
- pxms_do_deb:0,
- pxms_do_gen:0,
- pxms_do_ap2hw:pxmc_inp_rocon_ap2hw,
- pxms_ap:0, pxms_as:0,
- pxms_rp: 55*256, pxms_rs:0, pxms_subdiv:8,
- pxms_md:800<<8, pxms_ms:1000, pxms_ma:10,
- pxms_inp_info:3,
- pxms_out_info:3,
- pxms_ene:0, pxms_erc:0,
- pxms_p:80, pxms_i:30, pxms_d:200, pxms_s1:200, pxms_s2:0,
- pxms_me:0x7e00/*0x7fff*/,
- pxms_cfg:PXMS_CFG_SMTH_m|PXMS_CFG_MD2E_m|PXMS_CFG_HLS_m|PXMS_CFG_I2PT_m*0|
- 0x1,
-
- pxms_ptper:1,
- pxms_ptirc:1000,
+pxmc_state_t mcs7 =
+{
+pxms_flg:
+ PXMS_ENI_m,
+pxms_do_inp:
+ pxmc_inp_rocon_inp,
+pxms_do_con:
+ pxmc_pid_con,
+pxms_do_out:
+ pxmc_rocon_pwm_dc_out,
+ pxms_do_deb: 0,
+ pxms_do_gen: 0,
+pxms_do_ap2hw:
+ pxmc_inp_rocon_ap2hw,
+ pxms_ap: 0, pxms_as: 0,
+ pxms_rp: 55 * 256, pxms_rs: 0, pxms_subdiv: 8,
+ pxms_md: 800 << 8, pxms_ms: 500, pxms_ma: 10,
+ pxms_inp_info: 7,
+ pxms_out_info: 14,
+ pxms_ene: 0, pxms_erc: 0,
+ pxms_p: 80, pxms_i: 10, pxms_d: 200, pxms_s1: 200, pxms_s2: 0,
+ pxms_me: 0x7e00/*0x7fff*/,
+pxms_cfg:
+ PXMS_CFG_SMTH_m | PXMS_CFG_MD2E_m | PXMS_CFG_HLS_m | PXMS_CFG_I2PT_m * 0 |
+ 0x1,
+
+ pxms_ptper: 1,
+ pxms_ptirc: 1000,
/*pxms_ptamp: 0x7fff,*/
pxms_hal: 0x40,
};
-pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT]=
- {&mcs0,&mcs1,&mcs2,&mcs3};
+pxmc_state_t *pxmc_main_arr[PXML_MAIN_CNT] =
+{&mcs0, &mcs1, &mcs2, &mcs3, &mcs4, &mcs5, &mcs6, &mcs7};
-pxmc_state_list_t pxmc_main_list={
- pxml_arr:pxmc_main_arr,
- pxml_cnt:0
+pxmc_state_list_t pxmc_main_list =
+{
+pxml_arr:
+ pxmc_main_arr,
+ pxml_cnt: 0
};
void pxmc_lpc_qei_callback_index(struct lpc_qei_state_t *qst, void *context)
{
- pxmc_state_t *mcs=(pxmc_state_t *)context;
+ pxmc_state_t *mcs = (pxmc_state_t *)context;
short ofs;
short irc;
- irc=qst->index_pos;
+ irc = qst->index_pos;
- if((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg&PXMS_PTI_m)) {
+ if ((mcs->pxms_cfg & PXMS_CFG_I2PT_m) && (mcs->pxms_flg & PXMS_PTI_m))
+ {
short diff;
- ofs=irc-mcs->pxms_ptmark;
- diff=ofs-mcs->pxms_ptofs;
- if(diff>=mcs->pxms_ptirc/2)
- diff-=mcs->pxms_ptirc;
- if(diff<=-mcs->pxms_ptirc/2)
- diff+=mcs->pxms_ptirc;
- if(diff<0)
- diff=-diff;
- if(diff>=mcs->pxms_ptirc/6) {
+ ofs = irc - mcs->pxms_ptmark;
+ diff = ofs - mcs->pxms_ptofs;
+
+ if (diff >= mcs->pxms_ptirc / 2)
+ diff -= mcs->pxms_ptirc;
+
+ if (diff <= -mcs->pxms_ptirc / 2)
+ diff += mcs->pxms_ptirc;
+
+ if (diff < 0)
+ diff = -diff;
+
+ if (diff >= mcs->pxms_ptirc / 6)
+ {
pxmc_set_errno(mcs, PXMS_E_I2PT_TOOBIG);
- } else {
+ }
+ else
+ {
mcs->pxms_ptofs = ofs;
- pxmc_set_flag(mcs,PXMS_PHA_b);
+ pxmc_set_flag(mcs, PXMS_PHA_b);
}
} /*else {
+
ofs=irc-mcs->pxms_ptofs;
if((unsigned short)ofs>=(unsigned short)mcs->pxms_ptirc) {
if(ofs>0) {
int irc = lpc_qei_get_pos(qst);
int idx_rel;
- if(!qst->index_occ)
+ if (!qst->index_occ)
return 0;
- idx_rel=qst->index_pos-irc;
- idx_rel%=mcs->pxms_ptirc;
- if(idx_rel<0)
- idx_rel+=mcs->pxms_ptirc;
+ idx_rel = qst->index_pos - irc;
+ idx_rel %= mcs->pxms_ptirc;
- ptofs=irc-mcs->pxms_ptofs;
- ptmark=ptofs+idx_rel;
+ if (idx_rel < 0)
+ idx_rel += mcs->pxms_ptirc;
- if((unsigned short)ptmark>=mcs->pxms_ptirc) {
- if(ptmark>0)
- ptmark-=mcs->pxms_ptirc;
+ ptofs = irc - mcs->pxms_ptofs;
+ ptmark = ptofs + idx_rel;
+
+ if ((unsigned short)ptmark >= mcs->pxms_ptirc)
+ {
+ if (ptmark > 0)
+ ptmark -= mcs->pxms_ptirc;
else
- ptmark+=mcs->pxms_ptirc;
+ ptmark += mcs->pxms_ptirc;
}
- if((unsigned short)ptmark<mcs->pxms_ptirc) {
- mcs->pxms_ptmark=ptmark;
+ if ((unsigned short)ptmark < mcs->pxms_ptirc)
+ {
+ mcs->pxms_ptmark = ptmark;
}
+
return 0;
}
int res;
long r2acq;
long spd;
- pxmc_call_t *fin_fnc=pxmc_lpc_pthalalign_callback;
+ pxmc_call_t *fin_fnc = pxmc_lpc_pthalalign_callback;
lpc_qei_state_t *qst = &lpc_qei_state;
- mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
+ mcs->pxms_cfg &= ~PXMS_CFG_I2PT_m;
lpc_qei_setup_index_catch(qst);
- r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
- spd=1<<PXMC_SUBDIV(mcs);
+ r2acq = ((long)mcs->pxms_ptirc << PXMC_SUBDIV(mcs)) * periods;
+ spd = 1 << PXMC_SUBDIV(mcs);
- res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
+ res = pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
return res;
}
{
int var;
pxmc_state_t *mcs;
- pxmc_for_each_mcs(var, mcs) {
+ pxmc_for_each_mcs(var, mcs)
+ {
/* PXMS_ENI_m - check if input (IRC) update is enabled */
- if (mcs->pxms_flg&PXMS_ENI_m) {
+ if (mcs->pxms_flg & PXMS_ENI_m)
+ {
pxmc_call(mcs, mcs->pxms_do_inp);
}
}
{
int var;
pxmc_state_t *mcs;
- pxmc_for_each_mcs(var, mcs) {
+ pxmc_for_each_mcs(var, mcs)
+ {
/* PXMS_ENR_m - check if controller is enabled */
- if (mcs->pxms_flg&PXMS_ENR_m || mcs->pxms_flg&PXMS_ENO_m) {
+ if (mcs->pxms_flg & PXMS_ENR_m || mcs->pxms_flg & PXMS_ENO_m)
+ {
/* If output only is enabled, we skip the controller */
- if (mcs->pxms_flg&PXMS_ENR_m) {
+ if (mcs->pxms_flg & PXMS_ENR_m)
+ {
pxmc_call(mcs, mcs->pxms_do_con);
+
/* PXMS_ERR_m - if axis in error state */
- if (mcs->pxms_flg&PXMS_ERR_m) mcs->pxms_ene = 0;
+ if (mcs->pxms_flg & PXMS_ERR_m)
+ mcs->pxms_ene = 0;
}
/* for bushless motors, it is necessary to call do_out
{
int var;
pxmc_state_t *mcs;
- pxmc_for_each_mcs(var, mcs) {
+ pxmc_for_each_mcs(var, mcs)
+ {
/* PXMS_ENG_m - check if requested value (position) generator is enabled */
- if (mcs->pxms_flg&PXMS_ENG_m) {
+ if (mcs->pxms_flg & PXMS_ENG_m)
+ {
pxmc_call(mcs, mcs->pxms_do_gen);
}
}
{
int var;
pxmc_state_t *mcs;
- pxmc_for_each_mcs(var, mcs) {
- if (mcs->pxms_flg&PXMS_DBG_m) {
+ pxmc_for_each_mcs(var, mcs)
+ {
+ if (mcs->pxms_flg & PXMS_DBG_m)
+ {
pxmc_call(mcs, mcs->pxms_do_deb);
}
}
}
+int pxmc_rocon_pthalalign_callback(pxmc_state_t *mcs)
+{
+ short ptofs;
+ short ptmark;
+ int inp_chan = mcs->pxms_inp_info;
+ int idx_rel;
+ long irc = fpga_irc[inp_chan]->count;
+
+ if (!pxmc_inp_rocon_is_index_edge(mcs))
+ return 0;
+
+ idx_rel = fpga_irc[inp_chan]->count_index - irc;
+ idx_rel %= mcs->pxms_ptirc;
+ if(idx_rel < 0)
+ idx_rel += mcs->pxms_ptirc;
+
+ ptofs = irc-mcs->pxms_ptofs;
+ ptmark = ptofs+idx_rel;
+
+ if((unsigned short)ptmark >= mcs->pxms_ptirc) {
+ if(ptmark>0)
+ ptmark -= mcs->pxms_ptirc;
+ else
+ ptmark += mcs->pxms_ptirc;
+ }
+
+ if((unsigned short)ptmark < mcs->pxms_ptirc) {
+ mcs->pxms_ptmark = ptmark;
+ }
+ return 0;
+}
+
+int pxmc_rocon_pthalalign(pxmc_state_t *mcs, int periods)
+{
+ int res;
+ long r2acq;
+ long spd;
+ pxmc_call_t *fin_fnc = pxmc_rocon_pthalalign_callback;
+
+ mcs->pxms_cfg&=~PXMS_CFG_I2PT_m;
+
+ r2acq=((long)mcs->pxms_ptirc<<PXMC_SUBDIV(mcs))*periods;
+ spd=1<<PXMC_SUBDIV(mcs);
+
+ /* clear bit indicating previous index */
+ pxmc_inp_rocon_is_index_edge(mcs);
+
+ res=pxmc_pthalalign(mcs, r2acq, spd, fin_fnc);
+
+ return res;
+}
+
+int pxmc_axis_rdmode(pxmc_state_t *mcs)
+{
+ if (mcs->pxms_do_out == pxmc_rocon_pwm2ph_out)
+ return PXMC_AXIS_MODE_STEPPER_WITH_IRC;
+ if (mcs->pxms_do_out == pxmc_rocon_pwm_dc_out)
+ return PXMC_AXIS_MODE_DC;
+ if (mcs->pxms_do_out == pxmc_rocon_pwm3ph_out)
+ return PXMC_AXIS_MODE_BLDC;
+ return -1;
+}
+
+
int
pxmc_axis_pt4mode(pxmc_state_t *mcs, int mode)
{
- int res;
+ static const typeof(*mcs->pxms_ptptr1) dummy0 = 0;
+ int res = 0;
- mcs->pxms_ptvang=pxmc_ptvang_deg2irc(mcs,90);
+ if (mode == PXMC_AXIS_MODE_NOCHANGE)
+ mode = pxmc_axis_rdmode(mcs);
+ if (mode < 0)
+ return -1;
- /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
- #ifndef PXMC_WITH_PT_ZIC
- res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
- #else /*PXMC_WITH_PT_ZIC*/
- res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
- #endif /*PXMC_WITH_PT_ZIC*/
+ switch (mode) {
+ /*case PXMC_AXIS_MODE_STEPPER:*/
+ case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
+ res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin, 0, 0);
+ break;
+ /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
+ case PXMC_AXIS_MODE_DC:
+ /*profive some sane dummy values*/
+ mcs->pxms_ptptr1 = (typeof(mcs->pxms_ptptr1))&dummy0;
+ mcs->pxms_ptptr2 = (typeof(mcs->pxms_ptptr1))&dummy0;
+ mcs->pxms_ptptr3 = (typeof(mcs->pxms_ptptr1))&dummy0;
+
+ mcs->pxms_ptscale_mult=1;
+ mcs->pxms_ptscale_shift=15;
+ break;
+ case PXMC_AXIS_MODE_BLDC:
+ /* res = pxmc_init_ptable(mcs, PXMC_PTPROF_SIN3FUP); */
+ #ifndef PXMC_WITH_PT_ZIC
+ res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup, 0, 0);
+ #else /*PXMC_WITH_PT_ZIC*/
+ res = pxmc_ptable_set_profile(mcs, &pxmc_ptprofile_sin3phup_zic, 0, 0);
+ #endif /*PXMC_WITH_PT_ZIC*/
+ break;
+ default:
+ return -1;
+ }
+
+ mcs->pxms_ptvang = pxmc_ptvang_deg2irc(mcs, 90);
return res;
}
{
int res;
- pxmc_set_const_out(mcs,0);
- pxmc_clear_flag(mcs,PXMS_ENI_b);
- pxmc_clear_flag(mcs,PXMS_PHA_b);
- pxmc_clear_flag(mcs,PXMS_PTI_b);
+ pxmc_set_const_out(mcs, 0);
+ pxmc_clear_flag(mcs, PXMS_ENI_b);
+ pxmc_clear_flags(mcs,PXMS_ENO_m);
+ /* Clear possible stall index flags from hardware */
+ pxmc_inp_rocon_is_index_edge(mcs);
+ pxmc_clear_flag(mcs, PXMS_PHA_b);
+ pxmc_clear_flag(mcs, PXMS_PTI_b);
- if(!mode){
- /*mode=pxmc_axis_rdmode(mcs);*/
- if(!mode) mode=PXMC_AXIS_MODE_BLDC;
- }
- res=pxmc_axis_pt4mode(mcs, mode);
+ if (mode == PXMC_AXIS_MODE_NOCHANGE)
+ mode = pxmc_axis_rdmode(mcs);
+ if (mode < 0)
+ return -1;
+ if (!mode)
+ mode = PXMC_AXIS_MODE_DC;
+
+ res = pxmc_axis_pt4mode(mcs, mode);
+ if (res < 0)
+ return -1;
+
+ switch (mode) {
+ /*case PXMC_AXIS_MODE_STEPPER:*/
+ case PXMC_AXIS_MODE_STEPPER_WITH_IRC:
+ mcs->pxms_do_out = pxmc_rocon_pwm2ph_out;
+ break;
+ /*case PXMC_AXIS_MODE_STEPPER_WITH_PWM:*/
+ case PXMC_AXIS_MODE_DC:
+ mcs->pxms_do_out = pxmc_rocon_pwm_dc_out;
+ break;
+ case PXMC_AXIS_MODE_BLDC:
+ mcs->pxms_do_out = pxmc_rocon_pwm3ph_out;
+ break;
+ default:
+ return -1;
+ }
- pxmc_set_flag(mcs,PXMS_ENI_b);
+ /* Clear possible stall index flags from hardware */
+ pxmc_inp_rocon_is_index_edge(mcs);
+ /* Request new phases alignment for changed parameters */
+ pxmc_clear_flag(mcs, PXMS_PHA_b);
+ pxmc_clear_flag(mcs, PXMS_PTI_b);
+ pxmc_set_flag(mcs, PXMS_ENI_b);
return res;
}
void pxmc_sfi_isr(void)
{
+ unsigned long spent = pxmc_fast_tick_time();
+
pxmc_sfi_input();
pxmc_sfi_controller_and_output();
pxmc_sfi_generator();
pxmc_sfi_dbg();
+ /* Kick LX Master watchdog */
+ if (pxmc_main_list.pxml_cnt != 0)
+ *fpga_lx_master_transmitter_wdog = 1;
+
+ spent = pxmc_fast_tick_time() - spent;
+
+ if(spent > pxmc_sfi_spent_time_max)
+ pxmc_sfi_spent_time_max = spent;
+
+}
+
+int pxmc_clear_power_stop(void)
+{
+ return 0;
+}
+
+int pxmc_process_state_check(unsigned long *pbusy_bits,
+ unsigned long *perror_bits)
+{
+ unsigned short flg;
+ unsigned short chan;
+ unsigned long busy_bits = 0;
+ unsigned long error_bits = 0;
+ pxmc_state_t *mcs;
+ flg=0;
+ pxmc_for_each_mcs(chan, mcs) {
+ if(mcs) {
+ flg |= mcs->pxms_flg;
+ if (mcs->pxms_flg & PXMS_BSY_m)
+ busy_bits |= 1 << chan;
+ if (mcs->pxms_flg & PXMS_ERR_m)
+ error_bits |= 1 << chan;
+ }
+ }
+ if (appl_errstop_mode) {
+ if((flg & PXMS_ENG_m) && (flg & PXMS_ERR_m)) {
+ pxmc_for_each_mcs(chan, mcs) {
+ if(mcs&&(mcs->pxms_flg & PXMS_ENG_m)) {
+ pxmc_stop(mcs, 0);
+ }
+ }
+ }
+ }
+
+ if (pbusy_bits != NULL)
+ *pbusy_bits = busy_bits;
+ if (error_bits != NULL)
+ *perror_bits = error_bits;
+
+ return flg;
+}
+
+int pxmc_done(void)
+{
+ int var;
+ pxmc_state_t *mcs;
+
+ if (!pxmc_main_list.pxml_cnt)
+ return 0;
+
+ pxmc_for_each_mcs(var, mcs)
+ {
+ pxmc_set_const_out(mcs,0);
+ }
+
+ pxmc_main_list.pxml_cnt = 0;
+ __memory_barrier();
+
+ return 0;
}
int pxmc_initialize(void)
{
int res;
+ int i;
- pxmc_state_t *mcs=&mcs0;
+ pxmc_state_t *mcs = &mcs0;
lpc_qei_state_t *qst = &lpc_qei_state;
+ *fpga_irc_reset = 1;
+
+ for (i = 0; i < 8; i++) {
+ fpga_irc[i]->count = 0;
+ fpga_irc[i]->count_index = 0;
+ *fpga_irc_state[i] = FPGA_IRC_STATE_INDEX_EVENT_MASK;
+ }
+
/* Initialize QEI module for IRC counting */
pxmc_inp_lpc_qei_init(mcs->pxms_inp_info);
/* Initialize QEI events processing */
- if(lpc_qei_setup_irq(qst)<0)
+ if (lpc_qei_setup_irq(qst) < 0)
return -1;
- res=pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
+ *fpga_irc_reset = 0;
+
+ //res = pxmc_axis_pt4mode(mcs, PXMC_AXIS_MODE_BLDC);
/*pxmc_ctm4pwm3f_wr(mcs, 0, 0, 0);*/
- pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
+ //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
- pxmc_rocon_pwm_init(mcs, 0);
+ pxmc_rocon_pwm_master_init();
+ #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;
+ pxmc_main_list.pxml_cnt = 0;
+ pxmc_dbg_hist = NULL;
__memory_barrier();
- pxmc_main_list.pxml_cnt=PXML_MAIN_CNT;
+ pxmc_main_list.pxml_cnt = PXML_MAIN_CNT;
return 0;
}
projects / fpga / lx-cpu1 / lx-rocon.git / blobdiff