|
|
|
@ -13,33 +13,32 @@ void GPIO::pinMode(uint8_t pin, uint8_t output)
@@ -13,33 +13,32 @@ void GPIO::pinMode(uint8_t pin, uint8_t output)
|
|
|
|
|
|
|
|
|
|
int8_t GPIO::analogPinToDigitalPin(uint8_t pin) |
|
|
|
|
{ |
|
|
|
|
return pin; |
|
|
|
|
return pin; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
uint8_t GPIO::read(uint8_t pin) |
|
|
|
|
{ |
|
|
|
|
if (!sitlState->_sitl) { |
|
|
|
|
if (!_sitlState->_sitl) { |
|
|
|
|
return 0; |
|
|
|
|
} |
|
|
|
|
uint8_t mask = sitlState->_sitl->pin_mask.get(); |
|
|
|
|
return mask & (1U<<pin)? 1: 0; |
|
|
|
|
uint8_t mask = static_cast<uint8_t>(_sitlState->_sitl->pin_mask.get()); |
|
|
|
|
return static_cast<uint8_t>((mask & (1U << pin)) ? 1 : 0); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void GPIO::write(uint8_t pin, uint8_t value) |
|
|
|
|
{ |
|
|
|
|
if (!sitlState->_sitl) { |
|
|
|
|
if (!_sitlState->_sitl) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
uint8_t mask = sitlState->_sitl->pin_mask.get(); |
|
|
|
|
uint8_t mask = static_cast<uint8_t>(_sitlState->_sitl->pin_mask.get()); |
|
|
|
|
uint8_t new_mask = mask; |
|
|
|
|
if (value) { |
|
|
|
|
new_mask |= (1U<<pin); |
|
|
|
|
new_mask |= (1U << pin); |
|
|
|
|
} else { |
|
|
|
|
new_mask &= ~(1U<<pin); |
|
|
|
|
new_mask &= ~(1U << pin); |
|
|
|
|
} |
|
|
|
|
if (mask != new_mask) { |
|
|
|
|
sitlState->_sitl->pin_mask.set_and_notify(new_mask); |
|
|
|
|
_sitlState->_sitl->pin_mask.set_and_notify(new_mask); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -50,7 +49,12 @@ void GPIO::toggle(uint8_t pin)
@@ -50,7 +49,12 @@ void GPIO::toggle(uint8_t pin)
|
|
|
|
|
|
|
|
|
|
/* Alternative interface: */ |
|
|
|
|
AP_HAL::DigitalSource* GPIO::channel(uint16_t n) { |
|
|
|
|
return new DigitalSource(0); |
|
|
|
|
if (n < 8) { // (ie. sizeof(pin_mask)*8)
|
|
|
|
|
return new DigitalSource(static_cast<uint8_t>(n)); |
|
|
|
|
} else { |
|
|
|
|
return nullptr; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* Interrupt interface: */ |
|
|
|
@ -64,8 +68,8 @@ bool GPIO::usb_connected(void)
@@ -64,8 +68,8 @@ bool GPIO::usb_connected(void)
|
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
DigitalSource::DigitalSource(uint8_t v) : |
|
|
|
|
pin(v) |
|
|
|
|
DigitalSource::DigitalSource(uint8_t pin) : |
|
|
|
|
_pin(pin) |
|
|
|
|
{} |
|
|
|
|
|
|
|
|
|
void DigitalSource::mode(uint8_t output) |
|
|
|
@ -73,16 +77,16 @@ void DigitalSource::mode(uint8_t output)
@@ -73,16 +77,16 @@ void DigitalSource::mode(uint8_t output)
|
|
|
|
|
|
|
|
|
|
uint8_t DigitalSource::read() |
|
|
|
|
{ |
|
|
|
|
return hal.gpio->read(pin); |
|
|
|
|
return hal.gpio->read(_pin); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void DigitalSource::write(uint8_t value) |
|
|
|
|
{ |
|
|
|
|
value = value?1:0; |
|
|
|
|
return hal.gpio->write(pin, value); |
|
|
|
|
value = static_cast<uint8_t>(value ? 1 : 0); |
|
|
|
|
return hal.gpio->write(_pin, value); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void DigitalSource::toggle() |
|
|
|
|
{ |
|
|
|
|
return hal.gpio->write(pin, !hal.gpio->read(pin)); |
|
|
|
|
return hal.gpio->write(_pin, !hal.gpio->read(_pin)); |
|
|
|
|
} |
|
|
|
|