ardupilot/libraries/AP_RCProtocol/AP_RCProtocol_Joystick_SFML...

49 lines
1.3 KiB
C++

#include "AP_RCProtocol_config.h"
#if AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED
#include "AP_RCProtocol_Joystick_SFML.h"
#include <SITL/SITL.h>
#ifdef HAVE_SFML_GRAPHICS_HPP
#include <SFML/Window/Joystick.hpp>
#elif HAVE_SFML_GRAPHIC_H
#include <SFML/Window/Joystick.h>
#endif
void AP_RCProtocol_Joystick_SFML::update()
{
auto *_sitl = AP::sitl();
if (_sitl == nullptr) {
return;
}
sf::Joystick::update();
const unsigned int stick_id = _sitl->sfml_joystick_id;
if (!sf::Joystick::isConnected(stick_id)) {
return;
}
uint16_t pwm_values[ARRAY_SIZE(_sitl->sfml_joystick_axis)]{};
for (uint8_t ch=0; ch<ARRAY_SIZE(_sitl->sfml_joystick_axis); ch++) {
const sf::Joystick::Axis axis = sf::Joystick::Axis(_sitl->sfml_joystick_axis[ch].get());
if (!sf::Joystick::hasAxis(stick_id, axis)) {
continue;
}
// pos is a value between -100 and 100:
const auto pos = sf::Joystick::getAxisPosition(stick_id, axis);
// convert to a "pwm" value between 1000 and 2000:
const uint16_t pwm = (constrain_float(pos + 100, 0, 200) * 5) + 1000;
pwm_values[ch] = pwm;
}
// never in failsafe:
add_input(ARRAY_SIZE(pwm_values), pwm_values, false);
}
#endif // AP_RCPROTOCOL_JOYSTICK_SFML_ENABLED