Added support for safelink handling and status in IO driver

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()