From b239df6889a4d9d48de7a0334e0b2c69ca237063 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 16 Jul 2018 10:32:26 -0400 Subject: [PATCH 1/3] FMU PWM parameters respect instance for MAIN/AUX usage --- src/drivers/px4fmu/fmu.cpp | 42 +++++++++++++++++++++++++++----------- 1 file changed, 30 insertions(+), 12 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 61fcdf8d5205..230240a633ca 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -509,8 +509,6 @@ PX4FMU::init() /* initialize PWM limit lib */ pwm_limit_init(&_pwm_limit); - update_pwm_rev_mask(); - #ifdef RC_SERIAL_PORT # ifdef RF_RADIO_POWER_CONTROL @@ -533,12 +531,6 @@ PX4FMU::init() // Getting initial parameter values update_params(); - for (unsigned i = 0; i < _max_actuators; i++) { - char pname[16]; - sprintf(pname, "PWM_AUX_TRIM%d", i + 1); - param_find(pname); - } - return 0; } @@ -922,15 +914,28 @@ PX4FMU::update_pwm_rev_mask() { _reverse_pwm_mask = 0; + const char *pname_format; + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + pname_format = "PWM_MAIN_REV%d"; + + } else if (_class_instance == CLASS_DEVICE_SECONDARY) { + pname_format = "PWM_AUX_REV%d"; + + } else { + PX4_ERR("PWM REV only for MAIN and AUX"); + return; + } + for (unsigned i = 0; i < _max_actuators; i++) { char pname[16]; - int32_t ival; /* fill the channel reverse mask from parameters */ - sprintf(pname, "PWM_AUX_REV%d", i + 1); + sprintf(pname, pname_format, i + 1); param_t param_h = param_find(pname); if (param_h != PARAM_INVALID) { + int32_t ival = 0; param_get(param_h, &ival); _reverse_pwm_mask |= ((int16_t)(ival != 0)) << i; } @@ -946,15 +951,28 @@ PX4FMU::update_pwm_trims() int16_t values[_max_actuators] = {}; + const char *pname_format; + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + pname_format = "PWM_MAIN_TRIM%d"; + + } else if (_class_instance == CLASS_DEVICE_SECONDARY) { + pname_format = "PWM_AUX_TRIM%d"; + + } else { + PX4_ERR("PWM TRIM only for MAIN and AUX"); + return; + } + for (unsigned i = 0; i < _max_actuators; i++) { char pname[16]; - float pval; /* fill the struct from parameters */ - sprintf(pname, "PWM_AUX_TRIM%d", i + 1); + sprintf(pname, pname_format, i + 1); param_t param_h = param_find(pname); if (param_h != PARAM_INVALID) { + float pval = 0.0f; param_get(param_h, &pval); values[i] = (int16_t)(10000 * pval); PX4_DEBUG("%s: %d", pname, values[i]); From 9099128ae84b116ee82c0d0ddc245bddf48a42c3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 16 Jul 2018 10:45:26 -0400 Subject: [PATCH 2/3] PWM parameters centralize under sensors and add aux 7&8 --- src/drivers/px4fmu/px4fmu_params.c | 132 ------ src/drivers/px4io/px4io_params.c | 176 -------- src/modules/sensors/pwm_params_aux.c | 406 ++++++++++++++++++ .../{pwm_params.c => pwm_params_main.c} | 201 +++++---- 4 files changed, 534 insertions(+), 381 deletions(-) create mode 100644 src/modules/sensors/pwm_params_aux.c rename src/modules/sensors/{pwm_params.c => pwm_params_main.c} (70%) diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c index 9d308387a10b..a33c204257cb 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -31,138 +31,6 @@ * ****************************************************************************/ -/** - * Invert direction of aux output channel 1 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_AUX_REV1, 0); - -/** - * Invert direction of aux output channel 2 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_AUX_REV2, 0); - -/** - * Invert direction of aux output channel 3 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_AUX_REV3, 0); - -/** - * Invert direction of aux output channel 4 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_AUX_REV4, 0); - -/** - * Invert direction of aux output channel 5 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_AUX_REV5, 0); - -/** - * Invert direction of aux output channel 6 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_AUX_REV6, 0); - -/** - * Trim value for FMU PWM output channel 1 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_AUX_TRIM1, 0); - -/** - * Trim value for FMU PWM output channel 2 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_AUX_TRIM2, 0); - -/** - * Trim value for FMU PWM output channel 3 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_AUX_TRIM3, 0); - -/** - * Trim value for FMU PWM output channel 4 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_AUX_TRIM4, 0); - -/** - * Trim value for FMU PWM output channel 5 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_AUX_TRIM5, 0); - -/** - * Trim value for FMU PWM output channel 6 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_AUX_TRIM6, 0); - /** * Thrust to PWM model parameter * diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index baf69802ff6b..30dfeab55310 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -55,182 +55,6 @@ */ PARAM_DEFINE_INT32(SYS_USE_IO, 1); -/** - * Invert direction of main output channel 1 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_MAIN_REV1, 0); - -/** - * Invert direction of main output channel 2 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_MAIN_REV2, 0); - -/** - * Invert direction of main output channel 3 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_MAIN_REV3, 0); - -/** - * Invert direction of main output channel 4 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_MAIN_REV4, 0); - -/** - * Invert direction of main output channel 5 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_MAIN_REV5, 0); - -/** - * Invert direction of main output channel 6 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_MAIN_REV6, 0); - -/** - * Invert direction of main output channel 7 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0); - -/** - * Invert direction of main output channel 8 - * - * Enable to invert the channel. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0); - -/** - * Trim value for main output channel 1 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM1, 0); - -/** - * Trim value for main output channel 2 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM2, 0); - -/** - * Trim value for main output channel 3 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM3, 0); - -/** - * Trim value for main output channel 4 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM4, 0); - -/** - * Trim value for main output channel 5 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM5, 0); - -/** - * Trim value for main output channel 6 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM6, 0); - -/** - * Trim value for main output channel 7 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM7, 0); - -/** - * Trim value for main output channel 8 - * - * Set to normalized offset - * - * @min -0.2 - * @max 0.2 - * @decimal 2 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM8, 0); - /** * S.BUS out * diff --git a/src/modules/sensors/pwm_params_aux.c b/src/modules/sensors/pwm_params_aux.c new file mode 100644 index 000000000000..5bf94d0f2bef --- /dev/null +++ b/src/modules/sensors/pwm_params_aux.c @@ -0,0 +1,406 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pwm_params_aux.c + * + * Parameters defined for PWM output. + * + */ + +/** + * Set the PWM output frequency for the auxiliary outputs + * + * Set to 400 for industry default or 1000 for high frequency ESCs. + * + * Set to 0 for Oneshot125. + * + * @reboot_required true + * + * @min -1 + * @max 2000 + * @unit Hz + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_RATE, 50); + +/** + * Set the minimum PWM for the auxiliary outputs + * + * Set to 1000 for industry default or 900 to increase servo travel. + * + * @reboot_required true + * + * @min 800 + * @max 1400 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000); + +/** + * Set the maximum PWM for the auxiliary outputs + * + * Set to 2000 for industry default or 2100 to increase servo travel. + * + * @reboot_required true + * + * @min 1600 + * @max 2200 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000); + +/** + * Set the disarmed PWM for auxiliary outputs + * + * This is the PWM pulse the autopilot is outputting if not armed. + * The main use of this parameter is to silence ESCs when they are disarmed. + * + * @reboot_required true + * + * @min 0 + * @max 2200 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1500); + + + + +/** + * Set the disarmed PWM for the auxiliary 1 output + * + * This is the PWM pulse the autopilot is outputting if not armed. + * When set to -1 the value for PWM_AUX_DISARMED will be used + * + * @reboot_required true + * + * @min -1 + * @max 2200 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DIS1, -1); + +/** + * Set the disarmed PWM for the auxiliary 2 output + * + * This is the PWM pulse the autopilot is outputting if not armed. + * When set to -1 the value for PWM_AUX_DISARMED will be used + * + * @reboot_required true + * + * @min -1 + * @max 2200 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DIS2, -1); + +/** + * Set the disarmed PWM for the auxiliary 3 output + * + * This is the PWM pulse the autopilot is outputting if not armed. + * When set to -1 the value for PWM_AUX_DISARMED will be used + * + * @reboot_required true + * + * @min -1 + * @max 2200 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DIS3, -1); + +/** + * Set the disarmed PWM for the auxiliary 4 output + * + * This is the PWM pulse the autopilot is outputting if not armed. + * When set to -1 the value for PWM_AUX_DISARMED will be used + * + * @reboot_required true + * + * @min -1 + * @max 2200 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DIS4, -1); + +/** + * Set the disarmed PWM for the auxiliary 5 output + * + * This is the PWM pulse the autopilot is outputting if not armed. + * When set to -1 the value for PWM_AUX_DISARMED will be used + * + * @reboot_required true + * + * @min -1 + * @max 2200 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DIS5, -1); + +/** + * Set the disarmed PWM for the auxiliary 6 output + * + * This is the PWM pulse the autopilot is outputting if not armed. + * When set to -1 the value for PWM_AUX_DISARMED will be used + * + * @reboot_required true + * + * @min -1 + * @max 2200 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DIS6, -1); + +/** + * Set the disarmed PWM for the auxiliary 7 output + * + * This is the PWM pulse the autopilot is outputting if not armed. + * When set to -1 the value for PWM_AUX_DISARMED will be used + * + * @reboot_required true + * + * @min -1 + * @max 2200 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DIS7, -1); + +/** + * Set the disarmed PWM for the auxiliary 8 output + * + * This is the PWM pulse the autopilot is outputting if not armed. + * When set to -1 the value for PWM_AUX_DISARMED will be used + * + * @reboot_required true + * + * @min -1 + * @max 2200 + * @unit us + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DIS8, -1); + + + + +/** + * Invert direction of auxiliary output channel 1 + * + * Enable to invert the channel. + * + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_REV1, 0); + +/** + * Invert direction of auxiliary output channel 2 + * + * Enable to invert the channel. + * + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_REV2, 0); + +/** + * Invert direction of auxiliary output channel 3 + * + * Enable to invert the channel. + * + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_REV3, 0); + +/** + * Invert direction of auxiliary output channel 4 + * + * Enable to invert the channel. + * + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_REV4, 0); + +/** + * Invert direction of auxiliary output channel 5 + * + * Enable to invert the channel. + * + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_REV5, 0); + +/** + * Invert direction of auxiliary output channel 6 + * + * Enable to invert the channel. + * + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_REV6, 0); + +/** + * Invert direction of auxiliary output channel 7 + * + * Enable to invert the channel. + * + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_REV7, 0); + +/** + * Invert direction of auxiliary output channel 8 + * + * Enable to invert the channel. + * + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_REV8, 0); + + + + +/** + * Trim value for auxiliary output channel 1 + * + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM1, 0); + +/** + * Trim value for auxiliary output channel 2 + * + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM2, 0); + +/** + * Trim value for auxiliary output channel 3 + * + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM3, 0); + +/** + * Trim value for auxiliary output channel 4 + * + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM4, 0); + +/** + * Trim value for auxiliary output channel 5 + * + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM5, 0); + +/** + * Trim value for auxiliary output channel 6 + * + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM6, 0); + +/** + * Trim value for auxiliary output channel 7 + * + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM7, 0); + +/** + * Trim value for auxiliary output channel 8 + * + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_AUX_TRIM8, 0); + + + diff --git a/src/modules/sensors/pwm_params.c b/src/modules/sensors/pwm_params_main.c similarity index 70% rename from src/modules/sensors/pwm_params.c rename to src/modules/sensors/pwm_params_main.c index ef921a98d434..1ed4520086eb 100644 --- a/src/modules/sensors/pwm_params.c +++ b/src/modules/sensors/pwm_params_main.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file pwm_params.c + * @file pwm_params_main.c * * Parameters defined for PWM output. * @@ -97,6 +97,9 @@ PARAM_DEFINE_INT32(PWM_MAX, 2000); */ PARAM_DEFINE_INT32(PWM_DISARMED, 900); + + + /** * Set the disarmed PWM for the main 1 output * @@ -217,135 +220,187 @@ PARAM_DEFINE_INT32(PWM_MAIN_DIS7, -1); */ PARAM_DEFINE_INT32(PWM_MAIN_DIS8, -1); + + + /** - * Set the disarmed PWM for the AUX 1 output + * Invert direction of main output channel 1 * - * This is the PWM pulse the autopilot is outputting if not armed. - * When set to -1 the value for PWM_AUX_DISARMED will be used - * - * @reboot_required true + * Enable to invert the channel. * - * @min -1 - * @max 2200 - * @unit us + * @boolean * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_DIS1, -1); +PARAM_DEFINE_INT32(PWM_MAIN_REV1, 0); /** - * Set the disarmed PWM for the AUX 2 output + * Invert direction of main output channel 2 * - * This is the PWM pulse the autopilot is outputting if not armed. - * When set to -1 the value for PWM_AUX_DISARMED will be used - * - * @reboot_required true + * Enable to invert the channel. * - * @min -1 - * @max 2200 - * @unit us + * @boolean * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_DIS2, -1); +PARAM_DEFINE_INT32(PWM_MAIN_REV2, 0); /** - * Set the disarmed PWM for the AUX 3 output + * Invert direction of main output channel 3 * - * This is the PWM pulse the autopilot is outputting if not armed. - * When set to -1 the value for PWM_AUX_DISARMED will be used + * Enable to invert the channel. * - * @reboot_required true + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_REV3, 0); + +/** + * Invert direction of main output channel 4 * - * @min -1 - * @max 2200 - * @unit us + * Enable to invert the channel. + * + * @boolean * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_DIS3, -1); +PARAM_DEFINE_INT32(PWM_MAIN_REV4, 0); /** - * Set the disarmed PWM for the AUX 4 output + * Invert direction of main output channel 5 * - * This is the PWM pulse the autopilot is outputting if not armed. - * When set to -1 the value for PWM_AUX_DISARMED will be used + * Enable to invert the channel. * - * @reboot_required true + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_REV5, 0); + +/** + * Invert direction of main output channel 6 * - * @min -1 - * @max 2200 - * @unit us + * Enable to invert the channel. + * + * @boolean * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_DIS4, -1); +PARAM_DEFINE_INT32(PWM_MAIN_REV6, 0); /** - * Set the disarmed PWM for the AUX 5 output + * Invert direction of main output channel 7 * - * This is the PWM pulse the autopilot is outputting if not armed. - * When set to -1 the value for PWM_AUX_DISARMED will be used + * Enable to invert the channel. * - * @reboot_required true + * @boolean + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0); + +/** + * Invert direction of main output channel 8 * - * @min -1 - * @max 2200 - * @unit us + * Enable to invert the channel. + * + * @boolean * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_DIS5, -1); +PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0); + + + /** - * Set the disarmed PWM for the AUX 6 output + * Trim value for main output channel 1 * - * This is the PWM pulse the autopilot is outputting if not armed. - * When set to -1 the value for PWM_AUX_DISARMED will be used + * Set to normalized offset * - * @reboot_required true + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM1, 0); + +/** + * Trim value for main output channel 2 * - * @min -1 - * @max 2200 - * @unit us + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_DIS6, -1); +PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM2, 0); /** - * Set the minimum PWM for the auxiliary outputs + * Trim value for main output channel 3 * - * Set to 1000 for default or 900 to increase servo travel + * Set to normalized offset * - * @reboot_required true + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM3, 0); + +/** + * Trim value for main output channel 4 * - * @min 800 - * @max 1400 - * @unit us + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000); +PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM4, 0); /** - * Set the maximum PWM for the auxiliary outputs + * Trim value for main output channel 5 * - * Set to 2000 for default or 2100 to increase servo travel + * Set to normalized offset * - * @reboot_required true + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM5, 0); + +/** + * Trim value for main output channel 6 * - * @min 1600 - * @max 2200 - * @unit us + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000); +PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM6, 0); /** - * Set the disarmed PWM for auxiliary outputs + * Trim value for main output channel 7 * - * This is the PWM pulse the autopilot is outputting if not armed. - * The main use of this parameter is to silence ESCs when they are disarmed. + * Set to normalized offset * - * @reboot_required true + * @min -0.2 + * @max 0.2 + * @decimal 2 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM7, 0); + +/** + * Trim value for main output channel 8 * - * @min 0 - * @max 2200 - * @unit us + * Set to normalized offset + * + * @min -0.2 + * @max 0.2 + * @decimal 2 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1500); +PARAM_DEFINE_FLOAT(PWM_MAIN_TRIM8, 0); + + + From a195f198dc8d943ebaa934f47200b7b1d55681b3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 16 Jul 2018 10:58:45 -0400 Subject: [PATCH 3/3] FMU relocate MOT_SLEW_MAX and THR_MDL_FAC parameters centrally --- src/drivers/px4fmu/px4fmu_params.c | 27 ------------ src/modules/sensors/motor_params.c | 67 ++++++++++++++++++++++++++++++ 2 files changed, 67 insertions(+), 27 deletions(-) create mode 100644 src/modules/sensors/motor_params.c diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c index a33c204257cb..df4ed00bda44 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -31,33 +31,6 @@ * ****************************************************************************/ -/** - * Thrust to PWM model parameter - * - * Parameter used to model the relationship between static thrust and motor - * input PWM. Model is: thrust = (1-factor)*PWM + factor * PWM^2 - * - * @min 0.0 - * @max 1.0 - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(THR_MDL_FAC, 0.0f); - -/** - * Minimum motor rise time (slew rate limit). - * - * Minimum time allowed for the motor input signal to pass through - * a range of 1000 PWM units. A value x means that the motor signal - * can only go from 1000 to 2000 PWM in maximum x seconds. - * - * Zero means that slew rate limiting is disabled. - * - * @min 0.0 - * @unit s/(1000*PWM) - * @group PWM Outputs - */ -PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f); - /** * Motor Ordering * diff --git a/src/modules/sensors/motor_params.c b/src/modules/sensors/motor_params.c new file mode 100644 index 000000000000..fa625230dac0 --- /dev/null +++ b/src/modules/sensors/motor_params.c @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file motor_params.c + * + * Parameters for motors. + * + */ + + +/** + * Minimum motor rise time (slew rate limit). + * + * Minimum time allowed for the motor input signal to pass through + * a range of 1000 PWM units. A value x means that the motor signal + * can only go from 1000 to 2000 PWM in maximum x seconds. + * + * Zero means that slew rate limiting is disabled. + * + * @min 0.0 + * @unit s/(1000*PWM) + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f); + +/** + * Thrust to PWM model parameter + * + * Parameter used to model the relationship between static thrust and motor + * input PWM. Model is: thrust = (1-factor)*PWM + factor * PWM^2 + * + * @min 0.0 + * @max 1.0 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(THR_MDL_FAC, 0.0f);