diff --git a/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp b/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp index 925057a986..aa08edb37e 100644 --- a/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp +++ b/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp @@ -22,13 +22,18 @@ #include "AP_ServoRelayEvents.h" #include #include +#include extern const AP_HAL::HAL& hal; bool AP_ServoRelayEvents::do_set_servo(uint8_t _channel, uint16_t pwm) { - if (!(mask & 1U<<(_channel-1))) { - // not allowed + SRV_Channel *c = SRV_Channels::srv_channel(_channel-1); + 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))) { - // not allowed + SRV_Channel *c = SRV_Channels::srv_channel(_channel-1); + 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; diff --git a/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h b/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h index 400e42fb5b..bddb0977d3 100644 --- a/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h +++ b/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 {