From 59b72ac21a7189e402f90a288889174af4fe3015 Mon Sep 17 00:00:00 2001 From: Martin Prudek Date: Wed, 15 Apr 2015 19:25:08 +0200 Subject: [PATCH] Added testing SW in folder test_sw. --- pmsm-control/test_sw/Makefile | 53 +++ pmsm-control/test_sw/README | 8 + pmsm-control/test_sw/main_pmsm.c | 251 +++++++++++++ pmsm-control/test_sw/misc.c | 28 ++ pmsm-control/test_sw/misc.h | 31 ++ pmsm-control/test_sw/rp_spi.c | 257 +++++++++++++ pmsm-control/test_sw/rp_spi.h | 34 ++ pmsm-control/test_sw/rpi_hw.c | 595 +++++++++++++++++++++++++++++++ pmsm-control/test_sw/rpin.h | 109 ++++++ 9 files changed, 1366 insertions(+) create mode 100644 pmsm-control/test_sw/Makefile create mode 100644 pmsm-control/test_sw/README create mode 100644 pmsm-control/test_sw/main_pmsm.c create mode 100644 pmsm-control/test_sw/misc.c create mode 100644 pmsm-control/test_sw/misc.h create mode 100644 pmsm-control/test_sw/rp_spi.c create mode 100644 pmsm-control/test_sw/rp_spi.h create mode 100644 pmsm-control/test_sw/rpi_hw.c create mode 100644 pmsm-control/test_sw/rpin.h diff --git a/pmsm-control/test_sw/Makefile b/pmsm-control/test_sw/Makefile new file mode 100644 index 0000000..be5ddb2 --- /dev/null +++ b/pmsm-control/test_sw/Makefile @@ -0,0 +1,53 @@ +#author Martin Prudek +#sestavuje klienta a server pro ovladani DC motoru pomoci raspberry Pi +all: + echo "scp server client_rt client_wirpi" + +#spoustime na desktopu +#prenese na raspberry zdrojak klienta + zdrojak wiringPi +transfer: + make scp + cd /home/warg/CloudStation/BP/wiringPi; \ + make scp + +#spoustime na raspberry, nainstaluje wiringPi +wirpi: + cd wiringPi-d42e831; \ + make + +#spoustime raspberry 1. nainstaluje wiringPi a sestavi clienta +build: + make wirpi + cd ~/DCMotor; \ + make client + +#spoustime na desktopu +#sestavi udp server +server: main_ref_srv.o udp_srv.o + gcc -o server main_ref_srv.o udp_srv.o -lpthread + +#pro rpi +#sestavi klienta s pouzitim wiringPi pro non-rt +client_wirpi: main_ctrl.o udp_cli.o wir_Pi_api.o pid.o misc.o + gcc -o client main_ctrl.o udp_cli.o pid.o web_srv.c web_dat.c misc.o wir_Pi_api.o -lpthread -lwiringPi + +#pro rpi +#sestavi klienta pro PREEMPT_RT +client_rt: main_ctrl.o loc_pos_file.o udp_cli.o pid.o rpi_hw.o web_srv.o web_dat.o misc.o + gcc -o client main_ctrl.o udp_cli.o pid.o web_srv.o web_dat.o misc.o loc_pos_file.o rpi_hw.o -lpthread -lrt + +#poustim z desktopu +#prenese zdrojove kody klienta na raspberry +scp: + scp -rp ./ pi@10.0.0.22:/home/pi/motor + +#poustime z RPi +#sestavi "blikej" - jednoduchy testovaci nastroj +blikej: howto_gpio.o rpi_hw.o + gcc -o blikej howto_gpio.o rpi_hw.o +#pro rpi +spi: rp_spi.o + gcc -o spi rp_spi.c +#pro rpi +pmsm: main_pmsm.o rp_spi.o rpi_hw.o misc.o + gcc -o pmsm_controll main_pmsm.o rp_spi.o rpi_hw.o misc.o -lpthread diff --git a/pmsm-control/test_sw/README b/pmsm-control/test_sw/README new file mode 100644 index 0000000..10087cc --- /dev/null +++ b/pmsm-control/test_sw/README @@ -0,0 +1,8 @@ +How to use makefile: + to transfer files to raspberry (on default ip 10.0.0.22) use + $make scp + to build project on raspberry use + $make pmsm + +NOTE: +The rest of the lines of Makefile is intended to operate projects that share part of source code in this directory. diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c new file mode 100644 index 0000000..ec33728 --- /dev/null +++ b/pmsm-control/test_sw/main_pmsm.c @@ -0,0 +1,251 @@ +/** + * \file main_pmsm.c + * \author Martin Prudek + * \brief Mainfile pro pmsm control. + */ + +#ifndef NULL +#define NULL (void*) 0 +#endif /*NULL*/ + + +#include /*exit*/ +#include /*signal handler Ctrl+C*/ +#include /*printf*/ +#include /*sheduler*/ +#include /*usleep*/ +#include /*threads*/ + +#include "rpin.h" /*gpclk*/ +#include "rp_spi.h" /*spi*/ +#include "misc.h" /*structure for priorities*/ + + +#define PRUM_PROUD 2061 +#define PRUM_SOUC 6183 + +#define PRIOR_KERN 50 +#define PRIOR_HIGH 49 +#define PRIOR_LOW 20 + +#define THREAD_SHARED 0 +#define INIT_VALUE 0 /*init value for semaphor*/ + +struct sigaction sighnd; /*struktura pro signal handler*/ +struct rpi_in data; + +uint8_t test; +uint16_t pwm1, pwm2, pwm3; + + +/** + * \brief Initilizes GPCLK. + */ +int clk_init() +{ + initialise(); /*namapovani gpio*/ + initClock(PLLD_500_MHZ, 10, 0); + gpioSetMode(4, FSEL_ALT0); + return 0; +} +/* + * \brief Terminates GPCLK. + */ + +inline void clk_disable(){ + termClock(0); +} + +/** + * \brief Signal handler pro Ctrl+C + */ +void sighnd_fnc(){ + spi_disable(); + clk_disable(); + printf("\nprogram bezpecne ukoncen\n"); + exit(0); +} + +void substractOffset(struct rpi_in* data, struct rpi_in* offset){ + data->pozice-=offset->pozice; + return; +} +/* + * pocita procentualni odchylku od prumerneho proudu + */ +float diff_p(int16_t value){ + return ((float)value-PRUM_PROUD)*100/PRUM_PROUD; +} +/* + * pocita procentualni odchylku od prumerneho souctu proudu + */ +float diff_s(int16_t value){ + return ((float)value-PRUM_SOUC)*100/PRUM_SOUC; +} +/* + * tiskne potrebna data + */ +void printData(struct rpi_in data){ + printf("\npozice=%d\n",(int32_t)data.pozice); + printf("hal1=%d, hal2=%d, hal3=%d\n",data.hal1,data.hal2,data.hal3); + printf("en1=%d, en2=%d, en3=%d (Predchozi hodnoty)\n",data.en1,data.en2,data.en3); + printf("shdn1=%d, shdn2=%d, shdn3=%d (P.h.)\n",data.shdn1,data.shdn2,data.shdn3); + printf("PWM1(10d5)b54=%d %d %d %d %d %d=b49(P.h.)\n",data.b54,data.b53,data.b52,data.b51,data.b50,data.b49); + printf("PWM2(10d4)b48=%d %d %d %d %d %d %d=b42(P.h.)\n",data.b48,data.b47,data.b46,data.b45,data.b44,data.b43,data.b42); + printf("PWM3(10d5)b41=%d %d %d %d %d %d=b36(P.h.)\n",data.b41,data.b40,data.b39,data.b38,data.b37,data.b36); + printf("(pwm1)proud1 (ch1)=%d (%2.2f%%) %x\n",data.ch1,diff_p(data.ch1),(uint16_t)data.ch1); + printf("(pwm2)proud2 (ch2)=%d (%2.2f%%) %x\n",data.ch2,diff_p(data.ch2),(uint16_t)data.ch2); + printf("(pwm3)proud3 (ch0)=%d (%2.2f%%) %x\n",data.ch0,diff_p(data.ch0),(uint16_t)data.ch0); + printf("soucet proudu=%d (%2.2f%%)\n",data.ch0+data.ch1+data.ch2,diff_s(data.ch0+data.ch1+data.ch2)); +} +void prepare_tx(uint8_t * tx){ + + /*Data format: + * tx[0] - bity 95 downto 88 - bits that are sent first + * tx[1] - bity 87 downto 80 + * tx[2] - bity 79 downto 72 + * tx[3] - bity 71 downto 64 + * tx[4] - bity 63 downto 56 + * tx[5] - bity 55 downto 48 + * tx[6] - bity 47 downto 40 + * tx[7] - bity 39 downto 32 + * tx[8] - bity 31 downto 24 + * tx[9] - bity 23 downto 16 + * tx[10] - bity 15 downto 8 + * tx[11] - bity 7 downto 0 + * + * bit 95 - ADC reset + * bit 94 - enable PWM1 + * bit 93 - enable PWM2 + * bit 92 - enable PWM3 + * bit 91 - shutdown1 + * bit 90 - shutdown2 + * bit 89 - shutdown3 + * . + * . + * . + * bits 34 .. 24 - match PWM1 + * bits 23 .. 13 - match PWM2 + * bit 11,12 - Unused + * bits 10 .. 0 - match PWM3 + */ + + + uint16_t tmp; + + /* keep the cap*/ + if (pwm1>2047) pwm1=2047; + if (pwm2>2047) pwm2=2047; + if (pwm3>2047) pwm3=2047; + + tx[0]=test; /*bit 94 - enable PWM1*/ + + /*pwm1*/ + tx[7]=(tx[7] & 0xF8) | (0x07 & ((uint8_t*)&pwm1)[1]); /*MSB*/ + tx[8]=((uint8_t*)&pwm1)[0]; /*LSB*/ + + /*pwm2*/ + tmp=pwm2; + tmp<<=5; + tx[9]=((uint8_t*)&tmp)[1]; /*MSB*/ + tx[10]=(tx[10] & 0x1F) | (0xE0 & ((uint8_t*)&tmp)[0]); /*LSB*/ + + /*pwm3*/ + tx[10]=(tx[10] & 0xF8) | (0x07 & ((uint8_t*)&pwm3)[1]); /*MSB*/ + tx[11]=((uint8_t*)&pwm3)[0]; /*LSB*/ + + +} +/** + * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru + */ +void * pos_monitor(void* param){ + set_priority(param); /*set priority*/ + while(1){ + printData(data); + usleep(1000000); /*1 Hz*/ + } + return (void*)0; +} + +/** + * Funkce pravidelne vycita data z motoru + */ +void * read_data(void* param){ + int i; + struct rpi_in pocatek; + uint8_t tx[]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ; + set_priority(param); /*set priority*/ + pocatek = spi_read(tx); + while(1){ + prepare_tx(tx); + data = spi_read(tx); + substractOffset(&data,&pocatek); + usleep(1000); /*1kHz*/ + } +} + +/** + * \brief Main function. + */ + +int main(){ + uint16_t tmp; + + /*nastaveni priorit vlaken*/ + struct thread_param tsp; + tsp.sch_policy = SCHED_FIFO; + + /*nastaveni signalu pro vypnuti pomoci Ctrl+C*/ + sighnd.sa_handler=&sighnd_fnc; + sigaction(SIGINT, &sighnd, NULL ); + + clk_init(); /* inicializace gpio hodin */ + spi_init(); /* iniicializace spi*/ + + /*semafor pro detekci zpracovani parametru vlaken*/ + sem_init(&thd_par_sem,THREAD_SHARED,INIT_VALUE); + + /*vlakna*/ + pthread_t tid; /*identifikator vlakna*/ + pthread_attr_t attr; /*atributy vlakna*/ + pthread_attr_init(&attr); /*inicializuj implicitni atributy*/ + + + + /*ziskavani dat z motoru*//*vysoka priorita*/ + tsp.sch_prior = PRIOR_HIGH; + pthread_create(&tid, &attr, read_data, (void*)&tsp); + + /*vypisovani lokalni pozice*//*nizka priorita*/ + tsp.sch_prior = PRIOR_LOW; + pthread_create(&tid, &attr, pos_monitor, (void*)&tsp); + + + /*muzeme zavrit semafor*/ + sem_destroy(&thd_par_sem); + + while (1){ + scanf("%u",&tmp); + printf("volba=%x\n",tmp); + switch (tmp){ + case 1: + scanf("%u",&pwm1); + break; + case 2: + scanf("%u",&pwm2); + break; + case 3: + scanf("%u",&pwm3); + break; + case 4: + scanf("%u",&test); + break; + + default: + break; + } + + } + return 0; +} diff --git a/pmsm-control/test_sw/misc.c b/pmsm-control/test_sw/misc.c new file mode 100644 index 0000000..78d4eb5 --- /dev/null +++ b/pmsm-control/test_sw/misc.c @@ -0,0 +1,28 @@ +/** + * \brief Implementace pomocnych funkci. + * \file misc.c + * \date Feb 1, 2015 + * \author Martin Prudek + * + * Implementace pomocnych funkci. + * Vetsinou pro komunikaci mezi vlakny, jejich sychronizaci a predavani priorit. + */ + +#include "misc.h" +#include + +/** + * nastavi prioritu a scheduler podle parametru + * funkce na konci vrati semafor, aby mohlo dojit k opetovne zmene parametru + * \param pointer na struct thread_sched_param + */ +void set_priority(void * param){ + struct sched_param sp; + struct thread_param *tp = ((struct thread_param*) param); + + sp.sched_priority = tp->sch_prior; /*vysoka priorita*/ + if(sched_setscheduler(0, tp->sch_policy, &sp) == -1) { + perror("pid sched_setscheduler failed"); + } + sem_post(&thd_par_sem); +} diff --git a/pmsm-control/test_sw/misc.h b/pmsm-control/test_sw/misc.h new file mode 100644 index 0000000..14b1089 --- /dev/null +++ b/pmsm-control/test_sw/misc.h @@ -0,0 +1,31 @@ +/** + * \brief Interface pro pomocne funkce. + * \file misc.h + * \date Feb 1, 2015 + * \author Martin Prudek + */ + +#ifndef MISC_H_ +#define MISC_H_ + +#include + +sem_t thd_par_sem; ///< semafor pro detekci zpracovani parametru noveho vlakna + +/* + * struktura predana novym vlaknum + */ +struct thread_param{ + int sch_policy; + int sch_prior; +}; + +/** + * nastavi prioritu a scheduler podle parametru + * na konci vrati semafor + * \param pointer na struct thread_sched_param + */ +extern void set_priority(void *); + + +#endif /* MISC_H_ */ diff --git a/pmsm-control/test_sw/rp_spi.c b/pmsm-control/test_sw/rp_spi.c new file mode 100644 index 0000000..a1de3ce --- /dev/null +++ b/pmsm-control/test_sw/rp_spi.c @@ -0,0 +1,257 @@ +/* + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rp_spi.h" /*struct rpi_in */ + +//#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) +#define ARRAY_SIZE 16 +static void pabort(const char *s) +{ + perror(s); + abort(); +} + +static const char *device = "/dev/spidev0.1"; +static uint8_t mode = 0; +static uint8_t bits = 8; +static uint32_t speed = 500000; +static uint16_t delay = 0; +static int fd; + +/** + * \brief Testovaci funkce. Odesle same nuly. Delka 64bit. Vysledek vypise. + */ +void transfer() +{ + int ret; + uint8_t tx[]={0,0,0,0,0,0,0,0} ;/*= { + 0x00, 0x00, 0x01, 0x23, 0x45, 0x67, + 0x89, 0xAB, 0xCD, 0xEF, 0x01, 0x23, + 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, + 0xCD, 0xEF + };/**/ + printf("ARRAY_SIZE=%d\n",ARRAY_SIZE); + uint8_t rx[ARRAY_SIZE] = {0, }; /* inicializace vsech prvku na nulu? */ + struct spi_ioc_transfer tr = { + .tx_buf = (unsigned long)tx, + .rx_buf = (unsigned long)rx, + .len = ARRAY_SIZE, + .delay_usecs = delay, + .speed_hz = speed, + .bits_per_word = bits, + }; + + ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); + if (ret < 1) + pabort("can't send spi message"); + + for (ret = 0; ret < ARRAY_SIZE; ret++) { + if (!(ret % 6)) + puts(""); + printf("%.2X ", rx[ret]); + } + puts(""); +} +struct rpi_in spi_read(uint8_t * tx) +{ + uint8_t *uint8_p; + uint16_t tmp; + int sign; + struct rpi_in in; + int ret; + + uint8_t rx[ARRAY_SIZE] = {0, }; /* inicializace vsech prvku na nulu? */ + struct spi_ioc_transfer tr = { + .tx_buf = (unsigned long)tx, + .rx_buf = (unsigned long)rx, + .len = ARRAY_SIZE, + .delay_usecs = delay, + .speed_hz = speed, + .bits_per_word = bits, + }; + + ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr); + /*vypisovani prichozich dat */ + /*/ + if (ret < 1) + pabort("can't send spi message"); + + for (ret = 0; ret < ARRAY_SIZE; ret++) { + if (!(ret % 6)) + puts(""); + printf("%.2X ", rx[ret]); + } + puts(""); + /*/ + + /*prichozi data: + * rx[0] - bity 95 downto 88 - bits, that come first + * rx[1] - bity 87 downto 80 + * rx[2] - bity 79 downto 72 + * rx[3] - bity 71 downto 64 + * rx[4] - bity 63 downto 56 + * rx[5] - bity 55 downto 48 + * rx[6] - bity 47 downto 40 + * rx[7] - bity 39 downto 32 + * rx[8] - bity 31 downto 24 + * rx[9] - bity 23 downto 16 + * rx[10] - bity 15 downto 8 + * rx[11] - bity 7 downto 0 + */ + + /*uprava endianity pozice*/ + uint8_p=(uint8_t*)&in.pozice; + /* x86 je Little-Endian */ + uint8_p[0]=rx[3]; /* LSB */ + uint8_p[1]=rx[2]; + uint8_p[2]=rx[1]; + uint8_p[3]=rx[0]; /*MSB*/ /*with sign bit*/ + uint8_p[4]=uint8_p[5]=uint8_p[6]=uint8_p[7]=0; + + /*halove sondy + * hal1 - bit63 + * hal2 - bit62 + * hal3 - bit61 + */ + in.hal1=!!(0x80 & rx[4]); + in.hal2=!!(0x40 & rx[4]); + in.hal3=!!(0x20 & rx[4]); + + /*pwm enable + * en1 - bit60 + * en2 - bit59 + * en2 - bit58 + */ + in.en1=!!(0x10 & rx[4]); + in.en2=!!(0x08 & rx[4]); + in.en3=!!(0x04 & rx[4]); + + /*shutdown + * shdn1 - bit57 + * shdn2 - bit56 + * shdn3 - bit55 + */ + in.shdn1=!!(0x02 & rx[4]); + in.shdn2=!!(0x01 & rx[4]); + in.shdn3=!!(0x80 & rx[5]); + + /*debug bits + * + */ + in.b54=!!(0x40 & rx[5]); + in.b53=!!(0x20 & rx[5]); + in.b52=!!(0x10 & rx[5]); + in.b51=!!(0x08 & rx[5]); + in.b50=!!(0x04 & rx[5]); + in.b49=!!(0x02 & rx[5]); + in.b48=!!(0x01 & rx[5]); + in.b47=!!(0x80 & rx[6]); + in.b46=!!(0x40 & rx[6]); + in.b45=!!(0x20 & rx[6]); + in.b44=!!(0x10 & rx[6]); + in.b43=!!(0x08 & rx[6]); + in.b42=!!(0x04 & rx[6]); + in.b41=!!(0x02 & rx[6]); + in.b40=!!(0x01 & rx[6]); + in.b39=!!(0x80 & rx[7]); + in.b38=!!(0x40 & rx[7]); + in.b37=!!(0x20 & rx[7]); + in.b36=!!(0x10 & rx[7]); + + /** currents + * ch0 - bits 35 downto 24 + * 35..32 in rx[7] - last 4 + * 31..24 in rx[8] - all bytte + * ch1 - bits 23 downto 12 + * 23..16 in rx[9] - all byte + * 15..12 in rx[10] - first 4 + * ch2 - bits 11 downto 0 + * 11..8 in rx[10] - last 4 + * 7..0 in rx[11] - all byte + */ + + in.ch0=0x0F & rx[7]; + in.ch0<<=8; + in.ch0|=rx[8]; + + in.ch1= rx[9]; + in.ch1<<=8; + in.ch1|=(rx[10] & 0xF0); + in.ch1>>=4; + + in.ch2=(0xF & rx[10]); + in.ch2<<=8; + in.ch2|=rx[11]; + + + return in; + + +} + +int spi_init() +{ + int ret = 0; + + fd = open(device, O_RDWR); + if (fd < 0) + pabort("can't open device"); + printf("device open\n"); + /* + * spi mode + */ + ret = ioctl(fd, SPI_IOC_WR_MODE, &mode); + if (ret == -1) + pabort("can't set spi mode"); + + ret = ioctl(fd, SPI_IOC_RD_MODE, &mode); + if (ret == -1) + pabort("can't get spi mode"); + + /* + * bits per word + */ + ret = ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &bits); + if (ret == -1) + pabort("can't set bits per word"); + + ret = ioctl(fd, SPI_IOC_RD_BITS_PER_WORD, &bits); + if (ret == -1) + pabort("can't get bits per word"); + + /* + * max speed hz + */ + ret = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed); + if (ret == -1) + pabort("can't set max speed hz"); + + ret = ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &speed); + if (ret == -1) + pabort("can't get max speed hz"); + + printf("spi mode: %d\n", mode); + printf("bits per word: %d\n", bits); + printf("delay: %d\n", delay); + printf("max speed: %d Hz (%d KHz)\n", speed, speed/1000); + + + return ret; +} + +void spi_disable(){ + close(fd); + +} diff --git a/pmsm-control/test_sw/rp_spi.h b/pmsm-control/test_sw/rp_spi.h new file mode 100644 index 0000000..a5cc719 --- /dev/null +++ b/pmsm-control/test_sw/rp_spi.h @@ -0,0 +1,34 @@ +/** + * \file rp_spi.h + */ + +#include /*uint32_t*/ + +/** + * \brief Struktura pro prichozi data z fpga. + */ +struct rpi_in{ + uint64_t pozice; /*use twice the origin size to avoid underflow when sunstracting offset*/ + uint16_t ch0, ch1, ch2; + int8_t hal1,hal2,hal3; /* bool values */ + int8_t en1, en2, en3; /*(bool)last read pwm-enable values - !they are changed after reading ! */ + int8_t shdn1,shdn2,shdn3; /*(bool)last read shutdown values - !they are changed after reading ! */ + int8_t b54, b53, b52, b51, b50, b49, b48, b47, b46, b45, b44, b43, b42, b41, b40, b39, b38, b37, b36; /*bits for debug*/ +}; + +/** + * \brief Testovaci funkce. + */ +extern void transfer(void); + +/** + * \brief Inicializace pro spi. + */ +extern int spi_init(void); + +/** + * \brief Uzavreni spi. + */ +extern void spi_disable(void); + +extern struct rpi_in spi_read(uint8_t *); diff --git a/pmsm-control/test_sw/rpi_hw.c b/pmsm-control/test_sw/rpi_hw.c new file mode 100644 index 0000000..3fecd5d --- /dev/null +++ b/pmsm-control/test_sw/rpi_hw.c @@ -0,0 +1,595 @@ +/** + * \brief Interface pro ovladani pinu a pwm na raspberryPi. + * \file rpi_hw.c + * \date Jan 24, 2015 + * \author Martin Prudek + * + * Poskytuje rozhrani pro cteni a zapisovani na GPIO. + * Zajistuje funkcnoust PWM. + * Napsano pro Raspberry Pi. + * Inspired by wiringPi written by Gordon Henderson. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rpin.h" +#include "pwm.h" + +#define BCM_PASSWORD 0x5A000000 + +#ifndef NULL + #define NULL (void*)0 +#endif +#define PI_GPIO_MASK (0xFFFFFFC0) +#define WPI_MODE_UNINITIALISED 0 +#define WPI_MODE_PINS 1 + + + +/* pouzite GPIO piny */ +#define PWM_WIDTH 18 +#define PWM_DIR 22 +#define IRC_A 24 +#define IRC_B 8 + +/* Bazove adresy periferii */ +#define BCM2708_PERI_BASE 0x20000000 +#define GPIO_PADS (BCM2708_PERI_BASE + 0x00100000) +#define CLOCK_BASE (BCM2708_PERI_BASE + 0x00101000) +#define GPIO_BASE (BCM2708_PERI_BASE + 0x00200000) +#define GPIO_TIMER (BCM2708_PERI_BASE + 0x0000B000) +#define GPIO_PWM (BCM2708_PERI_BASE + 0x0020C000) + +#define PAGE_SIZE (4*1024) +#define BLOCK_SIZE (4*1024) +// PWM +// Word offsets into the PWM control region + +#define PWM_CONTROL 0 +#define PWM_STATUS 1 +#define PWM0_RANGE 4 +#define PWM0_DATA 5 +#define PWM1_RANGE 8 +#define PWM1_DATA 9 + +// Clock regsiter offsets + +#define PWMCLK_CNTL 40 +#define PWMCLK_DIV 41 + +#define PWM0_MS_MODE 0x0080 // Run in MS mode +#define PWM0_USEFIFO 0x0020 // Data from FIFO +#define PWM0_REVPOLAR 0x0010 // Reverse polarity +#define PWM0_OFFSTATE 0x0008 // Ouput Off state +#define PWM0_REPEATFF 0x0004 // Repeat last value if FIFO empty +#define PWM0_SERIAL 0x0002 // Run in serial mode +#define PWM0_ENABLE 0x0001 // Channel Enable + +#define PWM1_MS_MODE 0x8000 // Run in MS mode +#define PWM1_USEFIFO 0x2000 // Data from FIFO +#define PWM1_REVPOLAR 0x1000 // Reverse polarity +#define PWM1_OFFSTATE 0x0800 // Ouput Off state +#define PWM1_REPEATFF 0x0400 // Repeat last value if FIFO empty +#define PWM1_SERIAL 0x0200 // Run in serial mode +#define PWM1_ENABLE 0x0100 // Channel Enable + + + +// Locals to hold pointers to the hardware + +static volatile uint32_t *gpio ; +static volatile uint32_t *pwm ; +static volatile uint32_t *clk ; +static volatile uint32_t *pads ; + +static int initialised = WPI_MODE_UNINITIALISED ; + + +// gpioToGPFSEL: +// Map a BCM_GPIO pin to it's Function Selection +// control port. (GPFSEL 0-5) +// Groups of 10 - 3 bits per Function - 30 bits per port + +static uint8_t gpioToGPFSEL[] = +{ + 0,0,0,0,0,0,0,0,0,0, + 1,1,1,1,1,1,1,1,1,1, + 2,2,2,2,2,2,2,2,2,2, + 3,3,3,3,3,3,3,3,3,3, + 4,4,4,4,4,4,4,4,4,4, + 5,5,5,5,5,5,5,5,5,5, +} ; + + +// gpioToShift +// Define the shift up for the 3 bits per pin in each GPFSEL port + +static uint8_t gpioToShift[] = +{ + 0,3,6,9,12,15,18,21,24,27, + 0,3,6,9,12,15,18,21,24,27, + 0,3,6,9,12,15,18,21,24,27, + 0,3,6,9,12,15,18,21,24,27, + 0,3,6,9,12,15,18,21,24,27, +} ; + + +// gpioToGPSET: +// (Word) offset to the GPIO Set registers for each GPIO pin + +static uint8_t gpioToGPSET[] = +{ + 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, + 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, 8, +} ; + +// gpioToGPCLR: +// (Word) offset to the GPIO Clear registers for each GPIO pin + +static uint8_t gpioToGPCLR[] = +{ + 10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10,10, + 11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11,11, +} ; + + +// gpioToGPLEV: +// (Word) offset to the GPIO Input level registers for each GPIO pin + +static uint8_t gpioToGPLEV[] = +{ + 13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13, + 14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14,14, +} ; + +// GPPUD: +// GPIO Pin pull up/down register + +#define GPPUD 37 + + + + +// gpioToPwmALT +// the ALT value to put a GPIO pin into PWM mode + +static uint8_t gpioToPwmALT [] = +{ + 0, 0, 0, 0, 0, 0, 0, 0, // 0 -> 7 + 0, 0, 0, 0, FSEL_ALT0, FSEL_ALT0, 0, 0, // 8 -> 15 + 0, 0, FSEL_ALT5, FSEL_ALT5, 0, 0, 0, 0, // 16 -> 23 + 0, 0, 0, 0, 0, 0, 0, 0, // 24 -> 31 + 0, 0, 0, 0, 0, 0, 0, 0, // 32 -> 39 + FSEL_ALT0, FSEL_ALT0, 0, 0, 0, FSEL_ALT0, 0, 0, // 40 -> 47 + 0, 0, 0, 0, 0, 0, 0, 0, // 48 -> 55 + 0, 0, 0, 0, 0, 0, 0, 0, // 56 -> 63 +} ; +// gpioToPwmPort +// The port value to put a GPIO pin into PWM mode + +static uint8_t gpioToPwmPort [] = +{ + 0, 0, 0, 0, 0, 0, 0, 0, // 0 -> 7 + 0, 0, 0, 0, PWM0_DATA, PWM1_DATA, 0, 0, // 8 -> 15 + 0, 0, PWM0_DATA, PWM1_DATA, 0, 0, 0, 0, // 16 -> 23 + 0, 0, 0, 0, 0, 0, 0, 0, // 24 -> 31 + 0, 0, 0, 0, 0, 0, 0, 0, // 32 -> 39 + PWM0_DATA, PWM1_DATA, 0, 0, 0, PWM1_DATA, 0, 0, // 40 -> 47 + 0, 0, 0, 0, 0, 0, 0, 0, // 48 -> 55 + 0, 0, 0, 0, 0, 0, 0, 0, // 56 -> 63 + +} ; + +// gpioToClk: +// (word) Offsets to the clock Control and Divisor register +/* +static uint8_t gpioToClkCon [] = +{ + -1, -1, -1, -1, 28, 30, 32, -1, // 0 -> 7 + -1, -1, -1, -1, -1, -1, -1, -1, // 8 -> 15 + -1, -1, -1, -1, 28, 30, -1, -1, // 16 -> 23 + -1, -1, -1, -1, -1, -1, -1, -1, // 24 -> 31 + 28, -1, 28, -1, -1, -1, -1, -1, // 32 -> 39 + -1, -1, 28, 30, 28, -1, -1, -1, // 40 -> 47 + -1, -1, -1, -1, -1, -1, -1, -1, // 48 -> 55 + -1, -1, -1, -1, -1, -1, -1, -1, // 56 -> 63 +} ; + +static uint8_t gpioToClkDiv [] = +{ + -1, -1, -1, -1, 29, 31, 33, -1, // 0 -> 7 + -1, -1, -1, -1, -1, -1, -1, -1, // 8 -> 15 + -1, -1, -1, -1, 29, 31, -1, -1, // 16 -> 23 + -1, -1, -1, -1, -1, -1, -1, -1, // 24 -> 31 + 29, -1, 29, -1, -1, -1, -1, -1, // 32 -> 39 + -1, -1, 29, 31, 29, -1, -1, -1, // 40 -> 47 + -1, -1, -1, -1, -1, -1, -1, -1, // 48 -> 55 + -1, -1, -1, -1, -1, -1, -1, -1, // 56 -> 63 +} ; +*/ + + +// PWM + +#define PWM_MODE_MS 0 +#define PWM_MODE_BAL 1 + +/*****************************************************************************/ +/****************** implementace rpin.h **************************************/ +/* */ +/* Funkce pro nativni ovladani pinu, pwm a hodin */ +/* */ +/*****************************************************************************/ + +/** + * inicializuje pouziti GPIO pinu... + * (namapuje registry DMA) + */ +int initialise (void) +{ + int fd ; + int model, rev, mem, maker, overVolted ; + + if (geteuid() != 0){ + printf("Must be root. (Did you forget sudo?)\n") ; + return 1; + } + + // Open the master /dev/memory device + if ((fd = open ("/dev/mem", O_RDWR | O_SYNC | O_CLOEXEC) ) < 0){ + printf(" Unable to open /dev/mem: %s\n"); + return 1; + } + + // GPIO: + gpio = (uint32_t *)mmap(0, BLOCK_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, fd, GPIO_BASE) ; + if ((int32_t)gpio == -1){ + printf("mmap (GPIO) failed: %s\n") ; + return 1; + } + + + // PWM + pwm = (uint32_t *)mmap(0, BLOCK_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, fd, GPIO_PWM) ; + if ((int32_t)pwm == -1){ + printf("mmap (PWM) failed: %s\n"); + return 1; + } + + // Clock control (needed for PWM) + clk = (uint32_t *)mmap(0, BLOCK_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, fd, CLOCK_BASE) ; + if ((int32_t)clk == -1){ + printf("mmap (CLOCK) failed: %s\n"); + return 1; + } + + pads = (uint32_t *)mmap(0, BLOCK_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, fd, GPIO_PADS) ; + if ((int32_t)pads == -1){ + printf("mmap (PADS) failed: %s\n"); + return 1; + } + close(fd); + initialised = 1 ; + + return 0 ; +} +/** + * \brief Initialize gpclk. + */ +int initClock(enum ClkSource source, int divI, int divF) +{ + const int MASH = 0; + #define CLK_GP0_CTL 28 + #define CLK_GP0_DIV 29 + #define CLK_GP1_CTL 30 + #define CLK_GP1_DIV 31 + #define CLK_GP2_CTL 32 + #define CLK_GP2_DIV 33 + + #define CLK_CTL_SRC_OSC 1 /* 19.2 MHz */ + #define CLK_CTL_SRC_PLLC 5 /* 1000 MHz */ + #define CLK_CTL_SRC_PLLD 6 /* 500 MHz */ + #define CLK_CTL_SRC_HDMI 7 /* 216 MHz */ + + int src[] ={ + CLK_CTL_SRC_PLLD, + CLK_CTL_SRC_OSC, + CLK_CTL_SRC_HDMI, + CLK_CTL_SRC_PLLC + }; + + int clkSrc; + uint32_t setting; + + if (!initialised){ + return -1; + } + + + if ((source < 0) || (source > 3 )) return -2; + if ((divI < 2) || (divI > 4095)) return -3; + if ((divF < 0) || (divF > 4095)) return -4; + if ((MASH < 0) || (MASH > 3)) return -5; + + + + clkSrc = src[source]; + + #define CLK_PASSWD (0x5A<<24) + #define CLK_CTL_MASH(x)((x)<<9) + #define CLK_CTL_BUSY (1 <<7) + #define CLK_CTL_KILL (1 <<5) + #define CLK_CTL_ENAB (1 <<4) + #define CLK_CTL_SRC(x) ((x)<<0) + + clk[CLK_GP0_CTL] = CLK_PASSWD | CLK_CTL_KILL; + + /* wait for clock to stop */ + + while (clk[CLK_GP0_CTL] & CLK_CTL_BUSY){ + usleep(10); + } + #define CLK_DIV_DIVI(x) ((x)<<12) + #define CLK_DIV_DIVF(x) ((x)<< 0) + + clk[CLK_GP0_DIV] = (CLK_PASSWD | CLK_DIV_DIVI(divI) | CLK_DIV_DIVF(divF)); + + usleep(10); + + clk[CLK_GP0_CTL] = (CLK_PASSWD | CLK_CTL_MASH(MASH) | CLK_CTL_SRC(clkSrc)); + + usleep(10); + + clk[CLK_GP0_CTL] |= (CLK_PASSWD | CLK_CTL_ENAB); + return 0; +} +int termClock(int clock) +{ + int ctl[] = {CLK_GP0_CTL, CLK_GP2_CTL}; + + int clkCtl; + + if ((clock < 0) || (clock > 1)) return -1; + + clkCtl = ctl[clock]; + + clk[clkCtl] = CLK_PASSWD | CLK_CTL_KILL; + + /* wait for clock to stop */ + + while (clk[clkCtl] & CLK_CTL_BUSY){ + usleep(10); + } + + return 0; +} +/** + * \brief A different approach to set gpio mode. + */ +void gpioSetMode(unsigned gpio_n, unsigned mode) +{ + int reg, shift; + + reg = gpio_n/10; + shift = (gpio_n%10) * 3; + + gpio[reg] = (gpio[reg] & ~(7< + * after further study of the manual and testing with a 'scope + */ + +void pwmSetClock (int divisor){ + uint32_t pwm_control ; + divisor &= 4095 ; + if (initialised){ + pwm_control = *(pwm + PWM_CONTROL) ; // preserve PWM_CONTROL + + // We need to stop PWM prior to stopping PWM clock in MS mode otherwise BUSY + // stays high. + + *(pwm + PWM_CONTROL) = 0 ; // Stop PWM + + // Stop PWM clock before changing divisor. The delay after this does need to + // this big (95uS occasionally fails, 100uS OK), it's almost as though the BUSY + // flag is not working properly in balanced mode. Without the delay when DIV is + // adjusted the clock sometimes switches to very slow, once slow further DIV + // adjustments do nothing and it's difficult to get out of this mode. + + *(clk + PWMCLK_CNTL) = BCM_PASSWORD | 0x01 ; // Stop PWM Clock + usleep(110) ; // prevents clock going sloooow + + while ((*(clk + PWMCLK_CNTL) & 0x80) != 0) // Wait for clock to be !BUSY + usleep(1) ; + + *(clk + PWMCLK_DIV) = BCM_PASSWORD | (divisor << 12) ; + + *(clk + PWMCLK_CNTL) = BCM_PASSWORD | 0x11 ; // Start PWM clock + *(pwm + PWM_CONTROL) = pwm_control ; // restore PWM_CONTROL + + } +} + + +/** + * \brief nastavi mod dnaeho pinu + * \param pin [in] cislo GPIO pinu + * \param mode [in] mod pinu -> in/out/altx + * + * Nastavi pin jako vstupni / vystupni, pripadne nastavi slternativni funkci. + */ +void pinMode (int pin, int mode){ + int fSel, shift, alt ; + if ((pin & PI_GPIO_MASK) == 0){ //exituje-li pin + if (!initialised) return; + + fSel = gpioToGPFSEL[pin] ; + shift = gpioToShift [pin] ; + + if (mode == INPUT) + *(gpio + fSel) = (*(gpio + fSel) & ~(7 << shift)) ; // Sets bits to zero = input + else if (mode == OUTPUT) + *(gpio + fSel) = (*(gpio + fSel) & ~(7 << shift)) | (1 << shift) ; + else if (mode == PWM_OUTPUT) + { + if ((alt = gpioToPwmALT [pin]) == 0) // Not a hardware capable PWM pin + return ; + + // Set pin to PWM mode + + *(gpio + fSel) = (*(gpio + fSel) & ~(7 << shift)) | (alt << shift) ; + usleep(110) ; // See comments in pwmSetClockWPi + + pwmSetMode (PWM_MODE_BAL) ; // Pi default mode + pwmSetRange (1024) ; // Default range of 1024 + pwmSetClock (32) ; // 19.2 / 32 = 600KHz - Also starts the PWM + } + + } + +} +/** + * \brief Precte logickou uroven daneho pinu 0/1 + * v pripade neicnicializovaneho GPIO nebo neexistence pinu vrati 0 + * \param pin [in] cislo GPIO pinu + */ + +int digitalRead (int pin){ + char c ; + if ((pin & PI_GPIO_MASK) == 0) // On-Board Pin + { + + if (!initialised) return LOW; + + if ((*(gpio + gpioToGPLEV [pin]) & (1 << (pin & 31))) != 0) + return HIGH ; + else + return LOW ; + }else{ + return LOW; + } +} + + +/** + * \brief zapise logickou uroven na dany pin + * \param pin [in] cislo GPIO pinu + * \param value [in] logicka uroven 0/1 + */ +void digitalWrite (int pin, int value){ + if ((pin & PI_GPIO_MASK) == 0) // On-Board Pin + { + if (!initialised) return; + if (value == LOW) + *(gpio + gpioToGPCLR [pin]) = 1 << (pin & 31) ; + else + *(gpio + gpioToGPSET [pin]) = 1 << (pin & 31) ; + } +} + + +/** + * nastavi hodnotu pwm + * \param pin[in] cislo GPIO PWM pinu + * \param value[int] sirka plneni pwm (max=1024) + */ +void pwmWrite (int pin, int value){ + if ((pin & PI_GPIO_MASK) == 0) // On-Board Pin + { + if (!initialised) return; + *(pwm + gpioToPwmPort[pin]) = value ; + } +} +/*****************************************************************************/ +/************ implementace pwm.h *********************************************/ +/*****************************************************************************/ + +/** + * nastavi a pripadne prevrati polaitu pwm pro pohon motoru + * \param width[in] sirka plneni (max=1024) + */ +void pwm_width(int width){ + if (width >= 0) { + digitalWrite(PWM_DIR, 0); + pwmWrite(PWM_WIDTH, width) ; + } + else { + digitalWrite(PWM_DIR, 1); + pwmWrite(PWM_WIDTH, -width) ; + } +} + +/** + * \brief Inicializuje pwm. + */ +void pwm_init(){ + if (initialise() < 0) { + fprintf(stderr, "Unable to setup: %s\n", strerror(errno)); + return; + } + + pinMode(PWM_WIDTH, PWM_OUTPUT) ; + pinMode(PWM_DIR, OUTPUT) ; + pwm_width(0); +} + +/** + * \brief Odinicializuje pwm. + */ +void pwm_disable(){ + pwm_width(0); + pinMode (PWM_WIDTH, INPUT); + pinMode (PWM_DIR, INPUT); +} + + + + + + diff --git a/pmsm-control/test_sw/rpin.h b/pmsm-control/test_sw/rpin.h new file mode 100644 index 0000000..e43408f --- /dev/null +++ b/pmsm-control/test_sw/rpin.h @@ -0,0 +1,109 @@ +/** + * \brief Interface pro ovladani pinu na RPi. + * \file rpin.h + * \date March 21, 2015 + * \author Martin Prudek + * + * Deklaruje nativni funkce pro ovladani pinu a pwm na RPi. + * Implementace "rpi_hw.c" + * + * + */ + +#ifndef RPIN_H_ +#define RPIN_H_ + + +/* Pin modes */ +#define INPUT 0 +#define OUTPUT 1 +#define PWM_OUTPUT 2 +#define GPIO_CLOCK 3 +#define FSEL_INPT 0b000 /*0*/ +#define FSEL_OUTP 0b001 /*1*/ +#define FSEL_ALT0 0b100 /*4*/ +#define FSEL_ALT1 0b101 /*5*/ +#define FSEL_ALT2 0b110 /*6*/ +#define FSEL_ALT3 0b111 /*7*/ +#define FSEL_ALT4 0b011 /*3*/ +#define FSEL_ALT5 0b010 /*2*/ + +/* Logic levels */ +#define LOW 0 +#define HIGH 1 + +/* Clk sources */ +enum ClkSource{ + PLLD_500_MHZ=0, /*500 MHz*/ + OSC_19_MHZ_2=1, /*19.2 MHz*/ + HDMI_216_MHZ=2, /*216 MHz*/ + PLLC_1000_MHZ=3 /*1000 MHz, changes with overclock*/ +}; + +/** + * \brief inicializuje pouziti GPIO pinu... + * (namapuje registry DMA) + */ +extern int initialise (void); + +/** + * \brief + * Select the native "balanced" mode, or standard mark:space mode + */ +extern void pwmSetMode (int); + +/** + * \brief + * Set the PWM range register. We set both range registers to the same + * value. + */ +extern void pwmSetRange (unsigned int); + +/** + * \brief + * Set/Change the PWM clock. + * Not tested. + */ +extern void pwmSetClock (int); + +/** + * \brief nastavi mod dnaeho pinu + * \param pin [in] cislo GPIO pinu + * \param mode [in] mod pinu -> in/out/altx + * + * Nastavi pin jako vstupni / vystupni, pripadne nastavi slternativni funkci. + */ +extern void pinMode (int, int); + +/** + * \brief Precte logickou uroven daneho pinu 0/1 + * v pripade neicnicializovaneho GPIO nebo neexistence pinu vrati 0 + * \param pin [in] cislo GPIO pinu + */ +extern int digitalRead (int); + +/** + * \brief zapise logickou uroven na dany pin + * \param pin [in] cislo GPIO pinu + * \param value [in] logicka uroven 0/1 + */ +extern void digitalWrite (int, int); + +/** + * \brief nastavi hodnotu pwm + * \param pin[in] cislo GPIO PWM pinu + * \param value[int] sirka plneni pwm (max=1024) + */ +extern void pwmWrite (int, int); + +/** + * \brief A different approach to set gpio mode. + */ +extern void gpioSetMode(unsigned gpio_n, unsigned mode); + +/** + * \brief Initialize gpclk. + */ +extern int initClock(enum ClkSource,int, int); + +#endif /*RPIN_H_*/ -- 2.39.2