diff --git a/libraries/AP_HAL_SITL/RCInput.cpp b/libraries/AP_HAL_SITL/RCInput.cpp index f1f4d11c92..7932b0b40b 100644 --- a/libraries/AP_HAL_SITL/RCInput.cpp +++ b/libraries/AP_HAL_SITL/RCInput.cpp @@ -5,6 +5,14 @@ #include #include +#ifdef SFML_JOYSTICK + #ifdef HAVE_SFML_GRAPHICS_HPP + #include + #elif HAVE_SFML_GRAPHIC_H + #include + #endif +#endif // SFML_JOYSTICK + using namespace HALSITL; extern const AP_HAL::HAL& hal; @@ -39,7 +47,22 @@ uint16_t RCInput::read(uint8_t ch) if (ch >= num_channels()) { return 0; } +#ifdef SFML_JOYSTICK + SITL::SITL *_sitl = AP::sitl(); + if (_sitl) { + const sf::Joystick::Axis axis = sf::Joystick::Axis(_sitl->sfml_joystick_axis[ch].get()); + const unsigned int stickID = _sitl->sfml_joystick_id; + if (sf::Joystick::hasAxis(stickID, axis)) { + return (constrain_float(sf::Joystick::getAxisPosition(stickID, axis) + 100, 0, 200) * 5) + 1000; + } else { + return 0; + } + } else { + return 0; + } +#else return _sitlState->pwm_input[ch]; +#endif } uint8_t RCInput::read(uint16_t* periods, uint8_t len) @@ -60,7 +83,11 @@ uint8_t RCInput::num_channels() } SITL::SITL *_sitl = AP::sitl(); if (_sitl) { +#ifdef SFML_JOYSTICK + return (sf::Joystick::isConnected(_sitl->sfml_joystick_id.get())) ? ARRAY_SIZE(_sitl->sfml_joystick_axis) : 0; +#else return MIN(_sitl->rc_chancount.get(), SITL_RC_INPUT_CHANNELS); +#endif // SFML_JOYSTICK } return SITL_RC_INPUT_CHANNELS; }