mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 23:48:31 -04:00
AP_Relay: tidy, reduce flash usage
This commit is contained in:
parent
b6efcbcf3b
commit
1c3b2f9699
libraries/AP_Relay
@ -115,50 +115,33 @@ AP_Relay::AP_Relay(void)
|
||||
}
|
||||
|
||||
|
||||
void AP_Relay::init()
|
||||
void AP_Relay::init()
|
||||
{
|
||||
if (_default != 0 && _default != 1) {
|
||||
return;
|
||||
}
|
||||
for (uint8_t i=0; i<AP_RELAY_NUM_RELAYS; i++) {
|
||||
if (_pin[i].get() != -1) {
|
||||
switch (_default) {
|
||||
case 0:
|
||||
off(i);
|
||||
break;
|
||||
case 1:
|
||||
on(i);
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
set(i, _default);
|
||||
}
|
||||
}
|
||||
|
||||
void AP_Relay::on(uint8_t relay)
|
||||
{
|
||||
if (relay < AP_RELAY_NUM_RELAYS && _pin[relay] != -1) {
|
||||
hal.gpio->pinMode(_pin[relay], HAL_GPIO_OUTPUT);
|
||||
hal.gpio->write(_pin[relay], 1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void AP_Relay::off(uint8_t relay)
|
||||
void AP_Relay::set(const uint8_t relay, const bool value)
|
||||
{
|
||||
if (relay < AP_RELAY_NUM_RELAYS && _pin[relay] != -1) {
|
||||
hal.gpio->pinMode(_pin[relay], HAL_GPIO_OUTPUT);
|
||||
hal.gpio->write(_pin[relay], 0);
|
||||
if (relay >= AP_RELAY_NUM_RELAYS) {
|
||||
return;
|
||||
}
|
||||
if (_pin[relay] == -1) {
|
||||
return;
|
||||
}
|
||||
hal.gpio->pinMode(_pin[relay], HAL_GPIO_OUTPUT);
|
||||
hal.gpio->write(_pin[relay], value);
|
||||
}
|
||||
|
||||
|
||||
void AP_Relay::toggle(uint8_t relay)
|
||||
{
|
||||
if (relay < AP_RELAY_NUM_RELAYS && _pin[relay] != -1) {
|
||||
bool ison = hal.gpio->read(_pin[relay]);
|
||||
if (ison)
|
||||
off(relay);
|
||||
else
|
||||
on(relay);
|
||||
set(relay, !ison);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -27,10 +27,10 @@ public:
|
||||
void init();
|
||||
|
||||
// activate the relay
|
||||
void on(uint8_t relay);
|
||||
void on(uint8_t relay) { set(relay, true); }
|
||||
|
||||
// de-activate the relay
|
||||
void off(uint8_t relay);
|
||||
void off(uint8_t relay) { set(relay, false); }
|
||||
|
||||
// see if the relay is enabled
|
||||
bool enabled(uint8_t relay) { return relay < AP_RELAY_NUM_RELAYS && _pin[relay] != -1; }
|
||||
@ -47,6 +47,8 @@ private:
|
||||
|
||||
AP_Int8 _pin[AP_RELAY_NUM_RELAYS];
|
||||
AP_Int8 _default;
|
||||
|
||||
void set(uint8_t relay, bool value);
|
||||
};
|
||||
|
||||
namespace AP {
|
||||
|
Loading…
Reference in New Issue
Block a user