mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-21 05:17:35 +08:00
pwm_limit: rename to output_limit
As there is nothing pwm-specific about it.
This commit is contained in:
@@ -42,6 +42,6 @@ px4_add_module(
|
||||
ocpoc_mmap.cpp
|
||||
bbblue_pwm_rc.cpp
|
||||
DEPENDS
|
||||
pwm_limit
|
||||
output_limit
|
||||
)
|
||||
|
||||
|
||||
@@ -54,7 +54,7 @@
|
||||
#include <lib/mixer/mixer.h>
|
||||
#include <lib/mixer/mixer_load.h>
|
||||
#include <parameters/param.h>
|
||||
#include <pwm_limit/pwm_limit.h>
|
||||
#include <output_limit/output_limit.h>
|
||||
#include <perf/perf_counter.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_subscribed = 0;
|
||||
|
||||
pwm_limit_t _pwm_limit;
|
||||
output_limit_t _pwm_limit;
|
||||
|
||||
// esc parameters
|
||||
int32_t _pwm_disarmed;
|
||||
@@ -268,7 +268,7 @@ void task_main(int argc, char *argv[])
|
||||
_armed.armed = false;
|
||||
_armed.prearmed = false;
|
||||
|
||||
pwm_limit_init(&_pwm_limit);
|
||||
output_limit_init(&_pwm_limit);
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
@@ -332,7 +332,7 @@ void task_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
/* 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) {
|
||||
@@ -358,16 +358,16 @@ void task_main(int argc, char *argv[])
|
||||
uint16_t pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS];
|
||||
|
||||
// TODO FIXME: pre-armed seems broken
|
||||
pwm_limit_calc(_armed.armed,
|
||||
false/*_armed.prearmed*/,
|
||||
_outputs.noutputs,
|
||||
reverse_mask,
|
||||
disarmed_pwm,
|
||||
min_pwm,
|
||||
max_pwm,
|
||||
_outputs.output,
|
||||
pwm,
|
||||
&_pwm_limit);
|
||||
output_limit_calc(_armed.armed,
|
||||
false/*_armed.prearmed*/,
|
||||
_outputs.noutputs,
|
||||
reverse_mask,
|
||||
disarmed_pwm,
|
||||
min_pwm,
|
||||
max_pwm,
|
||||
_outputs.output,
|
||||
pwm,
|
||||
&_pwm_limit);
|
||||
|
||||
if (_armed.lockdown || _armed.manual_lockdown) {
|
||||
pwm_out->send_output_pwm(disarmed_pwm, _outputs.noutputs);
|
||||
|
||||
@@ -41,5 +41,5 @@ px4_add_module(
|
||||
circuit_breaker
|
||||
mixer
|
||||
mixer_module
|
||||
pwm_limit
|
||||
output_limit
|
||||
)
|
||||
|
||||
@@ -53,7 +53,6 @@
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/pwm_limit/pwm_limit.h>
|
||||
#include <px4_config.h>
|
||||
#include <px4_getopt.h>
|
||||
#include <px4_log.h>
|
||||
|
||||
@@ -38,5 +38,5 @@ px4_add_module(
|
||||
snapdragon_pwm_out.cpp
|
||||
DEPENDS
|
||||
mixer
|
||||
pwm_limit
|
||||
output_limit
|
||||
)
|
||||
|
||||
@@ -53,7 +53,7 @@
|
||||
#include <mixer/mixer_multirotor_normalized.generated.h>
|
||||
#include <parameters/param.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <pwm_limit/pwm_limit.h>
|
||||
#include <output_limit/output_limit.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;
|
||||
|
||||
// limit for pwm
|
||||
pwm_limit_t _pwm_limit;
|
||||
output_limit_t _pwm_limit;
|
||||
|
||||
// esc parameters
|
||||
int32_t _pwm_disarmed;
|
||||
@@ -364,7 +364,7 @@ void task_main(int argc, char *argv[])
|
||||
_armed.prearmed = false;
|
||||
|
||||
// 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");
|
||||
|
||||
@@ -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
|
||||
pwm_limit_calc(_armed.armed,
|
||||
false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm,
|
||||
min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit);
|
||||
output_limit_calc(_armed.armed,
|
||||
false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm,
|
||||
min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit);
|
||||
|
||||
// send and publish outputs
|
||||
if (_armed.lockdown || _armed.manual_lockdown || timeout) {
|
||||
|
||||
@@ -40,6 +40,5 @@ px4_add_module(
|
||||
tap_esc_common.cpp
|
||||
DEPENDS
|
||||
mixer
|
||||
pwm_limit
|
||||
)
|
||||
|
||||
|
||||
@@ -60,7 +60,6 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_mixer.h>
|
||||
#include <mixer/mixer.h>
|
||||
#include <pwm_limit/pwm_limit.h>
|
||||
|
||||
#include "tap_esc_common.h"
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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)};
|
||||
|
||||
@@ -31,4 +31,4 @@
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(pwm_limit pwm_limit.cpp)
|
||||
px4_add_library(output_limit output_limit.cpp)
|
||||
@@ -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];
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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_ */
|
||||
@@ -52,5 +52,5 @@ target_link_libraries(px4iofirmware
|
||||
mixer
|
||||
rc
|
||||
perf
|
||||
pwm_limit
|
||||
output_limit
|
||||
)
|
||||
|
||||
@@ -52,7 +52,7 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <mixer/mixer.h>
|
||||
#include <pwm_limit/pwm_limit.h>
|
||||
#include <output_limit/output_limit.h>
|
||||
#include <rc/sbus.h>
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
@@ -311,8 +311,8 @@ mixer_tick(void)
|
||||
mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
|
||||
|
||||
/* 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,
|
||||
r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
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);
|
||||
|
||||
/* clamp unused outputs to zero */
|
||||
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 */
|
||||
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 ||
|
||||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
|
||||
control_index == actuator_controls_s::INDEX_THROTTLE) {
|
||||
|
||||
@@ -55,7 +55,7 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <perf/perf_counter.h>
|
||||
#include <pwm_limit/pwm_limit.h>
|
||||
#include <output_limit/output_limit.h>
|
||||
|
||||
#include <stm32_uart.h>
|
||||
|
||||
@@ -68,7 +68,7 @@ struct sys_state_s system_state;
|
||||
|
||||
static struct hrt_call serial_dma_call;
|
||||
|
||||
pwm_limit_t pwm_limit;
|
||||
output_limit_t pwm_limit;
|
||||
|
||||
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);
|
||||
|
||||
/* 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
|
||||
|
||||
@@ -50,7 +50,7 @@
|
||||
|
||||
#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
|
||||
@@ -160,7 +160,7 @@ extern bool update_trims;
|
||||
/*
|
||||
* PWM limit structure
|
||||
*/
|
||||
extern pwm_limit_t pwm_limit;
|
||||
extern output_limit_t pwm_limit;
|
||||
|
||||
/*
|
||||
* GPIO handling.
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -39,7 +39,5 @@ px4_add_module(
|
||||
vtol_type.cpp
|
||||
tailsitter.cpp
|
||||
standard.cpp
|
||||
DEPENDS
|
||||
pwm_limit
|
||||
)
|
||||
|
||||
|
||||
@@ -102,7 +102,7 @@ px4_add_module(
|
||||
DEPENDS
|
||||
git_ecl
|
||||
ecl_geo_lookup # TODO: move this
|
||||
pwm_limit
|
||||
output_limit
|
||||
version
|
||||
)
|
||||
|
||||
|
||||
@@ -45,7 +45,7 @@
|
||||
#include <px4_config.h>
|
||||
#include <mixer/mixer.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_pwm_output.h>
|
||||
#include <px4iofirmware/mixer.h>
|
||||
@@ -349,9 +349,9 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade
|
||||
bool MixerTest::mixerTest()
|
||||
{
|
||||
/*
|
||||
* PWM limit structure
|
||||
* Output limit structure
|
||||
*/
|
||||
pwm_limit_t pwm_limit;
|
||||
output_limit_t output_limit;
|
||||
bool should_arm = false;
|
||||
uint16_t r_page_servo_disarmed[output_max];
|
||||
uint16_t r_page_servo_control_min[output_max];
|
||||
@@ -372,7 +372,7 @@ bool MixerTest::mixerTest()
|
||||
unsigned mixed;
|
||||
const int jmax = 5;
|
||||
|
||||
pwm_limit_init(&pwm_limit);
|
||||
output_limit_init(&output_limit);
|
||||
|
||||
/* run through arming phase */
|
||||
for (unsigned i = 0; i < output_max; i++) {
|
||||
@@ -388,8 +388,8 @@ bool MixerTest::mixerTest()
|
||||
should_prearm = true;
|
||||
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,
|
||||
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
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, &output_limit);
|
||||
|
||||
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
@@ -429,8 +429,8 @@ bool MixerTest::mixerTest()
|
||||
/* mix */
|
||||
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,
|
||||
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
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, &output_limit);
|
||||
|
||||
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
@@ -473,8 +473,8 @@ bool MixerTest::mixerTest()
|
||||
/* mix */
|
||||
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,
|
||||
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
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, &output_limit);
|
||||
|
||||
//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
|
||||
|
||||
@@ -501,8 +501,8 @@ bool MixerTest::mixerTest()
|
||||
/* mix */
|
||||
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,
|
||||
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
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, &output_limit);
|
||||
|
||||
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
@@ -538,8 +538,8 @@ bool MixerTest::mixerTest()
|
||||
/* mix */
|
||||
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,
|
||||
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
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, &output_limit);
|
||||
|
||||
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
|
||||
Reference in New Issue
Block a user