mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 12:14:10 -04:00
HAL_Linux: change to mem access method from sysfs access method
This step is taken to reduce the time for gpio access substantially
This commit is contained in:
bugobliterator
Andrew Tridgell
parent
eb95130441
commit
fd0685cc01
@ -10,85 +10,110 @@
|
|||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <poll.h>
|
#include <poll.h>
|
||||||
|
#include <sys/mman.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
|
||||||
using namespace Linux;
|
using namespace Linux;
|
||||||
|
|
||||||
|
struct GPIO{
|
||||||
|
volatile unsigned *base;
|
||||||
|
volatile unsigned *oe;
|
||||||
|
volatile unsigned *in;
|
||||||
|
volatile unsigned *out;
|
||||||
|
}gpio0,gpio1,gpio2,gpio3;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
LinuxGPIO::LinuxGPIO()
|
LinuxGPIO::LinuxGPIO()
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void LinuxGPIO::init()
|
void LinuxGPIO::init()
|
||||||
{
|
{
|
||||||
int fd, len;
|
int mem_fd;
|
||||||
char buf[64];
|
// Enable all GPIO banks
|
||||||
|
// Without this, access to deactivated banks (i.e. those with no clock source set up) will (logically) fail with SIGBUS
|
||||||
|
// Idea taken from https://groups.google.com/forum/#!msg/beagleboard/OYFp4EXawiI/Mq6s3sg14HoJ
|
||||||
|
system("echo 5 > /sys/class/gpio/export");
|
||||||
|
system("echo 65 > /sys/class/gpio/export");
|
||||||
|
system("echo 105 > /sys/class/gpio/export");
|
||||||
|
|
||||||
fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
|
/* open /dev/mem */
|
||||||
if (fd < 0) {
|
if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
|
||||||
perror("LinuxGPIO::init");
|
printf("can't open /dev/mem \n");
|
||||||
|
exit (-1);
|
||||||
}
|
}
|
||||||
|
|
||||||
len = snprintf(buf, sizeof(buf), "%d", LED_AMBER);
|
/* mmap GPIO */
|
||||||
::write(fd, buf, len);
|
gpio0.base = (volatile unsigned *)mmap(0, GPIO_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, GPIO0_BASE);
|
||||||
close(fd);
|
if ((char *)gpio0.base == MAP_FAILED) {
|
||||||
|
printf("mmap error %d\n", (int)gpio0.base);
|
||||||
fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
|
exit (-1);
|
||||||
if (fd < 0) {
|
}
|
||||||
perror("LinuxGPIO::init");
|
gpio1.base = (volatile unsigned *)mmap(0, GPIO_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, GPIO1_BASE);
|
||||||
|
if ((char *)gpio1.base == MAP_FAILED) {
|
||||||
|
printf("mmap error %d\n", (int)gpio1.base);
|
||||||
|
exit (-1);
|
||||||
|
}
|
||||||
|
gpio2.base = (volatile unsigned *)mmap(0, GPIO_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, GPIO2_BASE);
|
||||||
|
if ((char *)gpio2.base == MAP_FAILED) {
|
||||||
|
printf("mmap error %d\n", (int)gpio2.base);
|
||||||
|
exit (-1);
|
||||||
|
}
|
||||||
|
gpio3.base = (volatile unsigned *)mmap(0, GPIO_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, GPIO3_BASE);
|
||||||
|
if ((char *)gpio3.base == MAP_FAILED) {
|
||||||
|
printf("mmap error %d\n", (int)gpio3.base);
|
||||||
|
exit (-1);
|
||||||
}
|
}
|
||||||
|
|
||||||
len = snprintf(buf, sizeof(buf), "%d", LED_BLUE);
|
|
||||||
::write(fd, buf, len);
|
|
||||||
close(fd);
|
|
||||||
|
|
||||||
fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
|
gpio0.oe = gpio0.base + GPIO_OE;
|
||||||
if (fd < 0) {
|
gpio0.in = gpio0.base + GPIO_IN;
|
||||||
perror("LinuxGPIO::init");
|
gpio0.out = gpio0.base + GPIO_OUT;
|
||||||
}
|
|
||||||
|
|
||||||
len = snprintf(buf, sizeof(buf), "%d", LED_SAFETY);
|
gpio1.oe = gpio1.base + GPIO_OE;
|
||||||
::write(fd, buf, len);
|
gpio1.in = gpio1.base + GPIO_IN;
|
||||||
close(fd);
|
gpio1.out = gpio1.base + GPIO_OUT;
|
||||||
|
|
||||||
fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
|
gpio2.oe = gpio2.base + GPIO_OE;
|
||||||
if (fd < 0) {
|
gpio2.in = gpio2.base + GPIO_IN;
|
||||||
perror("LinuxGPIO::init");
|
gpio2.out = gpio2.base + GPIO_OUT;
|
||||||
}
|
|
||||||
|
|
||||||
len = snprintf(buf, sizeof(buf), "%d", SAFETY_SWITCH);
|
gpio3.oe = gpio3.base + GPIO_OE;
|
||||||
::write(fd, buf, len);
|
gpio3.in = gpio3.base + GPIO_IN;
|
||||||
close(fd);
|
gpio3.out = gpio3.base + GPIO_OUT;
|
||||||
|
close(mem_fd);
|
||||||
}
|
}
|
||||||
|
|
||||||
void LinuxGPIO::pinMode(uint8_t pin, uint8_t output)
|
void LinuxGPIO::pinMode(uint8_t pin, uint8_t output)
|
||||||
{
|
{
|
||||||
int fd,len;
|
if(output == GPIO_INPUT){
|
||||||
char buf[64];
|
if(pin/32 < 1){
|
||||||
|
*gpio0.oe = *gpio0.oe | (1<<pin);
|
||||||
snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/direction", pin);
|
|
||||||
|
|
||||||
fd = ::open(buf, O_WRONLY);
|
|
||||||
if (fd < 0) {
|
|
||||||
fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY); //try exporting GPIO pin
|
|
||||||
if (fd < 0) {
|
|
||||||
perror("LinuxGPIO::direction");
|
|
||||||
}
|
}
|
||||||
|
else if(pin/32 < 2){
|
||||||
len = snprintf(buf, sizeof(buf), "%d", pin);
|
*gpio1.oe = *gpio1.oe | (1<<(pin-32));
|
||||||
::write(fd, buf, len);
|
}
|
||||||
close(fd);
|
else if(pin/32 < 3){
|
||||||
|
*gpio2.oe = *gpio2.oe | (1<<(pin-64));
|
||||||
snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/direction", pin); //retry writing direction
|
}
|
||||||
fd = ::open(buf, O_WRONLY);
|
else if(pin/32 < 4){
|
||||||
if (fd < 0) {
|
*gpio3.oe = *gpio3.oe | (1<<(pin-96));
|
||||||
perror("LinuxGPIO::direction"); //report faillure
|
}
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
if(pin/32 < 1){
|
||||||
|
*gpio0.oe = *gpio0.oe & (~(1<<pin));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 2){
|
||||||
|
*gpio1.oe = *gpio1.oe & (~(1<<(pin-32)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 3){
|
||||||
|
*gpio2.oe = *gpio2.oe & (~(1<<(pin-64)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 4){
|
||||||
|
*gpio3.oe = *gpio3.oe & (~(1<<(pin-96)));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (output == GPIO_OUTPUT)
|
|
||||||
::write(fd, "out", 4);
|
|
||||||
else
|
|
||||||
::write(fd, "in", 3);
|
|
||||||
|
|
||||||
close(fd);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int8_t LinuxGPIO::analogPinToDigitalPin(uint8_t pin)
|
int8_t LinuxGPIO::analogPinToDigitalPin(uint8_t pin)
|
||||||
@ -98,49 +123,55 @@ int8_t LinuxGPIO::analogPinToDigitalPin(uint8_t pin)
|
|||||||
|
|
||||||
|
|
||||||
uint8_t LinuxGPIO::read(uint8_t pin) {
|
uint8_t LinuxGPIO::read(uint8_t pin) {
|
||||||
int fd;
|
|
||||||
char buf[64];
|
|
||||||
char ch;
|
|
||||||
|
|
||||||
snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", pin);
|
uint8_t value;
|
||||||
|
|
||||||
fd = open(buf, O_RDONLY);
|
if(pin/32 < 1){
|
||||||
if (fd < 0) {
|
value = (bool)(*gpio0.in & (1<<pin));
|
||||||
perror("LinuxGPIO::read");
|
}
|
||||||
|
else if(pin/32 < 2){
|
||||||
|
value = (bool)(*gpio1.in & (1<<(pin-32)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 3){
|
||||||
|
value = (bool)(*gpio2.in & (1<<(pin-64)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 4){
|
||||||
|
value = (bool)(*gpio3.in & (1<<(pin-96)));
|
||||||
}
|
}
|
||||||
|
|
||||||
::read(fd, &ch, 1);
|
return value;
|
||||||
|
|
||||||
close(fd);
|
|
||||||
|
|
||||||
if (ch == '0') {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
else if (ch == '1'){
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
return -1;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void LinuxGPIO::write(uint8_t pin, uint8_t value)
|
void LinuxGPIO::write(uint8_t pin, uint8_t value)
|
||||||
{
|
{
|
||||||
int fd;
|
if(value == HIGH){
|
||||||
char buf[64];
|
if(pin/32 < 1){
|
||||||
|
*gpio0.out = *gpio0.out | (1<<pin);
|
||||||
snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", pin);
|
}
|
||||||
|
else if(pin/32 < 2){
|
||||||
fd = open(buf, O_WRONLY);
|
*gpio1.out = *gpio1.out | (1<<(pin-32));
|
||||||
if (fd < 0) {
|
}
|
||||||
perror("LinuxGPIO::write");
|
else if(pin/32 < 3){
|
||||||
|
*gpio2.out = *gpio2.out | (1<<(pin-64));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 4){
|
||||||
|
*gpio3.out = *gpio3.out | (1<<(pin-96));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
if(pin/32 < 1){
|
||||||
|
*gpio0.out = *gpio0.out & (~(1<<pin));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 2){
|
||||||
|
*gpio1.out = *gpio1.out & (~(1<<(pin-32)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 3){
|
||||||
|
*gpio2.out = *gpio2.out & (~(1<<(pin-64)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 4){
|
||||||
|
*gpio3.out = *gpio3.out & (~(1<<(pin-96)));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (value==LOW)
|
|
||||||
::write(fd, "0", 2);
|
|
||||||
else
|
|
||||||
::write(fd, "1", 2);
|
|
||||||
|
|
||||||
close(fd);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void LinuxGPIO::toggle(uint8_t pin)
|
void LinuxGPIO::toggle(uint8_t pin)
|
||||||
@ -167,98 +198,95 @@ bool LinuxGPIO::usb_connected(void)
|
|||||||
LinuxDigitalSource::LinuxDigitalSource(uint8_t v) :
|
LinuxDigitalSource::LinuxDigitalSource(uint8_t v) :
|
||||||
_v(v)
|
_v(v)
|
||||||
{
|
{
|
||||||
int fd, len;
|
|
||||||
char buf[64];
|
|
||||||
|
|
||||||
fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
|
|
||||||
if (fd < 0) {
|
|
||||||
perror("LinuxGPIO::init");
|
|
||||||
}
|
|
||||||
|
|
||||||
len = snprintf(buf, sizeof(buf), "%d", v);
|
|
||||||
::write(fd, buf, len);
|
|
||||||
close(fd);
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void LinuxDigitalSource::mode(uint8_t output)
|
void LinuxDigitalSource::mode(uint8_t output)
|
||||||
{
|
{
|
||||||
|
uint8_t pin = _v;
|
||||||
|
|
||||||
int fd,len;
|
if(output == GPIO_INPUT){
|
||||||
char buf[64];
|
if(pin/32 < 1){
|
||||||
|
*gpio0.oe = *gpio0.oe | (1<<pin);
|
||||||
snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/direction",_v);
|
|
||||||
|
|
||||||
fd = ::open(buf, O_WRONLY);
|
|
||||||
if (fd < 0) {
|
|
||||||
fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY); //try exporting GPIO pin
|
|
||||||
if (fd < 0) {
|
|
||||||
perror("LinuxGPIO::direction");
|
|
||||||
}
|
}
|
||||||
|
else if(pin/32 < 2){
|
||||||
len = snprintf(buf, sizeof(buf), "%d",_v);
|
*gpio1.oe = *gpio1.oe | (1<<(pin-32));
|
||||||
::write(fd, buf, len);
|
}
|
||||||
close(fd);
|
else if(pin/32 < 3){
|
||||||
|
*gpio2.oe = *gpio2.oe | (1<<(pin-64));
|
||||||
snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/direction",_v); //retry writing direction
|
}
|
||||||
fd = ::open(buf, O_WRONLY);
|
else if(pin/32 < 4){
|
||||||
if (fd < 0) {
|
*gpio3.oe = *gpio3.oe | (1<<(pin-96));
|
||||||
perror("LinuxGPIO::direction"); //report faillure
|
}
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
if(pin/32 < 1){
|
||||||
|
*gpio0.oe = *gpio0.oe & (~(1<<pin));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 2){
|
||||||
|
*gpio1.oe = *gpio1.oe & (~(1<<(pin-32)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 3){
|
||||||
|
*gpio2.oe = *gpio2.oe & (~(1<<(pin-64)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 4){
|
||||||
|
*gpio3.oe = *gpio3.oe & (~(1<<(pin-96)));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (output == GPIO_OUTPUT)
|
|
||||||
::write(fd, "out", 4);
|
|
||||||
else
|
|
||||||
::write(fd, "in", 3);
|
|
||||||
|
|
||||||
close(fd);
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t LinuxDigitalSource::read() {
|
uint8_t LinuxDigitalSource::read() {
|
||||||
int fd;
|
uint8_t value, pin = _v;
|
||||||
char buf[64];
|
|
||||||
char ch;
|
|
||||||
|
|
||||||
snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value",_v);
|
if(pin/32 < 1){
|
||||||
|
value = (bool)(*gpio0.in & (1<<pin));
|
||||||
fd = open(buf, O_RDONLY);
|
}
|
||||||
if (fd < 0) {
|
else if(pin/32 < 2){
|
||||||
perror("LinuxGPIO::read");
|
value = (bool)(*gpio1.in & (1<<(pin-32)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 3){
|
||||||
|
value = (bool)(*gpio2.in & (1<<(pin-64)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 4){
|
||||||
|
value = (bool)(*gpio3.in & (1<<(pin-96)));
|
||||||
}
|
}
|
||||||
|
|
||||||
::read(fd, &ch, 1);
|
return value;
|
||||||
|
|
||||||
close(fd);
|
|
||||||
|
|
||||||
if (ch == '0') {
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
else if (ch == '1'){
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
return -1;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void LinuxDigitalSource::write(uint8_t value) {
|
void LinuxDigitalSource::write(uint8_t value) {
|
||||||
int fd;
|
|
||||||
char buf[64];
|
|
||||||
|
|
||||||
snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value",_v);
|
uint8_t pin = _v;
|
||||||
|
|
||||||
fd = open(buf, O_WRONLY);
|
if(value == HIGH){
|
||||||
if (fd < 0) {
|
if(pin/32 < 1){
|
||||||
perror("LinuxGPIO::write");
|
*gpio0.out = *gpio0.out | (1<<pin);
|
||||||
|
}
|
||||||
|
else if(pin/32 < 2){
|
||||||
|
*gpio1.out = *gpio1.out | (1<<(pin-32));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 3){
|
||||||
|
*gpio2.out = *gpio2.out | (1<<(pin-64));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 4){
|
||||||
|
*gpio3.out = *gpio3.out | (1<<(pin-96));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
if(pin/32 < 1){
|
||||||
|
*gpio0.out = *gpio0.out & (~(1<<pin));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 2){
|
||||||
|
*gpio1.out = *gpio1.out & (~(1<<(pin-32)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 3){
|
||||||
|
*gpio2.out = *gpio2.out & (~(1<<(pin-64)));
|
||||||
|
}
|
||||||
|
else if(pin/32 < 4){
|
||||||
|
*gpio3.out = *gpio3.out & (~(1<<(pin-96)));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (value==LOW)
|
|
||||||
::write(fd, "0", 2);
|
|
||||||
else
|
|
||||||
::write(fd, "1", 2);
|
|
||||||
|
|
||||||
close(fd);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void LinuxDigitalSource::toggle() {
|
void LinuxDigitalSource::toggle() {
|
||||||
|
|||||||
@ -5,6 +5,19 @@
|
|||||||
#include <AP_HAL_Linux.h>
|
#include <AP_HAL_Linux.h>
|
||||||
|
|
||||||
#define SYSFS_GPIO_DIR "/sys/class/gpio"
|
#define SYSFS_GPIO_DIR "/sys/class/gpio"
|
||||||
|
|
||||||
|
#define GPIO0_BASE 0x44E07000
|
||||||
|
#define GPIO1_BASE 0x4804C000
|
||||||
|
#define GPIO2_BASE 0x481AC000
|
||||||
|
#define GPIO3_BASE 0x481AE000
|
||||||
|
|
||||||
|
#define GPIO_SIZE 0x00000FFF
|
||||||
|
|
||||||
|
// OE: 0 is output, 1 is input
|
||||||
|
#define GPIO_OE 0x14d
|
||||||
|
#define GPIO_IN 0x14e
|
||||||
|
#define GPIO_OUT 0x14f
|
||||||
|
|
||||||
#define LED_AMBER 117
|
#define LED_AMBER 117
|
||||||
#define LED_BLUE 48
|
#define LED_BLUE 48
|
||||||
#define LED_SAFETY 61
|
#define LED_SAFETY 61
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user