diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-11-22 14:40:41 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-11-22 14:40:41 +0100 |
commit | 7b43b80c370253ba63477df15572e0c28390dc84 (patch) | |
tree | fde081ac13854f649f8d994e68d4a59f83d06dda /src/drivers/px4io/px4io.cpp | |
parent | 4656db01c215c94dda70d7b579037ec368d3275e (diff) | |
parent | 685d3965a81b3f6a1ada4aa8bf9ebdd16b029c58 (diff) | |
download | px4-firmware-7b43b80c370253ba63477df15572e0c28390dc84.tar.gz px4-firmware-7b43b80c370253ba63477df15572e0c28390dc84.tar.bz2 px4-firmware-7b43b80c370253ba63477df15572e0c28390dc84.zip |
Merge branch 'master' of github.com:PX4/Firmware into safety
Diffstat (limited to 'src/drivers/px4io/px4io.cpp')
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 98 |
1 files changed, 81 insertions, 17 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 821eb52ff..6f02ba62c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1250,30 +1250,43 @@ PX4IO::io_set_rc_config() * for compatibility reasons with existing * autopilots / GCS'. */ - param_get(param_find("RC_MAP_ROLL"), &ichan); - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + /* ROLL */ + param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 0; + } + /* PITCH */ param_get(param_find("RC_MAP_PITCH"), &ichan); - - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 1; + } + /* YAW */ param_get(param_find("RC_MAP_YAW"), &ichan); - - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 2; + } + /* THROTTLE */ param_get(param_find("RC_MAP_THROTTLE"), &ichan); - - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 3; + } - param_get(param_find("RC_MAP_MODE_SW"), &ichan); - - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + /* FLAPS */ + param_get(param_find("RC_MAP_FLAPS"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { input_map[ichan - 1] = 4; + } + + /* MAIN MODE SWITCH */ + param_get(param_find("RC_MAP_MODE_SW"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + /* use out of normal bounds index to indicate special channel */ + input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; + } /* * Iterate all possible RC inputs. @@ -1649,10 +1662,6 @@ PX4IO::io_publish_raw_rc() int PX4IO::io_publish_pwm_outputs() { - /* if no FMU comms(!) just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) - return OK; - /* data we are going to fetch */ actuator_outputs_s outputs; outputs.timestamp = hrt_absolute_time(); @@ -2053,7 +2062,7 @@ PX4IO::print_status(bool extended_status) ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), @@ -2063,7 +2072,8 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), - ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "") + ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "") ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -2276,6 +2286,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; + case PWM_SERVO_SET_FORCE_SAFETY_ON: + /* force safety switch on */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC); + break; + case PWM_SERVO_SET_FORCE_FAILSAFE: /* force failsafe mode instantly */ if (arg == 0) { @@ -2298,6 +2313,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_SET_OVERRIDE_IMMEDIATE: + /* control whether override on FMU failure is + immediate or waits for override threshold on mode + switch */ + if (arg == 0) { + /* clear override immediate flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0); + } else { + /* set override immediate flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ @@ -2559,6 +2587,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_RC_CONFIG: { + /* enable setting of RC configuration without relying + on param_get() + */ + struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; + if (config->channel >= _max_actuators) { + /* fail with error */ + return E2BIG; + } + + /* copy values to registers in IO */ + uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; + uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE; + regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min; + regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim; + regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max; + regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz; + regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment; + regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + if (config->rc_reverse) { + regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + } + ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + break; + } + + case PWM_SERVO_SET_OVERRIDE_OK: + /* set the 'OVERRIDE OK' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK); + break; + + case PWM_SERVO_CLEAR_OVERRIDE_OK: + /* clear the 'OVERRIDE OK' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + break; + default: /* see if the parent class can make any use of it */ ret = CDev::ioctl(filep, cmd, arg); |