|
|
|
@ -17,35 +17,50 @@ extern const AP_HAL::HAL& hal;
@@ -17,35 +17,50 @@ extern const AP_HAL::HAL& hal;
|
|
|
|
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL |
|
|
|
|
#define RELAY_PIN 26 |
|
|
|
|
#else |
|
|
|
|
#error "no RELAY_PIN defined for this board" |
|
|
|
|
// no relay for this board
|
|
|
|
|
#define RELAY_PIN -1 |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
void AP_Relay::init() { |
|
|
|
|
#if RELAY_PIN != -1 |
|
|
|
|
hal.gpio->pinMode(RELAY_PIN, GPIO_OUTPUT); |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AP_Relay::on() { |
|
|
|
|
#if RELAY_PIN != -1 |
|
|
|
|
hal.gpio->write(RELAY_PIN, 1); |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void AP_Relay::off() { |
|
|
|
|
#if RELAY_PIN != -1 |
|
|
|
|
hal.gpio->write(RELAY_PIN, 0); |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void AP_Relay::toggle() { |
|
|
|
|
#if RELAY_PIN != -1 |
|
|
|
|
bool ison = hal.gpio->read(RELAY_PIN); |
|
|
|
|
if (ison) |
|
|
|
|
off(); |
|
|
|
|
else |
|
|
|
|
on(); |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AP_Relay::set(bool status){ |
|
|
|
|
#if RELAY_PIN != -1 |
|
|
|
|
hal.gpio->write(RELAY_PIN, status); |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool AP_Relay::get() { |
|
|
|
|
#if RELAY_PIN != -1 |
|
|
|
|
return hal.gpio->read(RELAY_PIN);
|
|
|
|
|
#else |
|
|
|
|
return false; |
|
|
|
|
#endif |
|
|
|
|
} |
|
|
|
|