mirror of https://github.com/ArduPilot/ardupilot
49 lines
1.3 KiB
C++
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
|