Major workover of the PX4IO firmware for I2C operation.

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;
 }