mirror of https://github.com/ArduPilot/ardupilot
AP_ServoRelayEvents: use Relay singleton
This commit is contained in:
parent
8e5ec84317
commit
c515d17a4a
|
@ -54,7 +54,12 @@ bool AP_ServoRelayEvents::do_set_servo(uint8_t _channel, uint16_t pwm)
|
||||||
|
|
||||||
bool AP_ServoRelayEvents::do_set_relay(uint8_t relay_num, uint8_t state)
|
bool AP_ServoRelayEvents::do_set_relay(uint8_t relay_num, uint8_t state)
|
||||||
{
|
{
|
||||||
if (!relay.enabled(relay_num)) {
|
AP_Relay *relay = AP::relay();
|
||||||
|
if (relay == nullptr) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!relay->enabled(relay_num)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (type == EVENT_TYPE_RELAY &&
|
if (type == EVENT_TYPE_RELAY &&
|
||||||
|
@ -63,11 +68,11 @@ bool AP_ServoRelayEvents::do_set_relay(uint8_t relay_num, uint8_t state)
|
||||||
repeat = 0;
|
repeat = 0;
|
||||||
}
|
}
|
||||||
if (state == 1) {
|
if (state == 1) {
|
||||||
relay.on(relay_num);
|
relay->on(relay_num);
|
||||||
} else if (state == 0) {
|
} else if (state == 0) {
|
||||||
relay.off(relay_num);
|
relay->off(relay_num);
|
||||||
} else {
|
} else {
|
||||||
relay.toggle(relay_num);
|
relay->toggle(relay_num);
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -102,7 +107,11 @@ bool AP_ServoRelayEvents::do_repeat_servo(uint8_t _channel, uint16_t _servo_valu
|
||||||
|
|
||||||
bool AP_ServoRelayEvents::do_repeat_relay(uint8_t relay_num, int16_t _repeat, uint32_t _delay_ms)
|
bool AP_ServoRelayEvents::do_repeat_relay(uint8_t relay_num, int16_t _repeat, uint32_t _delay_ms)
|
||||||
{
|
{
|
||||||
if (!relay.enabled(relay_num)) {
|
AP_Relay *relay = AP::relay();
|
||||||
|
if (relay == nullptr) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (!relay->enabled(relay_num)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
type = EVENT_TYPE_RELAY;
|
type = EVENT_TYPE_RELAY;
|
||||||
|
@ -140,11 +149,14 @@ void AP_ServoRelayEvents::update_events(void)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case EVENT_TYPE_RELAY:
|
case EVENT_TYPE_RELAY: {
|
||||||
relay.toggle(channel);
|
AP_Relay *relay = AP::relay();
|
||||||
|
if (relay != nullptr) {
|
||||||
|
relay->toggle(channel);
|
||||||
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
}
|
||||||
if (repeat > 0) {
|
if (repeat > 0) {
|
||||||
repeat--;
|
repeat--;
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -11,9 +11,8 @@
|
||||||
|
|
||||||
class AP_ServoRelayEvents {
|
class AP_ServoRelayEvents {
|
||||||
public:
|
public:
|
||||||
AP_ServoRelayEvents(AP_Relay &_relay)
|
AP_ServoRelayEvents()
|
||||||
: relay(_relay)
|
: type(EVENT_TYPE_RELAY)
|
||||||
, type(EVENT_TYPE_RELAY)
|
|
||||||
{
|
{
|
||||||
_singleton = this;
|
_singleton = this;
|
||||||
}
|
}
|
||||||
|
@ -37,8 +36,6 @@ private:
|
||||||
|
|
||||||
static AP_ServoRelayEvents *_singleton;
|
static AP_ServoRelayEvents *_singleton;
|
||||||
|
|
||||||
AP_Relay &relay;
|
|
||||||
|
|
||||||
// event control state
|
// event control state
|
||||||
enum event_type {
|
enum event_type {
|
||||||
EVENT_TYPE_RELAY=0,
|
EVENT_TYPE_RELAY=0,
|
||||||
|
|
Loading…
Reference in New Issue