diff options
Diffstat (limited to 'apps/px4io/px4io.c')
-rw-r--r-- | apps/px4io/px4io.c | 130 |
1 files changed, 118 insertions, 12 deletions
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 88342816e..fec5eed23 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -37,20 +37,22 @@ */ #include <nuttx/config.h> -#include <stdio.h> + +#include <stdio.h> // required for task_create #include <stdbool.h> -#include <fcntl.h> -#include <unistd.h> -#include <debug.h> #include <stdlib.h> #include <errno.h> #include <string.h> - -#include <nuttx/clock.h> +#include <poll.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> +#include <systemlib/perf_counter.h> + +#include <stm32_uart.h> + +#define DEBUG #include "px4io.h" __EXPORT int user_start(int argc, char *argv[]); @@ -59,6 +61,76 @@ extern void up_cxxinitialize(void); struct sys_state_s system_state; +static struct hrt_call serial_dma_call; + +/* global debug level for isr_debug() */ +volatile uint8_t debug_level = 0; +volatile uint32_t i2c_loop_resets = 0; + +struct hrt_call loop_overtime_call; + + +/* + a set of debug buffers to allow us to send debug information from ISRs + */ + +static volatile uint32_t msg_counter; +static volatile uint32_t last_msg_counter; +static volatile uint8_t msg_next_out, msg_next_in; + +/* + * WARNING too large buffers here consume the memory required + * for mixer handling. Do not allocate more than 80 bytes for + * output. + */ +#define NUM_MSG 2 +static char msg[NUM_MSG][50]; + +/* + add a debug message to be printed on the console + */ +void isr_debug(uint8_t level, const char *fmt, ...) +{ + if (level > debug_level) { + return; + } + va_list ap; + va_start(ap, fmt); + vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap); + va_end(ap); + msg_next_in = (msg_next_in+1) % NUM_MSG; + msg_counter++; +} + +/* + show all pending debug messages + */ +static void show_debug_messages(void) +{ + if (msg_counter != last_msg_counter) { + uint32_t n = msg_counter - last_msg_counter; + if (n > NUM_MSG) n = NUM_MSG; + last_msg_counter = msg_counter; + while (n--) { + debug("%s", msg[msg_next_out]); + msg_next_out = (msg_next_out+1) % NUM_MSG; + } + } +} + +/* + catch I2C lockups + */ +static void loop_overtime(void *arg) +{ + debug("RESETTING\n"); + i2c_loop_resets++; + i2c_dump(); + i2c_reset(); + hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL); +} + + int user_start(int argc, char *argv[]) { /* run C++ ctors before we go any further */ @@ -66,12 +138,16 @@ int user_start(int argc, char *argv[]) /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); - /* default to 50 Hz PWM outputs */ - system_state.servo_rate = 50; /* configure the high-resolution time/callout interface */ hrt_init(); + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); + /* print some startup info */ lowsyslog("\nPX4IO: starting\n"); @@ -96,10 +172,40 @@ int user_start(int argc, char *argv[]) (main_t)controls_main, NULL); - struct mallinfo minfo = mallinfo(); lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); - /* we're done here, go run the communications loop */ - comms_main(); -}
\ No newline at end of file + debug("debug_level=%u\n", (unsigned)debug_level); + + /* start the i2c handler */ + i2c_init(); + + /* add a performance counter for mixing */ + perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); + + /* run the mixer at ~300Hz (for now...) */ + /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */ + uint16_t counter=0; + for (;;) { + /* + if we are not scheduled for 10ms then reset the I2C bus + */ + hrt_call_after(&loop_overtime_call, 10000, (hrt_callout)loop_overtime, NULL); + + poll(NULL, 0, 3); + perf_begin(mixer_perf); + mixer_tick(); + perf_end(mixer_perf); + show_debug_messages(); + if (counter++ == 800) { + counter = 0; + isr_debug(1, "d:%u stat=0x%x arm=0x%x feat=0x%x rst=%u", + (unsigned)debug_level, + (unsigned)r_status_flags, + (unsigned)r_setup_arming, + (unsigned)r_setup_features, + (unsigned)i2c_loop_resets); + } + } +} + |