HAL_Linux: added initial gpio library

libraries/AP_HAL_Linux/GPIO.cpp

@@ -3,6 +3,13 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_ERLE
 
 #include "GPIO.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <poll.h>
 
 using namespace Linux;
 
@@ -10,10 +17,66 @@ LinuxGPIO::LinuxGPIO()
 {}
 
 void LinuxGPIO::init()
-{}
+{
+    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", LED_AMBER);
+    ::write(fd, buf, len);
+    close(fd);
+    
+      fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::init");
+    }
+
+    len = snprintf(buf, sizeof(buf), "%d", LED_BLUE);
+    ::write(fd, buf, len);
+    close(fd);
+
+    fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::init");
+    }
+
+    len = snprintf(buf, sizeof(buf), "%d", LED_SAFETY);
+    ::write(fd, buf, len);
+    close(fd);
+
+    fd = open(SYSFS_GPIO_DIR "/export", O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::init");
+    }
+
+    len = snprintf(buf, sizeof(buf), "%d", SAFETY_SWITCH);
+    ::write(fd, buf, len);
+    close(fd);
+}
 
 void LinuxGPIO::pinMode(uint8_t pin, uint8_t output)
-{}
+{
+    int fd;
+    char buf[64];
+
+    snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR  "/gpio%d/direction", pin);
+
+    fd = ::open(buf, O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::direction");
+    }
+
+    if (output == GPIO_OUTPUT)
+        ::write(fd, "out", 4);
+    else
+        ::write(fd, "in", 3);
+
+    close(fd);
+}
 
 int8_t LinuxGPIO::analogPinToDigitalPin(uint8_t pin)
 {
@@ -22,14 +85,55 @@ int8_t LinuxGPIO::analogPinToDigitalPin(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);
+
+    fd = open(buf, O_RDONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::read");
+    }
+
+    ::read(fd, &ch, 1);
+    
+    close(fd);
+
+    if (ch == '0') {
         return 0;
     } 
+    else if (ch == '1'){
+        return 1;
+    }
+
+    return -1;
+}
 
 void LinuxGPIO::write(uint8_t pin, uint8_t value)
-{}
+{
+    int fd;
+    char buf[64];
+
+    snprintf(buf, sizeof(buf), SYSFS_GPIO_DIR "/gpio%d/value", pin);
+
+    fd = open(buf, O_WRONLY);
+    if (fd < 0) {
+        perror("LinuxGPIO::write");
+    }
+
+    if (value==LOW)
+       ::write(fd, "0", 2);
+    else
+       ::write(fd, "1", 2);
+
+    close(fd);
+}
 
 void LinuxGPIO::toggle(uint8_t pin)
-{}
+{
+    write(pin, !read(pin));
+}
 
 /* Alternative interface: */
 AP_HAL::DigitalSource* LinuxGPIO::channel(uint16_t n) {

libraries/AP_HAL_Linux/GPIO.h

@@ -4,6 +4,16 @@
 
 #include <AP_HAL_Linux.h>
 
+#define SYSFS_GPIO_DIR "/sys/class/gpio"
+#define LED_AMBER       117
+#define LED_BLUE        48
+#define LED_SAFETY      61
+#define SAFETY_SWITCH   116
+#define LOW             0
+#define HIGH            1
+
+
+
 class Linux::LinuxGPIO : public AP_HAL::GPIO {
 public:
     LinuxGPIO();

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -73,7 +73,7 @@ void HAL_Linux::init(int argc,char* const argv[]) const
     }
 
     scheduler->init(NULL);
-
+    gpio->init();
     rcout->init(NULL);
     uartA->begin(115200);
     i2c->begin();