Sub 4.5: autofix/warning for misconfigured relays and leak detectors

ArduSub/Parameters.cpp

@@ -834,6 +834,14 @@ void Sub::convert_old_parameters()
     SRV_Channels::upgrade_parameters();
 }
 
+// Helper function to set servo function by channel number, 1-indexed
+static void set_servo_function(uint8_t channel, SRV_Channel::Aux_servo_function_t function)
+{
+    char param_name[20];
+    snprintf(param_name, sizeof(param_name), "SERVO%u_FUNCTION", channel);
+    AP_Param::set_and_save_by_name(param_name, static_cast<int>(function));
+}
+
 
 void Sub::update_actuators_from_jsbuttons()
 {
@@ -921,9 +929,7 @@ void Sub::update_actuators_from_jsbuttons()
 
       if (has_assigned_button) {
           // Assign actuator function to preserve legacy behavior
-          char param_name[20];
-          snprintf(param_name, sizeof(param_name), "SERVO%u_FUNCTION", channel);
-          AP_Param::set_and_save_by_name(param_name, static_cast<int>(target_function));
+          set_servo_function(channel, target_function);
       }
   }
 }
@@ -972,11 +978,52 @@ void Sub::update_lights_from_rcin()
         for (uint8_t func_idx = 0; func_idx < FUNCTIONS_PER_LIGHT; func_idx++) {
             if (sub.jsbutton_function_is_assigned(lights_button_functions[light][func_idx])) {
                 // We have buttons assined to lights. set the channel to the new, dedicated lights function.
-                char param_name[20];
-                snprintf(param_name, sizeof(param_name), "SERVO%u_FUNCTION", existing_channel + 1);
-                AP_Param::set_and_save_by_name(param_name, static_cast<int>(lights_functions[light]));
+                set_servo_function(existing_channel + 1, lights_functions[light]);
                 break;
             }
         }
     }
 }
+
+#if LEAKDETECTOR_MAX_INSTANCES > 0
+
+void Sub::update_leak_pins() {
+    for (uint8_t instance = 0; instance < LEAKDETECTOR_MAX_INSTANCES; instance++) {
+        if (leak_detector.get_pin(instance) > 0) {
+            uint8_t servo_channel;
+            if (hal.gpio->pin_to_servo_channel(leak_detector.get_pin(instance), servo_channel)) {
+                if (!SRV_Channels::is_GPIO(servo_channel)) {
+                    if (SRV_Channels::channel_function(servo_channel) == SRV_Channel::Aux_servo_function_t::k_none) {
+                        gcs().send_text(MAV_SEVERITY_INFO, "Leak detector %u pin (servo %u) auto-set to GPIO", instance + 1, servo_channel + 1);
+                        set_servo_function(servo_channel + 1, SRV_Channel::Aux_servo_function_t::k_GPIO);
+                    }
+                    else {
+                        gcs().send_text(MAV_SEVERITY_WARNING, "Leak detector %u error. Please set SERVO%u_FUNCTION to GPIO", instance + 1, servo_channel + 1);
+                    }
+                }
+            }
+        }
+    }
+}
+#endif
+
+#if AP_RELAY_ENABLED
+void Sub::update_relay_pins() {
+    for (uint8_t instance = 0; instance < AP_RELAY_NUM_RELAYS; instance++) {
+        if (relay.get_gpio_pin(instance) > 0) {
+            uint8_t servo_channel;
+            if (hal.gpio->pin_to_servo_channel(relay.get_gpio_pin(instance), servo_channel)) {
+                if (relay.enabled(instance) && !SRV_Channels::is_GPIO(servo_channel)) {
+                    if (SRV_Channels::channel_function(servo_channel) == SRV_Channel::Aux_servo_function_t::k_none) {
+                        gcs().send_text(MAV_SEVERITY_INFO, "Relay %u pin (servo %u) auto-set to GPIO", instance + 1, servo_channel + 1);
+                        set_servo_function(servo_channel + 1, SRV_Channel::Aux_servo_function_t::k_GPIO);
+                    }
+                    else {
+                        gcs().send_text(MAV_SEVERITY_WARNING, "Relay %u error. Please set SERVO%u_FUNCTION to GPIO", instance + 1, servo_channel + 1);
+                    }
+                }
+            }
+        }
+    }
+}
+#endif

ArduSub/Sub.h

@@ -547,6 +547,12 @@ class Sub : public AP_Vehicle {
     void convert_old_parameters(void);
     void update_actuators_from_jsbuttons();
     void update_lights_from_rcin();
+#if LEAKDETECTOR_MAX_INSTANCES > 0
+    void update_leak_pins();
+#endif
+#if AP_RELAY_ENABLED
+    void update_relay_pins();
+#endif
     bool handle_do_motor_test(mavlink_command_int_t command);
     bool init_motor_test();
     bool verify_motor_test();

ArduSub/system.cpp

@@ -179,8 +179,14 @@ void Sub::init_ardupilot()
 
     g2.actuators.initialize_actuators();
 
-    // flag that initialisation has completed
     update_lights_from_rcin();
+#if LEAKDETECTOR_MAX_INSTANCES > 0
+    update_leak_pins();
+#endif
+#if AP_RELAY_ENABLED
+    update_relay_pins();
+#endif
+    // flag that initialisation has completed
     ap.initialised = true;
 }
 

libraries/AP_HAL_Linux/GPIO_Navigator.cpp

@@ -45,3 +45,12 @@ bool GPIO_Navigator::pinAllowed(uint8_t pin)
     }
     return false;
 }
+
+bool GPIO_Navigator::pin_to_servo_channel(uint8_t pin, uint8_t& servo_ch) const
+{
+    if (1 <= pin && pin <= 16) {
+        servo_ch = pin - 1;
+        return true;
+    }
+    return false;
+}

libraries/AP_HAL_Linux/GPIO_Navigator.h

@@ -13,6 +13,7 @@ class GPIO_Navigator : public GPIO_RPI
     void    pinMode(uint8_t pin, uint8_t output, uint8_t alt) override;
     uint8_t read(uint8_t pin) override;
     void    write(uint8_t pin, uint8_t value) override;
+    bool    pin_to_servo_channel(uint8_t pin, uint8_t& servo_ch) const override;
 private:
     uint8_t AllowedGPIOS[3] = {
         RPI_GPIO_<18>(), // Aux Output for PWMs

libraries/AP_LeakDetector/AP_LeakDetector.h

@@ -36,6 +36,8 @@ class AP_LeakDetector {
     // Update all drivers
     bool update(void);
 
+    int8_t get_pin(uint8_t instance) const { return _pin[instance]; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 private:

libraries/AP_Relay/AP_Relay.h

@@ -73,6 +73,8 @@ class AP_Relay {
     // see if the relay is enabled
     bool enabled(AP_Relay_Params::FUNCTION function) const;
 
+    int8_t get_gpio_pin(uint8_t instance) const { return _params[instance].pin.get(); }
+
 private:
     static AP_Relay *singleton;