|
|
@ -11,7 +11,16 @@ void GPIO::init() |
|
|
|
{} |
|
|
|
{} |
|
|
|
|
|
|
|
|
|
|
|
void GPIO::pinMode(uint8_t pin, uint8_t output) |
|
|
|
void GPIO::pinMode(uint8_t pin, uint8_t output) |
|
|
|
{} |
|
|
|
{ |
|
|
|
|
|
|
|
if (pin > 7) { |
|
|
|
|
|
|
|
return; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
if (output) { |
|
|
|
|
|
|
|
pin_mode_is_write |= (1U<<pin); |
|
|
|
|
|
|
|
} else { |
|
|
|
|
|
|
|
pin_mode_is_write &= ~(1U<<pin); |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
uint8_t GPIO::read(uint8_t pin) |
|
|
|
uint8_t GPIO::read(uint8_t pin) |
|
|
|
{ |
|
|
|
{ |
|
|
@ -33,6 +42,13 @@ void GPIO::write(uint8_t pin, uint8_t value) |
|
|
|
if (!_sitlState->_sitl) { |
|
|
|
if (!_sitlState->_sitl) { |
|
|
|
return; |
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (pin < 8) { |
|
|
|
|
|
|
|
if (!(pin_mode_is_write & (1U<<pin))) { |
|
|
|
|
|
|
|
// ignore setting of pull-up resistors
|
|
|
|
|
|
|
|
return; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
} |
|
|
|
uint16_t mask = static_cast<uint16_t>(_sitlState->_sitl->pin_mask.get()); |
|
|
|
uint16_t mask = static_cast<uint16_t>(_sitlState->_sitl->pin_mask.get()); |
|
|
|
uint16_t new_mask = mask; |
|
|
|
uint16_t new_mask = mask; |
|
|
|
|
|
|
|
|
|
|
|