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

support up to PWM 14

Browse Source 
- generate PWM_MAIN 1-14
 - generate PWM_AUX 1-8
 - generate PWM_EXTRA 1-8
 - px4io and pwm_out directly read configuration parameters
    - only available and active physical outputs are actually shown for configuration
 - overall saves flash despite adding many new parameters
This commit is contained in:
Daniel Agar
2021-01-30 15:55:53 +01:00
committed by Lorenz Meier
parent cdd6df4e27
commit 318c7e83b3
28 changed files with 869 additions and 2812 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+168 -45
View File
@@ -425,6 +425,8 @@ private:
*/
void answer_command(const vehicle_command_s &cmd, uint8_t result);
void update_params();
/**
* check and handle test_motor topic updates
*/
@@ -1075,51 +1077,6 @@ PX4IO::task_main()
}
}
/*
* Set invert mask for PWM outputs (does not apply to S.Bus)
*/
int16_t pwm_invert_mask = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
char pname[16];
int32_t ival;
/* fill the channel reverse mask from parameters */
sprintf(pname, "PWM_MAIN_REV%u", i + 1);
param_t param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
param_get(param_h, &ival);
pwm_invert_mask |= ((int16_t)(ival != 0)) << i;
}
}
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask);
// update trim values
struct pwm_output_values pwm_values;
// memset(&pwm_values, 0, sizeof(pwm_values));
// ret = io_reg_get(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, (uint16_t *)pwm_values.values, _max_actuators);
for (unsigned i = 0; i < _max_actuators; i++) {
char pname[16];
float pval;
/* fetch the trim values from parameters */
sprintf(pname, "PWM_MAIN_TRIM%u", i + 1);
param_t param_h = param_find(pname);
if (param_h != PARAM_INVALID) {
param_get(param_h, &pval);
pwm_values.values[i] = (int16_t)(10000 * pval);
}
}
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, pwm_values.values, _max_actuators);
float param_val;
param_t parm_handle;
@@ -1211,6 +1168,8 @@ PX4IO::task_main()
param_get(parm_handle, &param_val_int);
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_AIRMODE, SIGNED_TO_REG(param_val_int));
}
update_params();
}
}
@@ -1233,6 +1192,170 @@ out:
_exit(0);
}
void PX4IO::update_params()
{
// skip update when armed
if (_armed) {
return;
}
int32_t pwm_min_default = PWM_DEFAULT_MIN;
int32_t pwm_max_default = PWM_DEFAULT_MAX;
int32_t pwm_disarmed_default = 0;
int32_t pwm_rate_default = 50;
const char *prefix = "PWM_MAIN";
param_get(param_find("PWM_MIN"), &pwm_min_default);
param_get(param_find("PWM_MAX"), &pwm_max_default);
param_get(param_find("PWM_DISARMED"), &pwm_disarmed_default);
param_get(param_find("PWM_RATE"), &pwm_rate_default);
char str[17];
// PWM_MAIN_MINx
{
pwm_output_values pwm{};
pwm.channel_count = _max_actuators;
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_MIN%u", prefix, i + 1);
int32_t pwm_min = -1;
if (param_get(param_find(str), &pwm_min) == PX4_OK) {
if (pwm_min >= 0) {
pwm.values[i] = math::constrain(pwm_min, PWM_LOWEST_MIN, PWM_HIGHEST_MIN);
if (pwm_min != pwm.values[i]) {
int32_t pwm_min_new = pwm.values[i];
param_set(param_find(str), &pwm_min_new);
}
} else {
pwm.values[i] = pwm_min_default;
}
}
}
io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm.values, pwm.channel_count);
}
// PWM_MAIN_MAXx
{
pwm_output_values pwm{};
pwm.channel_count = _max_actuators;
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_MAX%u", prefix, i + 1);
int32_t pwm_max = -1;
if (param_get(param_find(str), &pwm_max) == PX4_OK) {
if (pwm_max >= 0) {
pwm.values[i] = math::constrain(pwm_max, PWM_LOWEST_MAX, PWM_HIGHEST_MAX);
if (pwm_max != pwm.values[i]) {
int32_t pwm_max_new = pwm.values[i];
param_set(param_find(str), &pwm_max_new);
}
} else {
pwm.values[i] = pwm_max_default;
}
}
}
io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm.values, pwm.channel_count);
}
// PWM_MAIN_FAILx
{
pwm_output_values pwm{};
pwm.channel_count = _max_actuators;
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_FAIL%u", prefix, i + 1);
int32_t pwm_fail = -1;
if (param_get(param_find(str), &pwm_fail) == PX4_OK) {
if (pwm_fail >= 0) {
pwm.values[i] = math::constrain(pwm_fail, 0, PWM_HIGHEST_MAX);
if (pwm_fail != pwm.values[i]) {
int32_t pwm_fail_new = pwm.values[i];
param_set(param_find(str), &pwm_fail_new);
}
}
}
}
io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, pwm.channel_count);
}
// PWM_MAIN_DISx
{
pwm_output_values pwm{};
pwm.channel_count = _max_actuators;
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_DIS%u", prefix, i + 1);
int32_t pwm_dis = -1;
if (param_get(param_find(str), &pwm_dis) == PX4_OK) {
if (pwm_dis >= 0) {
pwm.values[i] = math::constrain(pwm_dis, 0, PWM_HIGHEST_MAX);
if (pwm_dis != pwm.values[i]) {
int32_t pwm_dis_new = pwm.values[i];
param_set(param_find(str), &pwm_dis_new);
}
} else {
pwm.values[i] = pwm_disarmed_default;
}
}
}
io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm.values, pwm.channel_count);
}
// PWM_MAIN_REVx
{
int16_t reverse_pwm_mask = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_REV%u", prefix, i + 1);
int32_t pwm_rev = -1;
if (param_get(param_find(str), &pwm_rev) == PX4_OK) {
if (pwm_rev >= 1) {
reverse_pwm_mask |= (2 << i);
}
}
}
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, reverse_pwm_mask);
}
// PWM_MAIN_TRIMx
{
uint16_t values[8] {};
for (unsigned i = 0; i < _max_actuators; i++) {
sprintf(str, "%s_TRIM%u", prefix, i + 1);
float pwm_trim = 0.f;
if (param_get(param_find(str), &pwm_trim) == PX4_OK) {
values[i] = (int16_t)(10000 * pwm_trim);
}
}
// copy the trim values to the mixer offsets
io_reg_set(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, values, _max_actuators);
}
}
int
PX4IO::io_set_control_groups()
{