mirror of https://github.com/ArduPilot/ardupilot
108 lines
2.2 KiB
C++
108 lines
2.2 KiB
C++
|
|
#include <AP_HAL_Boards.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
|
|
|
|
#include "RCInput.h"
|
|
|
|
#include <hwf4/timer.h>
|
|
|
|
using namespace SMACCM;
|
|
|
|
/* Constrain captured pulse to be between min and max pulsewidth. */
|
|
static inline uint16_t constrain_pulse(uint16_t p)
|
|
{
|
|
if (p > RC_INPUT_MAX_PULSEWIDTH)
|
|
return RC_INPUT_MAX_PULSEWIDTH;
|
|
if (p < RC_INPUT_MIN_PULSEWIDTH)
|
|
return RC_INPUT_MIN_PULSEWIDTH;
|
|
|
|
return p;
|
|
}
|
|
|
|
SMACCMRCInput::SMACCMRCInput()
|
|
{
|
|
}
|
|
|
|
void SMACCMRCInput::init(void *unused)
|
|
{
|
|
clear_overrides();
|
|
}
|
|
|
|
uint8_t SMACCMRCInput::valid()
|
|
{
|
|
// If any of the overrides are positive, we have valid data.
|
|
for (int i = 0; i < SMACCM_RCINPUT_CHANNELS; ++i)
|
|
if (_override[i] > 0)
|
|
return true;
|
|
|
|
return timer_is_ppm_valid();
|
|
}
|
|
|
|
// It looks like the APM2 driver clears the PPM sample after this
|
|
// function is called, so we do the same thing here for compatibility.
|
|
uint16_t SMACCMRCInput::read(uint8_t ch)
|
|
{
|
|
uint16_t result = 0;
|
|
|
|
if (_override[ch] != 0) {
|
|
result = _override[ch];
|
|
} else {
|
|
timer_get_ppm_channel(ch, &result);
|
|
result = constrain_pulse(result);
|
|
}
|
|
|
|
timer_clear_ppm();
|
|
return constrain_pulse(result);
|
|
}
|
|
|
|
// It looks like the APM2 driver clears the PPM sample after this
|
|
// function is called, so we do the same thing here for compatibility.
|
|
uint8_t SMACCMRCInput::read(uint16_t *periods, uint8_t len)
|
|
{
|
|
uint8_t result;
|
|
result = timer_get_ppm(periods, len, NULL);
|
|
|
|
for (int i = 0; i < result; ++i) {
|
|
periods[i] = constrain_pulse(periods[i]);
|
|
if (_override[i] != 0)
|
|
periods[i] = _override[i];
|
|
}
|
|
|
|
timer_clear_ppm();
|
|
return result;
|
|
}
|
|
|
|
bool SMACCMRCInput::set_overrides(int16_t *overrides, uint8_t len)
|
|
{
|
|
bool result = false;
|
|
|
|
for (int i = 0; i < len; ++i)
|
|
result |= set_override(i, overrides[i]);
|
|
|
|
return result;
|
|
}
|
|
|
|
bool SMACCMRCInput::set_override(uint8_t channel, int16_t override)
|
|
{
|
|
if (override < 0)
|
|
return false;
|
|
|
|
if (channel < SMACCM_RCINPUT_CHANNELS) {
|
|
_override[channel] = override;
|
|
if (override != 0) {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void SMACCMRCInput::clear_overrides()
|
|
{
|
|
for (int i = 0; i < SMACCM_RCINPUT_CHANNELS; ++i)
|
|
_override[i] = 0;
|
|
}
|
|
|
|
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
|