From 9835043dd537818947ffd384c1cc9aed2f27f99b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 25 Nov 2015 13:06:43 +1100
Subject: [PATCH] HAL_PX4: support FMUv4

---
 libraries/AP_HAL_PX4/AnalogIn.cpp      |  6 ++---
 libraries/AP_HAL_PX4/AnalogIn.h        |  2 +-
 libraries/AP_HAL_PX4/GPIO.cpp          | 33 ++++++++++++++++++++++----
 libraries/AP_HAL_PX4/GPIO.h            |  8 +++----
 libraries/AP_HAL_PX4/HAL_PX4_Class.cpp |  2 +-
 5 files changed, 38 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_HAL_PX4/AnalogIn.cpp b/libraries/AP_HAL_PX4/AnalogIn.cpp
index 11fae8d8ae..5f2789979a 100644
--- a/libraries/AP_HAL_PX4/AnalogIn.cpp
+++ b/libraries/AP_HAL_PX4/AnalogIn.cpp
@@ -48,7 +48,7 @@ static const struct {
     { 11,  6.6f/4096  }, // analog airspeed input, 2:1 scaling
     { 12,  3.3f/4096  }, // analog2, on SPI port pin 3
     { 13, 16.8f/4096  }, // analog3, on SPI port pin 4
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
     { 2,   3.3f/4096  },    // 3DR Brick voltage, usually 10.1:1
                             // scaled from battery voltage
     { 3,   3.3f/4096  },    // 3DR Brick current, usually 17:1 scaled
@@ -269,7 +269,7 @@ void PX4AnalogIn::_timer_tick(void)
     if (ret > 0) {
         // match the incoming channels to the currently active pins
         for (uint8_t i=0; i<ret/sizeof(buf_adc[0]); i++) {
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
             if (buf_adc[i].am_channel == 4) {
                 // record the Vcc value for later use in
                 // voltage_average_ratiometric()
@@ -325,7 +325,7 @@ void PX4AnalogIn::_timer_tick(void)
     }
 #endif
 
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
     // check for new servorail data on FMUv2
     if (_servorail_handle != -1) {
         struct servorail_status_s servorail;
diff --git a/libraries/AP_HAL_PX4/AnalogIn.h b/libraries/AP_HAL_PX4/AnalogIn.h
index 4bd4d523de..f452364e0c 100644
--- a/libraries/AP_HAL_PX4/AnalogIn.h
+++ b/libraries/AP_HAL_PX4/AnalogIn.h
@@ -14,7 +14,7 @@
 // these are virtual pins that read from the ORB
 #define PX4_ANALOG_ORB_BATTERY_VOLTAGE_PIN     100
 #define PX4_ANALOG_ORB_BATTERY_CURRENT_PIN     101
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 #define PX4_ANALOG_VCC_5V_PIN                4
 #define PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN       102
 #define PX4_ANALOG_ORB_SERVO_VRSSI_PIN         103
diff --git a/libraries/AP_HAL_PX4/GPIO.cpp b/libraries/AP_HAL_PX4/GPIO.cpp
index e7b40f6ff6..cb56eddc3e 100644
--- a/libraries/AP_HAL_PX4/GPIO.cpp
+++ b/libraries/AP_HAL_PX4/GPIO.cpp
@@ -58,11 +58,13 @@ void PX4GPIO::init()
     }
 #endif
 
+#ifdef PX4IO_DEVICE_PATH
     // also try to setup for the relay pins on the IO board
     _gpio_io_fd = open(PX4IO_DEVICE_PATH, O_RDWR);
     if (_gpio_io_fd == -1) {
         hal.console->printf("GPIO: Unable to open px4io\n");
     }
+#endif
 }
 
 void PX4GPIO::pinMode(uint8_t pin, uint8_t output)
@@ -117,6 +119,9 @@ uint8_t PX4GPIO::read(uint8_t pin) {
 #ifdef PX4IO_P_SETUP_RELAYS_POWER1
         case PX4_GPIO_EXT_IO_RELAY1_PIN: {
             uint32_t relays = 0;
+            if (_gpio_io_fd == -1) {
+                return LOW;
+            }
             ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays);
             return (relays & PX4IO_P_SETUP_RELAYS_POWER1)?HIGH:LOW;
         }
@@ -125,6 +130,9 @@ uint8_t PX4GPIO::read(uint8_t pin) {
 #ifdef PX4IO_P_SETUP_RELAYS_POWER2
         case PX4_GPIO_EXT_IO_RELAY2_PIN: {
             uint32_t relays = 0;
+            if (_gpio_io_fd == -1) {
+                return LOW;
+            }
             ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays);
             return (relays & PX4IO_P_SETUP_RELAYS_POWER2)?HIGH:LOW;
         }
@@ -133,6 +141,9 @@ uint8_t PX4GPIO::read(uint8_t pin) {
 #ifdef PX4IO_P_SETUP_RELAYS_ACC1
         case PX4_GPIO_EXT_IO_ACC1_PIN: {
             uint32_t relays = 0;
+            if (_gpio_io_fd == -1) {
+                return LOW;
+            }
             ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays);
             return (relays & PX4IO_P_SETUP_RELAYS_ACC1)?HIGH:LOW;
         }
@@ -141,6 +152,9 @@ uint8_t PX4GPIO::read(uint8_t pin) {
 #ifdef PX4IO_P_SETUP_RELAYS_ACC2
         case PX4_GPIO_EXT_IO_ACC2_PIN: {
             uint32_t relays = 0;
+            if (_gpio_io_fd == -1) {
+                return LOW;
+            }
             ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays);
             return (relays & PX4IO_P_SETUP_RELAYS_ACC2)?HIGH:LOW;
         }
@@ -148,6 +162,9 @@ uint8_t PX4GPIO::read(uint8_t pin) {
 
     case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5): {
             uint32_t relays = 0;
+            if (_gpio_io_fd == -1) {
+                return LOW;
+            }
             ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays);
             return (relays & (1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0))))?HIGH:LOW;
         }
@@ -203,25 +220,33 @@ void PX4GPIO::write(uint8_t pin, uint8_t value)
 
 #ifdef PX4IO_P_SETUP_RELAYS_POWER1
         case PX4_GPIO_EXT_IO_RELAY1_PIN:
-            ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_POWER1);
+            if (_gpio_io_fd != -1) {
+                ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_POWER1);
+            }
             break;
 #endif
 
 #ifdef PX4IO_P_SETUP_RELAYS_POWER2
         case PX4_GPIO_EXT_IO_RELAY2_PIN:
-            ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_POWER2);
+            if (_gpio_io_fd != -1) {
+                ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_POWER2);
+            }
             break;
 #endif
 
 #ifdef PX4IO_P_SETUP_RELAYS_ACC1
         case PX4_GPIO_EXT_IO_ACC1_PIN:
-            ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_ACC1);
+            if (_gpio_io_fd != -1) {
+                ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_ACC1);
+            }
             break;
 #endif
 
 #ifdef PX4IO_P_SETUP_RELAYS_ACC2
         case PX4_GPIO_EXT_IO_ACC2_PIN:
-            ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_ACC2);
+            if (_gpio_io_fd != -1) {
+                ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_ACC2);
+            }
             break;
 #endif
 
diff --git a/libraries/AP_HAL_PX4/GPIO.h b/libraries/AP_HAL_PX4/GPIO.h
index 49d1cf1b07..59fbcdbc8b 100644
--- a/libraries/AP_HAL_PX4/GPIO.h
+++ b/libraries/AP_HAL_PX4/GPIO.h
@@ -50,10 +50,10 @@ public:
     void set_usb_connected(void) { _usb_connected = true; }
 
 private:
-    int _led_fd;
-    int _tone_alarm_fd;
-    int _gpio_fmu_fd;
-    int _gpio_io_fd;
+    int _led_fd = -1;
+    int _tone_alarm_fd = -1;
+    int _gpio_fmu_fd = -1;
+    int _gpio_io_fd = -1;
     bool _usb_connected = false;
 };
 
diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
index 43998c8e60..c928eaffda 100644
--- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
+++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
@@ -43,7 +43,7 @@ static PX4AnalogIn analogIn;
 static PX4Util utilInstance;
 static PX4GPIO gpioDriver;
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
 #define UARTB_DEFAULT_DEVICE "/dev/ttyS3"
 #define UARTC_DEFAULT_DEVICE "/dev/ttyS1"