Merged debuglevel command from Tridge

5 files changed, 37 insertions, 1 deletions

diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index 23883323e..64bb540b4 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/apps/drivers/drv_pwm_output.h
@@ -106,6 +106,9 @@ ORB_DECLARE(output_pwm);
 /** get the number of servos in *(unsigned *)arg */
 #define PWM_SERVO_GET_COUNT	_IOC(_PWM_SERVO_BASE, 3)
 
+/** set debug level for servo IO */
+#define PWM_SERVO_SET_DEBUG	_IOC(_PWM_SERVO_BASE, 4)
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 51ff3fb83..8bcf4bacb 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -1019,6 +1019,11 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		*(unsigned *)arg = _max_actuators;
 		break;
 
+	case PWM_SERVO_SET_DEBUG:
+		/* set the debug level */
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
+		break;
+
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): {
 
 		unsigned channel = cmd - PWM_SERVO_SET(0);
@@ -1287,6 +1292,27 @@ px4io_main(int argc, char *argv[])
 			exit(0);
 		}
 
+	if (!strcmp(argv[1], "debug")) {
+		if (argc <= 2) {
+			printf("usage: px4io debug LEVEL\n");
+			exit(1);
+		}
+		if (g_dev == nullptr) {
+			printf("px4io is not started\n");
+			exit(1);
+		}
+		uint8_t level = atoi(argv[2]);
+		// we can cheat and call the driver directly, as it
+		// doesn't reference filp in ioctl()
+		int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level);
+		if (ret != 0) {
+			printf("SET_DEBUG failed - %d\n", ret);
+			exit(1);
+		}
+		printf("SET_DEBUG %u OK\n", (unsigned)level);
+		exit(0);
+	}
+
 	/* note, stop not currently implemented */
 
 	if (!strcmp(argv[1], "update")) {
@@ -1353,5 +1379,5 @@ px4io_main(int argc, char *argv[])
 		monitor();
 
 	out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug' or 'update'");
 }
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 29a287e45..f64b82ad1 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -149,6 +149,7 @@
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
 #define PX4IO_P_SETUP_IBATT_SCALE		7	/* battery current scaling factor (float) */
 #define PX4IO_P_SETUP_IBATT_BIAS		8	/* battery current bias value */
+#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS		101		/* 0..CONFIG_CONTROL_COUNT */
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 92d55a47c..9c5989df7 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -185,3 +185,4 @@ extern void isr_debug(uint8_t level, const char *fmt, ...);
 
 void i2c_dump(void);
 void i2c_reset(void);
+
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
index f14cd8309..014608416 100644
--- a/apps/px4io/registers.c
+++ b/apps/px4io/registers.c
@@ -330,6 +330,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			POWER_ACC2(value & (1 << 3) ? 1 : 0);
 			break;
 
+		case PX4IO_P_SETUP_SET_DEBUG:
+			debug_level = value;
+			isr_debug(0, "set debug %u\n", (unsigned)debug_level);
+			break;
+
 		default:
 			return -1;
 		}