First try to prevent motors from stopping when armed

5 files changed, 192 insertions, 2 deletions

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index d728b7b76..6a596a987 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -126,6 +126,16 @@ public:
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
 	/**
+	 * Set the minimum PWM signals when armed
+	 */
+	int 			set_min_values(const uint16_t *vals, unsigned len);
+
+	/**
+	 * Set the maximum PWM signal when armed
+	 */
+	int 			set_max_values(const uint16_t *vals, unsigned len);
+
+	/**
 	* Print the current status of IO
 	*/
 	void			print_status();
@@ -712,6 +722,34 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
 }
 
 int
+PX4IO::set_min_values(const uint16_t *vals, unsigned len)
+{
+	uint16_t 		regs[_max_actuators];
+
+	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)
+{
+	uint16_t 		regs[_max_actuators];
+
+	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::io_set_arming_state()
 {
 	actuator_safety_s	safety;		///< system armed state
@@ -1792,6 +1830,76 @@ px4io_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "min")) {
+
+		if (argc < 3) {
+			errx(1, "min command needs at least one channel value (PWM)");
+		}
+
+		if (g_dev != nullptr) {
+
+			/* set values for first 8 channels, fill unassigned channels with 900. */
+			uint16_t min[8];
+
+			for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++)
+			{
+				/* set channel to commanline argument or to 900 for non-provided channels */
+				if (argc > i + 2) {
+					min[i] = atoi(argv[i+2]);
+					if (min[i] < 900 || min[i] > 1200) {
+						errx(1, "value out of range of 900 < value < 1200. Aborting.");
+					}
+				} else {
+					/* a zero value will the default */
+					min[i] = 900;
+				}
+			}
+
+			int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
+
+			if (ret != OK)
+				errx(ret, "failed setting min values");
+		} else {
+			errx(1, "not loaded");
+		}
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "max")) {
+
+		if (argc < 3) {
+			errx(1, "max command needs at least one channel value (PWM)");
+		}
+
+		if (g_dev != nullptr) {
+
+			/* set values for first 8 channels, fill unassigned channels with 2100. */
+			uint16_t max[8];
+
+			for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++)
+			{
+				/* set channel to commanline argument or to 2100 for non-provided channels */
+				if (argc > i + 2) {
+					max[i] = atoi(argv[i+2]);
+					if (max[i] < 1800 || max[i] > 2100) {
+						errx(1, "value out of range of 1800 < value < 2100. Aborting.");
+					}
+				} else {
+					/* a zero value will the default */
+					max[i] = 2100;
+				}
+			}
+
+			int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
+
+			if (ret != OK)
+				errx(ret, "failed setting max values");
+		} else {
+			errx(1, "not loaded");
+		}
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "recovery")) {
 
 		if (g_dev != nullptr) {
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index a2193b526..e257e7cb8 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -59,6 +59,11 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		200000
 
+/*
+ * Time that the ESCs need to initialize
+ */
+ #define ESC_INIT_TIME_US		500000
+
 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
 #define ROLL     0
 #define PITCH    1
@@ -69,6 +74,8 @@ extern "C" {
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
 
+static uint64_t time_armed;
+
 /* selected control values and count for mixing */
 enum mixer_source {
 	MIX_NONE,
@@ -176,8 +183,16 @@ mixer_tick(void)
 			/* save actuator values for FMU readback */
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 
-			/* scale to servo output */
-			r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
+			/* scale to control range after init time */
+			if (mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) {
+				r_page_servos[i] = (outputs[i]
+					* (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+					+ (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
+
+			/* but use init range from 900 to 2100 right after arming */
+			} else {
+				r_page_servos[i] = (outputs[i] * 600 + 1500);
+			}
 
 		}
 		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
@@ -206,6 +221,7 @@ mixer_tick(void)
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
+		time_armed = hrt_absolute_time();
 
 	} else if (!should_arm && mixer_servos_armed) {
 		/* armed but need to disarm */
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 674f9dddd..1c9715451 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -183,6 +183,12 @@
 /* PWM failsafe values - zero disables the output */
 #define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
+/* PWM minimum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MIN_PWM		106	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+ /* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM		107	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
 /**
  * As-needed mixer data upload.
  *
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 272cdb7bf..3e4027c9b 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -77,6 +77,8 @@ extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
 extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
 extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
 extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+extern uint16_t			r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
+extern uint16_t			r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
 
 /*
  * Register aliases.
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index df7d6dcd3..1fcfb2906 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -184,6 +184,22 @@ uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
  */
 uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
 
+/**
+ * PAGE 106
+ *
+ * minimum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+/**
+ * PAGE 107
+ *
+ * maximum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
+
 void
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
@@ -247,6 +263,42 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		}
 		break;
 
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+
+		/* copy channel data */
+		while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values > 1200)
+				r_page_servo_control_min[offset] = 1200;
+			else if (*values < 900)
+				r_page_servo_control_min[offset] = 900;
+			else
+				r_page_servo_control_min[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+	
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+
+		/* copy channel data */
+		while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values > 1200)
+				r_page_servo_control_max[offset] = 1200;
+			else if (*values < 900)
+				r_page_servo_control_max[offset] = 900;
+			else
+				r_page_servo_control_max[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
 		mixer_handle_text(values, num_values * sizeof(*values));
@@ -583,6 +635,12 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 	case PX4IO_PAGE_FAILSAFE_PWM:
 		SELECT_PAGE(r_page_servo_failsafe);
 		break;
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+		SELECT_PAGE(r_page_servo_control_min);
+		break;
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+		SELECT_PAGE(r_page_servo_control_max);
+		break;
 
 	default:
 		return -1;