From f057fe3d024c53c2564d3c6c8a5841174359a6d0 Mon Sep 17 00:00:00 2001 From: raspilot Date: Tue, 18 Aug 2015 12:32:29 +1000 Subject: [PATCH] AP_HAL_Linux: added support for raspilot GPIO --- libraries/AP_HAL_Linux/GPIO.h | 2 +- libraries/AP_HAL_Linux/GPIO_RPI.cpp | 109 ++++++++++++++++++++++++++-- libraries/AP_HAL_Linux/GPIO_RPI.h | 48 +++++++++--- 3 files changed, 143 insertions(+), 16 deletions(-) diff --git a/libraries/AP_HAL_Linux/GPIO.h b/libraries/AP_HAL_Linux/GPIO.h index e42cce1d67..afcd199316 100644 --- a/libraries/AP_HAL_Linux/GPIO.h +++ b/libraries/AP_HAL_Linux/GPIO.h @@ -6,7 +6,7 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI #include "GPIO_BBB.h" -#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT #include "GPIO_RPI.h" #endif diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.cpp b/libraries/AP_HAL_Linux/GPIO_RPI.cpp index 96a5afd6c9..fdde45c5b5 100644 --- a/libraries/AP_HAL_Linux/GPIO_RPI.cpp +++ b/libraries/AP_HAL_Linux/GPIO_RPI.cpp @@ -1,6 +1,6 @@ #include -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT #include "GPIO.h" #include @@ -56,7 +56,10 @@ int LinuxGPIO_RPI::getRaspberryPiVersion() const void LinuxGPIO_RPI::init() { - uint32_t address = getRaspberryPiVersion() == 1? GPIO_BASE(BCM2708_PERI_BASE): GPIO_BASE(BCM2709_PERI_BASE); + int rpi_version = getRaspberryPiVersion(); + uint32_t gpio_address = rpi_version == 1? GPIO_BASE(BCM2708_PERI_BASE): GPIO_BASE(BCM2709_PERI_BASE); + uint32_t pwm_address = rpi_version == 1? PWM_BASE(BCM2708_PERI_BASE): PWM_BASE(BCM2709_PERI_BASE); + uint32_t clk_address = rpi_version == 1? CLOCK_BASE(BCM2708_PERI_BASE): CLOCK_BASE(BCM2709_PERI_BASE); // open /dev/mem if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) { hal.scheduler->panic("Can't open /dev/mem"); @@ -69,16 +72,36 @@ void LinuxGPIO_RPI::init() PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory MAP_SHARED, // Shared with other processes mem_fd, // File to map - address // Offset to GPIO peripheral + gpio_address // Offset to GPIO peripheral + ); + + pwm_map = mmap( + NULL, // Any adddress in our space will do + BLOCK_SIZE, // Map length + PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory + MAP_SHARED, // Shared with other processes + mem_fd, // File to map + pwm_address // Offset to GPIO peripheral + ); + + clk_map = mmap( + NULL, // Any adddress in our space will do + BLOCK_SIZE, // Map length + PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory + MAP_SHARED, // Shared with other processes + mem_fd, // File to map + clk_address // Offset to GPIO peripheral ); close(mem_fd); // No need to keep mem_fd open after mmap - if (gpio_map == MAP_FAILED) { + if (gpio_map == MAP_FAILED || pwm_map == MAP_FAILED || clk_map == MAP_FAILED) { hal.scheduler->panic("Can't open /dev/mem"); } gpio = (volatile uint32_t *)gpio_map; // Always use volatile pointer! + pwm = (volatile uint32_t *)pwm_map; + clk = (volatile uint32_t *)clk_map; } void LinuxGPIO_RPI::pinMode(uint8_t pin, uint8_t output) @@ -91,6 +114,19 @@ void LinuxGPIO_RPI::pinMode(uint8_t pin, uint8_t output) } } +void LinuxGPIO_RPI::pinMode(uint8_t pin, uint8_t output, uint8_t alt) +{ + if (output == HAL_GPIO_INPUT) { + GPIO_MODE_IN(pin); + } else if (output == HAL_GPIO_ALT) { + GPIO_MODE_IN(pin); + GPIO_MODE_ALT(pin, alt); + } else { + GPIO_MODE_IN(pin); + GPIO_MODE_OUT(pin); + } +} + int8_t LinuxGPIO_RPI::analogPinToDigitalPin(uint8_t pin) { return -1; @@ -116,6 +152,69 @@ void LinuxGPIO_RPI::toggle(uint8_t pin) write(pin, !read(pin)); } +void LinuxGPIO_RPI::setPWMPeriod(uint8_t pin, uint32_t time_us) +{ + setPWM0Period(time_us); +} + +void LinuxGPIO_RPI::setPWMDuty(uint8_t pin, uint8_t percent) +{ + setPWM0Duty(percent); +} + +void LinuxGPIO_RPI::setPWM0Period(uint32_t time_us) +{ + // stop clock and waiting for busy flag doesn't work, so kill clock + *(clk + PWMCLK_CNTL) = 0x5A000000 | (1 << 5); + usleep(10); + + // set frequency + // DIVI is the integer part of the divisor + // the fractional part (DIVF) drops clock cycles to get the output frequency, bad for servo motors + // 320 bits for one cycle of 20 milliseconds = 62.5 us per bit = 16 kHz + int idiv = (int) (19200000.0f / (320000000.0f / time_us)); + if (idiv < 1 || idiv > 0x1000) { + hal.scheduler->panic("idiv out of range."); + return; + } + *(clk + PWMCLK_DIV) = 0x5A000000 | (idiv<<12); + + // source=osc and enable clock + *(clk + PWMCLK_CNTL) = 0x5A000011; + + // disable PWM + *(pwm + PWM_CTL) = 0; + + // needs some time until the PWM module gets disabled, without the delay the PWM module crashs + usleep(10); + + // filled with 0 for 20 milliseconds = 320 bits + *(pwm + PWM_RNG1) = 320; + + // init with 0% + setPWM0Duty(0); + + // start PWM1 in serializer mode + *(pwm + PWM_CTL) = 3; +} + +void LinuxGPIO_RPI::setPWM0Duty(uint8_t percent) +{ + int bitCount; + unsigned int bits = 0; + + bitCount = 320 * percent / 100; + if (bitCount > 320) bitCount = 320; + if (bitCount < 0) bitCount = 0; + bits = 0; + while (bitCount) { + bits <<= 1; + bits |= 1; + bitCount--; + } + *(pwm + PWM_DAT1) = bits; +} + /* Alternative interface: */ AP_HAL::DigitalSource* LinuxGPIO_RPI::channel(uint16_t n) { return new LinuxDigitalSource(n); @@ -132,4 +231,4 @@ bool LinuxGPIO_RPI::usb_connected(void) return false; } -#endif // CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO +#endif // CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.h b/libraries/AP_HAL_Linux/GPIO_RPI.h index 66f5914586..c2952ea310 100644 --- a/libraries/AP_HAL_Linux/GPIO_RPI.h +++ b/libraries/AP_HAL_Linux/GPIO_RPI.h @@ -12,6 +12,16 @@ #define BCM2708_PERI_BASE 0x20000000 #define BCM2709_PERI_BASE 0x3F000000 #define GPIO_BASE(address) (address + 0x200000) +#define PWM_BASE(address) (address + 0x20C000) /* PWM controller */ +#define CLOCK_BASE(address) (address + 0x101000) + +#define PWM_CTL 0 +#define PWM_RNG1 4 +#define PWM_DAT1 5 + +#define PWMCLK_CNTL 40 +#define PWMCLK_DIV 41 + #define PAGE_SIZE (4*1024) #define BLOCK_SIZE (4*1024) @@ -26,21 +36,30 @@ // Raspberry Pi GPIO mapping #define RPI_GPIO_2 2 // Pin 3 SDA #define RPI_GPIO_3 3 // Pin 5 SCL -#define RPI_GPIO_4 4 // Pin 7 NAVIO_PPM_INPUT -#define RPI_GPIO_7 7 // Pin 26 CE1 NAVIO_MPU9250_CS -#define RPI_GPIO_8 8 // Pin 24 CE0 NAVIO_UBLOX_CS +#define RPI_GPIO_4 4 // Pin 7 +#define RPI_GPIO_5 5 // Pin 29 +#define RPI_GPIO_6 6 // Pin 31 +#define RPI_GPIO_7 7 // Pin 26 CE1 MPU9250_CS +#define RPI_GPIO_8 8 // Pin 24 CE0 UBLOX_CS #define RPI_GPIO_9 9 // Pin 21 MISO #define RPI_GPIO_10 10 // Pin 19 MOSI #define RPI_GPIO_11 11 // Pin 23 SCLK +#define RPI_GPIO_12 12 // Pin 32 +#define RPI_GPIO_13 13 // Pin 33 #define RPI_GPIO_14 14 // Pin 8 TxD #define RPI_GPIO_15 15 // Pin 10 RxD -#define RPI_GPIO_17 17 // Pin 11 NAVIO_UART_PORT_5 -#define RPI_GPIO_18 18 // Pin 12 NAVIO_UART_PORT_4 -#define RPI_GPIO_22 22 // Pin 15 NAVIO_UBLOX_PPS -#define RPI_GPIO_23 23 // Pin 16 NAVIO_MPU9250_DRDY -#define RPI_GPIO_24 24 // Pin 18 NAVIO_SPI_PORT_6 -#define RPI_GPIO_25 25 // Pin 22 NAVIO_SPI_PORT_5 -#define RPI_GPIO_27 27 // Pin 13 NAVIO_PCA9685_OE +#define RPI_GPIO_16 16 // Pin 36 +#define RPI_GPIO_17 17 // Pin 11 UART_PORT_5 +#define RPI_GPIO_18 18 // Pin 12 UART_PORT_4 +#define RPI_GPIO_19 19 // Pin 35 +#define RPI_GPIO_20 20 // Pin 38 +#define RPI_GPIO_21 21 // Pin 40 +#define RPI_GPIO_22 22 // Pin 15 UBLOX_PPS +#define RPI_GPIO_23 23 // Pin 16 MPU9250_DRDY +#define RPI_GPIO_24 24 // Pin 18 SPI_PORT_6 +#define RPI_GPIO_25 25 // Pin 22 SPI_PORT_5 +#define RPI_GPIO_26 26 // Pin 37 +#define RPI_GPIO_27 27 // Pin 13 #define RPI_GPIO_28 28 // Pin 3 #define RPI_GPIO_29 29 // Pin 4 #define RPI_GPIO_30 30 // Pin 5 @@ -50,17 +69,26 @@ class Linux::LinuxGPIO_RPI : public AP_HAL::GPIO { private: int mem_fd; void *gpio_map; + void *pwm_map; + void *clk_map; volatile uint32_t *gpio; + volatile uint32_t *pwm; + volatile uint32_t *clk; int getRaspberryPiVersion() const; + void setPWM0Period(uint32_t time_us); + void setPWM0Duty(uint8_t percent); public: LinuxGPIO_RPI(); void init(); void pinMode(uint8_t pin, uint8_t output); + void pinMode(uint8_t pin, uint8_t output, uint8_t alt); int8_t analogPinToDigitalPin(uint8_t pin); uint8_t read(uint8_t pin); void write(uint8_t pin, uint8_t value); void toggle(uint8_t pin); + void setPWMPeriod(uint8_t pin, uint32_t time_us); + void setPWMDuty(uint8_t pin, uint8_t percent); /* Alternative interface: */ AP_HAL::DigitalSource* channel(uint16_t n);