fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 06:11:35 +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/drivers/linux_pwm_out/CMakeLists.txt
+1 -1
View File
@@ -42,6 +42,6 @@ px4_add_module(
ocpoc_mmap.cpp ocpoc_mmap.cpp
bbblue_pwm_rc.cpp bbblue_pwm_rc.cpp
DEPENDS DEPENDS
pwm_limit output_limit
) )
src/drivers/linux_pwm_out/linux_pwm_out.cpp
+14 -14
View File
@@ -54,7 +54,7 @@
#include <lib/mixer/mixer.h> #include <lib/mixer/mixer.h>
#include <lib/mixer/mixer_load.h> #include <lib/mixer/mixer_load.h>
#include <parameters/param.h> #include <parameters/param.h>
#include <pwm_limit/pwm_limit.h> #include <output_limit/output_limit.h>
#include <perf/perf_counter.h> #include <perf/perf_counter.h>
#include "common.h" #include "common.h"
@@ -98,7 +98,7 @@ px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
uint32_t _groups_required = 0; uint32_t _groups_required = 0;
uint32_t _groups_subscribed = 0; uint32_t _groups_subscribed = 0;
pwm_limit_t _pwm_limit; output_limit_t _pwm_limit;
// esc parameters // esc parameters
int32_t _pwm_disarmed; int32_t _pwm_disarmed;
@@ -268,7 +268,7 @@ void task_main(int argc, char *argv[])
_armed.armed = false; _armed.armed = false;
_armed.prearmed = false; _armed.prearmed = false;
pwm_limit_init(&_pwm_limit); output_limit_init(&_pwm_limit);
while (!_task_should_exit) { while (!_task_should_exit) {
@@ -332,7 +332,7 @@ void task_main(int argc, char *argv[])
} }
/* Switch off the PWM limit ramp for the calibration. */ /* Switch off the PWM limit ramp for the calibration. */
_pwm_limit.state = PWM_LIMIT_STATE_ON; _pwm_limit.state = OUTPUT_LIMIT_STATE_ON;
} }
if (_mixer_group != nullptr) { if (_mixer_group != nullptr) {
@@ -358,16 +358,16 @@ void task_main(int argc, char *argv[])
uint16_t pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS]; uint16_t pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];
// TODO FIXME: pre-armed seems broken // TODO FIXME: pre-armed seems broken
pwm_limit_calc(_armed.armed, output_limit_calc(_armed.armed,
false/*_armed.prearmed*/, false/*_armed.prearmed*/,
_outputs.noutputs, _outputs.noutputs,
reverse_mask, reverse_mask,
disarmed_pwm, disarmed_pwm,
min_pwm, min_pwm,
max_pwm, max_pwm,
_outputs.output, _outputs.output,
pwm, pwm,
&_pwm_limit); &_pwm_limit);
if (_armed.lockdown || _armed.manual_lockdown) { if (_armed.lockdown || _armed.manual_lockdown) {
pwm_out->send_output_pwm(disarmed_pwm, _outputs.noutputs); pwm_out->send_output_pwm(disarmed_pwm, _outputs.noutputs);
src/drivers/px4fmu/CMakeLists.txt
+1 -1
View File
@@ -41,5 +41,5 @@ px4_add_module(
circuit_breaker circuit_breaker
mixer mixer
mixer_module mixer_module
pwm_limit output_limit
) )
src/drivers/px4fmu/fmu.cpp
-1
View File
@@ -53,7 +53,6 @@
#include <lib/mixer_module/mixer_module.hpp> #include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h> #include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <lib/pwm_limit/pwm_limit.h>
#include <px4_config.h> #include <px4_config.h>
#include <px4_getopt.h> #include <px4_getopt.h>
#include <px4_log.h> #include <px4_log.h>
src/drivers/snapdragon_pwm_out/CMakeLists.txt
+1 -1
View File
@@ -38,5 +38,5 @@ px4_add_module(
snapdragon_pwm_out.cpp snapdragon_pwm_out.cpp
DEPENDS DEPENDS
mixer mixer
pwm_limit output_limit
) )
src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp
+6 -6
View File
@@ -53,7 +53,7 @@
#include <mixer/mixer_multirotor_normalized.generated.h> #include <mixer/mixer_multirotor_normalized.generated.h>
#include <parameters/param.h> #include <parameters/param.h>
#include <perf/perf_counter.h> #include <perf/perf_counter.h>
#include <pwm_limit/pwm_limit.h> #include <output_limit/output_limit.h>
#include <dev_fs_lib_pwm.h> #include <dev_fs_lib_pwm.h>
/* /*
@@ -105,7 +105,7 @@ px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
uint32_t _groups_required = 0; uint32_t _groups_required = 0;
// limit for pwm // limit for pwm
pwm_limit_t _pwm_limit; output_limit_t _pwm_limit;
// esc parameters // esc parameters
int32_t _pwm_disarmed; int32_t _pwm_disarmed;
@@ -364,7 +364,7 @@ void task_main(int argc, char *argv[])
_armed.prearmed = false; _armed.prearmed = false;
// set max min pwm // set max min pwm
pwm_limit_init(&_pwm_limit); output_limit_init(&_pwm_limit);
_perf_control_latency = perf_alloc(PC_ELAPSED, "snapdragon_pwm_out control latency"); _perf_control_latency = perf_alloc(PC_ELAPSED, "snapdragon_pwm_out control latency");
@@ -439,9 +439,9 @@ void task_main(int argc, char *argv[])
// TODO FIXME: pre-armed seems broken -> copied and pasted from pwm_out_rc_in: needs to be tested // TODO FIXME: pre-armed seems broken -> copied and pasted from pwm_out_rc_in: needs to be tested
pwm_limit_calc(_armed.armed, output_limit_calc(_armed.armed,
false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm, false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm,
min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit); min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit);
// send and publish outputs // send and publish outputs
if (_armed.lockdown || _armed.manual_lockdown || timeout) { if (_armed.lockdown || _armed.manual_lockdown || timeout) {
src/drivers/tap_esc/CMakeLists.txt
-1
View File
@@ -40,6 +40,5 @@ px4_add_module(
tap_esc_common.cpp tap_esc_common.cpp
DEPENDS DEPENDS
mixer mixer
pwm_limit
) )
src/drivers/tap_esc/tap_esc.cpp
-1
View File
@@ -60,7 +60,6 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h> #include <drivers/drv_mixer.h>
#include <mixer/mixer.h> #include <mixer/mixer.h>
#include <pwm_limit/pwm_limit.h>
#include "tap_esc_common.h" #include "tap_esc_common.h"
src/lib/CMakeLists.txt
+1 -1
View File
@@ -52,9 +52,9 @@ add_subdirectory(led)
add_subdirectory(mathlib) add_subdirectory(mathlib)
add_subdirectory(mixer) add_subdirectory(mixer)
add_subdirectory(mixer_module) add_subdirectory(mixer_module)
add_subdirectory(output_limit)
add_subdirectory(perf) add_subdirectory(perf)
add_subdirectory(pid) add_subdirectory(pid)
add_subdirectory(pwm_limit)
add_subdirectory(rc) add_subdirectory(rc)
add_subdirectory(systemlib) add_subdirectory(systemlib)
add_subdirectory(terrain_estimation) 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), _interface(interface),
_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")) _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency"))
{ {
/* initialize PWM limit lib */ output_limit_init(&_output_limit);
pwm_limit_init(&_pwm_limit); _output_limit.ramp_up = ramp_up;
_pwm_limit.ramp_up = ramp_up;
/* Safely initialize armed flags */ /* Safely initialize armed flags */
_armed.armed = false; _armed.armed = false;
@@ -252,8 +251,8 @@ bool MixingOutput::update()
/* except thrust to maximum. */ /* except thrust to maximum. */
_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; _controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
/* Switch off the PWM limit ramp for the calibration. */ /* Switch off the output limit ramp for the calibration. */
_pwm_limit.state = PWM_LIMIT_STATE_ON; _output_limit.state = OUTPUT_LIMIT_STATE_ON;
} }
} }
} }
@@ -262,11 +261,11 @@ bool MixingOutput::update()
float outputs[MAX_ACTUATORS] {}; float outputs[MAX_ACTUATORS] {};
const unsigned mixed_num_outputs = _mixers->mix(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] {}; uint16_t output_limited[MAX_ACTUATORS] {};
pwm_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask, output_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask,
_disarmed_value, _min_value, _max_value, outputs, output_limited, &_pwm_limit); _disarmed_value, _min_value, _max_value, outputs, output_limited, &_output_limit);
/* overwrite outputs in case of force_failsafe with _failsafe_value values */ /* overwrite outputs in case of force_failsafe with _failsafe_value values */
if (_armed.force_failsafe) { 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 */ /* 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 || if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) { 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 <board_config.h>
#include <lib/mixer/mixer.h> #include <lib/mixer/mixer.h>
#include <lib/perf/perf_counter.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_atomic.h>
#include <px4_module_params.h> #include <px4_module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
@@ -177,8 +177,8 @@ private:
Command _command; ///< incoming commands (from another thread) Command _command; ///< incoming commands (from another thread)
/** /**
* Reorder PWM outputs according to _param_mot_ordering * Reorder outputs according to _param_mot_ordering
* @param values PWM values to reorder * @param values values to reorder
*/ */
inline void reorderOutputs(uint16_t values[MAX_ACTUATORS]); inline void reorderOutputs(uint16_t values[MAX_ACTUATORS]);
@@ -192,7 +192,7 @@ private:
uint16_t _min_value[MAX_ACTUATORS] {}; uint16_t _min_value[MAX_ACTUATORS] {};
uint16_t _max_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 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 _armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _safety_sub{ORB_ID(safety)}; 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 @@
* *
****************************************************************************/ ****************************************************************************/
/** #include "output_limit.h"
* @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 <px4_defines.h> #include <px4_defines.h>
#include <math.h> #include <math.h>
@@ -50,21 +41,21 @@
#define PROGRESS_INT_SCALING 10000 #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->time_armed = 0;
limit->ramp_up = true; limit->ramp_up = true;
} }
void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, void output_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 uint16_t *disarmed_output, const uint16_t *min_output, const uint16_t *max_output,
const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) const float *output, uint16_t *effective_output, output_limit_t *limit)
{ {
/* first evaluate state changes */ /* first evaluate state changes */
switch (limit->state) { switch (limit->state) {
case PWM_LIMIT_STATE_INIT: case OUTPUT_LIMIT_STATE_INIT:
if (armed) { 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) { if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
limit->state = PWM_LIMIT_STATE_OFF; limit->state = OUTPUT_LIMIT_STATE_OFF;
} }
} }
break; break;
case PWM_LIMIT_STATE_OFF: case OUTPUT_LIMIT_STATE_OFF:
if (armed) { if (armed) {
if (limit->ramp_up) { if (limit->ramp_up) {
limit->state = PWM_LIMIT_STATE_RAMP; limit->state = OUTPUT_LIMIT_STATE_RAMP;
} else { } else {
limit->state = PWM_LIMIT_STATE_ON; limit->state = OUTPUT_LIMIT_STATE_ON;
} }
/* reset arming time, used for ramp timing */ /* 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; break;
case PWM_LIMIT_STATE_RAMP: case OUTPUT_LIMIT_STATE_RAMP:
if (!armed) { 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) { } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
limit->state = PWM_LIMIT_STATE_ON; limit->state = OUTPUT_LIMIT_STATE_ON;
} }
break; break;
case PWM_LIMIT_STATE_ON: case OUTPUT_LIMIT_STATE_ON:
if (!armed) { if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF; limit->state = OUTPUT_LIMIT_STATE_OFF;
} }
break; 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; unsigned local_limit_state = limit->state;
if (pre_armed) { if (pre_armed) {
local_limit_state = PWM_LIMIT_STATE_ON; local_limit_state = OUTPUT_LIMIT_STATE_ON;
} }
unsigned progress; unsigned progress;
/* then set effective_pwm based on state */ /* then set effective_output based on state */
switch (local_limit_state) { switch (local_limit_state) {
case PWM_LIMIT_STATE_OFF: case OUTPUT_LIMIT_STATE_OFF:
case PWM_LIMIT_STATE_INIT: case OUTPUT_LIMIT_STATE_INIT:
for (unsigned i = 0; i < num_channels; i++) { for (unsigned i = 0; i < num_channels; i++) {
effective_pwm[i] = disarmed_pwm[i]; effective_output[i] = disarmed_output[i];
} }
break; break;
case PWM_LIMIT_STATE_RAMP: { case OUTPUT_LIMIT_STATE_RAMP: {
hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; 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 */ /* check for invalid / disabled channels */
if (!PX4_ISFINITE(control_value)) { if (!PX4_ISFINITE(control_value)) {
effective_pwm[i] = disarmed_pwm[i]; effective_output[i] = disarmed_output[i];
continue; continue;
} }
uint16_t ramp_min_pwm; uint16_t ramp_min_output;
/* if a disarmed pwm value was set, blend between disarmed and min */ /* if a disarmed output value was set, blend between disarmed and min */
if (disarmed_pwm[i] > 0) { if (disarmed_output[i] > 0) {
/* safeguard against overflows */ /* safeguard against overflows */
unsigned disarmed = disarmed_pwm[i]; unsigned disarmed = disarmed_output[i];
if (disarmed > min_pwm[i]) { if (disarmed > min_output[i]) {
disarmed = min_pwm[i]; disarmed = min_output[i];
} }
unsigned disarmed_min_diff = min_pwm[i] - disarmed; unsigned disarmed_min_diff = min_output[i] - disarmed;
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; ramp_min_output = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING;
} else { } else {
/* no disarmed pwm value set, choose min pwm */ /* no disarmed output value set, choose min output */
ramp_min_pwm = min_pwm[i]; ramp_min_output = min_output[i];
} }
if (reverse_mask & (1 << i)) { if (reverse_mask & (1 << i)) {
control_value = -1.0f * control_value; 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 */ /* last line of defense against invalid inputs */
if (effective_pwm[i] < ramp_min_pwm) { if (effective_output[i] < ramp_min_output) {
effective_pwm[i] = ramp_min_pwm; effective_output[i] = ramp_min_output;
} else if (effective_pwm[i] > max_pwm[i]) { } else if (effective_output[i] > max_output[i]) {
effective_pwm[i] = max_pwm[i]; effective_output[i] = max_output[i];
} }
} }
} }
break; break;
case PWM_LIMIT_STATE_ON: case OUTPUT_LIMIT_STATE_ON:
for (unsigned i = 0; i < num_channels; i++) { 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 */ /* check for invalid / disabled channels */
if (!PX4_ISFINITE(control_value)) { if (!PX4_ISFINITE(control_value)) {
effective_pwm[i] = disarmed_pwm[i]; effective_output[i] = disarmed_output[i];
continue; 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; 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 */ /* last line of defense against invalid inputs */
if (effective_pwm[i] < min_pwm[i]) { if (effective_output[i] < min_output[i]) {
effective_pwm[i] = min_pwm[i]; effective_output[i] = min_output[i];
} else if (effective_pwm[i] > max_pwm[i]) { } else if (effective_output[i] > max_output[i]) {
effective_pwm[i] = max_pwm[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> * @author Julian Oes <julian@px4.io>
*/ */
#ifndef PWM_LIMIT_H_ #pragma once
#define PWM_LIMIT_H_
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
@@ -49,7 +48,7 @@ __BEGIN_DECLS
/* /*
* time for the ESCs to initialize * 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 #define INIT_TIME_US 50000
/* /*
@@ -57,26 +56,25 @@ __BEGIN_DECLS
*/ */
#define RAMP_TIME_US 500000 #define RAMP_TIME_US 500000
enum pwm_limit_state { enum output_limit_state {
PWM_LIMIT_STATE_OFF = 0, OUTPUT_LIMIT_STATE_OFF = 0,
PWM_LIMIT_STATE_INIT, OUTPUT_LIMIT_STATE_INIT,
PWM_LIMIT_STATE_RAMP, OUTPUT_LIMIT_STATE_RAMP,
PWM_LIMIT_STATE_ON OUTPUT_LIMIT_STATE_ON
}; };
typedef struct { typedef struct {
enum pwm_limit_state state; enum output_limit_state state;
uint64_t time_armed; uint64_t time_armed;
bool ramp_up; ///< if true, motors will ramp up from disarmed to min_pwm after arming bool ramp_up; ///< if true, motors will ramp up from disarmed to min_output after arming
} pwm_limit_t; } 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, __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_pwm, const uint16_t reverse_mask, const uint16_t *disarmed_output,
const uint16_t *min_pwm, const uint16_t *max_pwm, const uint16_t *min_output, const uint16_t *max_output,
const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); const float *output, uint16_t *effective_output, output_limit_t *limit);
__END_DECLS __END_DECLS
#endif /* PWM_LIMIT_H_ */
src/modules/px4iofirmware/CMakeLists.txt
+1 -1
View File
@@ -52,5 +52,5 @@ target_link_libraries(px4iofirmware
mixer mixer
rc rc
perf perf
pwm_limit output_limit
) )
src/modules/px4iofirmware/mixer.cpp
+4 -4
View File
@@ -52,7 +52,7 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <mixer/mixer.h> #include <mixer/mixer.h>
#include <pwm_limit/pwm_limit.h> #include <output_limit/output_limit.h>
#include <rc/sbus.h> #include <rc/sbus.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
@@ -311,8 +311,8 @@ mixer_tick(void)
mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits); mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
/* the pwm limit call takes care of out of band errors */ /* the pwm limit call takes care of out of band errors */
pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, output_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
/* clamp unused outputs to zero */ /* clamp unused outputs to zero */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
@@ -482,7 +482,7 @@ mixer_callback(uintptr_t handle,
} }
/* motor spinup phase - lock throttle to zero */ /* motor spinup phase - lock throttle to zero */
if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if ((pwm_limit.state == OUTPUT_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) { control_index == actuator_controls_s::INDEX_THROTTLE) {
src/modules/px4iofirmware/px4io.c
+3 -3
View File
@@ -55,7 +55,7 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <perf/perf_counter.h> #include <perf/perf_counter.h>
#include <pwm_limit/pwm_limit.h> #include <output_limit/output_limit.h>
#include <stm32_uart.h> #include <stm32_uart.h>
@@ -68,7 +68,7 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call; static struct hrt_call serial_dma_call;
pwm_limit_t pwm_limit; output_limit_t pwm_limit;
float dt; float dt;
@@ -338,7 +338,7 @@ user_start(int argc, char *argv[])
syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */ /* initialize PWM limit lib */
pwm_limit_init(&pwm_limit); output_limit_init(&pwm_limit);
/* /*
* P O L I C E L I G H T S * P O L I C E L I G H T S
src/modules/px4iofirmware/px4io.h
+2 -2
View File
@@ -50,7 +50,7 @@
#include "protocol.h" #include "protocol.h"
#include <pwm_limit/pwm_limit.h> #include <output_limit/output_limit.h>
/* /*
hotfix: we are critically short of memory in px4io and this is the hotfix: we are critically short of memory in px4io and this is the
@@ -160,7 +160,7 @@ extern bool update_trims;
/* /*
* PWM limit structure * PWM limit structure
*/ */
extern pwm_limit_t pwm_limit; extern output_limit_t pwm_limit;
/* /*
* GPIO handling. * GPIO handling.
src/modules/systemlib/param/param_new.c
+1321
View File
File diff suppressed because it is too large Load Diff
src/modules/vtol_att_control/CMakeLists.txt
-2
View File
@@ -39,7 +39,5 @@ px4_add_module(
vtol_type.cpp vtol_type.cpp
tailsitter.cpp tailsitter.cpp
standard.cpp standard.cpp
DEPENDS
pwm_limit
) )
src/systemcmds/tests/CMakeLists.txt
+1 -1
View File
@@ -102,7 +102,7 @@ px4_add_module(
DEPENDS DEPENDS
git_ecl git_ecl
ecl_geo_lookup # TODO: move this ecl_geo_lookup # TODO: move this
pwm_limit output_limit
version version
) )
src/systemcmds/tests/test_mixer.cpp
+14 -14
View File
@@ -45,7 +45,7 @@
#include <px4_config.h> #include <px4_config.h>
#include <mixer/mixer.h> #include <mixer/mixer.h>
#include <mixer/mixer_load.h> #include <mixer/mixer_load.h>
#include <pwm_limit/pwm_limit.h> #include <output_limit/output_limit.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <px4iofirmware/mixer.h> #include <px4iofirmware/mixer.h>
@@ -349,9 +349,9 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade
bool MixerTest::mixerTest() bool MixerTest::mixerTest()
{ {
/* /*
* PWM limit structure * Output limit structure
*/ */
pwm_limit_t pwm_limit; output_limit_t output_limit;
bool should_arm = false; bool should_arm = false;
uint16_t r_page_servo_disarmed[output_max]; uint16_t r_page_servo_disarmed[output_max];
uint16_t r_page_servo_control_min[output_max]; uint16_t r_page_servo_control_min[output_max];
@@ -372,7 +372,7 @@ bool MixerTest::mixerTest()
unsigned mixed; unsigned mixed;
const int jmax = 5; const int jmax = 5;
pwm_limit_init(&pwm_limit); output_limit_init(&output_limit);
/* run through arming phase */ /* run through arming phase */
for (unsigned i = 0; i < output_max; i++) { for (unsigned i = 0; i < output_max; i++) {
@@ -388,8 +388,8 @@ bool MixerTest::mixerTest()
should_prearm = true; should_prearm = true;
mixed = mixer_group.mix(&outputs[0], output_max); mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); r_page_servo_control_max, outputs, r_page_servos, &output_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max); //warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) { for (unsigned i = 0; i < mixed; i++) {
@@ -429,8 +429,8 @@ bool MixerTest::mixerTest()
/* mix */ /* mix */
mixed = mixer_group.mix(&outputs[0], output_max); mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); r_page_servo_control_max, outputs, r_page_servos, &output_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max); //warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) { for (unsigned i = 0; i < mixed; i++) {
@@ -473,8 +473,8 @@ bool MixerTest::mixerTest()
/* mix */ /* mix */
mixed = mixer_group.mix(&outputs[0], output_max); mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); r_page_servo_control_max, outputs, r_page_servos, &output_limit);
//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max); //fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
@@ -501,8 +501,8 @@ bool MixerTest::mixerTest()
/* mix */ /* mix */
mixed = mixer_group.mix(&outputs[0], output_max); mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); r_page_servo_control_max, outputs, r_page_servos, &output_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max); //warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) { for (unsigned i = 0; i < mixed; i++) {
@@ -538,8 +538,8 @@ bool MixerTest::mixerTest()
/* mix */ /* mix */
mixed = mixer_group.mix(&outputs[0], output_max); mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, output_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); r_page_servo_control_max, outputs, r_page_servos, &output_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max); //warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) { for (unsigned i = 0; i < mixed; i++) {