fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 15:08:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Moved PWM ramp to systemlib

Browse Source
This commit is contained in:
Julian Oes
2013-10-08 17:03:57 +02:00
parent 19879432ad
commit 1b9e2af742
7 changed files with 241 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+1 -1
View File
@@ -2402,7 +2402,7 @@ px4io_main(int argc, char *argv[])
/* set channel to commandline argument or to 900 for non-provided channels */
if (argc > i + 2) {
failsafe[i] = atoi(argv[i+2]);
if (failsafe[i] < PWM_MIN || failsafe[i] > PWM_AX) {
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 {
src/modules/px4iofirmware/mixer.cpp
+31 -112
View File
@@ -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 {
@@ -165,102 +151,6 @@ mixer_tick(void)
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
}
/*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
}
} else if (source != MIX_NONE) {
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)*PWM_MAX - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*PWM_MIN)/2/1000
+ (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*PWM_MAX + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*PWM_MIN)/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;
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.
*
@@ -285,6 +175,36 @@ mixer_tick(void)
&& (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) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
}
} else if (source != MIX_NONE) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
pwm_limit.nchannels = mixed;
pwm_limit_calc(should_arm, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
@@ -298,7 +218,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) {
src/modules/px4iofirmware/module.mk
+1
View File
@@ -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
src/modules/px4iofirmware/px4io.c
+6
View File
@@ -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
*/
@@ -174,6 +177,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) {
src/modules/px4iofirmware/px4io.h
+7
View File
@@ -46,6 +46,8 @@
#include "protocol.h"
#include <systemlib/pwm_limit/pwm_limit.h>
/*
* Constants and limits.
*/
@@ -122,6 +124,11 @@ struct sys_state_s {
extern struct sys_state_s system_state;
/*
* PWM limit structure
*/
extern pwm_limit_t pwm_limit;
/*
* GPIO handling.
*/
src/modules/systemlib/pwm_limit/pwm_limit.c
+116
View File
@@ -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;
}
src/modules/systemlib/pwm_limit/pwm_limit.h
+79
View File
@@ -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_ */