From 93f6edfe64780f1132cf8cb44afe3c7f1477f94e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 14:30:56 -0800 Subject: [PATCH 1/7] Fix reporting of R/C input config --- apps/drivers/px4io/px4io.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2488f8022b..3872d7201f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1228,7 +1228,7 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); printf("\n"); - /* setup */ + /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s\n", @@ -1251,6 +1251,20 @@ PX4IO::print_status() for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); printf("\n"); + for (unsigned i = 0; i < _max_rc_input; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); + printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + } } int From f35c5d600aa1d936207e3e6988058093dccacdf7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 14:32:04 -0800 Subject: [PATCH 2/7] Don't mask out the enable bit when accepting R/C input config updates. --- apps/px4io/registers.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 5fb90b9b0c..5ec2f7258b 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -165,8 +165,8 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; */ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; -/* valid options excluding ENABLE */ -#define PX4IO_P_RC_CONFIG_OPTIONS_VALID PX4IO_P_RC_CONFIG_OPTIONS_REVERSE +/* valid options */ +#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) /* * PAGE 104 uses r_page_servos. From dc74eeb421bce204a3064bcc60d524bf3fb53ab2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:31:01 -0800 Subject: [PATCH 3/7] Report the control values from the FMU in the status output. Count them separately from the actuators. --- apps/drivers/px4io/px4io.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 3872d7201f..a3b9957dd5 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -101,6 +101,7 @@ public: private: // XXX unsigned _max_actuators; + unsigned _max_controls; unsigned _max_rc_input; unsigned _max_relays; unsigned _max_transfer; @@ -277,6 +278,7 @@ PX4IO *g_dev; PX4IO::PX4IO() : I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), + _max_controls(0), _max_rc_input(0), _max_relays(0), _max_transfer(16), /* sensible default */ @@ -342,6 +344,7 @@ PX4IO::init() /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); + _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); @@ -637,11 +640,11 @@ PX4IO::io_set_control_state() orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &controls); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_controls; i++) regs[i] = FLOAT_TO_REG(controls.control[i]); /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators); + return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); } int @@ -1247,9 +1250,9 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - printf("failsafe"); - for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("controls"); + for (unsigned i = 0; i < _max_controls; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); printf("\n"); for (unsigned i = 0; i < _max_rc_input; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; @@ -1265,6 +1268,10 @@ PX4IO::print_status() ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); } + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\n"); } int From 3d9901dfaf687e375569cbc3256b818ff01721c6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:31:40 -0800 Subject: [PATCH 4/7] If we have seen control input from FMU, update the FMU_OK status flag. --- apps/px4io/mixer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 77f28cd7a6..ed74cb3d30 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -94,9 +94,13 @@ mixer_tick(void) if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { debug("AP RX timeout"); } - r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; + + /* XXX this is questionable - vehicle may not make sense for direct control */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + } else { + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } source = MIX_FAILSAFE; From 345b1a091554c92aa2d3e8e8df2b91cba2431aa5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:55:38 -0800 Subject: [PATCH 5/7] Print mapped R/C inputs as signed values (since they are zero-relative) --- apps/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index a3b9957dd5..4006f88cf0 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1222,7 +1222,7 @@ PX4IO::print_status() printf("mapped R/C inputs 0x%04x", mapped_inputs); for (unsigned i = 0; i < _max_rc_input; i++) { if (mapped_inputs & (1 << i)) - printf(" %u:%u", i, io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)); + printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); } printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); From 6ac7e8b7e4662c297e02ffc43e2cd52126753fa2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:56:02 -0800 Subject: [PATCH 6/7] Scale R/C inputs around the preset center, not the nominal center. --- apps/px4io/controls.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 6b51647569..37872d3569 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -119,7 +119,6 @@ controls_tick() { ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - /* * In some cases we may have received a frame, but input has still * been lost. @@ -168,8 +167,8 @@ controls_tick() { int16_t scaled = raw; - /* adjust to zero-relative (-500..500) */ - scaled -= 1500; + /* adjust to zero-relative around center */ + scaled -= conf[PX4IO_P_RC_CONFIG_CENTER]; /* scale to fixed-point representation (-10000..10000) */ scaled *= 20; From e6228355557aa7e06551711fc5a5a2127ca683f3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 16:20:04 -0800 Subject: [PATCH 7/7] Bump the task stack up to 1200 bytes to give the mixer loader some headroom. This addresses the last reported issue with this branch. --- apps/px4io/controls.c | 2 +- nuttx/configs/px4io/io/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 37872d3569..21b4edcc37 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -167,7 +167,7 @@ controls_tick() { int16_t scaled = raw; - /* adjust to zero-relative around center */ + /* adjust to zero-relative around center (nominal -500..500) */ scaled -= conf[PX4IO_P_RC_CONFIG_CENTER]; /* scale to fixed-point representation (-10000..10000) */ diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 1145fb349f..bb937cf4ee 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -541,7 +541,7 @@ CONFIG_BOOT_COPYTORAM=n CONFIG_CUSTOM_STACK=n CONFIG_STACK_POINTER= CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=800 +CONFIG_USERMAIN_STACKSIZE=1200 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=1024 CONFIG_HEAP_BASE=