diff --git a/libraries/AP_HAL_SITL/GPIO.cpp b/libraries/AP_HAL_SITL/GPIO.cpp index 5654148adb..ec195e7c08 100644 --- a/libraries/AP_HAL_SITL/GPIO.cpp +++ b/libraries/AP_HAL_SITL/GPIO.cpp @@ -11,7 +11,16 @@ void GPIO::init() {} void GPIO::pinMode(uint8_t pin, uint8_t output) -{} +{ + if (pin > 7) { + return; + } + if (output) { + pin_mode_is_write |= (1U<_sitl) { return; } + + if (pin < 8) { + if (!(pin_mode_is_write & (1U<(_sitlState->_sitl->pin_mask.get()); uint16_t new_mask = mask; diff --git a/libraries/AP_HAL_SITL/GPIO.h b/libraries/AP_HAL_SITL/GPIO.h index 4e94379091..38af0571e6 100644 --- a/libraries/AP_HAL_SITL/GPIO.h +++ b/libraries/AP_HAL_SITL/GPIO.h @@ -19,6 +19,8 @@ public: private: SITL_State *_sitlState; + + uint8_t pin_mode_is_write; }; class HALSITL::DigitalSource : public AP_HAL::DigitalSource {