--- /dev/null
+/**
+ * \file main_pmsm.c
+ * \author Martin Prudek
+ * \brief Mainfile pro pmsm control.
+ */
+
+#ifndef NULL
+#define NULL (void*) 0
+#endif /*NULL*/
+
+
+#include <stdlib.h> /*exit*/
+#include <signal.h> /*signal handler Ctrl+C*/
+#include <stdio.h> /*printf*/
+#include <sched.h> /*sheduler*/
+#include <unistd.h> /*usleep*/
+#include <pthread.h> /*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;
+}
--- /dev/null
+/*
+ *
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <getopt.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <linux/types.h>
+#include <linux/spi/spidev.h>
+
+#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);
+
+}
--- /dev/null
+/**
+ * \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 <stdio.h>
+#include <stdarg.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include <poll.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <time.h>
+#include <fcntl.h>
+#include <pthread.h>
+#include <sys/time.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/wait.h>
+#include <sys/ioctl.h>
+
+#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<<shift)) | (mode<<shift);
+}
+
+/**
+ * pwmSetMode:
+ * Select the native "balanced" mode, or standard mark:space mode
+ */
+
+void pwmSetMode (int mode){
+ if (initialised){
+ if (mode == PWM_MODE_MS){
+ *(pwm + PWM_CONTROL) = PWM0_ENABLE | PWM1_ENABLE | PWM0_MS_MODE | PWM1_MS_MODE ;
+ }else{
+ *(pwm + PWM_CONTROL) = PWM0_ENABLE | PWM1_ENABLE ;
+ }
+ }
+}
+
+/**
+ * pwmSetRange:
+ * Set the PWM range register. We set both range registers to the same
+ * value. If you want different in your own code, then write your own.
+ */
+
+void pwmSetRange (unsigned int range){
+ if (initialised){
+ *(pwm + PWM0_RANGE) = range ; usleep(10) ;
+ *(pwm + PWM1_RANGE) = range ; usleep(10) ;
+ }
+}
+/**
+ * pwmSetClock:
+ * Set/Change the PWM clock. Originally Drogon's code, but changed
+ * (for the better!) by Chris Hall, <chris@kchall.plus.com>
+ * 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);
+}
+
+
+
+
+
+
projects / fpga / rpi-motor-control.git / commitdiff