pwm systemcmd can now set the failsafe values, fmu uses failsafe values as well now, fix to only send the appropriate number of pwm values to IO at once

3 files changed, 182 insertions, 166 deletions

diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 3357e67a5..f0dd713b2 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -154,23 +154,29 @@ ORB_DECLARE(output_pwm);
 /** power up DSM receiver */
 #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
 
+/** set the PWM value for failsafe */
+#define PWM_SERVO_SET_FAILSAFE_PWM	_IOC(_PWM_SERVO_BASE, 12)
+
+/** get the PWM value for failsafe */
+#define PWM_SERVO_GET_FAILSAFE_PWM	_IOC(_PWM_SERVO_BASE, 13)
+
 /** set the PWM value when disarmed - should be no PWM (zero) by default */
-#define PWM_SERVO_SET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 12)
+#define PWM_SERVO_SET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 14)
 
 /** get the PWM value when disarmed */
-#define PWM_SERVO_GET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 13)
+#define PWM_SERVO_GET_DISARMED_PWM	_IOC(_PWM_SERVO_BASE, 15)
 
 /** set the minimum PWM value the output will send */
-#define PWM_SERVO_SET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 14)
+#define PWM_SERVO_SET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 16)
 
 /** get the minimum PWM value the output will send */
-#define PWM_SERVO_GET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 15)
+#define PWM_SERVO_GET_MIN_PWM	_IOC(_PWM_SERVO_BASE, 17)
 
 /** set the maximum PWM value the output will send */
-#define PWM_SERVO_SET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 16)
+#define PWM_SERVO_SET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 18)
 
 /** get the maximum PWM value the output will send */
-#define PWM_SERVO_GET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 17)
+#define PWM_SERVO_GET_MAX_PWM	_IOC(_PWM_SERVO_BASE, 19)
 
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index e729612cc..b7b2f7b33 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -78,6 +78,12 @@
 # include <systemlib/ppm_decode.h>
 #endif
 
+/*
+ * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
+ */
+
+#define CONTROL_INPUT_DROP_LIMIT_MS		20
+
 class PX4FMU : public device::CDev
 {
 public:
@@ -130,9 +136,11 @@ private:
 	actuator_controls_s _controls;
 
 	pwm_limit_t	_pwm_limit;
+	uint16_t	_failsafe_pwm[_max_actuators];
 	uint16_t	_disarmed_pwm[_max_actuators];
 	uint16_t	_min_pwm[_max_actuators];
 	uint16_t	_max_pwm[_max_actuators];
+	unsigned	_num_failsafe_set;
 	unsigned	_num_disarmed_set;
 
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -218,7 +226,9 @@ PX4FMU::PX4FMU() :
 	_armed(false),
 	_pwm_on(false),
 	_mixers(nullptr),
+	_failsafe_pwm({0}),
 	_disarmed_pwm({0}),
+	_num_failsafe_set(0),
 	_num_disarmed_set(0)
 {
 	for (unsigned i = 0; i < _max_actuators; i++) {
@@ -515,98 +525,103 @@ PX4FMU::task_main()
 
 		/* sleep waiting for data, stopping to check for PPM
 		 * input at 100Hz */
-		int ret = ::poll(&fds[0], 2, 10);
+		int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
 
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
 			usleep(1000000);
 			continue;
-		}
+		} else if (ret == 0) {
+			/* timeout: no control data, switch to failsafe values */
+//			warnx("no PWM: failsafe");
 
-		/* do we have a control update? */
-		if (fds[0].revents & POLLIN) {
-
-			/* get controls - must always do this to avoid spinning */
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
-
-			/* can we mix? */
-			if (_mixers != nullptr) {
-
-				unsigned num_outputs;
-
-				switch (_mode) {
-				case MODE_2PWM:
-					num_outputs = 2;
-					break;
-				case MODE_4PWM:
-					num_outputs = 4;
-					break;
-				case MODE_6PWM:
-					num_outputs = 6;
-					break;
-				default:
-					num_outputs = 0;
-					break;
-				}
+		} else {
 
-				/* do mixing */
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
-				outputs.timestamp = hrt_absolute_time();
-
-				/* iterate actuators */
-				for (unsigned i = 0; i < num_outputs; i++) {
-					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i >= outputs.noutputs ||
-						!isfinite(outputs.output[i]) ||
-						outputs.output[i] < -1.0f ||
-						outputs.output[i] > 1.0f) {
-						/*
-						 * Value is NaN, INF or out of band - set to the minimum value.
-						 * This will be clearly visible on the servo status and will limit the risk of accidentally
-						 * spinning motors. It would be deadly in flight.
-						 */
-						outputs.output[i] = -1.0f;
+			/* do we have a control update? */
+			if (fds[0].revents & POLLIN) {
+
+				/* get controls - must always do this to avoid spinning */
+				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+
+				/* can we mix? */
+				if (_mixers != nullptr) {
+
+					unsigned num_outputs;
+
+					switch (_mode) {
+					case MODE_2PWM:
+						num_outputs = 2;
+						break;
+					case MODE_4PWM:
+						num_outputs = 4;
+						break;
+					case MODE_6PWM:
+						num_outputs = 6;
+						break;
+					default:
+						num_outputs = 0;
+						break;
 					}
-				}
 
-				uint16_t pwm_limited[num_outputs];
+					/* do mixing */
+					outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+					outputs.timestamp = hrt_absolute_time();
+
+					/* iterate actuators */
+					for (unsigned i = 0; i < num_outputs; i++) {
+						/* last resort: catch NaN, INF and out-of-band errors */
+						if (i >= outputs.noutputs ||
+							!isfinite(outputs.output[i]) ||
+							outputs.output[i] < -1.0f ||
+							outputs.output[i] > 1.0f) {
+							/*
+							 * Value is NaN, INF or out of band - set to the minimum value.
+							 * This will be clearly visible on the servo status and will limit the risk of accidentally
+							 * spinning motors. It would be deadly in flight.
+							 */
+							outputs.output[i] = -1.0f;
+						}
+					}
 
-				pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
+					uint16_t pwm_limited[num_outputs];
 
-				/* output actual limited values */
-				for (unsigned i = 0; i < num_outputs; i++) {
-					controls_effective.control_effective[i] = (float)pwm_limited[i];
-				}
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
+					pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
 
-				/* output to the servos */
-				for (unsigned i = 0; i < num_outputs; i++) {
-					up_pwm_servo_set(i, pwm_limited[i]);
-				}
+					/* output actual limited values */
+					for (unsigned i = 0; i < num_outputs; i++) {
+						controls_effective.control_effective[i] = (float)pwm_limited[i];
+					}
+					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
 
-				/* and publish for anyone that cares to see */
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+					/* output to the servos */
+					for (unsigned i = 0; i < num_outputs; i++) {
+						up_pwm_servo_set(i, pwm_limited[i]);
+					}
+
+					/* and publish for anyone that cares to see */
+					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+				}
 			}
-		}
 
-		/* how about an arming update? */
-		if (fds[1].revents & POLLIN) {
-			actuator_armed_s aa;
+			/* how about an arming update? */
+			if (fds[1].revents & POLLIN) {
+				actuator_armed_s aa;
 
-			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
+				/* get new value */
+				orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
 
-			/* update the armed status and check that we're not locked down */
-			bool set_armed = aa.armed && !aa.lockdown;
-			if (_armed != set_armed)
-				_armed = set_armed;
+				/* update the armed status and check that we're not locked down */
+				bool set_armed = aa.armed && !aa.lockdown;
+				if (_armed != set_armed)
+					_armed = set_armed;
 
-			/* update PWM status if armed or if disarmed PWM values are set */
-			bool pwm_on = (aa.armed || _num_disarmed_set > 0);
-			if (_pwm_on != pwm_on) {
-				_pwm_on = pwm_on;
-				up_pwm_servo_arm(pwm_on);
+				/* update PWM status if armed or if disarmed PWM values are set */
+				bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+				if (_pwm_on != pwm_on) {
+					_pwm_on = pwm_on;
+					up_pwm_servo_arm(pwm_on);
+				}
 			}
 		}
 
@@ -737,6 +752,45 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		*(uint32_t *)arg = _pwm_alt_rate_channels;
 		break;
 
+	case PWM_SERVO_SET_FAILSAFE_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		/* discard if too many values are sent */
+		if (pwm->channel_count > _max_actuators) {
+			ret = -EINVAL;
+			break;
+		}
+		for (unsigned i = 0; i < pwm->channel_count; i++) {
+			if (pwm->values[i] == 0) {
+				/* ignore 0 */
+			} else if (pwm->values[i] > PWM_MAX) {
+				_failsafe_pwm[i] = PWM_MAX;
+			} else if (pwm->values[i] < PWM_MIN) {
+				_failsafe_pwm[i] = PWM_MIN;
+			} else {
+				_failsafe_pwm[i] = pwm->values[i];
+			}
+		}
+
+		/*
+		 * update the counter
+		 * this is needed to decide if disarmed PWM output should be turned on or not
+		 */
+		_num_failsafe_set = 0;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			if (_failsafe_pwm[i] > 0)
+				_num_failsafe_set++;
+		}
+		break;
+	}
+	case PWM_SERVO_GET_FAILSAFE_PWM: {
+		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
+		for (unsigned i = 0; i < _max_actuators; i++) {
+			pwm->values[i] = _failsafe_pwm[i];
+		}
+		pwm->channel_count = _max_actuators;
+		break;
+	}
+
 	case PWM_SERVO_SET_DISARMED_PWM: {
 		struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
 		/* discard if too many values are sent */
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index d709f1cc8..b603ffc8d 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -943,53 +943,6 @@ PX4IO::io_set_control_state()
 	return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
 }
 
-int
-PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
-{
-	if (len > _max_actuators)
-		/* fail with error */
-		return E2BIG;
-
-	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
-}
-
-int
-PX4IO::set_min_values(const uint16_t *vals, unsigned len)
-{
-
-	if (len > _max_actuators)
-		/* fail with error */
-		return E2BIG;
-
-	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len);
-}
-
-int
-PX4IO::set_max_values(const uint16_t *vals, unsigned len)
-{
-
-	if (len > _max_actuators)
-		/* fail with error */
-		return E2BIG;
-
-	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
-}
-
-int
-PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
-{
-
-	if (len > _max_actuators)
-		/* fail with error */
-		return E2BIG;
-
-	/* copy values to registers in IO */
-	return io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, vals, len);
-}
-
 
 int
 PX4IO::io_set_arming_state()
@@ -1803,7 +1756,7 @@ PX4IO::print_status()
 	printf("failsafe");
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
-	printf("\nidle values");
+	printf("\ndisarmed values");
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i));
 	printf("\n");
@@ -1874,15 +1827,39 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
 		break;
 
+	case PWM_SERVO_SET_FAILSAFE_PWM: {
+		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
+		break;
+	}
+
+	case PWM_SERVO_GET_FAILSAFE_PWM:
+
+		ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators);
+		if (ret != OK) {
+			ret = -EIO;
+		}
+		break;
+
 	case PWM_SERVO_SET_DISARMED_PWM: {
 		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
-		set_idle_values(pwm->values, pwm->channel_count);
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
 		break;
 	}
 
 	case PWM_SERVO_GET_DISARMED_PWM:
 
-		ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t));
+		ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators);
 		if (ret != OK) {
 			ret = -EIO;
 		}
@@ -1890,13 +1867,18 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET_MIN_PWM: {
 		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
-		set_min_values(pwm->values, pwm->channel_count);
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
 		break;
 	}
 
 	case PWM_SERVO_GET_MIN_PWM:
 
-		ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t));
+		ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators);
 		if (ret != OK) {
 			ret = -EIO;
 		}
@@ -1904,13 +1886,18 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET_MAX_PWM: {
 		struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
-		set_max_values(pwm->values, pwm->channel_count);
+		if (pwm->channel_count > _max_actuators)
+			/* fail with error */
+			return E2BIG;
+
+		/* copy values to registers in IO */
+		ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
 		break;
 	}
 
 	case PWM_SERVO_GET_MAX_PWM:
 
-		ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, sizeof(struct pwm_output_values)/sizeof(uint16_t));
+		ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators);
 		if (ret != OK) {
 			ret = -EIO;
 		}
@@ -2518,37 +2505,6 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "failsafe")) {
-
-		if (argc < 3) {
-			errx(1, "failsafe command needs at least one channel value (ppm)");
-		}
-
-		/* set values for first 8 channels, fill unassigned channels with 1500. */
-		uint16_t failsafe[8];
-
-		for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
-
-			/* set channel to commandline argument or to 900 for non-provided channels */
-			if ((unsigned)argc > i + 2) {
-				failsafe[i] = atoi(argv[i+2]);
-				if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_MAX) {
-					errx(1, "value out of range of %d < value < %d. Aborting.", PWM_MIN, PWM_MAX);
-				}
-			} else {
-				/* a zero value will result in stopping to output any pulse */
-				failsafe[i] = 0;
-			}
-		}
-
-		int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
-
-		if (ret != OK)
-			errx(ret, "failed setting failsafe values");
-		exit(0);
-	}
-
-
 
 	if (!strcmp(argv[1], "recovery")) {
 
@@ -2616,5 +2572,5 @@ px4io_main(int argc, char *argv[])
 		bind(argc, argv);
 
 	out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'bind' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
 }