Moved PWM ramp to systemlib

2 files changed, 195 insertions, 0 deletions

diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
new file mode 100644
index 000000000..675247578
--- /dev/null
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pwm_limit.c
+ *
+ * Lib to limit PWM output
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#include "pwm_limit.h"
+#include <math.h>
+#include <stdbool.h>
+#include <drivers/drv_hrt.h>
+
+__EXPORT pwm_limit_init(pwm_limit_t *limit)
+{
+	limit->nchannels = 0;
+	limit->state = LIMIT_STATE_OFF;
+	limit->time_armed = 0;
+	return;
+}
+
+__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, uint16_t *effective_pwm, pwm_limit_t *limit)
+{
+	/* first evaluate state changes */
+	switch (limit->state) {
+		case LIMIT_STATE_OFF:
+			if (armed)
+				limit->state = LIMIT_STATE_RAMP;
+			limit->time_armed = hrt_absolute_time();
+			break;
+		case LIMIT_STATE_INIT:
+			if (!armed)
+				limit->state = LIMIT_STATE_OFF;
+			else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US)
+				limit->state = LIMIT_STATE_RAMP;
+			break;
+		case LIMIT_STATE_RAMP:
+			if (!armed)
+				limit->state = LIMIT_STATE_OFF;
+			else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US)
+				limit->state = LIMIT_STATE_ON;
+			break;
+		case LIMIT_STATE_ON:
+			if (!armed)
+				limit->state = LIMIT_STATE_OFF;
+			break;
+		default:
+			break;
+	}
+
+	unsigned progress;
+	uint16_t temp_pwm;
+
+	/* then set effective_pwm based on state */
+	switch (limit->state) {
+		case LIMIT_STATE_OFF:
+		case LIMIT_STATE_INIT:
+			for (unsigned i=0; i<limit->nchannels; i++) {
+				effective_pwm[i] = disarmed_pwm[i];
+			}
+			break;
+		case LIMIT_STATE_RAMP:
+
+			progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
+			for (unsigned i=0; i<limit->nchannels; i++) {
+
+				temp_pwm = output_requested[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
+				/* already follow user/controller input if higher than min_pwm */
+				effective_pwm[i] = (disarmed_pwm[i]*(10000-progress) + (temp_pwm > min_pwm[i] ? temp_pwm : min_pwm[i])*progress)/10000;
+
+			}
+			break;
+		case LIMIT_STATE_ON:
+			for (unsigned i=0; i<limit->nchannels; i++) {
+				effective_pwm[i] = output_requested[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
+			}
+			break;
+		default:
+			break;
+	}
+	return;
+}
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h
new file mode 100644
index 000000000..f8c6370e8
--- /dev/null
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.h
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pwm_limit.h
+ *
+ * Lib to limit PWM output
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#ifndef PWM_LIMIT_H_
+#define PWM_LIMIT_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+__BEGIN_DECLS
+
+/*
+ * time for the ESCs to initialize
+ * (this is not actually needed if PWM is sent right after boot)
+ */
+#define INIT_TIME_US 500000
+/*
+ * time to slowly ramp up the ESCs
+ */
+#define RAMP_TIME_US 2500000
+
+typedef struct {
+	enum {
+		LIMIT_STATE_OFF = 0,
+		LIMIT_STATE_INIT,
+		LIMIT_STATE_RAMP,
+		LIMIT_STATE_ON
+	} state;
+	uint64_t time_armed;
+	unsigned nchannels;
+} pwm_limit_t;
+
+__EXPORT void pwm_limit_init(pwm_limit_t *limit);
+
+__EXPORT void pwm_limit_calc(const bool armed, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output_requested, uint16_t *effective_pwm, pwm_limit_t *limit);
+
+
+__END_DECLS
+
+#endif /* PWM_LIMIT_H_ */