diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/drv_pwm_output.h | 4 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 53 |
2 files changed, 55 insertions, 2 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index c3eea310f..7a364d52f 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -199,6 +199,10 @@ ORB_DECLARE(output_pwm); /** get the lockdown override flag to enable outputs in HIL */ #define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) +#define SAFELINK_CONTROL_INPUT _IOC(_PWM_SERVO_BASE, 23) + +#define SAFELINK_CONTROL_OUTPUT _IOC(_PWM_SERVO_BASE, 24) + /* * * diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index efcf4d12b..50ec30536 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1805,7 +1805,8 @@ PX4IO::print_status() ((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_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); + ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_SAFELINK_FS) ? " SAFELINK_FAILSAFE" : "")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n", alarms, @@ -1898,7 +1899,9 @@ PX4IO::print_status() ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), - ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") + ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : ""), + ((features & PX4IO_P_SETUP_FEATURES_SAFELINK_IN) ? " SAFELINK_IN" : ""), + ((features & PX4IO_P_SETUP_FEATURES_SAFELINK_OUT) ? " SAFELINK_OUT" : "") ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s%s\n", @@ -2362,6 +2365,26 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; + case SAFELINK_CONTROL_INPUT: + + if (arg == 1) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SAFELINK_IN); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_SAFELINK_IN, 0); + } + + break; + + case SAFELINK_CONTROL_OUTPUT: + + if (arg == 1) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SAFELINK_OUT); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_SAFELINK_OUT, 0); + } + + break; + default: /* not a recognized value */ ret = -ENOTTY; @@ -3099,6 +3122,32 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "safelink_in")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SAFELINK_CONTROL_INPUT, 1); + + if (ret != 0) { + errx(ret, "SAFELINK input failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "safelink_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SAFELINK_CONTROL_OUTPUT, 1); + + if (ret != 0) { + errx(ret, "SAFELINK output failed"); + } + + exit(0); + } + if (!strcmp(argv[1], "rssi_analog")) { /* we can cheat and call the driver directly, as it * doesn't reference filp in ioctl() |