From 5b081a704c6dbae5388a10bced6b4fa0aa99f445 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 22 Jul 2016 10:58:02 +1000
Subject: [PATCH] HAL_SITL: added GPIO simulation

using SIM_PIN_MASK
---
 libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h |  2 +
 libraries/AP_HAL_SITL/GPIO.cpp                | 85 +++++++++++++++++++
 libraries/AP_HAL_SITL/GPIO.h                  | 41 +++++++++
 libraries/AP_HAL_SITL/HAL_SITL_Class.cpp      |  5 +-
 libraries/AP_HAL_SITL/SITL_State.h            |  1 +
 5 files changed, 132 insertions(+), 2 deletions(-)
 create mode 100644 libraries/AP_HAL_SITL/GPIO.cpp
 create mode 100644 libraries/AP_HAL_SITL/GPIO.h

diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
index fd7ba7d3d8..58e9abcb2b 100644
--- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
+++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
@@ -12,4 +12,6 @@ class ADCSource;
 class RCInput;
 class Util;
 class Semaphore;
+class GPIO;
+class DigitalSource;
 }
diff --git a/libraries/AP_HAL_SITL/GPIO.cpp b/libraries/AP_HAL_SITL/GPIO.cpp
new file mode 100644
index 0000000000..f628648d86
--- /dev/null
+++ b/libraries/AP_HAL_SITL/GPIO.cpp
@@ -0,0 +1,85 @@
+
+#include "GPIO.h"
+
+using namespace HALSITL;
+
+extern const AP_HAL::HAL& hal;
+
+void GPIO::init()
+{}
+
+void GPIO::pinMode(uint8_t pin, uint8_t output)
+{}
+
+int8_t GPIO::analogPinToDigitalPin(uint8_t pin)
+{
+	return pin;
+}
+
+
+uint8_t GPIO::read(uint8_t pin)
+{
+    if (!sitlState->_sitl) {
+        return 0;
+    }
+    uint8_t mask = sitlState->_sitl->pin_mask.get();
+    return mask & (1U<<pin)? 1: 0;
+}
+
+void GPIO::write(uint8_t pin, uint8_t value)
+{
+    if (!sitlState->_sitl) {
+        return;
+    }
+    uint8_t mask = sitlState->_sitl->pin_mask.get();
+    if (value) {
+        mask |= (1U<<pin);
+    } else {
+        mask &= ~(1U<<pin);
+    }
+    sitlState->_sitl->pin_mask.set(mask);
+}
+
+void GPIO::toggle(uint8_t pin)
+{
+    write(pin, !read(pin));
+}
+
+/* Alternative interface: */
+AP_HAL::DigitalSource* GPIO::channel(uint16_t n) {
+    return new DigitalSource(0);
+}
+
+/* Interrupt interface: */
+bool GPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p,
+        uint8_t mode) {
+    return true;
+}
+
+bool GPIO::usb_connected(void)
+{
+    return false;
+}
+
+DigitalSource::DigitalSource(uint8_t v) :
+    pin(v)
+{}
+
+void DigitalSource::mode(uint8_t output)
+{}
+
+uint8_t DigitalSource::read()
+{
+    return hal.gpio->read(pin);
+}
+
+void DigitalSource::write(uint8_t value)
+{
+    value = value?1:0;
+    return hal.gpio->write(pin, value);
+}
+
+void DigitalSource::toggle()
+{
+    return hal.gpio->write(pin, !hal.gpio->read(pin));
+}
diff --git a/libraries/AP_HAL_SITL/GPIO.h b/libraries/AP_HAL_SITL/GPIO.h
new file mode 100644
index 0000000000..d31f443799
--- /dev/null
+++ b/libraries/AP_HAL_SITL/GPIO.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include "AP_HAL_SITL.h"
+
+class HALSITL::GPIO : public AP_HAL::GPIO {
+public:
+    GPIO(SITL_State *_sitlState) {
+        sitlState = _sitlState;
+    }
+    void    init();
+    void    pinMode(uint8_t pin, uint8_t output);
+    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);
+
+    /* Alternative interface: */
+    AP_HAL::DigitalSource* channel(uint16_t n);
+
+    /* Interrupt interface: */
+    bool    attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p,
+            uint8_t mode);
+
+    /* return true if USB cable is connected */
+    bool    usb_connected(void);
+
+private:
+    SITL_State *sitlState;
+};
+
+class HALSITL::DigitalSource : public AP_HAL::DigitalSource {
+public:
+    DigitalSource(uint8_t _pin);
+    void    mode(uint8_t output);
+    uint8_t read();
+    void    write(uint8_t value); 
+    void    toggle();
+
+private:
+    uint8_t pin;
+};
diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
index 5dbf1315a8..8a2fb55127 100644
--- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
+++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
@@ -15,6 +15,7 @@
 #include "Storage.h"
 #include "RCInput.h"
 #include "RCOutput.h"
+#include "GPIO.h"
 #include "SITL_State.h"
 #include "Util.h"
 
@@ -29,9 +30,9 @@ static Scheduler sitlScheduler(&sitlState);
 static RCInput  sitlRCInput(&sitlState);
 static RCOutput sitlRCOutput(&sitlState);
 static AnalogIn sitlAnalogIn(&sitlState);
+static GPIO sitlGPIO(&sitlState);
 
 // use the Empty HAL for hardware we don't emulate
-static Empty::GPIO emptyGPIO;
 static Empty::Semaphore emptyI2Csemaphore;
 static Empty::I2CDriver emptyI2C(&emptyI2Csemaphore);
 static Empty::I2CDeviceManager i2c_mgr_instance;
@@ -63,7 +64,7 @@ HAL_SITL::HAL_SITL() :
         &sitlAnalogIn, /* analogin */
         &sitlEEPROMStorage, /* storage */
         &sitlUart0Driver, /* console */
-        &emptyGPIO, /* gpio */
+        &sitlGPIO, /* gpio */
         &sitlRCInput,  /* rcinput */
         &sitlRCOutput, /* rcoutput */
         &sitlScheduler, /* scheduler */
diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h
index 32f0621307..6471475c62 100644
--- a/libraries/AP_HAL_SITL/SITL_State.h
+++ b/libraries/AP_HAL_SITL/SITL_State.h
@@ -30,6 +30,7 @@ class HAL_SITL;
 class HALSITL::SITL_State {
     friend class HALSITL::Scheduler;
     friend class HALSITL::Util;
+    friend class HALSITL::GPIO;
 public:
     void init(int argc, char * const argv[]);