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

pwm_limit: rename to output_limit

Browse Source 
As there is nothing pwm-specific about it.
This commit is contained in:
Beat Küng
2019-08-31 10:05:00 -04:00
committed by Daniel Agar
parent 1bee984bb0
commit a2ebbe9066
22 changed files with 1445 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CMakeLists.txt
+1 -1
View File
@@ -52,9 +52,9 @@ add_subdirectory(led)
add_subdirectory(mathlib)
add_subdirectory(mixer)
add_subdirectory(mixer_module)
add_subdirectory(output_limit)
add_subdirectory(perf)
add_subdirectory(pid)
add_subdirectory(pwm_limit)
add_subdirectory(rc)
add_subdirectory(systemlib)
add_subdirectory(terrain_estimation)
src/lib/mixer_module/mixer_module.cpp
+8 -9
View File
@@ -51,9 +51,8 @@ _support_esc_calibration(support_esc_calibration),
_interface(interface),
_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency"))
{
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
_pwm_limit.ramp_up = ramp_up;
output_limit_init(&_output_limit);
_output_limit.ramp_up = ramp_up;
/* Safely initialize armed flags */
_armed.armed = false;
@@ -252,8 +251,8 @@ bool MixingOutput::update()
/* except thrust to maximum. */
_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
/* Switch off the PWM limit ramp for the calibration. */
_pwm_limit.state = PWM_LIMIT_STATE_ON;
/* Switch off the output limit ramp for the calibration. */
_output_limit.state = OUTPUT_LIMIT_STATE_ON;
}
}
}
@@ -262,11 +261,11 @@ bool MixingOutput::update()
float outputs[MAX_ACTUATORS] {};
const unsigned mixed_num_outputs = _mixers->mix(outputs, MAX_ACTUATORS);
/* the PWM limit call takes care of out of band errors, NaN and constrains */
/* the output limit call takes care of out of band errors, NaN and constrains */
uint16_t output_limited[MAX_ACTUATORS] {};
pwm_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask,
_disarmed_value, _min_value, _max_value, outputs, output_limited, &_pwm_limit);
output_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask,
_disarmed_value, _min_value, _max_value, outputs, output_limited, &_output_limit);
/* overwrite outputs in case of force_failsafe with _failsafe_value values */
if (_armed.force_failsafe) {
@@ -387,7 +386,7 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8
}
/* motor spinup phase - lock throttle to zero */
if (output->_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
if (output->_output_limit.state == OUTPUT_LIMIT_STATE_RAMP) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
src/lib/mixer_module/mixer_module.hpp
+4 -4
View File
@@ -36,7 +36,7 @@
#include <board_config.h>
#include <lib/mixer/mixer.h>
#include <lib/perf/perf_counter.h>
#include <lib/pwm_limit/pwm_limit.h>
#include <lib/output_limit/output_limit.h>
#include <px4_atomic.h>
#include <px4_module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
@@ -177,8 +177,8 @@ private:
Command _command; ///< incoming commands (from another thread)
/**
* Reorder PWM outputs according to _param_mot_ordering
* @param values PWM values to reorder
* Reorder outputs according to _param_mot_ordering
* @param values values to reorder
*/
inline void reorderOutputs(uint16_t values[MAX_ACTUATORS]);
@@ -192,7 +192,7 @@ private:
uint16_t _min_value[MAX_ACTUATORS] {};
uint16_t _max_value[MAX_ACTUATORS] {};
uint16_t _reverse_output_mask{0}; ///< reverses the interval [min, max] -> [max, min], NOT motor direction
pwm_limit_t _pwm_limit;
output_limit_t _output_limit;
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
src/lib/pwm_limit/CMakeLists.txt → src/lib/output_limit/CMakeLists.txt
+1 -1
View File
@@ -31,4 +31,4 @@
#
############################################################################
px4_add_library(pwm_limit pwm_limit.cpp)
px4_add_library(output_limit output_limit.cpp)
src/lib/pwm_limit/pwm_limit.cpp → src/lib/output_limit/output_limit.cpp
+45 -54
View File
@@ -31,16 +31,7 @@
*
****************************************************************************/
/**
* @file pwm_limit.c
*
* Library for PWM output limiting
*
* @author Julian Oes <julian@px4.io>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include "pwm_limit.h"
#include "output_limit.h"
#include <px4_defines.h>
#include <math.h>
@@ -50,21 +41,21 @@
#define PROGRESS_INT_SCALING 10000
void pwm_limit_init(pwm_limit_t *limit)
void output_limit_init(output_limit_t *limit)
{
limit->state = PWM_LIMIT_STATE_INIT;
limit->state = OUTPUT_LIMIT_STATE_INIT;
limit->time_armed = 0;
limit->ramp_up = true;
}
void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask,
const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm,
const float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask,
const uint16_t *disarmed_output, const uint16_t *min_output, const uint16_t *max_output,
const float *output, uint16_t *effective_output, output_limit_t *limit)
{
/* first evaluate state changes */
switch (limit->state) {
case PWM_LIMIT_STATE_INIT:
case OUTPUT_LIMIT_STATE_INIT:
if (armed) {
@@ -74,19 +65,19 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
}
if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
limit->state = PWM_LIMIT_STATE_OFF;
limit->state = OUTPUT_LIMIT_STATE_OFF;
}
}
break;
case PWM_LIMIT_STATE_OFF:
case OUTPUT_LIMIT_STATE_OFF:
if (armed) {
if (limit->ramp_up) {
limit->state = PWM_LIMIT_STATE_RAMP;
limit->state = OUTPUT_LIMIT_STATE_RAMP;
} else {
limit->state = PWM_LIMIT_STATE_ON;
limit->state = OUTPUT_LIMIT_STATE_ON;
}
/* reset arming time, used for ramp timing */
@@ -95,19 +86,19 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
break;
case PWM_LIMIT_STATE_RAMP:
case OUTPUT_LIMIT_STATE_RAMP:
if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF;
limit->state = OUTPUT_LIMIT_STATE_OFF;
} else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
limit->state = PWM_LIMIT_STATE_ON;
limit->state = OUTPUT_LIMIT_STATE_ON;
}
break;
case PWM_LIMIT_STATE_ON:
case OUTPUT_LIMIT_STATE_ON:
if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF;
limit->state = OUTPUT_LIMIT_STATE_OFF;
}
break;
@@ -126,22 +117,22 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
unsigned local_limit_state = limit->state;
if (pre_armed) {
local_limit_state = PWM_LIMIT_STATE_ON;
local_limit_state = OUTPUT_LIMIT_STATE_ON;
}
unsigned progress;
/* then set effective_pwm based on state */
/* then set effective_output based on state */
switch (local_limit_state) {
case PWM_LIMIT_STATE_OFF:
case PWM_LIMIT_STATE_INIT:
case OUTPUT_LIMIT_STATE_OFF:
case OUTPUT_LIMIT_STATE_INIT:
for (unsigned i = 0; i < num_channels; i++) {
effective_pwm[i] = disarmed_pwm[i];
effective_output[i] = disarmed_output[i];
}
break;
case PWM_LIMIT_STATE_RAMP: {
case OUTPUT_LIMIT_STATE_RAMP: {
hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US;
@@ -156,49 +147,49 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
/* check for invalid / disabled channels */
if (!PX4_ISFINITE(control_value)) {
effective_pwm[i] = disarmed_pwm[i];
effective_output[i] = disarmed_output[i];
continue;
}
uint16_t ramp_min_pwm;
uint16_t ramp_min_output;
/* if a disarmed pwm value was set, blend between disarmed and min */
if (disarmed_pwm[i] > 0) {
/* if a disarmed output value was set, blend between disarmed and min */
if (disarmed_output[i] > 0) {
/* safeguard against overflows */
unsigned disarmed = disarmed_pwm[i];
unsigned disarmed = disarmed_output[i];
if (disarmed > min_pwm[i]) {
disarmed = min_pwm[i];
if (disarmed > min_output[i]) {
disarmed = min_output[i];
}
unsigned disarmed_min_diff = min_pwm[i] - disarmed;
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING;
unsigned disarmed_min_diff = min_output[i] - disarmed;
ramp_min_output = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING;
} else {
/* no disarmed pwm value set, choose min pwm */
ramp_min_pwm = min_pwm[i];
/* no disarmed output value set, choose min output */
ramp_min_output = min_output[i];
}
if (reverse_mask & (1 << i)) {
control_value = -1.0f * control_value;
}
effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm) / 2 + (max_pwm[i] + ramp_min_pwm) / 2;
effective_output[i] = control_value * (max_output[i] - ramp_min_output) / 2 + (max_output[i] + ramp_min_output) / 2;
/* last line of defense against invalid inputs */
if (effective_pwm[i] < ramp_min_pwm) {
effective_pwm[i] = ramp_min_pwm;
if (effective_output[i] < ramp_min_output) {
effective_output[i] = ramp_min_output;
} else if (effective_pwm[i] > max_pwm[i]) {
effective_pwm[i] = max_pwm[i];
} else if (effective_output[i] > max_output[i]) {
effective_output[i] = max_output[i];
}
}
}
break;
case PWM_LIMIT_STATE_ON:
case OUTPUT_LIMIT_STATE_ON:
for (unsigned i = 0; i < num_channels; i++) {
@@ -206,7 +197,7 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
/* check for invalid / disabled channels */
if (!PX4_ISFINITE(control_value)) {
effective_pwm[i] = disarmed_pwm[i];
effective_output[i] = disarmed_output[i];
continue;
}
@@ -214,14 +205,14 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
control_value = -1.0f * control_value;
}
effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2;
effective_output[i] = control_value * (max_output[i] - min_output[i]) / 2 + (max_output[i] + min_output[i]) / 2;
/* last line of defense against invalid inputs */
if (effective_pwm[i] < min_pwm[i]) {
effective_pwm[i] = min_pwm[i];
if (effective_output[i] < min_output[i]) {
effective_output[i] = min_output[i];
} else if (effective_pwm[i] > max_pwm[i]) {
effective_pwm[i] = max_pwm[i];
} else if (effective_output[i] > max_output[i]) {
effective_output[i] = max_output[i];
}
}
src/lib/pwm_limit/pwm_limit.h → src/lib/output_limit/output_limit.h
+17 -19
View File
@@ -32,15 +32,14 @@
****************************************************************************/
/**
* @file pwm_limit.c
* @file output_limit.h
*
* Library for PWM output limiting
* Library for output limiting (PWM for example)
*
* @author Julian Oes <julian@px4.io>
*/
#ifndef PWM_LIMIT_H_
#define PWM_LIMIT_H_
#pragma once
#include <stdint.h>
#include <stdbool.h>
@@ -49,7 +48,7 @@ __BEGIN_DECLS
/*
* time for the ESCs to initialize
* (this is not actually needed if PWM is sent right after boot)
* (this is not actually needed if the signal is sent right after boot)
*/
#define INIT_TIME_US 50000
/*
@@ -57,26 +56,25 @@ __BEGIN_DECLS
*/
#define RAMP_TIME_US 500000
enum pwm_limit_state {
PWM_LIMIT_STATE_OFF = 0,
PWM_LIMIT_STATE_INIT,
PWM_LIMIT_STATE_RAMP,
PWM_LIMIT_STATE_ON
enum output_limit_state {
OUTPUT_LIMIT_STATE_OFF = 0,
OUTPUT_LIMIT_STATE_INIT,
OUTPUT_LIMIT_STATE_RAMP,
OUTPUT_LIMIT_STATE_ON
};
typedef struct {
enum pwm_limit_state state;
enum output_limit_state state;
uint64_t time_armed;
bool ramp_up; ///< if true, motors will ramp up from disarmed to min_pwm after arming
} pwm_limit_t;
bool ramp_up; ///< if true, motors will ramp up from disarmed to min_output after arming
} output_limit_t;
__EXPORT void pwm_limit_init(pwm_limit_t *limit);
__EXPORT void output_limit_init(output_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels,
const uint16_t reverse_mask, const uint16_t *disarmed_pwm,
const uint16_t *min_pwm, const uint16_t *max_pwm,
const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__EXPORT void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels,
const uint16_t reverse_mask, const uint16_t *disarmed_output,
const uint16_t *min_output, const uint16_t *max_output,
const float *output, uint16_t *effective_output, output_limit_t *limit);
__END_DECLS
#endif /* PWM_LIMIT_H_ */