mirror of https://github.com/ArduPilot/ardupilot
96 lines
2.1 KiB
C++
96 lines
2.1 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
|
|
|
|
#include "RCInput.h"
|
|
#include <SITL/SITL.h>
|
|
#include <AP_RCProtocol/AP_RCProtocol.h>
|
|
|
|
#ifdef SFML_JOYSTICK
|
|
#ifdef HAVE_SFML_GRAPHICS_HPP
|
|
#include <SFML/Window/Joystick.hpp>
|
|
#elif HAVE_SFML_GRAPHIC_H
|
|
#include <SFML/Window/Joystick.h>
|
|
#endif
|
|
#endif // SFML_JOYSTICK
|
|
|
|
using namespace HALSITL;
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
void RCInput::init()
|
|
{
|
|
AP::RC().init();
|
|
}
|
|
|
|
bool RCInput::new_input()
|
|
{
|
|
if (!using_rc_protocol) {
|
|
if (AP::RC().new_input()) {
|
|
using_rc_protocol = true;
|
|
}
|
|
}
|
|
if (using_rc_protocol) {
|
|
return AP::RC().new_input();
|
|
}
|
|
if (_sitlState->new_rc_input) {
|
|
_sitlState->new_rc_input = false;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
uint16_t RCInput::read(uint8_t ch)
|
|
{
|
|
if (using_rc_protocol) {
|
|
return AP::RC().read(ch);
|
|
}
|
|
if (ch >= num_channels()) {
|
|
return 0;
|
|
}
|
|
#ifdef SFML_JOYSTICK
|
|
SITL::SIM *_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)
|
|
{
|
|
if (len > num_channels()) {
|
|
len = num_channels();
|
|
}
|
|
for (uint8_t i=0; i < len; i++) {
|
|
periods[i] = read(i);
|
|
}
|
|
return len;
|
|
}
|
|
|
|
uint8_t RCInput::num_channels()
|
|
{
|
|
if (using_rc_protocol) {
|
|
return AP::RC().num_channels();
|
|
}
|
|
SITL::SIM *_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;
|
|
}
|
|
|
|
#endif
|