AP_ServoRelayEvents: More robust check if a channel is available

libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp

@@ -22,13 +22,18 @@
 #include "AP_ServoRelayEvents.h"
 #include <RC_Channel/RC_Channel.h>
 #include <SRV_Channel/SRV_Channel.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
 bool AP_ServoRelayEvents::do_set_servo(uint8_t _channel, uint16_t pwm)
 {
-    if (!(mask & 1U<<(_channel-1))) {
+    SRV_Channel *c = SRV_Channels::srv_channel(_channel-1);
-        // not allowed
+    if (c == nullptr) {
+        return false;
+    }
+    if (c->get_function() != SRV_Channel::k_none) {
+        gcs().send_text(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
         return false;
     }
     if (type == EVENT_TYPE_SERVO && 
@@ -36,10 +41,6 @@ bool AP_ServoRelayEvents::do_set_servo(uint8_t _channel, uint16_t pwm)
         // cancel previous repeat
         repeat = 0;
     }
-    SRV_Channel *c = SRV_Channels::srv_channel(_channel-1);
-    if (c == nullptr) {
-        return false;
-    }
     c->set_output_pwm(pwm);
     return true;
 }
@@ -67,8 +68,12 @@ bool AP_ServoRelayEvents::do_set_relay(uint8_t relay_num, uint8_t state)
 bool AP_ServoRelayEvents::do_repeat_servo(uint8_t _channel, uint16_t _servo_value, 
                                           int16_t _repeat, uint16_t _delay_ms)
 {
-    if (!(mask & 1U<<(_channel-1))) {
+    SRV_Channel *c = SRV_Channels::srv_channel(_channel-1);
-        // not allowed
+    if (c == nullptr) {
+        return false;
+    }
+    if (c->get_function() != SRV_Channel::k_none) {
+        gcs().send_text(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
         return false;
     }
     channel = _channel;

libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h

@@ -27,9 +27,6 @@ public:
         return _singleton;
     }
 
-    // set allowed servo channel mask
-    void set_channel_mask(uint16_t _mask) { mask = _mask; }
-
     bool do_set_servo(uint8_t channel, uint16_t pwm);
     bool do_set_relay(uint8_t relay_num, uint8_t state);
     bool do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repeat, uint16_t delay_time_ms);
@@ -41,7 +38,6 @@ private:
     static AP_ServoRelayEvents *_singleton;
 
     AP_Relay &relay;
-    uint16_t mask;
 
     // event control state
     enum event_type {