From 5c12b6a91113e924f2264e1b0d04d6f865eb3c64 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 6 Mar 2013 22:52:19 +0100 Subject: [PATCH 1/4] Request result of rc config upload from IO --- apps/drivers/px4io/px4io.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2611c4e9c7..fee49b1aa0 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -83,6 +83,7 @@ #include #include "uploader.h" +#include class PX4IO : public device::I2C @@ -771,9 +772,17 @@ PX4IO::io_set_rc_config() /* send channel config to IO */ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); if (ret != OK) { - log("RC config update failed"); + log("rc config upload failed"); break; } + + /* check the IO initialisation flag */ + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK; + if (ret != OK) { + log("config for RC%d rejected by IO", i + 1); + break; + } + offset += PX4IO_P_RC_CONFIG_STRIDE; } From 8d1f80a9e8ef988949eed006995384800ac91e70 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 7 Mar 2013 01:03:38 +0100 Subject: [PATCH 2/4] Fix how we check for rc config init status --- apps/drivers/px4io/px4io.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index fee49b1aa0..8fb53295f4 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -777,8 +777,7 @@ PX4IO::io_set_rc_config() } /* check the IO initialisation flag */ - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK; - if (ret != OK) { + if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) { log("config for RC%d rejected by IO", i + 1); break; } @@ -1195,7 +1194,7 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), @@ -1206,7 +1205,8 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC")); + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); printf("alarms 0x%04x%s%s%s%s%s%s\n", alarms, From 8f5dac3740c87636f1f000b7e67df6f8ad58822a Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 7 Mar 2013 01:47:02 +0100 Subject: [PATCH 3/4] Let's just init the status flag every time we send a config update --- apps/px4io/px4io.c | 3 --- apps/px4io/registers.c | 7 +------ 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index a0e0002a64..5892646612 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -153,9 +153,6 @@ user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); - /* initialise the registry space */ - registers_init(); - /* initialise the control inputs */ controls_init(); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index dac09021db..d97fd8d86e 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -179,12 +179,6 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; -void -registers_init(void) -{ - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; -} - void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -389,6 +383,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_RC_CONFIG_OPTIONS: value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; From 4797c192be73fee2f99769a4063044c97632899e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 09:49:12 +0100 Subject: [PATCH 4/4] Fixed RC calibration scaling / assignment --- apps/px4io/controls.c | 45 +++++++++++++++++++++++++++---------------- 1 file changed, 28 insertions(+), 17 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index dabdde0af9..908334c478 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -147,32 +147,43 @@ controls_tick() { uint16_t raw = r_raw_rc_values[i]; - /* 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; - /* constrain to min/max values */ + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ 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]; - int16_t scaled = raw; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if center is min 0..20000, if center is max -20000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 20.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - /* adjust to zero-relative around center (nominal -500..500) */ - scaled -= conf[PX4IO_P_RC_CONFIG_CENTER]; + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 20.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - /* scale to fixed-point representation (-10000..10000) */ - scaled *= 20; + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } + /* invert channel if requested */ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled;