diff options
Diffstat (limited to 'apps/px4io/controls.c')
-rw-r--r-- | apps/px4io/controls.c | 268 |
1 files changed, 195 insertions, 73 deletions
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 169d5bb81..072d296da 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -37,34 +37,22 @@ * R/C inputs and servo outputs. */ - #include <nuttx/config.h> -#include <stdio.h> #include <stdbool.h> -#include <fcntl.h> -#include <unistd.h> -#include <debug.h> -#include <stdlib.h> -#include <errno.h> -#include <termios.h> -#include <string.h> #include <poll.h> -#include <nuttx/clock.h> - #include <drivers/drv_hrt.h> -#include <systemlib/hx_stream.h> #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> -#define DEBUG +//#define DEBUG #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ -#define RC_CHANNEL_HIGH_THRESH 1700 -#define RC_CHANNEL_LOW_THRESH 1300 +#define RC_CHANNEL_HIGH_THRESH 5000 +#define RC_CHANNEL_LOW_THRESH -5000 -static void ppm_input(void); +static bool ppm_input(uint16_t *values, uint16_t *num_values); void controls_main(void) @@ -79,9 +67,26 @@ controls_main(void) fds[1].fd = sbus_init("/dev/ttyS2"); fds[1].events = POLLIN; + ASSERT(fds[0].fd >= 0); + ASSERT(fds[1].fd >= 0); + + /* default to a 1:1 input map */ + for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 50; + r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i; + } + for (;;) { /* run this loop at ~100Hz */ - poll(fds, 2, 10); + int result = poll(fds, 2, 10); + + ASSERT(result >= 0); /* * Gather R/C control inputs from supported sources. @@ -90,95 +95,212 @@ controls_main(void) * one control input source, they're going to fight each * other. Don't do that. */ - bool locked = false; + + bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); + if (dsm_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); + if (sbus_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; /* - * Store RC channel count to detect switch to RC loss sooner - * than just by timeout + * XXX each S.bus frame will cause a PPM decoder interrupt + * storm (lots of edges). It might be sensible to actually + * disable the PPM decoder completely if we have S.bus signal. */ - unsigned rc_channels = system_state.rc_channels; + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); + if (ppm_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; - if (fds[0].revents & POLLIN) - locked |= dsm_input(); + ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - if (fds[1].revents & POLLIN) - locked |= sbus_input(); + /* + * In some cases we may have received a frame, but input has still + * been lost. + */ + bool rc_input_lost = false; /* - * If we don't have lock from one of the serial receivers, - * look for PPM. It shares an input with S.bus, so there's - * a possibility it will mis-parse an S.bus frame. - * - * XXX each S.bus frame will cause a PPM decoder interrupt - * storm (lots of edges). It might be sensible to actually - * disable the PPM decoder completely if we have an alternate - * receiver lock. + * If we received a new frame from any of the RC sources, process it. */ - if (!locked) - ppm_input(); + if (dsm_updated | sbus_updated | ppm_updated) { + + /* update RC-received timestamp */ + system_state.rc_channels_timestamp = hrt_absolute_time(); + + /* record a bitmask of channels assigned */ + unsigned assigned_channels = 0; + + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { + + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] && PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + + uint16_t raw = r_raw_rc_values[i]; + + /* implement the deadzone */ + if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) { + raw += conf[PX4IO_P_RC_CONFIG_DEADZONE]; + if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) + raw = conf[PX4IO_P_RC_CONFIG_CENTER]; + } + if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) { + raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE]; + if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) + raw = conf[PX4IO_P_RC_CONFIG_CENTER]; + } + + /* constrain to min/max values */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; + + int16_t scaled = raw; + + /* adjust to zero-relative (-500..500) */ + scaled -= 1500; + + /* scale to fixed-point representation (-10000..10000) */ + scaled *= 20; + + ASSERT(scaled >= -15000); + ASSERT(scaled <= 15000); + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* check for manual override status */ - if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { - /* force manual input override */ - system_state.mixer_manual_override = true; + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + ASSERT(mapped < MAX_CONTROL_CHANNELS); - } else { - /* override not engaged, use FMU */ - system_state.mixer_manual_override = false; + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); + } + } + + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } + + /* + * If we got an update with zero channels, treat it as + * a loss of input. + */ + if (assigned_channels == 0) + rc_input_lost = true; + + /* + * Export the valid channel bitmap + */ + r_rc_valid = assigned_channels; } /* * If we haven't seen any new control data in 200ms, assume we - * have lost input and tell FMU. + * have lost input. */ if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { + rc_input_lost = true; + } - if (system_state.rc_channels > 0) { - /* - * If the RC signal status (lost / present) has - * just changed, request an update immediately. - */ - system_state.fmu_report_due = true; - } + /* + * Handle losing RC input + */ + if (rc_input_lost) { + + /* Clear the RC input status flags, clear manual override control */ + r_status_flags &= ~( + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK | + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS); + + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; - /* set the number of channels to zero - no inputs */ - system_state.rc_channels = 0; + /* Mark the arrays as empty */ + r_raw_rc_count = 0; + r_rc_valid = 0; } /* - * PWM output updates are performed in addition on each comm update. - * the updates here are required to ensure operation if FMU is not started - * or stopped responding. + * Check for manual override. + * + * The OVERRIDE_OK feature must be set, and we must have R/C input. + * Override is enabled if either the hardcoded channel / value combination + * is selected, or the AP has requested it. */ - mixer_tick(); + if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + + bool override = false; + + /* + * Check mapped channel 5; if the value is 'high' then the pilot has + * requested override. + */ + if ((r_rc_valid & (1 << 4)) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) + override = true; + + /* + * Check for an explicit manual override request from the AP. + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)) + override = true; + + if (override) { + + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + + /* mix new RC input control values to servos */ + if (dsm_updated | sbus_updated | ppm_updated) + mixer_tick(); + + } else { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + } + } + } } -static void -ppm_input(void) +static bool +ppm_input(uint16_t *values, uint16_t *num_values) { + bool result = false; + + /* avoid racing with PPM updates */ + irqstate_t state = irqsave(); + /* - * Look for new PPM input. + * Look for recent PPM input. */ - if (ppm_last_valid_decode != 0) { - - /* avoid racing with PPM updates */ - irqstate_t state = irqsave(); + if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) { /* PPM data exists, copy it */ - system_state.rc_channels = ppm_decoded_channels; + result = true; + *num_values = ppm_decoded_channels; + if (*num_values > MAX_CONTROL_CHANNELS) + *num_values = MAX_CONTROL_CHANNELS; - for (unsigned i = 0; i < ppm_decoded_channels; i++) { - system_state.rc_channel_data[i] = ppm_buffer[i]; - } + for (unsigned i = 0; i < *num_values; i++) + values[i] = ppm_buffer[i]; - /* copy the timestamp and clear it */ - system_state.rc_channels_timestamp = ppm_last_valid_decode; + /* clear validity */ ppm_last_valid_decode = 0; + } - irqrestore(state); + irqrestore(state); - /* trigger an immediate report to the FMU */ - system_state.fmu_report_due = true; - } + return result; } |