From ebac51cad8e144b64938e6726e26bdc23aaf45e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 19:47:43 +0100 Subject: [PATCH] Working on restart resilience, hunting down multi-load mixer issue (still present) --- apps/drivers/px4io/px4io.cpp | 38 ++++++++++++++++++++-------- apps/px4io/mixer.cpp | 16 +++++++++--- apps/px4io/registers.c | 7 +++++ apps/systemlib/mixer/mixer.cpp | 1 + apps/systemlib/mixer/mixer_group.cpp | 1 + 5 files changed, 49 insertions(+), 14 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 8fb53295f4..882ca9fed8 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1556,7 +1556,7 @@ px4io_main(int argc, char *argv[]) errx(1, "already loaded"); /* create the driver - it will set g_dev */ - (void)new PX4IO; + (void)new PX4IO(); if (g_dev == nullptr) errx(1, "driver alloc failed"); @@ -1567,7 +1567,7 @@ px4io_main(int argc, char *argv[]) } /* look for the optional pwm update rate for the supported modes */ - if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if (argc > 2 && strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { if (argc > 2 + 1) { #warning implement this } else { @@ -1579,16 +1579,31 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "recovery")) { + + if (g_dev != nullptr) { + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "stop")) { - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } - exit(0); + if (g_dev != nullptr) { + /* stop the driver */ + delete g_dev; + } else { + errx(1, "not loaded"); } + exit(0); + } if (!strcmp(argv[1], "status")) { @@ -1613,8 +1628,9 @@ px4io_main(int argc, char *argv[]) exit(1); } uint8_t level = atoi(argv[2]); - // we can cheat and call the driver directly, as it - // doesn't reference filp in ioctl() + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 0fba2cbe55..54584e6851 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -108,9 +108,11 @@ mixer_tick(void) /* * Decide which set of controls we're using. */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - /* don't actually mix anything - we already have raw PWM values */ + /* don't actually mix anything - we already have raw PWM values or + not a valid mixer. */ source = MIX_NONE; } else { @@ -239,6 +241,11 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { + /* do not allow a mixer change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + return; + } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -252,9 +259,12 @@ mixer_handle_text(const void *buffer, size_t length) switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); + + /* FIRST mark the mixer as invalid */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index d97fd8d86e..511a47f8df 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -361,6 +361,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_PAGE_RC_CONFIG: { + + /* do not allow a RC config change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + break; + } + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 72f765d90c..df0dfe838d 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -54,6 +54,7 @@ #include "mixer.h" Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), _control_cb(control_cb), _cb_handle(cb_handle) { diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 60eeff2256..1dbc512cdb 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -93,6 +93,7 @@ MixerGroup::reset() mixer = _first; _first = mixer->_next; delete mixer; + mixer = nullptr; } }