Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: added support for raspilot GPIO

This commit is contained in:
raspilot 2015-08-18 12:32:29 +10:00 committed by Andrew Tridgell
parent 9a73df1bea
commit f057fe3d02
3 changed files with 143 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_HAL_Linux/GPIO.h
View File

@ -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
View File

@ -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
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

48
libraries/AP_HAL_Linux/GPIO_RPI.h
View File

@ -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);