#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 HAL_ERR_SENSITIVITY 20
#define HAL_ERR_MAX_COUNT 5
+#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];
return &pxmc_rocon_receiver_dummy_reg;
rec_reg = fpga_lx_master_receiver_base;
- #if 0 /* FPGA design version 2 */
- rec_reg += 1 + (chan >> 8) + chan;
- #else /* FPGA design version 3 */
- rec_reg += chan + 8;
- #endif
+
+ #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;
}
unsigned h = 0;
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 >> 14) & 1;
- h |= (*rec_reg_b >> 13) & 2;
- h |= (*rec_reg_c >> 12) & 4;
+ 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;
return &pxmc_rocon_pwm_dummy_reg;
pwm_reg = fpga_lx_master_transmitter_base;
- #if 0 /* FPGA design version 2 */
- pwm_reg += 1 + (chan >> 8) + chan;
- #else /* FPGA design version 3 */
- pwm_reg += chan + 8;
- #endif
+
+ #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;
}
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_rx_done_isr_setup(irq_handler_t rx_done_isr_handler)
+{
+
+ 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;
+
+ 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 < 8 + 16; i ++)
+ for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
fpga_lx_master_receiver_base[i] = 0;
- fpga_lx_master_receiver_base[grp_in++] = 0x0808;
+ 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 < 8 + 16; i ++)
+ for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
fpga_lx_master_transmitter_base[i] = 0;
- fpga_lx_master_transmitter_base[grp_out++] = 0x1008;
- fpga_lx_master_transmitter_base[grp_out++] = 0x0808;
+ 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;
}
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_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: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 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 |
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_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: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 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 |
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_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: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 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 |
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_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: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 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 |
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_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: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 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 |
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_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: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 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 |
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_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: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 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 |
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_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: 30, pxms_d: 200, pxms_s1: 200, pxms_s2: 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 |
//pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 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;