From 7daa3201fd1ce8600c7053022a6413b9f3cf2454 Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Tue, 24 Mar 2015 15:44:54 -0300
Subject: [PATCH] AP_HAL_Linux: add test for GPIO

---
 .../examples/GPIOTest/GPIOTest.cpp            | 123 ++++++++++++++++++
 .../AP_HAL_Linux/examples/GPIOTest/Makefile   |   1 +
 .../AP_HAL_Linux/examples/GPIOTest/make.inc   |   2 +
 3 files changed, 126 insertions(+)
 create mode 100644 libraries/AP_HAL_Linux/examples/GPIOTest/GPIOTest.cpp
 create mode 100644 libraries/AP_HAL_Linux/examples/GPIOTest/Makefile
 create mode 100644 libraries/AP_HAL_Linux/examples/GPIOTest/make.inc

diff --git a/libraries/AP_HAL_Linux/examples/GPIOTest/GPIOTest.cpp b/libraries/AP_HAL_Linux/examples/GPIOTest/GPIOTest.cpp
new file mode 100644
index 0000000000..272cd96841
--- /dev/null
+++ b/libraries/AP_HAL_Linux/examples/GPIOTest/GPIOTest.cpp
@@ -0,0 +1,123 @@
+#include <stdio.h>
+#include <unistd.h>
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL_Linux/AP_HAL_Linux.h>
+#include <AP_Menu/AP_Menu.h>
+
+#define MENU_FUNC(func) FUNCTOR_BIND(&commands, &MenuCommands::func, int8_t, uint8_t, const Menu::arg *)
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+int parse_gpio_pin_number(uint8_t argc, const Menu::arg *argv) {
+    if (argc != 2) {
+        fprintf(stderr, "Input and output commands take only one argument, which is the GPIO pin number\n");
+        return -1;
+    }
+
+    long int pin = argv[1].i;
+    if (pin <= 0) {
+        fprintf(stderr, "Invalid pin number: %ld\n", pin);
+        return -1;
+    }
+
+    return pin;
+}
+
+static int8_t test_gpio_input(uint8_t argc, const Menu::arg *argv, bool use_channel) {
+    AP_HAL::DigitalSource *ch = NULL;
+    int pin = parse_gpio_pin_number(argc, argv);
+
+    if (pin <= 0) return -1;
+
+    if (use_channel) {
+        ch = hal.gpio->channel(pin);
+        ch->mode(HAL_GPIO_INPUT);
+    } else {
+        hal.gpio->pinMode(pin, HAL_GPIO_INPUT);
+    }
+
+    hal.console->printf("Ok, I'll start reading pin number %d  and printing the value read on intervals of 1 second.", pin);
+    while (1) {
+        hal.console->printf("%u ", use_channel ? ch->read() : hal.gpio->read(pin));
+        sleep(1);
+    }
+    return 0;
+}
+
+static int8_t test_gpio_output(uint8_t argc, const Menu::arg *argv, bool use_channel) {
+    AP_HAL::DigitalSource *ch = NULL;
+    int pin = parse_gpio_pin_number(argc, argv);
+
+    if (pin <= 0) return -1;
+
+    if (use_channel) {
+        ch = hal.gpio->channel(pin);
+        ch->mode(HAL_GPIO_OUTPUT);
+    } else {
+        hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT);
+    }
+
+    hal.console->printf("Now I'll start toggling the signal on the pin number %d on intervals of 1 second."
+            " It's recommended to verify that the signal is reaching it (e.g. by using a LED)\n", pin);
+    uint8_t signal = 0;
+    while (1) {
+        signal ^= 1;
+        if (use_channel) {
+            ch->write(signal);
+        } else {
+            hal.gpio->write(pin, signal);
+        }
+        sleep(1);
+    }
+    return 0;
+}
+
+class MenuCommands {
+public:
+    int8_t gpio_input(uint8_t argc, const Menu::arg *argv) {
+        return ::test_gpio_input(argc, argv, false);
+    }
+
+    int8_t gpio_output(uint8_t argc, const Menu::arg *argv) {
+        return ::test_gpio_output(argc, argv, false);
+    }
+
+    int8_t gpio_input_channel(uint8_t argc, const Menu::arg *argv) {
+        hal.console->printf("GPIO Input using digital source\n");
+        return test_gpio_input(argc, argv, true);
+    }
+
+    int8_t gpio_output_channel(uint8_t argc, const Menu::arg *argv) {
+        hal.console->printf("GPIO Output using digital source\n");
+        return test_gpio_output(argc, argv, true);
+    }
+
+};
+
+MenuCommands commands;
+
+const struct Menu::command test_menu_commands[] PROGMEM = {
+    {"input",          MENU_FUNC(gpio_input)},
+    {"output",         MENU_FUNC(gpio_output)},
+    {"input_ch",       MENU_FUNC(gpio_input_channel)},
+    {"output_ch",      MENU_FUNC(gpio_output_channel)}
+};
+
+MENU(main_menu, "GPIOTest: Please select one of the operations followed by the GPIO pin number", test_menu_commands);
+
+void setup(void)
+{
+    Menu::set_port(hal.console);
+    hal.console->set_blocking_writes(true);
+
+    while (1) {
+        main_menu.run();
+    }
+}
+
+void loop(void)
+{
+}
+
+AP_HAL_MAIN();
diff --git a/libraries/AP_HAL_Linux/examples/GPIOTest/Makefile b/libraries/AP_HAL_Linux/examples/GPIOTest/Makefile
new file mode 100644
index 0000000000..f5daf25151
--- /dev/null
+++ b/libraries/AP_HAL_Linux/examples/GPIOTest/Makefile
@@ -0,0 +1 @@
+include ../../../../mk/apm.mk
diff --git a/libraries/AP_HAL_Linux/examples/GPIOTest/make.inc b/libraries/AP_HAL_Linux/examples/GPIOTest/make.inc
new file mode 100644
index 0000000000..084e7b58dd
--- /dev/null
+++ b/libraries/AP_HAL_Linux/examples/GPIOTest/make.inc
@@ -0,0 +1,2 @@
+LIBRARIES += StorageManager
+LIBRARIES += AP_Menu