From 48407cad35d152771ac1a86652dae3eede6741a2 Mon Sep 17 00:00:00 2001 From: Martin Prudek Date: Wed, 20 May 2015 19:08:21 +0200 Subject: [PATCH] Speed, index and position computations moved to separate file. --- pmsm-control/test_sw/Makefile | 4 +- pmsm-control/test_sw/comp.c | 108 +++++++++++++++++++++++++++++++ pmsm-control/test_sw/comp.h | 37 +++++++++++ pmsm-control/test_sw/main_pmsm.c | 105 ++---------------------------- 4 files changed, 151 insertions(+), 103 deletions(-) create mode 100644 pmsm-control/test_sw/comp.c create mode 100644 pmsm-control/test_sw/comp.h diff --git a/pmsm-control/test_sw/Makefile b/pmsm-control/test_sw/Makefile index 24d2ea5..778b336 100644 --- a/pmsm-control/test_sw/Makefile +++ b/pmsm-control/test_sw/Makefile @@ -49,5 +49,5 @@ blikej: howto_gpio.o rpi_hw.o spi: rp_spi.o gcc -o spi rp_spi.c #pro rpi -pmsm: main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o - gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o -lpthread -lrt +pmsm: main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o comp.o + gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o misc.o pxmc_sin_fixtab.o cmd_proc.o controllers.o commutators.o comp.o -lpthread -lrt diff --git a/pmsm-control/test_sw/comp.c b/pmsm-control/test_sw/comp.c new file mode 100644 index 0000000..49d3546 --- /dev/null +++ b/pmsm-control/test_sw/comp.c @@ -0,0 +1,108 @@ +/** + * \brief + * Computatations of position, index, speed. + * + */ + +#include "comp.h" +#include "pmsm_state.h" +#include "rp_spi.h" + +/** + * \brief + * Substact initial position. + */ +void substractOffset(struct rpi_in* data, struct rpi_in* offset){ + data->pozice=data->pozice_raw-offset->pozice_raw; + return; +} + +/* + * \brief + * Multiplication of 11 bit + * Zaporne vysledky prvede na nulu. + */ +uint16_t mult_cap(int32_t s,int d){ + int j; + int res=0; + for(j=0;j!=11;j++){ + /* multiplicate as if maximum sinus value was unity */ + res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j)); + } + return res; +} + + +/** + * \brief + * Computation of distance to index. + * + * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu + * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu - + * to je 3999 bodu + * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate + * signalu indexu + */ +void comIndDist(struct rpi_state* this){ + uint16_t pos = 0x0FFF & this->spi_dat->pozice_raw; + uint16_t dist; + uint16_t index = this->spi_dat->index_position; + + if (index<1999){ /*index e<0,1998> */ + if (pos */ + /*proti smeru h.r. od indexu*/ + dist=pos+2000-index; + }else if (pos<=index+1999){ /*pozice e */ + /*po smeru h.r. od indexu*/ + dist=pos-index; + }else if (pos */ + goto index_lost; + }else{ /*pozice e */ + /*proti smeru h.r. od indexu - podtecena pozice*/ + dist=pos-index-2096; + } + }else if (index<=2096){ /*index e<1999,2096>*/ + if (pos */ + goto index_lost; + }else if (pos */ + /*proti smeru h.r. od indexu*/ + dist=pos+2000-index; + }else if (pos<=index+1999){ /*pozice e */ + /*po smeru h.r. od indexu*/ + dist=pos-index; + }else { /*pozice e */ + goto index_lost; + } + }else{ /*index e<2097,4095> */ + if (pos<=index-2097){ /*pozice e<0,index-2097> */ + /*po smeru h.r. od indexu - pretecena pozice*/ + dist=4096+pos-index; + }else if (pos */ + goto index_lost; + }else if (pos */ + /*proti smeru h.r. od indexu*/ + dist=pos+2000-index; + }else{ /*pozice e */ + /*po smeru h.r. od indexu*/ + dist=pos-index; + } + } + + this->index_dist = dist; + return; + + index_lost: + this->index_ok=0; + return; +} + + +/* + * \brief + * Computate speed. + */ +void compSpeed(struct rpi_state* this){ + signed long int spd; + spd=this->spi_dat->pozice-this->old_pos[this->tf_count%OLD_POS_NUM]; + this->speed=(int32_t)spd; +} diff --git a/pmsm-control/test_sw/comp.h b/pmsm-control/test_sw/comp.h new file mode 100644 index 0000000..d71f066 --- /dev/null +++ b/pmsm-control/test_sw/comp.h @@ -0,0 +1,37 @@ +/** + * \brief + * Computatations of position, index, speed. + * + */ +#ifndef COMP +#define COMP + +struct rpi_in; +struct rpi_state; + +/** + * \brief + * Substact initial position. + */ +void substractOffset(struct rpi_in* data, struct rpi_in* offset); + +/** + * \brief + * Computation of distance to index. + * + * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu + * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu - + * to je 3999 bodu + * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate + * signalu indexu + */ +void comIndDist(struct rpi_state*); + +/* + * \brief + * Computates speed. + */ +void compSpeed(struct rpi_state*); + +#endif /*COMP*/ + diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c index f4ce30b..56b6544 100644 --- a/pmsm-control/test_sw/main_pmsm.c +++ b/pmsm-control/test_sw/main_pmsm.c @@ -25,6 +25,7 @@ #include "cmd_proc.h" #include "controllers.h" #include "commutators.h" +#include "comp.h" @@ -163,13 +164,6 @@ void appl_stop(){ printf("\nprogram bezpecne ukoncen\n"); } -void substractOffset(struct rpi_in* data, struct rpi_in* offset){ - data->pozice=data->pozice_raw-offset->pozice_raw; - return; -} - - - /** * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru */ @@ -180,97 +174,6 @@ void * pos_monitor(void* param){ } return (void*)0; } -/* - * \brief - * Multiplication of 11 bit - * Zaporne vysledky prvede na nulu. - */ -inline uint16_t mult_cap(int32_t s,int d){ - int j; - int res=0; - for(j=0;j!=11;j++){ - /* multiplicate as if maximum sinus value was unity */ - res+=(!(s & 0x10000000))*(((1 << j) & s)>>j)*(d>>(10-j)); - } - return res; -} - - -/** - * \brief - * Computation of distance to index. - * - * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu - * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu - - * to je 3999 bodu - * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate - * signalu indexu - */ -void comIndDist(){ - uint16_t pos = 0x0FFF & data.pozice_raw; - uint16_t dist; - uint16_t index = data.index_position; - - if (index<1999){ /*index e<0,1998> */ - if (pos */ - /*proti smeru h.r. od indexu*/ - dist=pos+2000-index; - }else if (pos<=index+1999){ /*pozice e */ - /*po smeru h.r. od indexu*/ - dist=pos-index; - }else if (pos */ - goto index_lost; - }else{ /*pozice e */ - /*proti smeru h.r. od indexu - podtecena pozice*/ - dist=pos-index-2096; - } - }else if (index<=2096){ /*index e<1999,2096>*/ - if (pos */ - goto index_lost; - }else if (pos */ - /*proti smeru h.r. od indexu*/ - dist=pos+2000-index; - }else if (pos<=index+1999){ /*pozice e */ - /*po smeru h.r. od indexu*/ - dist=pos-index; - }else { /*pozice e */ - goto index_lost; - } - }else{ /*index e<2097,4095> */ - if (pos<=index-2097){ /*pozice e<0,index-2097> */ - /*po smeru h.r. od indexu - pretecena pozice*/ - dist=4096+pos-index; - }else if (pos */ - goto index_lost; - }else if (pos */ - /*proti smeru h.r. od indexu*/ - dist=pos+2000-index; - }else{ /*pozice e */ - /*po smeru h.r. od indexu*/ - dist=pos-index; - } - } - - rps.index_dist = dist; - return; - - index_lost: - rps.index_ok=0; - return; -} - - -/* - * \brief - * Computate speed. - */ -void compSpeed(){ - signed long int spd; - spd=rps.spi_dat->pozice-rps.old_pos[rps.tf_count%OLD_POS_NUM]; - rps.speed=(int32_t)spd; -} - - /* * \brief @@ -304,7 +207,7 @@ void * read_data(void* param){ /*subtract initiate postion */ rps.tf_count++; substractOffset(&data,poc.spi_dat); - compSpeed(); /*spocita rychlost*/ + compSpeed(&rps); /*spocita rychlost*/ if (!rps.index_ok){ if (first){ @@ -312,10 +215,10 @@ void * read_data(void* param){ first=0; }else if (last_index!=data.index_position){ rps.index_ok=1; - comIndDist(); /*vypocet vzdalenosti indexu*/ + comIndDist(&rps); /*vypocet vzdalenosti indexu*/ } }else{ /*index je v poradku*/ - comIndDist(); /*vypocet vzdalenosti indexu*/ + comIndDist(&rps); /*vypocet vzdalenosti indexu*/ } /* pocitame sirku plneni podle potreb rizeni*/ -- 2.39.2