From 5c6479bf18633a1c2738aa47c9754c4b57104198 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Mon, 23 Oct 2023 12:31:58 -0700
Subject: [PATCH] Relay

---
 libraries/AP_Relay/AP_Relay.cpp | 41 +++++++++++++++++++++++++++++++--
 libraries/AP_Relay/AP_Relay.h   |  1 +
 2 files changed, 40 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp
index 1a2d03e9726d0b..0c5092861faa90 100644
--- a/libraries/AP_Relay/AP_Relay.cpp
+++ b/libraries/AP_Relay/AP_Relay.cpp
@@ -169,6 +169,28 @@ void AP_Relay::init()
           set(i, (bool)default_state);
         }
     }
+
+    for (uint8_t instance = 0; instance < AP_RELAY_NUM_RELAYS; instance++) {
+        const int8_t pin = _params[instance].pin;
+        if (pin == -1) {
+            // no valid pin to set it on, skip it
+            continue;
+        }
+
+        const AP_Relay_Params::Function function = _params[instance].function;
+        if (function <= AP_Relay_Params::Function::relay || function >= AP_Relay_Params::Function::num_functions) {
+            // invalid function, skip it
+            continue;
+        }
+
+        // all functions are interpreted to be off at power on, so if we tag our pins to the wrong
+        // state that will trigger the first call to output to update the state, and will get the
+        // pins into the correct state.
+        //
+        // This will need to be made aware of pin inversion when that support is added
+        _pin_states = _pin_states | (1U<<instance);
+    }
+
 }
 
 void AP_Relay::set(const AP_Relay_Params::Function function, const bool value) {
@@ -186,6 +208,13 @@ void AP_Relay::set(const uint8_t instance, const bool value)
         return;
     }
 
+    if (!_relay_by_instance_has_been_asserted.get(instance)) {
+        // this is the first time this pin has been assigned make sure the tagged previous state was wrong
+        // so that update is guranteed to cause it to output
+        _pin_states = !value ? _pin_states | (1U<<instance) : _pin_states & ~(1U<<instance);
+        _relay_by_instance_has_been_asserted.set(instance);
+    }
+
     _relay_by_instance.setonoff(instance, value);
 }
 
@@ -267,8 +296,16 @@ void AP_Relay::update()
             continue;
         }
 
-        const bool instance_numbered_relay = (function == AP_Relay_Params::Function::relay);
-        const bool value = instance_numbered_relay ? _relay_by_instance.get (instance) : _desired_state.get((uint16_t)function);
+        bool value;
+        if (function == AP_Relay_Params::Function::relay) {
+            if (_relay_by_instance_has_been_asserted.get(instance)) {
+                value = _relay_by_instance.get (instance);
+            } else {
+                continue; // still in the default no change state
+            }
+        } else {
+            value = _desired_state.get((uint16_t)function);
+        }
         const uint8_t old_pin_states = _pin_states;
         _pin_states = value ? _pin_states | (1U<<instance) : _pin_states & ~(1U<<instance);
         if (old_pin_states != _pin_states) {
diff --git a/libraries/AP_Relay/AP_Relay.h b/libraries/AP_Relay/AP_Relay.h
index af422a3c6cf82d..aa3750435bdb55 100644
--- a/libraries/AP_Relay/AP_Relay.h
+++ b/libraries/AP_Relay/AP_Relay.h
@@ -74,6 +74,7 @@ class AP_Relay {
     void convert_params();
 
     Bitmask<AP_RELAY_NUM_RELAYS> _relay_by_instance;
+    Bitmask<AP_RELAY_NUM_RELAYS> _relay_by_instance_has_been_asserted;
     Bitmask<(uint16_t)AP_Relay_Params::Function::num_functions> _desired_state;
 };