diff options
| -rw-r--r-- | apps/px4io/controls.c | 1 | ||||
| -rw-r--r-- | apps/px4io/dsm.c | 6 | ||||
| -rw-r--r-- | apps/px4io/mixer.cpp | 67 | ||||
| -rw-r--r-- | apps/px4io/px4io.c | 31 | ||||
| -rw-r--r-- | apps/px4io/sbus.c | 6 | ||||
| -rwxr-xr-x | nuttx/configs/px4io/io/defconfig | 6 |
6 files changed, 34 insertions, 83 deletions
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index b4a18bae6..829cc7671 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -45,7 +45,6 @@ #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> -//#define DEBUG #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index f0e8e0f32..1719cf58c 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -248,18 +248,18 @@ dsm_guess_format(bool reset) if ((votes11 == 1) && (votes10 == 0)) { channel_shift = 11; - debug("DSM: detected 11-bit format"); + debug("DSM: 11-bit format"); return; } if ((votes10 == 1) && (votes11 == 0)) { channel_shift = 10; - debug("DSM: detected 10-bit format"); + debug("DSM: 10-bit format"); return; } /* call ourselves to reset our state ... we have to try again */ - debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); + debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); dsm_guess_format(true); } diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 3ae2a3115..505bc8a69 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -157,71 +157,6 @@ mixer_tick(void) r_page_servos[i] = 0; } -#if 0 - /* if everything is ok */ - - if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { - /* we have recent control data from the FMU */ - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - - } else if (system_state.rc_channels > 0) { - /* when override is on or the fmu is not available, but RC is present */ - control_count = system_state.rc_channels; - - sched_lock(); - - /* remap roll, pitch, yaw and throttle from RC specific to internal ordering */ - rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1]; - rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1]; - rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1]; - rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1]; - //rc_channel_data[OVERRIDE] = system_state.rc_channel_data[system_state.rc_map[OVERRIDE] - 1]; - - /* get the remaining channels, no remapping needed */ - for (unsigned i = 4; i < system_state.rc_channels; i++) { - rc_channel_data[i] = system_state.rc_channel_data[i]; - } - - /* scale the control inputs */ - rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / - (float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000; - - if (rc_channel_data[THROTTLE] > 2000) { - rc_channel_data[THROTTLE] = 2000; - } - - if (rc_channel_data[THROTTLE] < 1000) { - rc_channel_data[THROTTLE] = 1000; - } - - // lowsyslog("Tmin: %d Ttrim: %d Tmax: %d T: %d \n", - // (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]), - // (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE])); - - control_values = &rc_channel_data[0]; - sched_unlock(); - } else { - /* we have no control input (no FMU, no RC) */ - - // XXX builtin failsafe would activate here - control_count = 0; - } - //lowsyslog("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]); - - /* this is for multicopters, etc. where manual override does not make sense */ - } else { - /* if the fmu is available whe are good */ - if (system_state.mixer_fmu_available) { - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - /* we better shut everything off */ - } else { - control_count = 0; - } - } -#endif - /* * Decide whether the servos should be armed right now. * @@ -301,7 +236,7 @@ mixer_handle_text(const void *buffer, size_t length) px4io_mixdata *msg = (px4io_mixdata *)buffer; - isr_debug(2, "mixer text %u", length); + isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) return; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 56923a674..b4135aba5 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -83,12 +83,12 @@ 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 + * 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]; +static char msg[NUM_MSG][40]; /* add a debug message to be printed on the console @@ -127,7 +127,7 @@ static void show_debug_messages(void) */ static void loop_overtime(void *arg) { - debug("RESETTING\n"); + lowsyslog("I2C RESET\n"); i2c_loop_resets++; i2c_dump(); i2c_reset(); @@ -136,7 +136,7 @@ static void loop_overtime(void *arg) static void wakeup_handler(int signo, siginfo_t *info, void *ucontext) { - // nothing to do - we just want poll() to return + /* nothing to do - we just want poll() to return */ } @@ -185,16 +185,31 @@ int user_start(int argc, char *argv[]) up_pwm_servo_init(0xff); /* start the flight control signal handler */ - task_create("FCon", + int ret = task_create("FCon", SCHED_PRIORITY_DEFAULT, 1024, (main_t)controls_main, NULL); struct mallinfo minfo = mallinfo(); - lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); + lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + + /* not enough memory, lock down */ + if (ret != OK || minfo.mxordblk < 500) { + lowsyslog("ERR: not enough MEM"); + bool phase = false; + + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } - debug("debug_level=%u\n", (unsigned)debug_level); + phase = !phase; + usleep(300000); + } /* start the i2c handler */ i2c_init(); @@ -211,7 +226,7 @@ int user_start(int argc, char *argv[]) sigfillset(&sa.sa_mask); sigdelset(&sa.sa_mask, SIGUSR1); if (sigaction(SIGUSR1, &sa, NULL) != OK) { - debug("Failed to setup SIGUSR1 handler\n"); + lowsyslog("SIGUSR1 init fail\n"); } /* diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index d199a9361..073ddeaba 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -53,7 +53,7 @@ #include "debug.h" #define SBUS_FRAME_SIZE 25 -#define SBUS_INPUT_CHANNELS 18 +#define SBUS_INPUT_CHANNELS 16 static int sbus_fd = -1; @@ -239,7 +239,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) } /* decode switch channels if data fields are wide enough */ - if (chancount > 17) { + if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) { + chancount = 18; + /* channel 17 (index 16) */ values[16] = (frame[23] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 5db94c6f1..87d500161 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -475,12 +475,12 @@ CONFIG_ARCH_BZERO=n # timer structures to minimize dynamic allocations. Set to # zero for all dynamic allocations. # -CONFIG_MAX_TASKS=8 +CONFIG_MAX_TASKS=4 CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=4 +CONFIG_NPTHREAD_KEYS=2 CONFIG_NFILE_DESCRIPTORS=8 CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=32 +CONFIG_NAME_MAX=12 CONFIG_STDIO_BUFFER_SIZE=32 CONFIG_STDIO_LINEBUFFER=n CONFIG_NUNGET_CHARS=2 |