ardupilot/libraries/AP_HAL_SITL/RCInput.cpp

77 lines
1.5 KiB
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"
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
{
clear_overrides();
}
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)
{
if (ch >= SITL_RC_INPUT_CHANNELS) {
return 0;
}
2016-05-03 23:50:02 -03:00
if (_override[ch]) {
return _override[ch];
}
return _sitlState->pwm_input[ch];
2012-12-17 23:56:21 -04:00
}
uint8_t RCInput::read(uint16_t* periods, uint8_t len)
{
if (len > SITL_RC_INPUT_CHANNELS) {
len = SITL_RC_INPUT_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
}
bool RCInput::set_overrides(int16_t *overrides, uint8_t len)
{
2012-12-17 23:56:21 -04:00
bool res = false;
if (len > SITL_RC_INPUT_CHANNELS) {
len = SITL_RC_INPUT_CHANNELS;
}
2012-12-17 23:56:21 -04:00
for (uint8_t i = 0; i < len; i++) {
res |= set_override(i, overrides[i]);
}
return res;
}
bool RCInput::set_override(uint8_t channel, int16_t override)
{
if (override < 0) {
return false; /* -1: no change. */
}
if (channel < SITL_RC_INPUT_CHANNELS) {
_override[channel] = static_cast<uint16_t>(override);
2012-12-17 23:56:21 -04:00
if (override != 0) {
return true;
}
}
return false;
}
void RCInput::clear_overrides()
2012-12-17 23:56:21 -04:00
{
memset(_override, 0, sizeof(_override));
2012-12-17 23:56:21 -04:00
}
2012-12-18 05:04:47 -04:00
#endif