From 12bcd8daffd6c638c6966198b60719f55b56e443 Mon Sep 17 00:00:00 2001 From: Pavel Pisa Date: Wed, 10 Dec 2014 00:00:25 +0100 Subject: [PATCH] The first firmware version for DQ to 3 phase PWM computation. Signed-off-by: Pavel Pisa --- hw/lx-rocon_firmware/firmware.c | 158 +++++++++++++++++++++++++++++--- 1 file changed, 146 insertions(+), 12 deletions(-) diff --git a/hw/lx-rocon_firmware/firmware.c b/hw/lx-rocon_firmware/firmware.c index d08cb56..171b2a2 100644 --- a/hw/lx-rocon_firmware/firmware.c +++ b/hw/lx-rocon_firmware/firmware.c @@ -1,25 +1,63 @@ -/* Firmware file for lx-rocon tumbl coprocessor */ +/******************************************************************* + Components for embedded applications builded for + laboratory and medical instruments firmware + + firmware.c - multi axis motion controller comprocesor + firmware for FPGA tumble CPU of lx-rocon system + + (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz + (C) 2002-2014 by PiKRON Ltd. http://www.pikron.com + + This file can be used and copied according to next + license alternatives + - GPL - GNU Public License + - other license provided by project originators + + *******************************************************************/ + #include #include "tumbl_addr.h" +/* k3 = math.sqrt(3) / 2 */ + +/* 1 / k3 * 65536 */ + +#define RECI16_K3 75674 + +/* 1 / (2 * k3) * 65536 */ +#define RECI16_2_K3 37837 + typedef struct pxmcc_axis_data_t { + uint32_t ccflg; + int32_t pwm_d; + int32_t pwm_q; + uint32_t ptindx; /* index into phase table / irc in the cycle */ uint32_t ptirc; /* IRC count per phase table */ - uint32_t ptper; /* number of periods per table */ - uint32_t ptreci; /* number of periods per table */ + uint32_t ptreci; /* Reciprocal value of ptirc * 63356 */ uint32_t ptofs; /* offset between table and IRC counter */ int32_t ptsin; int32_t ptcos; + uint32_t ptphs; + int32_t pwm_alp; + int32_t pwm_bet; + int32_t pwm_bet2; + + uint32_t act_idle; + uint32_t min_idle; + uint32_t rx_done_sqn; } pxmcc_axis_data_t; -uint32_t sin_lat[7]; - pxmcc_axis_data_t pxmcc_axis[1]; void init_defvals(void) { } +#if 0 + +uint32_t sin_lat[7]; + void find_sin_lat(void) { int i; @@ -39,32 +77,42 @@ void find_sin_lat(void) a5 = *FPGA_FNCAPPROX_SIN; asm volatile("": : : "memory"); - sin_lat[0] = 0x1234; + sin_lat[0] = a0; sin_lat[1] = a1; sin_lat[2] = a2; sin_lat[3] = a3; sin_lat[4] = a4; sin_lat[5] = a5; sin_lat[6] = 0x4321; + sin_lat[0] = 0x1234; } +#endif + void main(void) { + uint32_t last_rx_done_sqn = 0; pxmcc_axis_data_t *pxmcc = pxmcc_axis; - pxmcc->ptirc = *FPGA_IRC0; - pxmcc->ptofs = pxmcc->ptirc; - pxmcc->ptper = 2000; - pxmcc->ptreci = 2147484; /* (1LL<<32)/ptper */ + pxmcc->ccflg = 0; + pxmcc->ptindx = 0; + pxmcc->ptofs = *FPGA_IRC0; + pxmcc->ptirc = 1000; + pxmcc->ptreci = 4294967; /* (1LL<<32)*ptper/ptirc */ + pxmcc->min_idle = 0; + pxmcc->pwm_d = 0; + pxmcc->pwm_q = 0; asm volatile("": : : "memory"); + #if 0 find_sin_lat(); + #endif while (1) { uint32_t irc = *FPGA_IRC0; uint32_t ofs = pxmcc->ptofs; - uint32_t per = pxmcc->ptper; + uint32_t per = pxmcc->ptirc; int32_t pti; uint32_t pta; uint32_t dummy; @@ -79,7 +127,7 @@ void main(void) pti = irc - ofs; pxmcc->ptofs = ofs; } - pxmcc->ptirc = pti; + pxmcc->ptindx = pti; pta = pti * pxmcc->ptreci; @@ -92,6 +140,92 @@ void main(void) dummy = *FPGA_FNCAPPROX_COS; pxmcc->ptcos = *FPGA_FNCAPPROX_COS; + if (pxmcc->ccflg) { + int32_t pwm_alp; + int32_t pwm_bet; + int32_t pwm_bet_div_2_k3; + uint32_t phs; + uint32_t pwm1; + uint32_t pwm2; + uint32_t pwm3; + + pwm_alp = pxmcc->pwm_d * pxmcc->ptcos + pxmcc->pwm_q * pxmcc->ptsin; + pwm_bet = pxmcc->pwm_d * pxmcc->ptsin - pxmcc->pwm_q * pxmcc->ptcos; + + pwm_bet_div_2_k3 = RECI16_2_K3 * (pwm_bet >> 16); + + if (pwm_bet > 0) + if (pwm_alp > 0) + /* pwm_bet > 2 * k3 * pwm_alp */ + if (pwm_bet_div_2_k3 > pwm_alp) + phs = 1; + else + phs = 0; + else + /* -pwm_bet > 2 * k3 * pwm_alp */ + if (pwm_bet_div_2_k3 < -pwm_alp) + phs = 2; + else + phs = 1; + else + if (pwm_alp > 0) + /* pwm_bet > -2 * k3 * pwm_alp */ + if (pwm_bet_div_2_k3 > -pwm_alp) + phs = 5; + else + phs = 4; + else + /* pwm_bet > 2 * k3 * u_alp */ + if (pwm_bet_div_2_k3 > pwm_alp) + phs = 3; + else + phs = 4; + + if (phs <= 1) { + /* pwm1 = pwm_alp + 1.0/(2.0*k3) * pwm_bet */ + pwm1 = (pwm_alp + pwm_bet_div_2_k3) >> 16; + /* pwm2 = 1/k3 * pwm_bet */ + pwm2 = pwm_bet_div_2_k3 >> 15; + pwm3 = 0; + } else if (phs <= 3) { + pwm1 = 0; + /* pwm2 = 1.0/(2.0*k3) * pwm_bet - pwm_alp */ + pwm2 = (pwm_bet_div_2_k3 - pwm_alp) >> 16; + /* pwm3 = -1.0/(2.0*k3) * pwm_bet - pwm_alp */ + pwm3 = (-pwm_bet_div_2_k3 - pwm_alp) >>16; + } else { + /* pwm1 = pwm_alp - 1.0/(2.0*k3) * pwm_bet */ + pwm1 = (pwm_alp - pwm_bet_div_2_k3) >>16; + pwm2 = 0; + /* pwm3 = -1/k3 * pwm_bet */ + pwm3 = -pwm_bet_div_2_k3 >> 15; + } + + pxmcc->ptphs = phs; + + *FPGA_LX_MASTER_TX_PWM0 = pwm2 | 0x4000; + *FPGA_LX_MASTER_TX_PWM1 = pwm3 | 0x4000; + *FPGA_LX_MASTER_TX_PWM2 = pwm1 | 0x4000; + } + asm volatile("": : : "memory"); + + { + uint32_t idlecnt = 0; + uint32_t sqn; + do { + sqn = *FPGA_LX_MASTER_RX_DDIV; + idlecnt++; + } while (sqn == last_rx_done_sqn); + pxmcc->act_idle = idlecnt; + if (((idlecnt < pxmcc->min_idle) || + (pxmcc->min_idle == 0)) && + last_rx_done_sqn) { + pxmcc->min_idle = idlecnt; + } + last_rx_done_sqn = sqn; + pxmcc->rx_done_sqn = last_rx_done_sqn; + asm volatile("": : : "memory"); + } } } -- 2.39.2