42 #include <px4_platform_common/px4_config.h> 56 #define RC_CHANNEL_HIGH_THRESH 5000 57 #define RC_CHANNEL_LOW_THRESH -8000 59 static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
60 static bool dsm_port_input(uint16_t *rssi,
bool *dsm_updated,
bool *st24_updated,
bool *sumd_updated);
79 bool dsm_port_input(uint16_t *rssi,
bool *dsm_updated,
bool *st24_updated,
bool *sumd_updated)
101 if (spektrum_rssi >= 0 && spektrum_rssi <= 100) {
106 *rssi = spektrum_rssi;
113 uint8_t st24_rssi, lost_count;
114 uint16_t st24_channel_count = 0;
116 *st24_updated =
false;
118 for (
unsigned i = 0; i < n_bytes; i++) {
121 *st24_updated |= (
OK ==
st24_decode(bytes[i], &st24_rssi, &lost_count,
125 if (*st24_updated && lost_count == 0) {
140 uint8_t sumd_rssi, sumd_rx_count;
141 uint16_t sumd_channel_count = 0;
142 bool sumd_failsafe_state;
144 *sumd_updated =
false;
146 for (
unsigned i = 0; i < n_bytes; i++) {
149 *sumd_updated |= (
OK ==
sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
161 if (sumd_failsafe_state) {
169 return (*dsm_updated | *st24_updated | *sumd_updated);
221 if (counts != 0xffff) {
244 bool sbus_failsafe, sbus_frame_drop;
253 if (sbus_frame_drop) {
294 bool dsm_updated =
false, st24_updated =
false, sumd_updated =
false;
328 bool rc_input_lost =
false;
333 if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {
336 unsigned assigned_channels = 0;
425 assigned_channels |= (1 << mapped);
436 if (!(assigned_channels & (1 << i))) {
446 if (assigned_channels > 4) {
464 rc_input_lost =
true;
468 PX4IO_P_STATUS_FLAGS_RC_PPM |
470 PX4IO_P_STATUS_FLAGS_RC_SBUS |
521 bool override =
false;
562 ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
566 if (!(num_values) || !(values) || !(frame_len)) {
571 irqstate_t
state = px4_enter_critical_section();
597 result = (*num_values > 0);
600 px4_leave_critical_section(state);
int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count, bool *failsafe)
Decoder for SUMD/SUMH protocol.
#define PX4IO_P_STATUS_FLAGS_FMU_OK
static unsigned _rssi_adc_counts
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE
RC protocol definition for Spektrum RC.
RC protocol definition for Graupner HoTT transmitter.
#define SIGNED_TO_REG(_signed)
measure the time elapsed performing an event
__EXPORT uint16_t ppm_frame_length
length of the decoded PPM frame (includes gap)
bool dsm_input(int fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes, int8_t *rssi, unsigned max_values)
Called periodically to check for input data from the DSM UART.
#define r_setup_rc_thr_failsafe
#define PX4IO_P_STATUS_FLAGS_RC_SBUS
void controls_init(void)
R/C receiver handling.
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK
#define PX4IO_RC_INPUT_CHANNELS
static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
#define PX4IO_P_STATUS_ALARMS_RC_LOST
High-resolution timer with callouts and timekeeping.
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
void atomic_modify_clear(volatile uint16_t *target, uint16_t modification)
#define PX4IO_P_RC_CONFIG_MAX
highest input value
#define RC_CHANNEL_LOW_THRESH
volatile uint64_t rc_channels_timestamp_valid
__EXPORT hrt_abstime ppm_last_valid_decode
timestamp of the last valid decode
RC protocol definition for Yuneec ST24 transmitter.
static perf_counter_t c_gather_dsm
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED
void atomic_modify_or(volatile uint16_t *target, uint16_t modification)
#define PX4IO_P_STATUS_FLAGS_MIXER_OK
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
#define PX4IO_P_RC_CONFIG_DEADZONE
band around center that is ignored
void perf_end(perf_counter_t handle)
End a performance event.
#define PX4IO_CONTROL_CHANNELS
#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH
magic value for mode switch
#define PX4IO_P_STATUS_FLAGS_OVERRIDE
#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP
#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11
#define PX4IO_P_RAW_RC_FLAGS_RC_OK
#define PPM_MAX_CHANNELS
Maximum number of channels that we will decode.
#define PX4IO_P_RC_CONFIG_ASSIGNMENT
mapped input value
General defines and structures for the PX4IO module firmware.
__EXPORT unsigned ppm_decoded_channels
count of decoded channels
struct sys_state_s system_state
int dsm_init(const char *device)
Initialize the DSM receive functionality.
#define PX4IO_P_RC_CONFIG_STRIDE
spacing between channel config data
int sbus_init(const char *device, bool singlewire)
#define PX4IO_P_RC_CONFIG_CENTER
center input value
uint16_t r_page_rc_input_config[]
PAGE 103.
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED
static uint16_t rc_value_override
#define PX4IO_P_STATUS_FLAGS_RC_OK
#define PX4IO_P_STATUS_FLAGS_RC_ST24
static perf_counter_t c_gather_sbus
int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *channel_count, uint16_t *channels, uint16_t max_chan_count)
Decoder for ST24 protocol.
#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK
RC protocol definition for S.BUS.
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI
enable ADC RSSI parsing
__BEGIN_DECLS __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]
decoded PPM channel values
#define PX4IO_P_STATUS_FLAGS_RC_SUMD
#define PX4IO_P_RAW_RC_NRSSI
#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE
#define REG_TO_SIGNED(_reg)
#define PX4IO_P_RAW_RC_DATA
static perf_counter_t c_gather_ppm
bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
uint16_t r_page_raw_rc_input[]
PAGE 4.
#define PX4IO_P_RC_CONFIG_MIN
lowest input value
#define PX4IO_P_RC_CONFIG_OPTIONS
channel options bitmask
volatile uint64_t rc_channels_timestamp_received
void perf_begin(perf_counter_t handle)
Begin a performance event.
uint16_t adc_measure(unsigned channel)
#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
#define PX4IO_P_STATUS_FLAGS_RC_PPM
Performance measuring tools.
#define PX4IO_P_STATUS_FLAGS_RC_DSM
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE
__EXPORT hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.