aboutsummaryrefslogtreecommitdiff
path: root/apps/px4io/controls.c
diff options
context:
space:
mode:
Diffstat (limited to 'apps/px4io/controls.c')
-rw-r--r--apps/px4io/controls.c338
1 files changed, 243 insertions, 95 deletions
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 169d5bb81..e80a41f15 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -37,148 +37,296 @@
* 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
#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 bool ppm_input(uint16_t *values, uint16_t *num_values);
-static void ppm_input(void);
+static perf_counter_t c_gather_dsm;
+static perf_counter_t c_gather_sbus;
+static perf_counter_t c_gather_ppm;
void
-controls_main(void)
+controls_init(void)
{
- struct pollfd fds[2];
-
/* DSM input */
- fds[0].fd = dsm_init("/dev/ttyS0");
- fds[0].events = POLLIN;
+ dsm_init("/dev/ttyS0");
/* S.bus input */
- fds[1].fd = sbus_init("/dev/ttyS2");
- fds[1].events = POLLIN;
+ sbus_init("/dev/ttyS2");
- for (;;) {
- /* run this loop at ~100Hz */
- poll(fds, 2, 10);
+ /* default to a 1:1 input map, all enabled */
+ for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+ unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
- /*
- * Gather R/C control inputs from supported sources.
- *
- * Note that if you're silly enough to connect more than
- * one control input source, they're going to fight each
- * other. Don't do that.
- */
- bool locked = false;
+ 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] = 30;
+ r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
+ r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+ }
- /*
- * Store RC channel count to detect switch to RC loss sooner
- * than just by timeout
- */
- unsigned rc_channels = system_state.rc_channels;
+ c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
+ c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
+ c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm");
+}
+
+void
+controls_tick() {
+
+ /*
+ * Gather R/C control inputs from supported sources.
+ *
+ * Note that if you're silly enough to connect more than
+ * one control input source, they're going to fight each
+ * other. Don't do that.
+ */
+
+ perf_begin(c_gather_dsm);
+ 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;
+ perf_end(c_gather_dsm);
+
+ perf_begin(c_gather_sbus);
+ 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;
+ perf_end(c_gather_sbus);
+
+ /*
+ * 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.
+ */
+ perf_begin(c_gather_ppm);
+ 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;
+ perf_end(c_gather_ppm);
+
+ ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
+
+ /*
+ * In some cases we may have received a frame, but input has still
+ * been lost.
+ */
+ bool rc_input_lost = false;
+
+ /*
+ * If we received a new frame from any of the RC sources, process it.
+ */
+ 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];
+
+ int16_t scaled;
+
+ /*
+ * 1) Constrain to min/max values, as later processing depends on bounds.
+ */
+ 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];
+
+ /*
+ * 2) Scale around the mid point differently for lower and upper range.
+ *
+ * This is necessary as they don't share the same endpoints and slope.
+ *
+ * First normalize to 0..1 range with correct sign (below or above center),
+ * then scale to 20000 range (if center is an actual center, -10000..10000,
+ * if parameters only support half range, scale to 10000 range, e.g. if
+ * center == min 0..10000, if center == max -10000..0).
+ *
+ * As the min and max bounds were enforced in step 1), division by zero
+ * cannot occur, as for the case of center == min or center == max the if
+ * statement is mutually exclusive with the arithmetic NaN case.
+ *
+ * DO NOT REMOVE OR ALTER STEP 1!
+ */
+ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+ scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+
+ } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+ scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
- if (fds[0].revents & POLLIN)
- locked |= dsm_input();
+ } else {
+ /* in the configured dead zone, output zero */
+ scaled = 0;
+ }
- if (fds[1].revents & POLLIN)
- locked |= sbus_input();
+ /* invert channel if requested */
+ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
+ scaled = -scaled;
+
+ /* and update the scaled/mapped version */
+ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+ ASSERT(mapped < MAX_CONTROL_CHANNELS);
+
+ /* invert channel if pitch - pulling the lever down means pitching up by convention */
+ if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
+ scaled = -scaled;
+
+ 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 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.
+ * If we got an update with zero channels, treat it as
+ * a loss of input.
*
- * 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.
+ * This might happen if a protocol-based receiver returns an update
+ * that contains no channels that we have mapped.
*/
- if (!locked)
- ppm_input();
-
- /* 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;
-
+ if (assigned_channels == 0) {
+ rc_input_lost = true;
} else {
- /* override not engaged, use FMU */
- system_state.mixer_manual_override = false;
+ /* set RC OK flag and clear RC lost alarm */
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+ r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/*
- * If we haven't seen any new control data in 200ms, assume we
- * have lost input and tell FMU.
+ * Export the valid channel bitmap
*/
- if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
+ r_rc_valid = assigned_channels;
+ }
- 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;
- }
+ /*
+ * If we haven't seen any new control data in 200ms, assume we
+ * have lost input.
+ */
+ if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
+ rc_input_lost = true;
- /* set the number of channels to zero - no inputs */
- system_state.rc_channels = 0;
- }
+ /* clear the input-kind flags here */
+ r_status_flags &= ~(
+ PX4IO_P_STATUS_FLAGS_RC_PPM |
+ PX4IO_P_STATUS_FLAGS_RC_DSM |
+ PX4IO_P_STATUS_FLAGS_RC_SBUS);
+ }
+
+ /*
+ * Handle losing RC input
+ */
+ if (rc_input_lost) {
+
+ /* Clear the RC input status flag, clear manual override flag */
+ r_status_flags &= ~(
+ PX4IO_P_STATUS_FLAGS_OVERRIDE |
+ PX4IO_P_STATUS_FLAGS_RC_OK);
+
+ /* Set the RC_LOST alarm */
+ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
+
+ /* Mark the arrays as empty */
+ r_raw_rc_count = 0;
+ r_rc_valid = 0;
+ }
+
+ /*
+ * Check for manual override.
+ *
+ * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag 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.
+ */
+ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+
+ bool override = false;
/*
- * 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 mapped channel 5 (can be any remote channel,
+ * depends on RC_MAP_OVER parameter);
+ * If the value is 'high' then the pilot has
+ * requested override.
+ *
*/
- mixer_tick();
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
+ 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.
+ * If we have received a new PPM frame within the last 200ms, accept it
+ * and then invalidate it.
*/
- if (ppm_last_valid_decode != 0) {
-
- /* avoid racing with PPM updates */
- irqstate_t state = irqsave();
+ if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) {
/* PPM data exists, copy it */
- system_state.rc_channels = ppm_decoded_channels;
+ *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);
-
- /* trigger an immediate report to the FMU */
- system_state.fmu_report_due = true;
+ /* good if we got any channels */
+ result = (*num_values > 0);
}
+
+ irqrestore(state);
+
+ return result;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-16 06:11:59 +0000