From e42ba853ec6c0e716b3fb4885edf4a3d5de37e65 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 20 Jan 2014 12:59:21 +1100
Subject: [PATCH] AP_Relay: enable up to 4 relay pins

---
 libraries/AP_Relay/AP_Relay.cpp | 64 +++++++++++++++++++++++----------
 libraries/AP_Relay/AP_Relay.h   | 10 +++---
 2 files changed, 52 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp
index 3fef02c215..f9937fbc38 100644
--- a/libraries/AP_Relay/AP_Relay.cpp
+++ b/libraries/AP_Relay/AP_Relay.cpp
@@ -15,19 +15,44 @@
 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 #define RELAY_PIN 13
 #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 #define RELAY_PIN 111
 #else
+#define RELAY_PIN 54
+#endif
+#else
 // no relay for this board
 #define RELAY_PIN -1
 #endif
 
 const AP_Param::GroupInfo AP_Relay::var_info[] PROGMEM = {
     // @Param: PIN
-    // @DisplayName: Relay Pin
-    // @Description: Digital pin number for relay control.
+    // @DisplayName: First Relay Pin
+    // @Description: Digital pin number for first relay control. This is the pin used for camera control.
     // @User: Standard
-    // @Values: 13:APM2 A9 pin,47:APM1 relay,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
-    AP_GROUPINFO("PIN",  0, AP_Relay, _pin, RELAY_PIN),
+    // @Values: 13:APM2 A9 pin,47:APM1 relay,54:Pixhawk FMU AUX1,55:Pixhawk FMU AUX2,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
+    AP_GROUPINFO("PIN",  0, AP_Relay, _pin[0], RELAY_PIN),
+
+    // @Param: PIN2
+    // @DisplayName: Second Relay Pin
+    // @Description: Digital pin number for 2nd relay control.
+    // @User: Standard
+    // @Values: 13:APM2 A9 pin,47:APM1 relay,54:Pixhawk FMU AUX1,55:Pixhawk FMU AUX2,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
+    AP_GROUPINFO("PIN2",  1, AP_Relay, _pin[1], -1),
+
+    // @Param: PIN3
+    // @DisplayName: Third Relay Pin
+    // @Description: Digital pin number for 3rd relay control.
+    // @User: Standard
+    // @Values: 13:APM2 A9 pin,47:APM1 relay,54:Pixhawk FMU AUX1,55:Pixhawk FMU AUX2,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
+    AP_GROUPINFO("PIN3",  2, AP_Relay, _pin[2], -1),
+
+    // @Param: PIN4
+    // @DisplayName: Fourth Relay Pin
+    // @Description: Digital pin number for 4th relay control.
+    // @User: Standard
+    // @Values: 13:APM2 A9 pin,47:APM1 relay,54:Pixhawk FMU AUX1,55:Pixhawk FMU AUX2,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
+    AP_GROUPINFO("PIN4",  3, AP_Relay, _pin[3], -1),
 
     AP_GROUPEND
 };
@@ -43,36 +68,39 @@ AP_Relay::AP_Relay(void)
 
 void AP_Relay::init() 
 {
-    if (_pin != -1) {
-        hal.gpio->pinMode(_pin, GPIO_OUTPUT);
-        off();
+    for (uint8_t i=0; i<AP_RELAY_NUM_RELAYS; i++) {
+        if (_pin[i].get() != -1) {
+            off(i);
+        }
     }
 }
 
-void AP_Relay::on() 
+void AP_Relay::on(uint8_t relay) 
 {    
-    if (_pin != -1) {
-        hal.gpio->write(_pin, 1);
+    if (relay < AP_RELAY_NUM_RELAYS && _pin[relay] != -1) {
+        hal.gpio->pinMode(_pin[relay], GPIO_OUTPUT);
+        hal.gpio->write(_pin[relay], 1);
     }
 }
 
 
-void AP_Relay::off() 
+void AP_Relay::off(uint8_t relay) 
 {
-    if (_pin != -1) {
-        hal.gpio->write(_pin, 0);
+    if (relay < AP_RELAY_NUM_RELAYS && _pin[relay] != -1) {
+        hal.gpio->pinMode(_pin[relay], GPIO_OUTPUT);
+        hal.gpio->write(_pin[relay], 0);
     }
 }
 
 
-void AP_Relay::toggle() 
+void AP_Relay::toggle(uint8_t relay) 
 {
-    if (_pin != -1) {
-        bool ison = hal.gpio->read(_pin);
+    if (relay < AP_RELAY_NUM_RELAYS && _pin[relay] != -1) {
+        bool ison = hal.gpio->read(_pin[relay]);
         if (ison)
-            off();
+            off(relay);
         else
-            on();
+            on(relay);
     }
 }
 
diff --git a/libraries/AP_Relay/AP_Relay.h b/libraries/AP_Relay/AP_Relay.h
index 3d72c1855b..7fb6416ffd 100644
--- a/libraries/AP_Relay/AP_Relay.h
+++ b/libraries/AP_Relay/AP_Relay.h
@@ -15,6 +15,8 @@
 
 #include <AP_Param.h>
 
+#define AP_RELAY_NUM_RELAYS 4
+
 /// @class	AP_Relay
 /// @brief	Class to manage the APM relay
 class AP_Relay {
@@ -25,18 +27,18 @@ public:
     void        init();
 
     // activate the relay
-    void        on();
+    void        on(uint8_t relay);
 
     // de-activate the relay
-    void        off();
+    void        off(uint8_t relay);
 
     // toggle the relay status
-    void        toggle();
+    void        toggle(uint8_t relay);
 
     static const struct AP_Param::GroupInfo        var_info[];
 
 private:
-    AP_Int8 _pin;
+    AP_Int8 _pin[AP_RELAY_NUM_RELAYS];
 };
 
 #endif /* AP_RELAY_H_ */