zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_HAL_Linux: added support for raspilot GPIO

master
raspilot 10 years ago committed by Andrew Tridgell
parent
9a73df1bea
commit
f057fe3d02
3 changed files with 143 additions and 16 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      libraries/AP_HAL_Linux/GPIO.h
  2. 109
      libraries/AP_HAL_Linux/GPIO_RPI.cpp
  3. 48
      libraries/AP_HAL_Linux/GPIO_RPI.h

2
libraries/AP_HAL_Linux/GPIO.h
Unescape View File

@ -6,7 +6,7 @@ @@ -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

109
libraries/AP_HAL_Linux/GPIO_RPI.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
#include <AP_HAL/AP_HAL.h>
#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 <stdio.h>
@ -56,7 +56,10 @@ int LinuxGPIO_RPI::getRaspberryPiVersion() const @@ -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() @@ -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) @@ -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) @@ -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) @@ -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

48
libraries/AP_HAL_Linux/GPIO_RPI.h
Unescape View File

@ -12,6 +12,16 @@ @@ -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 @@ @@ -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 { @@ -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);

Write Preview
Loading…
Cancel
Save