diff options
| -rw-r--r-- | apps/px4io/controls.c | 319 | ||||
| -rw-r--r-- | apps/px4io/px4io.c | 115 | ||||
| -rw-r--r-- | apps/px4io/px4io.h | 8 | ||||
| -rw-r--r-- | apps/px4io/registers.c | 4 | ||||
| -rwxr-xr-x | nuttx/configs/px4io/io/defconfig | 6 |
5 files changed, 206 insertions, 246 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; } - } } diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index b4135aba5..589264661 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -68,14 +68,8 @@ static struct hrt_call serial_dma_call; volatile uint8_t debug_level = 0; volatile uint32_t i2c_loop_resets = 0; -struct hrt_call loop_overtime_call; - -// this allows wakeup of the main task via a signal -static pid_t daemon_pid; - - /* - a set of debug buffers to allow us to send debug information from ISRs + * a set of debug buffers to allow us to send debug information from ISRs */ static volatile uint32_t msg_counter; @@ -91,9 +85,10 @@ static volatile uint8_t msg_next_out, msg_next_in; static char msg[NUM_MSG][40]; /* - add a debug message to be printed on the console + * add a debug message to be printed on the console */ -void isr_debug(uint8_t level, const char *fmt, ...) +void +isr_debug(uint8_t level, const char *fmt, ...) { if (level > debug_level) { return; @@ -107,9 +102,10 @@ void isr_debug(uint8_t level, const char *fmt, ...) } /* - show all pending debug messages + * show all pending debug messages */ -static void show_debug_messages(void) +static void +show_debug_messages(void) { if (msg_counter != last_msg_counter) { uint32_t n = msg_counter - last_msg_counter; @@ -122,36 +118,9 @@ static void show_debug_messages(void) } } -/* - catch I2C lockups - */ -static void loop_overtime(void *arg) -{ - lowsyslog("I2C RESET\n"); - i2c_loop_resets++; - i2c_dump(); - i2c_reset(); - hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL); -} - -static void wakeup_handler(int signo, siginfo_t *info, void *ucontext) -{ - /* nothing to do - we just want poll() to return */ -} - - -/* - wakeup the main task using a signal - */ -void daemon_wakeup(void) +int +user_start(int argc, char *argv[]) { - kill(daemon_pid, SIGUSR1); -} - -int user_start(int argc, char *argv[]) -{ - daemon_pid = getpid(); - /* run C++ ctors before we go any further */ up_cxxinitialize(); @@ -184,18 +153,27 @@ int user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); - /* start the flight control signal handler */ - int ret = task_create("FCon", - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)controls_main, - NULL); + /* initialise the control inputs */ + controls_init(); + + /* start the i2c handler */ + i2c_init(); + + /* add a performance counter for mixing */ + perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); + + /* add a performance counter for controls */ + perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); + + /* and one for measuring the loop rate */ + perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); +#if 0 /* not enough memory, lock down */ - if (ret != OK || minfo.mxordblk < 500) { + if (minfo.mxordblk < 500) { lowsyslog("ERR: not enough MEM"); bool phase = false; @@ -210,46 +188,33 @@ int user_start(int argc, char *argv[]) phase = !phase; usleep(300000); } +#endif - /* start the i2c handler */ - i2c_init(); - - /* add a performance counter for mixing */ - perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); - - /* - * setup a null handler for SIGUSR1 - we will use this for wakeup from poll() + /* + * Run everything in a tight loop. */ - struct sigaction sa; - memset(&sa, 0, sizeof(sa)); - sa.sa_sigaction = wakeup_handler; - sigfillset(&sa.sa_mask); - sigdelset(&sa.sa_mask, SIGUSR1); - if (sigaction(SIGUSR1, &sa, NULL) != OK) { - lowsyslog("SIGUSR1 init fail\n"); - } - /* - run the mixer at ~50Hz, using signals to run it early if - need be - */ uint64_t last_debug_time = 0; for (;;) { - /* - if we are not scheduled for 30ms then reset the I2C bus - */ - hrt_call_after(&loop_overtime_call, 30000, (hrt_callout)loop_overtime, NULL); - // we use usleep() instead of poll() as poll() is not - // interrupted by signals in nuttx, whereas usleep() is - usleep(20000); + /* track the rate at which the loop is running */ + perf_count(loop_perf); + /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); + /* kick the control inputs */ + perf_begin(controls_perf); + controls_tick(); + perf_end(controls_perf); + + /* check for debug activity */ show_debug_messages(); - if (hrt_absolute_time() - last_debug_time > 1000000) { + + /* post debug state at ~1Hz */ + if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", (unsigned)debug_level, (unsigned)r_status_flags, diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index cd5977258..22993fb52 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -170,7 +170,8 @@ extern uint16_t adc_measure(unsigned channel); * * Input functions return true when they receive an update from the RC controller. */ -extern void controls_main(void); +extern void controls_init(void); +extern void controls_tick(void); extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern int sbus_init(const char *device); @@ -179,11 +180,6 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; -/** - * Wake up mixer. - */ -void daemon_wakeup(void); - /* send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index ec1980aaa..5fb90b9b0 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -203,8 +203,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; - // wake up daemon to trigger mixer - daemon_wakeup(); break; /* handle raw PWM input */ @@ -224,8 +222,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; - // wake up the main thread to trigger mixer - daemon_wakeup(); break; /* handle setup for servo failsafe values */ diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 87d500161..1145fb349 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -401,11 +401,11 @@ CONFIG_SCHED_ATEXIT=n CONFIG_DISABLE_CLOCK=n CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_SIGNALS=y CONFIG_DISABLE_MQUEUE=y CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=n +CONFIG_DISABLE_POLL=y # # Misc libc settings @@ -541,7 +541,7 @@ CONFIG_BOOT_COPYTORAM=n CONFIG_CUSTOM_STACK=n CONFIG_STACK_POINTER= CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=800 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=1024 CONFIG_HEAP_BASE= |