aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/px4iofirmware/mixer.cpp139
-rw-r--r--src/modules/px4iofirmware/module.mk1
-rw-r--r--src/modules/px4iofirmware/protocol.h4
-rw-r--r--src/modules/px4iofirmware/px4io.c6
-rw-r--r--src/modules/px4iofirmware/px4io.h9
-rw-r--r--src/modules/px4iofirmware/registers.c111
-rw-r--r--src/modules/systemlib/pwm_limit/pwm_limit.c133
-rw-r--r--src/modules/systemlib/pwm_limit/pwm_limit.h77
-rw-r--r--src/modules/uORB/topics/actuator_outputs.h2
9 files changed, 320 insertions, 162 deletions
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 30aff7d20..05897b4ce 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -47,6 +47,7 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
#include <systemlib/mixer/mixer.h>
extern "C" {
@@ -59,12 +60,6 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
-/*
- * Time that the ESCs need to initialize
- */
- #define ESC_INIT_TIME_US 1000000
- #define ESC_RAMP_TIME_US 2000000
-
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
@@ -76,15 +71,6 @@ extern "C" {
static bool mixer_servos_armed = false;
static bool should_arm = false;
static bool should_always_enable_pwm = false;
-static uint64_t esc_init_time;
-
-enum esc_state_e {
- ESC_OFF,
- ESC_INIT,
- ESC_RAMP,
- ESC_ON
-};
-static esc_state_e esc_state;
/* selected control values and count for mixing */
enum mixer_source {
@@ -166,6 +152,30 @@ mixer_tick(void)
}
/*
+ * Decide whether the servos should be armed right now.
+ *
+ * We must be armed, and we must have a PWM source; either raw from
+ * FMU or from the mixer.
+ *
+ * XXX correct behaviour for failsafe may require an additional case
+ * here.
+ */
+ should_arm = (
+ /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+ /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+ /* and FMU is armed */ && (
+ ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+ /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+ /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+ /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+ )
+ );
+
+ should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+ && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+ && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+ /*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
@@ -184,107 +194,15 @@ mixer_tick(void)
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
- uint16_t ramp_promille;
-
- /* update esc init state, but only if we are truely armed and not just PWM enabled */
- if (mixer_servos_armed && should_arm) {
-
- switch (esc_state) {
-
- /* after arming, some ESCs need an initalization period, count the time from here */
- case ESC_OFF:
- esc_init_time = hrt_absolute_time();
- esc_state = ESC_INIT;
- break;
-
- /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
- case ESC_INIT:
- if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
- esc_state = ESC_RAMP;
- }
- break;
-
- /* then ramp until the min speed is reached */
- case ESC_RAMP:
- if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
- esc_state = ESC_ON;
- }
- break;
-
- case ESC_ON:
- default:
-
- break;
- }
- } else {
- esc_state = ESC_OFF;
- }
-
- /* do the calculations during the ramp for all at once */
- if (esc_state == ESC_RAMP) {
- ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
- }
-
-
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
- /* scale to PWM and update the servo outputs as required */
- for (unsigned i = 0; i < mixed; i++) {
-
- /* save actuator values for FMU readback */
- r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
-
- switch (esc_state) {
- case ESC_INIT:
- r_page_servos[i] = (outputs[i] * 600 + 1500);
- break;
-
- case ESC_RAMP:
- r_page_servos[i] = (outputs[i]
- * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
- + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
- break;
-
- case ESC_ON:
- 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);
- break;
+ pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
- case ESC_OFF:
- default:
- break;
- }
- }
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
- /*
- * Decide whether the servos should be armed right now.
- *
- * We must be armed, and we must have a PWM source; either raw from
- * FMU or from the mixer.
- *
- * XXX correct behaviour for failsafe may require an additional case
- * here.
- */
- should_arm = (
- /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
- /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
- /* and FMU is armed */ && (
- ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
- /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
- /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
- /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
- )
- );
-
- should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
- && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
- && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
-
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
@@ -298,7 +216,6 @@ mixer_tick(void)
mixer_servos_armed = false;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
isr_debug(5, "> PWM disabled");
-
}
if (mixer_servos_armed && should_arm) {
@@ -307,9 +224,9 @@ mixer_tick(void)
up_pwm_servo_set(i, r_page_servos[i]);
} else if (mixer_servos_armed && should_always_enable_pwm) {
- /* set the idle servo outputs. */
+ /* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
- up_pwm_servo_set(i, r_page_servo_idle[i]);
+ up_pwm_servo_set(i, r_page_servo_disarmed[i]);
}
}
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 59f470a94..01869569f 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -14,6 +14,7 @@ SRCS = adc.c \
../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \
../systemlib/mixer/mixer_simple.cpp \
+ ../systemlib/pwm_limit/pwm_limit.c
ifeq ($(BOARD),px4io-v1)
SRCS += i2c.c
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 0e2cd1689..5e5396782 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -220,8 +220,8 @@ enum { /* DSM bind states */
/* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
-/* PWM idle values that are active, even when SAFETY_SAFE */
-#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+/* PWM disarmed values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index e28106971..ff9eecd74 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -50,6 +50,7 @@
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
+#include <systemlib/pwm_limit/pwm_limit.h>
#include <stm32_uart.h>
@@ -64,6 +65,8 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call;
+pwm_limit_t pwm_limit;
+
/*
* a set of debug buffers to allow us to send debug information from ISRs
*/
@@ -171,6 +174,9 @@ user_start(int argc, char *argv[])
struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
+ /* initialize PWM limit lib */
+ pwm_limit_init(&pwm_limit);
+
#if 0
/* not enough memory, lock down */
if (minfo.mxordblk < 500) {
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 66c4ca906..4fea0288c 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -46,6 +46,8 @@
#include "protocol.h"
+#include <systemlib/pwm_limit/pwm_limit.h>
+
/*
* Constants and limits.
*/
@@ -80,7 +82,7 @@ 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 */
-extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */
+extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
/*
* Register aliases.
@@ -123,6 +125,11 @@ struct sys_state_s {
extern struct sys_state_s system_state;
/*
+ * PWM limit structure
+ */
+extern pwm_limit_t pwm_limit;
+
+/*
* GPIO handling.
*/
#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9d9ef7c6d..40597adf1 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -199,7 +199,7 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI
*
* Disable pulses as default.
*/
-uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
+uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
/**
* PAGE 106
@@ -207,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
* minimum PWM values when armed
*
*/
-uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN };
/**
* PAGE 107
@@ -215,15 +215,15 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 90
* maximum PWM values when armed
*
*/
-uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
+uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX };
/**
* PAGE 108
*
- * idle PWM values for difficult ESCs
+ * disarmed PWM values for difficult ESCs
*
*/
-uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
@@ -276,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
- /* XXX range-check value? */
- r_page_servo_failsafe[offset] = *values;
+ if (*values == 0) {
+ /* ignore 0 */
+ } else if (*values < PWM_MIN) {
+ r_page_servo_failsafe[offset] = PWM_MIN;
+ } else if (*values > PWM_MAX) {
+ r_page_servo_failsafe[offset] = PWM_MAX;
+ } else {
+ r_page_servo_failsafe[offset] = *values;
+ }
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
@@ -293,16 +300,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
- if (*values == 0)
- /* set to default */
- r_page_servo_control_min[offset] = 900;
-
- else if (*values > 1200)
- r_page_servo_control_min[offset] = 1200;
- else if (*values < 900)
- r_page_servo_control_min[offset] = 900;
- else
+ if (*values == 0) {
+ /* ignore 0 */
+ } else if (*values > PWM_HIGHEST_MIN) {
+ r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
+ } else if (*values < PWM_MIN) {
+ r_page_servo_control_min[offset] = PWM_MIN;
+ } else {
r_page_servo_control_min[offset] = *values;
+ }
offset++;
num_values--;
@@ -315,16 +321,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
- if (*values == 0)
- /* set to default */
- r_page_servo_control_max[offset] = 2100;
-
- else if (*values > 2100)
- r_page_servo_control_max[offset] = 2100;
- else if (*values < 1800)
- r_page_servo_control_max[offset] = 1800;
- else
+ if (*values == 0) {
+ /* ignore 0 */
+ } else if (*values > PWM_MAX) {
+ r_page_servo_control_max[offset] = PWM_MAX;
+ } else if (*values < PWM_LOWEST_MAX) {
+ r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
+ } else {
r_page_servo_control_max[offset] = *values;
+ }
offset++;
num_values--;
@@ -332,28 +337,40 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
}
break;
- case PX4IO_PAGE_IDLE_PWM:
-
- /* copy channel data */
- while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
-
- if (*values == 0)
- /* set to default */
- r_page_servo_idle[offset] = 0;
-
- else if (*values < 900)
- r_page_servo_idle[offset] = 900;
- else if (*values > 2100)
- r_page_servo_idle[offset] = 2100;
- else
- r_page_servo_idle[offset] = *values;
+ case PX4IO_PAGE_DISARMED_PWM:
+ {
+ /* flag for all outputs */
+ bool all_disarmed_off = true;
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0) {
+ /* 0 means disabling always PWM */
+ r_page_servo_disarmed[offset] = 0;
+ } else if (*values < PWM_MIN) {
+ r_page_servo_disarmed[offset] = PWM_MIN;
+ all_disarmed_off = false;
+ } else if (*values > PWM_MAX) {
+ r_page_servo_disarmed[offset] = PWM_MAX;
+ all_disarmed_off = false;
+ } else {
+ r_page_servo_disarmed[offset] = *values;
+ all_disarmed_off = false;
+ }
- /* flag the failsafe values as custom */
- r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+ offset++;
+ num_values--;
+ values++;
+ }
- offset++;
- num_values--;
- values++;
+ if (all_disarmed_off) {
+ /* disable PWM output if disarmed */
+ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
+ } else {
+ /* enable PWM output always */
+ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+ }
}
break;
@@ -767,8 +784,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max);
break;
- case PX4IO_PAGE_IDLE_PWM:
- SELECT_PAGE(r_page_servo_idle);
+ case PX4IO_PAGE_DISARMED_PWM:
+ SELECT_PAGE(r_page_servo_disarmed);
break;
default:
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..cac3dc82a
--- /dev/null
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ * 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>
+
+void pwm_limit_init(pwm_limit_t *limit)
+{
+ limit->state = LIMIT_STATE_OFF;
+ limit->time_armed = 0;
+ return;
+}
+
+void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, 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<num_channels; i++) {
+ effective_pwm[i] = disarmed_pwm[i];
+ output[i] = 0.0f;
+ }
+ break;
+ case LIMIT_STATE_RAMP:
+
+ progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
+ for (unsigned i=0; i<num_channels; i++) {
+
+ uint16_t ramp_min_pwm;
+
+ /* if a disarmed pwm value was set, blend between disarmed and min */
+ if (disarmed_pwm[i] > 0) {
+
+ /* safeguard against overflows */
+ uint16_t disarmed = disarmed_pwm[i];
+ if (disarmed > min_pwm[i])
+ disarmed = min_pwm[i];
+
+ uint16_t disarmed_min_diff = min_pwm[i] - disarmed;
+ ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
+ } else {
+
+ /* no disarmed pwm value set, choose min pwm */
+ ramp_min_pwm = min_pwm[i];
+ }
+
+ effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
+ output[i] = (float)progress/10000.0f * output[i];
+ }
+ break;
+ case LIMIT_STATE_ON:
+ for (unsigned i=0; i<num_channels; i++) {
+ effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
+ /* effective_output stays the same */
+ }
+ 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..9974770be
--- /dev/null
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.h
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ * 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>
+
+/*
+ * 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;
+} pwm_limit_t;
+
+__BEGIN_DECLS
+
+__EXPORT void pwm_limit_init(pwm_limit_t *limit);
+
+__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
+
+__END_DECLS
+
+#endif /* PWM_LIMIT_H_ */
diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h
index 30895ca83..446140423 100644
--- a/src/modules/uORB/topics/actuator_outputs.h
+++ b/src/modules/uORB/topics/actuator_outputs.h
@@ -60,7 +60,7 @@
struct actuator_outputs_s {
uint64_t timestamp; /**< output timestamp in us since system boot */
float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */
- int noutputs; /**< valid outputs */
+ unsigned noutputs; /**< valid outputs */
};
/**
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-03 02:44:12 +0000