dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

px4io+pwm_out: set the PWM rate and disarmed value when a channel is first set to a servo 

This should simplify the first setup a bit.
This commit is contained in:
Beat Küng 2022-07-29 14:01:35 +02:00 committed by Daniel Agar
parent f929017618
commit e7588d2da0
4 changed files with 114 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/drivers/pwm_out/PWMOut.cpp
View File

@ -494,8 +494,57 @@ void PWMOut::Run()
void PWMOut::update_params()
{
uint32_t previously_set_functions = 0;
for (size_t i = 0; i < _num_outputs; i++) {
previously_set_functions |= (uint32_t)_mixing_output.isFunctionSet(i) << i;
}
updateParams();
// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
if (_mixing_output.useDynamicMixing() && !_first_param_update) {
for (size_t i = 0; i < _num_outputs; i++) {
if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
int32_t output_function;
if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
&& output_function >= (int)OutputFunction::Servo1
&& output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
int32_t val = 1500;
PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
param_set(_mixing_output.disarmedParamHandle(i), &val);
// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
uint32_t channels = io_timer_get_group(timer);
if ((channels & (1u << i)) == 0) {
continue;
}
if ((channels & previously_set_functions) == 0) { // None of the channels was set
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
int32_t tim_config = 0;
param_t handle = param_find(param_name);
if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
tim_config = 50;
PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
param_set(handle, &tim_config);
}
}
}
}
}
}
}
_first_param_update = false;
if (_mixing_output.useDynamicMixing()) {
return;
}

1
src/drivers/pwm_out/PWMOut.hpp
View File

@ -147,6 +147,7 @@ private:
bool _pwm_on{false};
uint32_t _pwm_mask{0};
bool _pwm_initialized{false};
bool _first_param_update{true};
unsigned _num_disarmed_set{0};

63
src/drivers/px4io/px4io.cpp
View File

@ -179,6 +179,8 @@ private:
unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO
int _class_instance{-1};
bool _first_param_update{true};
uint32_t _group_channels[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {};
hrt_abstime _poll_last{0};
@ -477,6 +479,12 @@ int PX4IO::init()
return ret;
}
/* initialize _group_channels */
for (uint8_t group = PX4IO_P_SETUP_PWM_RATE_GROUP0; group <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++group) {
unsigned group_idx = group - PX4IO_P_SETUP_PWM_RATE_GROUP0;
_group_channels[group_idx] = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + group_idx);
}
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
@ -605,8 +613,6 @@ void PX4IO::Run()
_param_update_force = false;
ModuleParams::updateParams();
update_params();
/* Check if the flight termination circuit breaker has been updated */
@ -699,14 +705,67 @@ void PX4IO::updateTimerRateGroups()
void PX4IO::update_params()
{
uint32_t previously_set_functions = 0;
for (size_t i = 0; i < _max_actuators; i++) {
previously_set_functions |= (uint32_t)_mixing_output.isFunctionSet(i) << i;
}
updateParams();
if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) {
// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
if (!_first_param_update) {
for (size_t i = 0; i < _max_actuators; i++) {
if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
int32_t output_function;
if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
&& output_function >= (int)OutputFunction::Servo1
&& output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
int32_t val = 1500;
PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
param_set(_mixing_output.disarmedParamHandle(i), &val);
// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) {
uint32_t channels = _group_channels[timer];
if ((channels & (1u << i)) == 0) {
continue;
}
if ((channels & previously_set_functions) == 0) { // None of the channels was set
char param_name[17];
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
int32_t tim_config = 0;
param_t handle = param_find(param_name);
if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
tim_config = 50;
PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
param_set(handle, &tim_config);
}
}
}
}
}
}
}
// sync params to IO
updateTimerRateGroups();
updateFailsafe();
updateDisarmed();
_first_param_update = false;
return;
}
_first_param_update = false;
// skip update when armed or PWM disabled
if (_mixing_output.armed().armed || _class_instance == -1 || _mixing_output.useDynamicMixing()) {
return;

3
src/lib/mixer_module/mixer_module.hpp
View File

@ -193,6 +193,9 @@ public:
uint16_t &minValue(int index) { return _min_value[index]; }
uint16_t &maxValue(int index) { return _max_value[index]; }
param_t functionParamHandle(int index) const { return _param_handles[index].function; }
param_t disarmedParamHandle(int index) const { return _param_handles[index].disarmed; }
/**
* Returns the actual failsafe value taking into account the assigned function
*/