Merge branch 'master' of github.com:PX4/Firmware

2 files changed, 108 insertions, 29 deletions

diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index 57eb12f4e..3021321da 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -209,6 +209,8 @@ FMUServo::task_main()
 
 	unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
 
+	log("starting");
+
 	/* loop until killed */
 	while (!_task_should_exit) {
 
@@ -235,7 +237,6 @@ FMUServo::task_main()
 
 				/* if the actuator is configured */
 				if (_mixer[i] != nullptr) {
-
 					/* mix controls to the actuator */
 					float output = mixer_mix(_mixer[i], &controls[0]);
 
@@ -263,6 +264,8 @@ FMUServo::task_main()
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
 
+	log("stopping");
+
 	/* note - someone else is responsible for restoring the GPIO config */
 
 	/* tell the dtor that we are exiting */
@@ -379,13 +382,16 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* get the caller-supplied mixer and check */
 		mm = (struct mixer_s *)arg;
 
-		if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) {	/* only the attitude group is supported */
-			ret = -EINVAL;
-			break;
-		}
-
 		/* allocate local storage and copy from the caller*/
 		if (mm != nullptr) {
+
+			if (mixer_check(mm, 1, NUM_ACTUATOR_CONTROLS)) {
+				/* only the attitude group is supported */
+				ret = -EINVAL;
+				break;
+			}
+
+			/* allocate a new mixer struct */
 			tmm = (struct mixer_s *)malloc(MIXER_SIZE(mm->control_count));
 			memcpy(tmm, mm, MIXER_SIZE(mm->control_count));
 
@@ -512,6 +518,56 @@ fmu_new_mode(PortMode new_mode)
 	return ret;
 }
 
+void
+test(void)
+{
+	int	fd;
+
+	fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		puts("open fail");
+		exit(1);
+	}
+
+	ioctl(fd, PWM_SERVO_ARM, 0);
+	ioctl(fd, PWM_SERVO_SET(0), 1000);
+
+	close(fd);
+
+	exit(0);
+}
+
+void
+fake(int argc, char *argv[])
+{
+	if (argc < 5) {
+		puts("fmu fake <roll> <pitch> <yaw> <thrust> (values -100 - 100)");
+		exit(1);
+	}
+
+	struct actuator_controls_s ac;
+
+	ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;
+
+	ac.control[1] = strtol(argv[2], 0, 0) / 100.0f;
+
+	ac.control[2] = strtol(argv[3], 0, 0) / 100.0f;
+
+	ac.control[3] = strtol(argv[4], 0, 0) / 100.0f;
+
+	int handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac);
+
+	if (handle < 0) {
+		puts("advertise failed");
+		exit(1);
+	}
+
+	close(handle);
+
+	exit(0);
+}
+
 } // namespace
 
 extern "C" __EXPORT int fmu_main(int argc, char *argv[]);
@@ -521,10 +577,16 @@ fmu_main(int argc, char *argv[])
 {
 	PortMode new_mode = PORT_MODE_UNSET;
 
+	if (!strcmp(argv[1], "test"))
+		test();
+
+	if (!strcmp(argv[1], "fake"))
+		fake(argc - 1, argv + 1);
+
 	/*
 	 * Mode switches.
 	 *
-	 * XXX use getopt
+	 * XXX use getopt?
 	 */
 	if (!strcmp(argv[1], "mode_gpio")) {
 		new_mode = PORT_FULL_GPIO;
diff --git a/apps/systemlib/mixer.c b/apps/systemlib/mixer.c
index 8b1dcc054..b068d3958 100644
--- a/apps/systemlib/mixer.c
+++ b/apps/systemlib/mixer.c
@@ -163,29 +163,46 @@ mixer_mix(struct mixer_s *mixer, float **controls)
 static int
 mixer_getline(int fd, char *line, unsigned maxlen)
 {
-	int	ret;
-	char	c;
-
-	while (--maxlen) {
-		ret = read(fd, &c, 1);
-
-		if (ret <= 0)
-			return ret;
-
-		if (c == '\r')
-			continue;
-
-		if (c == '\n') {
-			*line = '\0';
-			return 1;
+	/* reduce line budget by 1 to account for terminal NUL */
+	maxlen--;
+
+	/* loop looking for a non-comment line */
+	for (;;) {
+		int	ret;
+		char	c;
+		char	*p = line;
+
+		/* loop reading characters for this line */
+		for (;;) {
+			ret = read(fd, &c, 1);
+
+			/* on error or EOF, return same */
+			if (ret <= 0)
+				return ret;
+
+			/* ignore carriage returns */
+			if (c == '\r')
+				continue;
+
+			/* line termination */
+			if (c == '\n') {
+				/* ignore malformed lines */
+				if ((p - line) < 4)
+					break;
+
+				if (line[1] != ':')
+					break;
+
+				/* terminate line as string and return */
+				*p = '\0';
+				return 1;
+			}
+
+			/* if we have space, accumulate the byte and go on */
+			if ((p - line) < maxlen)
+				*p++ = c;
 		}
-
-		*line++ = c;
 	}
-
-	/* line too long */
-	puts("line too long");
-	return -1;
 }
 
 static int
@@ -214,7 +231,7 @@ mixer_load(int fd, struct mixer_s **mp)
 {
 	int		ret, result = -1;
 	struct mixer_s *mixer = NULL;
-	char		buf[100];
+	char		buf[60];
 	unsigned	scalers;
 
 	ret = mixer_getline(fd, buf, sizeof(buf));