From 8c7e2546ed5222145a6d1745e77d01f7c21c24fc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 00:09:37 -0800 Subject: Simplify the PX4IO main loop to cut down on memory consumption. --- apps/px4io/controls.c | 319 +++++++++++++++++++++++++------------------------- 1 file changed, 161 insertions(+), 158 deletions(-) (limited to 'apps/px4io/controls.c') 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 #include -#include #include #include @@ -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; } - } } -- cgit v1.2.3