ardupilot/libraries/AP_HAL_SITL/RCInput.cpp

53 lines
910 B
C++
Raw Normal View History

#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2012-12-17 23:56:21 -04:00
#include "RCInput.h"
2018-07-23 23:46:31 -03:00
#include <SITL/SITL.h>
2012-12-17 23:56:21 -04:00
using namespace HALSITL;
2012-12-17 23:56:21 -04:00
extern const AP_HAL::HAL& hal;
void RCInput::init()
2012-12-17 23:56:21 -04:00
{
}
bool RCInput::new_input()
{
if (_sitlState->new_rc_input) {
_sitlState->new_rc_input = false;
return true;
}
return false;
2012-12-17 23:56:21 -04:00
}
uint16_t RCInput::read(uint8_t ch)
{
2018-07-23 23:46:31 -03:00
if (ch >= num_channels()) {
return 0;
}
2016-05-03 23:50:02 -03:00
return _sitlState->pwm_input[ch];
2012-12-17 23:56:21 -04:00
}
uint8_t RCInput::read(uint16_t* periods, uint8_t len)
{
2018-07-23 23:46:31 -03:00
if (len > num_channels()) {
len = num_channels();
}
for (uint8_t i=0; i < len; i++) {
2016-05-03 23:50:02 -03:00
periods[i] = read(i);
2012-12-17 23:56:21 -04:00
}
return len;
2012-12-17 23:56:21 -04:00
}
2018-07-23 23:46:31 -03:00
uint8_t RCInput::num_channels()
{
SITL::SITL *_sitl = AP::sitl();
if (_sitl) {
return MIN(_sitl->rc_chancount.get(), SITL_RC_INPUT_CHANNELS);
}
return SITL_RC_INPUT_CHANNELS;
}
2012-12-18 05:04:47 -04:00
#endif