Simplify the PX4IO main loop to cut down on memory consumption.

1 files changed, 161 insertions, 158 deletions

diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 829cc7671..cad368ae4 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -39,7 +39,6 @@
 
 #include <nuttx/config.h>
 #include <stdbool.h>
-#include <poll.h>
 
 #include <drivers/drv_hrt.h>
 #include <systemlib/perf_counter.h>
@@ -53,21 +52,18 @@
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values);
 
+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;
-
-	ASSERT(fds[0].fd >= 0);
-	ASSERT(fds[1].fd >= 0);
+	sbus_init("/dev/ttyS2");
 
 	/* default to a 1:1 input map, all enabled */
 	for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
@@ -82,200 +78,207 @@ controls_main(void)
 		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
 	}
 
-	for (;;) {
-		/* run this loop at ~100Hz */
-		int result = poll(fds, 2, 10);
+	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");
+}
 
-		ASSERT(result >= 0);
+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.
-		 */
+	/*
+	 * 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 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_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);
 
-		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_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.
-		 */
-		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;
+	/*
+	 * 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);
+	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) {
+	/*
+	 * In some cases we may have received a frame, but input has still
+	 * been lost.
+	 */
+	bool rc_input_lost = false;
 
-			/* update RC-received timestamp */
-			system_state.rc_channels_timestamp = hrt_absolute_time();
+	/*
+	 * If we received a new frame from any of the RC sources, process it.
+	 */
+	if (dsm_updated || sbus_updated || ppm_updated) {
 
-			/* record a bitmask of channels assigned */
-			unsigned assigned_channels = 0;
+		/* update RC-received timestamp */
+		system_state.rc_channels_timestamp = hrt_absolute_time();
 
-			/* map raw inputs to mapped inputs */
-			/* XXX mapping should be atomic relative to protocol */
-			for (unsigned i = 0; i < r_raw_rc_count; i++) {
+		/* record a bitmask of channels assigned */
+		unsigned assigned_channels = 0;
 
-				/* map the input channel */
-				uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+		/* map raw inputs to mapped inputs */
+		/* XXX mapping should be atomic relative to protocol */
+		for (unsigned i = 0; i < r_raw_rc_count; i++) {
 
-				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+			/* map the input channel */
+			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
 
-					uint16_t raw = r_raw_rc_values[i];
+			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
 
-					/* 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];
-					}
+				uint16_t raw = r_raw_rc_values[i];
 
-					/* 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];
+				/* 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];
+				}
 
-					int16_t scaled = raw;
+				/* 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];
 
-					/* adjust to zero-relative (-500..500) */
-					scaled -= 1500;
+				int16_t scaled = raw;
 
-					/* scale to fixed-point representation (-10000..10000) */
-					scaled *= 20;
+				/* adjust to zero-relative (-500..500) */
+				scaled -= 1500;
 
-					ASSERT(scaled >= -15000);
-					ASSERT(scaled <= 15000);
+				/* scale to fixed-point representation (-10000..10000) */
+				scaled *= 20;
 
-					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
-						scaled = -scaled;
+				ASSERT(scaled >= -15000);
+				ASSERT(scaled <= 15000);
 
-					/* and update the scaled/mapped version */
-					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-					ASSERT(mapped < MAX_CONTROL_CHANNELS);
+				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
+					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;
-			}
+				/* and update the scaled/mapped version */
+				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+				ASSERT(mapped < MAX_CONTROL_CHANNELS);
 
-			/*
-			 * If we got an update with zero channels, treat it as 
-			 * a loss of input.
-			 *
-			 * This might happen if a protocol-based receiver returns an update
-			 * that contains no channels that we have mapped.
-			 */
-			if (assigned_channels == 0) {
-				rc_input_lost = true;
-			} else {
-				r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+				r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+				assigned_channels |= (1 << mapped);
 			}
+		}
 
-			/*
-			 * Export the valid channel bitmap
-			 */
-			r_rc_valid = assigned_channels;
+		/* 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 haven't seen any new control data in 200ms, assume we
-		 * have lost input.
+		 * If we got an update with zero channels, treat it as 
+		 * a loss of input.
+		 *
+		 * This might happen if a protocol-based receiver returns an update
+		 * that contains no channels that we have mapped.
 		 */
-		if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
+		if (assigned_channels == 0) {
 			rc_input_lost = true;
-
-			/* 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);
+		} else {
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
 		}
 
 		/*
-		 * Handle losing RC input
+		 * Export the valid channel bitmap
 		 */
-		if (rc_input_lost) {
+		r_rc_valid = assigned_channels;
+	}
+
+	/*
+	 * If we haven't seen any new control data in 200ms, assume we
+	 * have lost input.
+	 */
+	if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
+		rc_input_lost = true;
+
+		/* 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);
+		/* 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;
+		/* 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;
-		}
+		/* 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;
 
 		/*
-		 * Check for manual override.
+		 * Check mapped channel 5; if the value is 'high' then the pilot has
+		 * requested 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.
+		 * XXX This should be configurable.
 		 */
-		if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
-			(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
-
-			bool override = false;
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH))
+			override = true;
 
-			/*
-			 * Check mapped channel 5; if the value is 'high' then the pilot has
-			 * requested override.
-			 *
-			 * XXX This should be configurable.
-			 */
-			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH))
-				override = true;
+		if (override) {
 
-			if (override) {
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_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();
 
-				/* 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;
-			}
+		} else {
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
 		}
-
 	}
 }