-/* Firmware file for lx-rocon tumbl coprocessor */
+/*******************************************************************
+ Components for embedded applications builded for
+ laboratory and medical instruments firmware
-#include <stdint.h>
+ firmware.c - multi axis motion controller comprocesor
+ firmware for FPGA tumble CPU of lx-rocon system
-typedef struct
-{
- int16_t p;
- int16_t i;
- int32_t irc_period;
- int16_t req_current;
- int16_t acc_dev;
- int16_t max_acc;
-} axis_settings;
-
-typedef struct
-{
- /* Input */
- int16_t current;
- int32_t irc;
- int32_t irc_last;
- int32_t irc_norm;
+ (C) 2001-2014 by Pavel Pisa pisa@cmp.felk.cvut.cz
+ (C) 2002-2014 by PiKRON Ltd. http://www.pikron.com
- /* Output */
- uint16_t pwm[3];
-} axis_io;
+ This file can be used and copied according to next
+ license alternatives
+ - GPL - GNU Public License
+ - other license provided by project originators
-axis_settings a_settings;
+ *******************************************************************/
-axis_io a_io;
-int32_t irc_a_reg;
-int16_t current_a_reg;
-uint32_t count;
-int16_t phase_table[1][1];
-void init_defvals()
-{
- a_settings.p = 20;
- a_settings.i = 2;
- a_settings.irc_period = 7000;
- a_settings.req_current = 500;
- a_settings.acc_dev = 0;
-}
+#include <stdint.h>
+#include "tumbl_addr.h"
-void update_axis(axis_settings *settings, axis_io* io)
-{
- int i;
- int16_t dev;
- int32_t mag, irc_diff;
+/* k3 = math.sqrt(3) / 2 */
- dev = settings->req_current - io->current;
- settings->acc_dev += dev;
+/* 1 / k3 * 65536 */
- if (settings->acc_dev > settings->max_acc)
- settings->acc_dev = settings->max_acc;
- else if (-(settings->acc_dev) <= -(settings->max_acc))
- settings->acc_dev = -(settings->max_acc);
+#define RECI16_K3 75674
- mag = settings->acc_dev * settings->i + dev * settings->p;
+/* 1 / (2 * k3) * 65536 */
+#define RECI16_2_K3 37837
- irc_diff = io->irc - io->irc_last;
- io->irc_last = io->irc;
- io->irc_norm += irc_diff;
+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 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;
- if (io->irc_norm > settings->irc_period)
- io->irc_norm -= settings->irc_period;
- else if (io->irc_norm < 0)
- io->irc_norm += settings->irc_period;
+ uint32_t act_idle;
+ uint32_t min_idle;
+ uint32_t rx_done_sqn;
+} pxmcc_axis_data_t;
- for (i = 0; i < 3; i++)
- io->pwm[i] = (uint16_t)((mag * phase_table[/*i*/ 0][/*irc_norm*/ 0]) >> 16);
+pxmcc_axis_data_t pxmcc_axis[1];
+
+void init_defvals(void)
+{
}
-void main()
+#if 0
+
+uint32_t sin_lat[7];
+
+void find_sin_lat(void)
{
- while (1)
- {
- update_axis(&a_settings, &a_io);
- a_io.irc = irc_a_reg;
- a_io.current = current_a_reg;
- count++;
- }
+ int i;
+ register uint32_t a0, a1, a2, a3, a4, a5;
+
+ *FPGA_FNCAPPROX_SIN = 0;
+
+ for (i = 0; i < 20; i++)
+ asm volatile("": : : "memory");
+
+ *FPGA_FNCAPPROX_SIN = 0x40000000;
+ a0 = *FPGA_FNCAPPROX_SIN;
+ a1 = *FPGA_FNCAPPROX_SIN;
+ a2 = *FPGA_FNCAPPROX_SIN;
+ a3 = *FPGA_FNCAPPROX_SIN;
+ a4 = *FPGA_FNCAPPROX_SIN;
+ a5 = *FPGA_FNCAPPROX_SIN;
+ asm volatile("": : : "memory");
+
+ 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->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->ptirc;
+ int32_t pti;
+ uint32_t pta;
+ uint32_t dummy;
+
+ pti = irc - ofs;
+ if ((uint32_t)pti >= per) {
+ if (pti < 0) {
+ ofs -= per;
+ } else {
+ ofs += per;
+ }
+ pti = irc - ofs;
+ pxmcc->ptofs = ofs;
+ }
+ pxmcc->ptindx = pti;
+
+ pta = pti * pxmcc->ptreci;
+
+ *FPGA_FNCAPPROX_SIN = pta;
+
+ dummy = *FPGA_FNCAPPROX_SIN;
+ dummy = *FPGA_FNCAPPROX_SIN;
+ dummy = *FPGA_FNCAPPROX_SIN;
+ pxmcc->ptsin = *FPGA_FNCAPPROX_SIN;
+ 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");
+ }
+ }
+}
projects / fpga / lx-cpu1 / lx-rocon.git / blobdiff