--- /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 / blobdiff