diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol index 28eb3ad57c..32c9fdb4b9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol @@ -76,8 +76,6 @@ param set-default NAV_ACC_RAD 5 param set-default VT_FWD_THRUST_EN 4 param set-default VT_F_TRANS_THR 0.75 -param set-default VT_MOT_ID 1234 -param set-default VT_FW_MOT_OFFID 1234 param set-default VT_B_TRANS_DUR 8 param set-default VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index ac4349858d..2d49754ae1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -55,8 +55,6 @@ param set-default RTL_RETURN_ALT 30 param set-default SDLOG_DIRS_MAX 7 param set-default VT_F_TRANS_THR 0.75 -param set-default VT_MOT_ID 1234 -param set-default VT_FW_MOT_OFFID 1234 param set-default VT_TYPE 2 param set-default CA_AIRFRAME 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index 1c6d1874c3..984ea09e99 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad @@ -116,8 +116,6 @@ param set-default MAV_1_FORWARD 1 param set-default SER_TEL2_BAUD 57600 param set-default VT_TYPE 2 -param set-default VT_MOT_ID 1234 -param set-default VT_FW_MOT_OFFID 1234 param set-default VT_F_TRANS_THR 1 param set-default VT_PITCH_MIN 8 param set-default VT_FW_QC_P 55 @@ -126,7 +124,6 @@ param set-default VT_TRANS_MIN_TM 15 param set-default VT_B_TRANS_DUR 8 param set-default VT_FWD_THRUST_SC 4 param set-default VT_F_TRANS_DUR 1 -param set-default VT_IDLE_PWM_MC 1025 param set-default VT_B_REV_OUT 0.5 param set-default VT_B_TRANS_THR 0.7 param set-default VT_TRANS_TIMEOUT 22 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark index 9f9f9faf87..519c0279ff 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark @@ -86,10 +86,7 @@ param set-default VT_B_DEC_MSS 1.5 param set-default VT_B_TRANS_DUR 12 param set-default VT_ELEV_MC_LOCK 0 param set-default VT_FWD_THRUST_SC 1.2 -param set-default VT_FW_MOT_OFFID 5678 param set-default VT_F_TR_OL_TM 8 -param set-default VT_IDLE_PWM_MC 1000 -param set-default VT_MOT_ID 5678 param set-default VT_PSHER_RMP_DT 2 param set-default VT_TRANS_MIN_TM 4 param set-default VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor b/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor index 0824e531e1..7c0c1af8a5 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor +++ b/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor @@ -16,10 +16,7 @@ param set-default MAV_TYPE 21 -param set-default VT_IDLE_PWM_MC 1100 param set-default VT_TYPE 1 -param set-default VT_MOT_ID 1234 -param set-default VT_FW_MOT_OFFID 24 param set-default CA_AIRFRAME 3 param set-default CA_ROTOR_COUNT 4 diff --git a/boards/airmind/mindpx-v2/src/init.c b/boards/airmind/mindpx-v2/src/init.c index 30a3e6a679..836bea7c42 100644 --- a/boards/airmind/mindpx-v2/src/init.c +++ b/boards/airmind/mindpx-v2/src/init.c @@ -74,7 +74,6 @@ #include #include -#include #include /**************************************************************************** diff --git a/boards/ark/cannode/src/init.c b/boards/ark/cannode/src/init.c index 491bc348b4..7eb98e4a15 100644 --- a/boards/ark/cannode/src/init.c +++ b/boards/ark/cannode/src/init.c @@ -69,7 +69,6 @@ #include #include -#include #include # if defined(FLASH_BASED_PARAMS) diff --git a/boards/px4/fmu-v4/src/init.c b/boards/px4/fmu-v4/src/init.c index adf97b6800..6c3ee01335 100644 --- a/boards/px4/fmu-v4/src/init.c +++ b/boards/px4/fmu-v4/src/init.c @@ -77,7 +77,6 @@ #include #include -#include #include /**************************************************************************** diff --git a/boards/uvify/core/src/init.c b/boards/uvify/core/src/init.c index 3418877c6f..6fb6034fba 100644 --- a/boards/uvify/core/src/init.c +++ b/boards/uvify/core/src/init.c @@ -77,7 +77,6 @@ #include #include -#include #include /**************************************************************************** diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c index 07edc7f334..d29ca58b02 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c @@ -61,12 +61,12 @@ //#include -int up_pwm_servo_set(unsigned channel, servo_position_t value) +int up_pwm_servo_set(unsigned channel, uint16_t value) { return io_timer_set_ccr(channel, value); } -servo_position_t up_pwm_servo_get(unsigned channel) +uint16_t up_pwm_servo_get(unsigned channel) { return io_channel_get_ccr(channel); } diff --git a/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp index 71ec010595..ddee5aa8ca 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp @@ -287,15 +287,15 @@ led_pwm_servo_set(unsigned channel, uint8_t cvalue) return 0; } -unsigned -led_pwm_servo_get(unsigned channel) + +unsigned led_pwm_servo_get(unsigned channel) { if (channel >= 3) { return 0; } unsigned timer = led_pwm_channels[channel].timer_index; - servo_position_t value = 0; + uint16_t value = 0; /* test timer for validity */ if ((led_pwm_timers[timer].base == 0) || @@ -307,8 +307,8 @@ led_pwm_servo_get(unsigned channel) unsigned period = led_pwm_timer_get_period(timer); return ((value + 1) * 255 / period); } -int -led_pwm_servo_init(void) + +int led_pwm_servo_init() { /* do basic timer initialisation first */ for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c index 408fbb37f1..fd083cdb74 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c @@ -62,12 +62,12 @@ #include -int up_pwm_servo_set(unsigned channel, servo_position_t value) +int up_pwm_servo_set(unsigned channel, uint16_t value) { return io_timer_set_ccr(channel, value); } -servo_position_t up_pwm_servo_get(unsigned channel) +uint16_t up_pwm_servo_get(unsigned channel) { return io_channel_get_ccr(channel); } diff --git a/platforms/nuttx/src/px4/nxp/kinetis/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/kinetis/led_pwm/led_pwm.cpp index e59021d556..4bbf7fc4e9 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/led_pwm/led_pwm.cpp +++ b/platforms/nuttx/src/px4/nxp/kinetis/led_pwm/led_pwm.cpp @@ -278,15 +278,15 @@ led_pwm_servo_set(unsigned channel, uint8_t cvalue) return 0; } -unsigned -led_pwm_servo_get(unsigned channel) + +unsigned led_pwm_servo_get(unsigned channel) { if (channel >= 3) { return 0; } unsigned timer = led_pwm_channels[channel].timer_index; - servo_position_t value = 0; + uint16_t value = 0; /* test timer for validity */ if ((led_pwm_timers[timer].base == 0) || @@ -298,8 +298,8 @@ led_pwm_servo_get(unsigned channel) unsigned period = led_pwm_timer_get_period(timer); return ((value + 1) * 255 / period); } -int -led_pwm_servo_init(void) + +int led_pwm_servo_init() { /* do basic timer initialisation first */ for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c index 9af9b4c8e8..b1dc315f4b 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c @@ -59,12 +59,12 @@ #include "s32k1xx_pin.h" -int up_pwm_servo_set(unsigned channel, servo_position_t value) +int up_pwm_servo_set(unsigned channel, uint16_t value) { return io_timer_set_ccr(channel, value); } -servo_position_t up_pwm_servo_get(unsigned channel) +uint16_t up_pwm_servo_get(unsigned channel) { return io_channel_get_ccr(channel); } diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp index f0a36da96f..4ec0c5a20d 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp @@ -279,15 +279,15 @@ led_pwm_servo_set(unsigned channel, uint8_t cvalue) return 0; } -unsigned -led_pwm_servo_get(unsigned channel) + +unsigned led_pwm_servo_get(unsigned channel) { if (channel >= 3) { return 0; } unsigned timer = led_pwm_channels[channel].timer_index; - servo_position_t value = 0; + uint16_t value = 0; /* test timer for validity */ if ((led_pwm_timers[timer].base == 0) || @@ -299,8 +299,8 @@ led_pwm_servo_get(unsigned channel) unsigned period = led_pwm_timer_get_period(timer); return ((value + 1) * 255 / period); } -int -led_pwm_servo_init(void) + +int led_pwm_servo_init() { /* do basic timer initialisation first */ for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c index b8ab8db721..83145a786c 100644 --- a/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c @@ -58,12 +58,12 @@ #include -int up_pwm_servo_set(unsigned channel, servo_position_t value) +int up_pwm_servo_set(unsigned channel, uint16_t value) { return io_timer_set_ccr(channel, value); } -servo_position_t up_pwm_servo_get(unsigned channel) +uint16_t up_pwm_servo_get(unsigned channel) { return io_channel_get_ccr(channel); } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index d9b8d013b0..f71ebc0cd7 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -43,7 +43,7 @@ #include #include #include -#include +#include #define MOTOR_PWM_BIT_1 14u diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c index d2b6d202ec..a94a153cc5 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c @@ -62,12 +62,12 @@ #include -int up_pwm_servo_set(unsigned channel, servo_position_t value) +int up_pwm_servo_set(unsigned channel, uint16_t value) { return io_timer_set_ccr(channel, value); } -servo_position_t up_pwm_servo_get(unsigned channel) +uint16_t up_pwm_servo_get(unsigned channel) { return io_channel_get_ccr(channel); } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp index cfe55b1868..e5c3da8ded 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp @@ -56,11 +56,8 @@ #include #include -#include #include - - #if defined(BOARD_HAS_LED_PWM) /* Board can override rate */ @@ -251,15 +248,15 @@ led_pwm_servo_set(unsigned channel, uint8_t cvalue) return 0; } -unsigned -led_pwm_servo_get(unsigned channel) + +unsigned led_pwm_servo_get(unsigned channel) { if (channel >= 3) { return 0; } unsigned timer = led_pwm_channels[channel].timer_index; - servo_position_t value = 0; + uint16_t value = 0; /* test timer for validity */ if ((led_pwm_timers[timer].base == 0) || @@ -289,8 +286,8 @@ led_pwm_servo_get(unsigned channel) unsigned period = led_pwm_timer_get_period(timer); return ((value + 1) * 255 / period); } -int -led_pwm_servo_init(void) + +int led_pwm_servo_init() { /* do basic timer initialisation first */ for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h new file mode 100644 index 0000000000..2e8dab2d6d --- /dev/null +++ b/src/drivers/drv_dshot.h @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2022 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 DShot servo output interface. + * + */ + +#pragma once + +#include + +#include +#include +#include + +#include "drv_orb_dev.h" + +__BEGIN_DECLS + +typedef enum { + DShot_cmd_motor_stop = 0, + DShot_cmd_beacon1, + DShot_cmd_beacon2, + DShot_cmd_beacon3, + DShot_cmd_beacon4, + DShot_cmd_beacon5, + DShot_cmd_esc_info, // V2 includes settings + DShot_cmd_spin_direction_1, + DShot_cmd_spin_direction_2, + DShot_cmd_3d_mode_off, + DShot_cmd_3d_mode_on, + DShot_cmd_settings_request, // Currently not implemented + DShot_cmd_save_settings, + DShot_cmd_spin_direction_normal = 20, + DShot_cmd_spin_direction_reversed = 21, + DShot_cmd_led0_on, // BLHeli32 only + DShot_cmd_led1_on, // BLHeli32 only + DShot_cmd_led2_on, // BLHeli32 only + DShot_cmd_led3_on, // BLHeli32 only + DShot_cmd_led0_off, // BLHeli32 only + DShot_cmd_led1_off, // BLHeli32 only + DShot_cmd_led2_off, // BLHeli32 only + DShot_cmd_led4_off, // BLHeli32 only + DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off + DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off + DShot_cmd_signal_line_telemetry_disable = 32, + DShot_cmd_signal_line_continuous_erpm_telemetry = 33, + DShot_cmd_MAX = 47, // >47 are throttle values + DShot_cmd_MIN_throttle = 48, + DShot_cmd_MAX_throttle = 2047 +} dshot_command_t; + + +/** + * Intialise the Dshot outputs using the specified configuration. + * + * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. + * This allows some of the channels to remain configured + * as GPIOs or as another function. Already used channels/timers will not be configured as DShot + * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 + * @return <0 on error, the initialized channels mask. + */ +__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq); + +/** + * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method) + */ +__EXPORT extern void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry); + +/** + * Set the current dshot throttle value for a channel (motor). + * + * @param channel The channel to set. + * @param throttle The output dshot throttle value in [0 = DSHOT_DISARM_VALUE, 1 = DSHOT_MIN_THROTTLE, 1999 = DSHOT_MAX_THROTTLE]. + * @param telemetry If true, request telemetry from that motor + */ +static inline void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) +{ + dshot_motor_data_set(channel, throttle + DShot_cmd_MIN_throttle, telemetry); +} + +/** + * Send DShot command to a channel (motor). + * + * @param channel The channel to set. + * @param command dshot_command_t + * @param telemetry If true, request telemetry from that motor + */ +static inline void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry) +{ + dshot_motor_data_set(channel, command, telemetry); +} + +/** + * Trigger dshot data transfer. + */ +__EXPORT extern void up_dshot_trigger(void); + +/** + * Arm or disarm dshot outputs (This will enable/disable complete timer for safety purpose.). + * + * When disarmed, dshot output no pulse. + * + * @param armed If true, outputs are armed; if false they + * are disarmed. + */ +__EXPORT extern int up_dshot_arm(bool armed); + +__END_DECLS diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 43721f9cb0..13251bfebd 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015, 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 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 @@ -34,10 +34,6 @@ /** * @file PWM servo output interface. * - * Servo values can be set with the PWM_SERVO_SET ioctl, by writing a - * pwm_output_values structure to the device - * Writing a value of 0 to a channel suppresses any output for that - * channel. */ #pragma once @@ -52,24 +48,8 @@ __BEGIN_DECLS -/** - * Path for the default PWM output device. - * - * Note that on systems with more than one PWM output path (e.g. - * PX4FMU with PX4IO connected) there may be other devices that - * respond to this protocol. - */ -#define PWM_OUTPUT_BASE_DEVICE_PATH "/dev/pwm_output" -#define PWM_OUTPUT0_DEVICE_PATH "/dev/pwm_output0" -#define PWM_OUTPUT1_DEVICE_PATH "/dev/pwm_output1" - #define PWM_OUTPUT_MAX_CHANNELS 16 -struct pwm_output_values { - uint32_t channel_count; - uint16_t values[PWM_OUTPUT_MAX_CHANNELS]; -}; - /* Use defaults unless the board override the defaults by providing * PX4_PWM_ALTERNATE_RANGES and a replacement set of * constants @@ -123,12 +103,6 @@ struct pwm_output_values { */ #define PWM_IGNORE_THIS_CHANNEL UINT16_MAX -/** - * Servo output signal type, value is actual servo output pulse - * width in microseconds. - */ -typedef uint16_t servo_position_t; - /* * ioctl() definitions * @@ -137,112 +111,14 @@ typedef uint16_t servo_position_t; */ #define _PWM_SERVO_BASE 0x2a00 -/** arm all servo outputs handle by this driver */ -#define PWM_SERVO_ARM _PX4_IOC(_PWM_SERVO_BASE, 0) - -/** disarm all servo outputs (stop generating pulses) */ -#define PWM_SERVO_DISARM _PX4_IOC(_PWM_SERVO_BASE, 1) - -/** get default servo update rate */ -#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 2) - -/** set alternate servo update rate */ -#define PWM_SERVO_SET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 3) - -/** get alternate servo update rate */ -#define PWM_SERVO_GET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 4) - -/** get the number of servos in *(unsigned *)arg */ -#define PWM_SERVO_GET_COUNT _PX4_IOC(_PWM_SERVO_BASE, 5) - -/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */ -#define PWM_SERVO_SET_SELECT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 6) - -/** check the selected update rates */ -#define PWM_SERVO_GET_SELECT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 7) - -/** set the 'ARM ok' bit, which activates the safety switch */ -#define PWM_SERVO_SET_ARM_OK _PX4_IOC(_PWM_SERVO_BASE, 8) - -/** clear the 'ARM ok' bit, which deactivates the safety switch */ -#define PWM_SERVO_CLEAR_ARM_OK _PX4_IOC(_PWM_SERVO_BASE, 9) - /** start DSM bind */ #define DSM_BIND_START _PX4_IOC(_PWM_SERVO_BASE, 10) -/** get the PWM value for failsafe */ -#define PWM_SERVO_GET_FAILSAFE_PWM _PX4_IOC(_PWM_SERVO_BASE, 13) - -/** get the PWM value when disarmed */ -#define PWM_SERVO_GET_DISARMED_PWM _PX4_IOC(_PWM_SERVO_BASE, 15) - -/** set the minimum PWM value the output will send */ -#define PWM_SERVO_SET_MIN_PWM _PX4_IOC(_PWM_SERVO_BASE, 16) - -/** get the minimum PWM value the output will send */ -#define PWM_SERVO_GET_MIN_PWM _PX4_IOC(_PWM_SERVO_BASE, 17) - -/** set the maximum PWM value the output will send */ -#define PWM_SERVO_SET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 18) - -/** get the maximum PWM value the output will send */ -#define PWM_SERVO_GET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 19) - -/* - * - * - * WARNING WARNING WARNING! DO NOT EXCEED 47 IN IOC INDICES HERE! - * - * - */ - -/** get a single specific servo value */ -#define PWM_SERVO_GET(_servo) _PX4_IOC(_PWM_SERVO_BASE, 0x50 + _servo) - -/** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels - * whose update rates must be the same. - */ -#define PWM_SERVO_GET_RATEGROUP(_n) _PX4_IOC(_PWM_SERVO_BASE, 0x70 + _n) - /** specific rates for configuring the timer for OneShot or PWM */ #define PWM_RATE_ONESHOT 0u #define PWM_RATE_LOWER_LIMIT 1u #define PWM_RATE_UPPER_LIMIT 10000u -typedef enum { - DShot_cmd_motor_stop = 0, - DShot_cmd_beacon1, - DShot_cmd_beacon2, - DShot_cmd_beacon3, - DShot_cmd_beacon4, - DShot_cmd_beacon5, - DShot_cmd_esc_info, // V2 includes settings - DShot_cmd_spin_direction_1, - DShot_cmd_spin_direction_2, - DShot_cmd_3d_mode_off, - DShot_cmd_3d_mode_on, - DShot_cmd_settings_request, // Currently not implemented - DShot_cmd_save_settings, - DShot_cmd_spin_direction_normal = 20, - DShot_cmd_spin_direction_reversed = 21, - DShot_cmd_led0_on, // BLHeli32 only - DShot_cmd_led1_on, // BLHeli32 only - DShot_cmd_led2_on, // BLHeli32 only - DShot_cmd_led3_on, // BLHeli32 only - DShot_cmd_led0_off, // BLHeli32 only - DShot_cmd_led1_off, // BLHeli32 only - DShot_cmd_led2_off, // BLHeli32 only - DShot_cmd_led4_off, // BLHeli32 only - DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off - DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off - DShot_cmd_signal_line_telemetry_disable = 32, - DShot_cmd_signal_line_continuous_erpm_telemetry = 33, - DShot_cmd_MAX = 47, // >47 are throttle values - DShot_cmd_MIN_throttle = 48, - DShot_cmd_MAX_throttle = 2047 -} dshot_command_t; - - /* * Low-level PWM output interface. * @@ -331,7 +207,7 @@ __EXPORT extern void up_pwm_update(unsigned channel_mask); * @param channel The channel to set. * @param value The output pulse width in microseconds. */ -__EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value); +__EXPORT extern int up_pwm_servo_set(unsigned channel, uint16_t value); /** * Get the current output value for a channel. @@ -340,62 +216,7 @@ __EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value); * @return The output pulse width in microseconds, or zero if * outputs are not armed or not configured. */ -__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel); +__EXPORT extern uint16_t up_pwm_servo_get(unsigned channel); -/** - * Intialise the Dshot outputs using the specified configuration. - * - * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. - * This allows some of the channels to remain configured - * as GPIOs or as another function. Already used channels/timers will not be configured as DShot - * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 - * @return <0 on error, the initialized channels mask. - */ -__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq); - -/** - * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method) - */ -__EXPORT extern void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry); - -/** - * Set the current dshot throttle value for a channel (motor). - * - * @param channel The channel to set. - * @param throttle The output dshot throttle value in [0 = DSHOT_DISARM_VALUE, 1 = DSHOT_MIN_THROTTLE, 1999 = DSHOT_MAX_THROTTLE]. - * @param telemetry If true, request telemetry from that motor - */ -static inline void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) -{ - dshot_motor_data_set(channel, throttle + DShot_cmd_MIN_throttle, telemetry); -} - -/** - * Send DShot command to a channel (motor). - * - * @param channel The channel to set. - * @param command dshot_command_t - * @param telemetry If true, request telemetry from that motor - */ -static inline void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry) -{ - dshot_motor_data_set(channel, command, telemetry); -} - - -/** - * Trigger dshot data transfer. - */ -__EXPORT extern void up_dshot_trigger(void); - -/** - * Arm or disarm dshot outputs (This will enable/disable complete timer for safety purpose.). - * - * When disarmed, dshot output no pulse. - * - * @param armed If true, outputs are armed; if false they - * are disarmed. - */ -__EXPORT extern int up_dshot_arm(bool armed); __END_DECLS diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index f6fe7b3754..f0cb4458f4 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 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 @@ -32,8 +32,7 @@ ****************************************************************************/ #pragma once -#include -#include +#include #include #include #include @@ -59,16 +58,16 @@ static constexpr int DSHOT_DISARM_VALUE = 0; static constexpr int DSHOT_MIN_THROTTLE = 1; static constexpr int DSHOT_MAX_THROTTLE = 1999; -class DShot : public ModuleBase, public OutputModuleInterface +class DShot final : public ModuleBase, public OutputModuleInterface { public: DShot(); - virtual ~DShot(); + ~DShot() override; /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); - virtual int init(); + int init(); void mixerChanged() override; diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index a4b8a532f1..df1e803562 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -51,7 +51,6 @@ static bool is_running() } PWMOut::PWMOut(int instance, uint8_t output_base) : - CDev((instance == 0) ? PX4FMU_DEVICE_PATH : PX4FMU_DEVICE_PATH"1"), OutputModuleInterface((instance == 0) ? MODULE_NAME"0" : MODULE_NAME"1", px4::wq_configurations::hp_default), _instance(instance), _output_base(output_base), @@ -65,31 +64,12 @@ PWMOut::~PWMOut() /* make sure servos are off */ up_pwm_servo_deinit(_pwm_mask); - /* clean up the alternate device node */ - unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); - perf_free(_cycle_perf); perf_free(_interval_perf); } int PWMOut::init() { - /* do regular cdev init */ - int ret = CDev::init(); - - if (ret != OK) { - return ret; - } - - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); - - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* lets not be too verbose */ - } else if (_class_instance < 0) { - PX4_ERR("FAILED registering class device"); - } - _num_outputs = FMU_MAX_ACTUATORS; _pwm_mask = ((1u << _num_outputs) - 1) << _output_base; @@ -103,37 +83,6 @@ int PWMOut::init() return 0; } -void PWMOut::update_current_rate() -{ - /* - * Adjust actuator topic update rate to keep up with - * the highest servo update rate configured. - * - * We always mix at max rate; some channels may update slower. - */ - int max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; - - // oneshot - if ((_pwm_default_rate == 0) || (_pwm_alt_rate == 0)) { - max_rate = 2000; - - } else { - // run up to twice PWM rate to reduce end-to-end latency - // actual pulse width only updated for next period regardless of output module - max_rate *= 2; - } - - // max interval 0.5 - 100 ms (10 - 2000Hz) - const int update_interval_in_us = math::constrain(1000000 / max_rate, 500, 100000); - - if (_current_update_rate != max_rate) { - PX4_INFO("instance: %d, max rate: %d, default: %d, alt: %d", _instance, max_rate, _pwm_default_rate, _pwm_alt_rate); - } - - _current_update_rate = max_rate; - _mixing_output.setMaxTopicUpdateRate(update_interval_in_us); -} - int PWMOut::task_spawn(int argc, char *argv[]) { for (unsigned instance = 0; instance < (sizeof(_objects) / sizeof(_objects[0])); instance++) { @@ -282,8 +231,6 @@ void PWMOut::Run() return; } - SmartLock lock_guard(_lock); - perf_begin(_cycle_perf); perf_count(_interval_perf); @@ -369,186 +316,6 @@ void PWMOut::update_params() _first_param_update = false; } -int PWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - SmartLock lock_guard(_lock); - - int ret = pwm_ioctl(filp, cmd, arg); - - /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) { - ret = CDev::ioctl(filp, cmd, arg); - } - - return ret; -} - -int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - int ret = OK; - - PX4_DEBUG("pwm_out%u: ioctl cmd: %d, arg: %ld", _instance, cmd, arg); - - switch (cmd) { - case PWM_SERVO_ARM: - update_pwm_out_state(true); - break; - - case PWM_SERVO_SET_ARM_OK: - case PWM_SERVO_CLEAR_ARM_OK: - break; - - case PWM_SERVO_DISARM: - - /* Ignore disarm if disarmed PWM is set already. */ - if (_num_disarmed_set == 0) { - update_pwm_out_state(false); - } - - break; - - case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: - *(uint32_t *)arg = _pwm_default_rate; - break; - - case PWM_SERVO_SET_UPDATE_RATE: - ret = -EINVAL; - break; - - case PWM_SERVO_GET_UPDATE_RATE: - *(uint32_t *)arg = _pwm_alt_rate; - break; - - case PWM_SERVO_SET_SELECT_UPDATE_RATE: - ret = -EINVAL; - break; - - case PWM_SERVO_GET_SELECT_UPDATE_RATE: - *(uint32_t *)arg = _pwm_alt_rate_channels; - break; - - case PWM_SERVO_GET_FAILSAFE_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.failsafeValue(i); - } - - pwm->channel_count = FMU_MAX_ACTUATORS; - break; - } - - case PWM_SERVO_GET_DISARMED_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.disarmedValue(i); - } - - pwm->channel_count = FMU_MAX_ACTUATORS; - break; - } - - case PWM_SERVO_SET_MIN_PWM: - ret = -EINVAL; - break; - - case PWM_SERVO_GET_MIN_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.minValue(i); - } - - pwm->channel_count = FMU_MAX_ACTUATORS; - arg = (unsigned long)&pwm; - break; - } - - case PWM_SERVO_SET_MAX_PWM: - ret = -EINVAL; - break; - - case PWM_SERVO_GET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.maxValue(i); - } - - pwm->channel_count = FMU_MAX_ACTUATORS; - arg = (unsigned long)&pwm; - } - break; - -#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 14 - - case PWM_SERVO_GET(13): - case PWM_SERVO_GET(12): - case PWM_SERVO_GET(11): - case PWM_SERVO_GET(10): - case PWM_SERVO_GET(9): - case PWM_SERVO_GET(8): -#endif -#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 8 - case PWM_SERVO_GET(7): - case PWM_SERVO_GET(6): -#endif -#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 6 - case PWM_SERVO_GET(5): -#endif -#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 5 - case PWM_SERVO_GET(4): -#endif - case PWM_SERVO_GET(3): - case PWM_SERVO_GET(2): - case PWM_SERVO_GET(1): - case PWM_SERVO_GET(0): - if (cmd - PWM_SERVO_GET(0) >= (int)_num_outputs) { - ret = -EINVAL; - break; - } - - *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0) + _output_base); - break; - - case PWM_SERVO_GET_RATEGROUP(0): - case PWM_SERVO_GET_RATEGROUP(1): - case PWM_SERVO_GET_RATEGROUP(2): - case PWM_SERVO_GET_RATEGROUP(3): -#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 5 - case PWM_SERVO_GET_RATEGROUP(4): -#endif -#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 6 - case PWM_SERVO_GET_RATEGROUP(5): -#endif -#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 8 - case PWM_SERVO_GET_RATEGROUP(6): - case PWM_SERVO_GET_RATEGROUP(7): -#endif -#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 14 - case PWM_SERVO_GET_RATEGROUP(8): - case PWM_SERVO_GET_RATEGROUP(9): - case PWM_SERVO_GET_RATEGROUP(10): - case PWM_SERVO_GET_RATEGROUP(11): - case PWM_SERVO_GET_RATEGROUP(12): - case PWM_SERVO_GET_RATEGROUP(13): -#endif - *(uint32_t *)arg = _pwm_mask & up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); - break; - - case PWM_SERVO_GET_COUNT: - *(unsigned *)arg = _num_outputs; - break; - - default: - ret = -ENOTTY; - break; - } - - return ret; -} - int PWMOut::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); @@ -556,16 +323,6 @@ int PWMOut::custom_command(int argc, char *argv[]) int PWMOut::print_status() { - if (_class_instance == CLASS_DEVICE_PRIMARY) { - PX4_INFO("%d - PWM_MAIN 0x%04" PRIx32, _instance, _pwm_mask); - - } else if (_class_instance == CLASS_DEVICE_SECONDARY) { - PX4_INFO("%d - PWM_AUX 0x%04" PRIx32, _instance, _pwm_mask); - - } else if (_class_instance == CLASS_DEVICE_TERTIARY) { - PX4_INFO("%d - PWM_EXTRA 0x%04" PRIx32, _instance, _pwm_mask); - } - PX4_INFO("%d - Max update rate: %i Hz", _instance, _current_update_rate); perf_print_counter(_cycle_perf); diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index c7599ae26a..d1d810ab69 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 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 @@ -41,10 +41,8 @@ #include #include #include -#include #include #include -#include #include #include #include @@ -56,22 +54,15 @@ #include #include #include -#include #include #include using namespace time_literals; -#if !defined(DIRECT_PWM_OUTPUT_CHANNELS) -# error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS" -#endif - -#define PX4FMU_DEVICE_PATH "/dev/px4fmu" - static constexpr int PWM_OUT_MAX_INSTANCES{(DIRECT_PWM_OUTPUT_CHANNELS > 8) ? 2 : 1}; extern pthread_mutex_t pwm_out_module_mutex; -class PWMOut : public cdev::CDev, public OutputModuleInterface +class PWMOut : public OutputModuleInterface { public: PWMOut() = delete; @@ -102,9 +93,7 @@ public: static int test(const char *dev); - int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - - int init() override; + int init(); uint32_t get_pwm_mask() const { return _pwm_mask; } void set_pwm_mask(uint32_t mask) { _pwm_mask = mask; } @@ -139,7 +128,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; unsigned _num_outputs{0}; - int _class_instance{-1}; bool _pwm_on{false}; uint32_t _pwm_mask{0}; @@ -151,10 +139,6 @@ private: perf_counter_t _cycle_perf; perf_counter_t _interval_perf; - void update_current_rate(); - int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); - int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); - bool update_pwm_out_state(bool on); void update_params(); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b9588a68ba..fba6a2bd67 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -177,7 +177,6 @@ private: unsigned _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO 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] {}; @@ -357,11 +356,6 @@ PX4IO::~PX4IO() { delete _interface; - /* clean up the alternate device node */ - if (_class_instance >= 0) { - unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); - } - /* deallocate perfs */ perf_free(_cycle_perf); perf_free(_interval_perf); @@ -476,8 +470,6 @@ int PX4IO::init() /* 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); - _mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL); } @@ -492,24 +484,24 @@ int PX4IO::init() void PX4IO::updateDisarmed() { - pwm_output_values pwm{}; + uint16_t values[PX4IO_MAX_ACTUATORS] {}; for (unsigned i = 0; i < _max_actuators; i++) { - pwm.values[i] = _mixing_output.disarmedValue(i); + values[i] = _mixing_output.disarmedValue(i); } - io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm.values, _max_actuators); + io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, values, _max_actuators); } void PX4IO::updateFailsafe() { - pwm_output_values pwm{}; + uint16_t values[PX4IO_MAX_ACTUATORS] {}; for (unsigned i = 0; i < _max_actuators; i++) { - pwm.values[i] = _mixing_output.actualFailsafeValue(i); + values[i] = _mixing_output.actualFailsafeValue(i); } - io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, _max_actuators); + io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, values, _max_actuators); } void PX4IO::Run() @@ -1312,187 +1304,11 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* regular ioctl? */ switch (cmd) { - case PWM_SERVO_ARM: - PX4_DEBUG("PWM_SERVO_ARM"); - /* set the 'armed' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED); - break; - - case PWM_SERVO_SET_ARM_OK: - PX4_DEBUG("PWM_SERVO_SET_ARM_OK"); - /* set the 'OK to arm' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK); - break; - - case PWM_SERVO_CLEAR_ARM_OK: - PX4_DEBUG("PWM_SERVO_CLEAR_ARM_OK"); - /* clear the 'OK to arm' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0); - break; - - case PWM_SERVO_DISARM: - PX4_DEBUG("PWM_SERVO_DISARM"); - /* clear the 'armed' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); - break; - - case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: - PX4_DEBUG("PWM_SERVO_GET_DEFAULT_UPDATE_RATE"); - /* get the default update rate */ - *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); - break; - - case PWM_SERVO_SET_UPDATE_RATE: - PX4_DEBUG("PWM_SERVO_SET_UPDATE_RATE"); - - ret = -EINVAL; - break; - - case PWM_SERVO_GET_UPDATE_RATE: - PX4_DEBUG("PWM_SERVO_GET_UPDATE_RATE"); - /* get the alternative update rate */ - *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); - break; - - case PWM_SERVO_SET_SELECT_UPDATE_RATE: { - PX4_DEBUG("PWM_SERVO_SET_SELECT_UPDATE_RATE"); - - ret = -EINVAL; - break; - } - - case PWM_SERVO_GET_SELECT_UPDATE_RATE: - PX4_DEBUG("PWM_SERVO_GET_SELECT_UPDATE_RATE"); - *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); - break; - - case PWM_SERVO_GET_FAILSAFE_PWM: { - PX4_DEBUG("PWM_SERVO_GET_FAILSAFE_PWM"); - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - pwm->channel_count = _max_actuators; - - for (unsigned i = 0; i < _max_actuators; i++) { - pwm->values[i] = _mixing_output.failsafeValue(i); - } - - break; - } - - case PWM_SERVO_GET_DISARMED_PWM: { - PX4_DEBUG("PWM_SERVO_GET_DISARMED_PWM"); - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - pwm->channel_count = _max_actuators; - - for (unsigned i = 0; i < _max_actuators; i++) { - pwm->values[i] = _mixing_output.disarmedValue(i); - } - - break; - } - - case PWM_SERVO_SET_MIN_PWM: { - PX4_DEBUG("PWM_SERVO_SET_MIN_PWM"); - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - if (pwm->channel_count > _max_actuators) { - /* fail with error */ - return -E2BIG; - } - - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] != 0 && false) { - _mixing_output.minValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MIN); - } - } - - break; - } - - case PWM_SERVO_GET_MIN_PWM: { - PX4_DEBUG("PWM_SERVO_GET_MIN_PWM"); - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - pwm->channel_count = _max_actuators; - - for (unsigned i = 0; i < _max_actuators; i++) { - pwm->values[i] = _mixing_output.minValue(i); - } - - break; - } - - case PWM_SERVO_SET_MAX_PWM: { - PX4_DEBUG("PWM_SERVO_SET_MAX_PWM"); - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - if (pwm->channel_count > _max_actuators) { - /* fail with error */ - return -E2BIG; - } - - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] != 0 && false) { - _mixing_output.maxValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MAX, (uint16_t)PWM_HIGHEST_MAX); - } - } - } - break; - - case PWM_SERVO_GET_MAX_PWM: { - PX4_DEBUG("PWM_SERVO_GET_MAX_PWM"); - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - pwm->channel_count = _max_actuators; - - for (unsigned i = 0; i < _max_actuators; i++) { - pwm->values[i] = _mixing_output.maxValue(i); - } - } - break; - - case PWM_SERVO_GET_COUNT: - PX4_DEBUG("PWM_SERVO_GET_COUNT"); - *(unsigned *)arg = _max_actuators; - break; - case DSM_BIND_START: /* bind a DSM receiver */ ret = dsm_bind_ioctl(arg); break; - case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { - - unsigned channel = cmd - PWM_SERVO_GET(0); - - if (channel >= _max_actuators) { - ret = -EINVAL; - - } else { - /* fetch a current PWM value */ - uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); - - if (value == _io_reg_get_error) { - ret = -EIO; - - } else { - *(servo_position_t *)arg = value; - } - } - - break; - } - - case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - - *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); - - if (*(uint32_t *)arg == _io_reg_get_error) { - ret = -EIO; - } - - break; - } - case PX4IO_SET_DEBUG: PX4_DEBUG("PX4IO_SET_DEBUG"); diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 865017ac27..e2e8e99b4c 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -91,8 +91,10 @@ static void set_motor_actuators(uORB::Publication &publisher, f } } -int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub) +int do_esc_calibration(orb_advert_t *mavlink_log_pub) { + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); + int return_code = PX4_OK; uORB::Publication actuator_test_pub{ORB_ID(actuator_test)}; // since we publish multiple at once, make sure the output driver subscribes before we publish @@ -154,112 +156,3 @@ int do_esc_calibration_ctrl_alloc(orb_advert_t *mavlink_log_pub) return return_code; } - -static int do_esc_calibration_ioctl(orb_advert_t *mavlink_log_pub) -{ - int return_code = PX4_OK; - hrt_abstime timeout_start = 0; - - uORB::SubscriptionData batt_sub{ORB_ID(battery_status)}; - const battery_status_s &battery = batt_sub.get(); - batt_sub.update(); - bool batt_connected = battery.connected; - - int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0); - - if (fd < 0) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device"); - return_code = PX4_ERROR; - goto Out; - } - - /* tell IO/FMU that its ok to disable its safety with the switch */ - if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch"); - return_code = PX4_ERROR; - goto Out; - } - - /* tell IO/FMU that the system is armed (it will output values if safety is off) */ - if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system"); - return_code = PX4_ERROR; - goto Out; - } - - calibration_log_info(mavlink_log_pub, "[cal] Connect battery now"); - - timeout_start = hrt_absolute_time(); - - while (true) { - // We are either waiting for the user to connect the battery. Or we are waiting to let the PWM - // sit high. - static constexpr hrt_abstime battery_connect_wait_timeout{20_s}; - static constexpr hrt_abstime pwm_high_timeout{3_s}; - hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout; - - if (hrt_elapsed_time(&timeout_start) > timeout_wait) { - if (!batt_connected) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery"); - return_code = PX4_ERROR; - goto Out; - } - - // PWM was high long enough - break; - } - - if (!batt_connected) { - if (batt_sub.update()) { - if (battery.connected) { - // Battery is connected, signal to user and start waiting again - batt_connected = true; - timeout_start = hrt_absolute_time(); - calibration_log_info(mavlink_log_pub, "[cal] Battery connected"); - } - } - } - - px4_usleep(50000); - } - -Out: - - if (fd != -1) { - - if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) { - calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed"); - } - - if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) { - calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated"); - } - - px4_close(fd); - } - - if (return_code == PX4_OK) { - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); - } - - return return_code; -} - -int do_esc_calibration(orb_advert_t *mavlink_log_pub) -{ - calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); - - param_t p_ctrl_alloc = param_find("SYS_CTRL_ALLOC"); - int32_t ctrl_alloc = 0; - - if (p_ctrl_alloc != PARAM_INVALID) { - param_get(p_ctrl_alloc, &ctrl_alloc); - } - - if (ctrl_alloc == 1) { - return do_esc_calibration_ctrl_alloc(mavlink_log_pub); - - } else { - return do_esc_calibration_ioctl(mavlink_log_pub); - } -} diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index a7d23ea693..3ae575ba0a 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -42,7 +42,6 @@ #include PWMSim::PWMSim(bool hil_mode_enabled) : - CDev(PWM_OUTPUT0_DEVICE_PATH), OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { _mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC); @@ -51,8 +50,6 @@ PWMSim::PWMSim(bool hil_mode_enabled) : _mixing_output.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC); _mixing_output.setIgnoreLockdown(hil_mode_enabled); - - CDev::init(); } void @@ -66,8 +63,6 @@ PWMSim::Run() return; } - SmartLock lock_guard(_lock); - _mixing_output.update(); // check for parameter updates @@ -121,127 +116,6 @@ PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne return false; } -int -PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - SmartLock lock_guard(_lock); - - int ret = OK; - - switch (cmd) { - case PWM_SERVO_ARM: - break; - - case PWM_SERVO_DISARM: - break; - - case PWM_SERVO_SET_MIN_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (i < OutputModuleInterface::MAX_ACTUATORS && false) { - _mixing_output.minValue(i) = pwm->values[i]; - } - } - - break; - } - - case PWM_SERVO_SET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (i < OutputModuleInterface::MAX_ACTUATORS && false) { - _mixing_output.maxValue(i) = pwm->values[i]; - } - } - - break; - } - - case PWM_SERVO_SET_UPDATE_RATE: - // PWMSim does not limit the update rate - break; - - case PWM_SERVO_SET_SELECT_UPDATE_RATE: - break; - - case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: - *(uint32_t *)arg = 9999; - break; - - case PWM_SERVO_GET_UPDATE_RATE: - *(uint32_t *)arg = 9999; - break; - - case PWM_SERVO_GET_SELECT_UPDATE_RATE: - *(uint32_t *)arg = 0; - break; - - case PWM_SERVO_GET_FAILSAFE_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.failsafeValue(i); - } - - pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS; - break; - } - - case PWM_SERVO_GET_DISARMED_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.disarmedValue(i); - } - - pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS; - break; - } - - case PWM_SERVO_GET_MIN_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.minValue(i); - } - - pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS; - break; - } - - case PWM_SERVO_GET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.maxValue(i); - } - - pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS; - break; - } - - case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - // no restrictions on output grouping - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - - *(uint32_t *)arg = (1 << channel); - break; - } - - case PWM_SERVO_GET_COUNT: - *(unsigned *)arg = OutputModuleInterface::MAX_ACTUATORS; - break; - - default: - ret = -ENOTTY; - break; - } - - return ret; -} - int PWMSim::task_spawn(int argc, char *argv[]) { diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index 5fa2ea156f..a35b092db3 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -52,10 +52,9 @@ #define PARAM_PREFIX "HIL_ACT" #endif - using namespace time_literals; -class PWMSim : public cdev::CDev, public ModuleBase, public OutputModuleInterface +class PWMSim : public ModuleBase, public OutputModuleInterface { public: PWMSim(bool hil_mode_enabled); @@ -72,8 +71,6 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 20cd0d0b2d..156f27e937 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -290,11 +290,9 @@ void Standard::update_transition_state() mc_weight = time_since_trans_start / _param_vt_b_trans_ramp.get(); } - set_all_motor_state(motor_state::ENABLED); - // set idle speed for MC actuators if (!_flag_idle_mc) { - _flag_idle_mc = set_idle_mc(); + _flag_idle_mc = true; } } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index a4209531dc..d451551322 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -256,7 +256,7 @@ void Tailsitter::update_transition_state() const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_b_trans_dur.get(), 0.1f); if (!_flag_idle_mc) { - _flag_idle_mc = set_idle_mc(); + _flag_idle_mc = true; } if (tilt > 0.01f) { diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 84e6e4adc6..304de97aba 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -263,13 +263,7 @@ void Tiltrotor::update_mc_state() // normal operation _tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get(); _mc_yaw_weight = 1.0f; - - // do thrust compensation only for legacy (static) allocation - if (!_param_sys_ctrl_alloc.get()) { - _v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt(); - } } - } void Tiltrotor::update_fw_state() @@ -309,7 +303,6 @@ void Tiltrotor::update_transition_state() if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) { // for the first part of the transition all rotors are enabled - set_all_motor_state(motor_state::ENABLED); // tilt rotors forward up to certain angle if (_tilt_control <= _param_vt_tilt_trans.get()) { @@ -357,12 +350,6 @@ void Tiltrotor::update_transition_state() _mc_roll_weight = 0.0f; _mc_yaw_weight = 0.0f; - // ramp down motors not used in fixed-wing flight (setting MAX_PWM down scales the given output into the new range) - int ramp_down_value = (1.0f - time_since_trans_start / _param_vt_trans_p2_dur.get()) * - (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN; - - set_alternate_motor_state(motor_state::VALUE, ramp_down_value); - // add minimum throttle for front transition _thrust_transition = math::max(_thrust_transition, FRONTTRANS_THR_MIN); @@ -378,7 +365,7 @@ void Tiltrotor::update_transition_state() // set idle speed for rotary wing mode if (!_flag_idle_mc) { - _flag_idle_mc = set_idle_mc(); + _flag_idle_mc = true; } // tilt rotors back once motors are idle @@ -407,7 +394,6 @@ void Tiltrotor::update_transition_state() // while we quickly rotate back the motors keep throttle at idle // turn on all MC motors - set_all_motor_state(motor_state::ENABLED); _mc_throttle_weight = 0.0f; _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index aa41a9f1d0..7cfb6e09e0 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -50,7 +50,6 @@ #pragma once #include -#include #include #include #include diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 035426ac07..2fefc5c5bd 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -39,18 +39,6 @@ * @author Sander Smeets */ -/** - * Idle speed of VTOL when in multicopter mode - * - * @unit us - * @min 900 - * @max 2000 - * @increment 1 - * @decimal 0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900); - /** * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) * @@ -263,28 +251,6 @@ PARAM_DEFINE_INT32(VT_FW_QC_R, 0); */ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f); -/** - * The channel number of motors that must be turned off in fixed wing mode. - * - * @min 0 - * @max 12345678 - * @increment 1 - * @decimal 0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0); - -/** - * The channel number of motors which provide lift during hover. - * - * @min 0 - * @max 12345678 - * @increment 1 - * @decimal 0 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_MOT_ID, 0); - /** * Differential thrust in forwards flight. * @@ -336,18 +302,6 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_FF, 0.f); */ PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f); -/** - * Enable the usage of AUX outputs for hover motors. - * - * Set this parameter to true if the vehicle's hover motors are connected to the FMU (AUX) port. - * Not required for boards that only have a FMU, and no IO. - * Only applies for standard VTOL and tiltrotor. - * - * @boolean - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_MC_ON_FMU, 0); - /** * Minimum pitch angle during hover. * diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 3d958e4867..37ec142ed7 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -75,72 +75,14 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) : _airspeed_validated = _attc->get_airspeed(); _tecs_status = _attc->get_tecs_status(); _land_detected = _attc->get_land_detected(); - - for (auto &pwm_max : _max_mc_pwm_values.values) { - pwm_max = PWM_DEFAULT_MAX; - } - - for (auto &pwm_disarmed : _disarmed_pwm_values.values) { - pwm_disarmed = PWM_MOTOR_OFF; - } } bool VtolType::init() { - if (!_param_sys_ctrl_alloc.get()) { - const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH; - - int fd = px4_open(dev, 0); - - if (fd < 0) { - PX4_ERR("can't open %s", dev); - return false; - } - - int ret = px4_ioctl(fd, PWM_SERVO_GET_MAX_PWM, (long unsigned int)&_max_mc_pwm_values); - _current_max_pwm_values = _max_mc_pwm_values; - - if (ret != PX4_OK) { - PX4_ERR("failed getting max values"); - px4_close(fd); - return false; - } - - ret = px4_ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&_min_mc_pwm_values); - - if (ret != PX4_OK) { - PX4_ERR("failed getting min values"); - px4_close(fd); - return false; - } - - ret = px4_ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (long unsigned int)&_disarmed_pwm_values); - - if (ret != PX4_OK) { - PX4_ERR("failed getting disarmed values"); - px4_close(fd); - return false; - } - - px4_close(fd); - - _main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()); - _alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_param_vt_fw_mot_offid.get()); - - - // in order to get the main motors we take all motors and clear the alternate motor bits - for (int i = 0; i < 8; i++) { - if (_alternate_motor_channel_bitmap & (1 << i)) { - _main_motor_channel_bitmap &= ~(1 << i); - } - } - } - _flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRateVtol); _spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRateVtol); return true; - } void VtolType::parameters_update() @@ -156,13 +98,11 @@ void VtolType::parameters_update() void VtolType::update_mc_state() { if (!_flag_idle_mc) { - _flag_idle_mc = set_idle_mc(); + _flag_idle_mc = true; } resetAccelToPitchPitchIntegrator(); - VtolType::set_all_motor_state(motor_state::ENABLED); - // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); @@ -185,14 +125,12 @@ void VtolType::update_mc_state() void VtolType::update_fw_state() { if (_flag_idle_mc) { - _flag_idle_mc = !set_idle_fw(); + _flag_idle_mc = false; } resetAccelToPitchPitchIntegrator(); _last_thr_in_fw_mode = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - VtolType::set_alternate_motor_state(motor_state::DISABLED); - // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); _mc_roll_weight = 0.0f; @@ -354,193 +292,6 @@ void VtolType::check_quadchute_condition() } } -bool VtolType::set_idle_mc() -{ - if (_param_sys_ctrl_alloc.get()) { - return true; - } - - unsigned pwm_value = _param_vt_idle_pwm_mc.get(); - struct pwm_output_values pwm_values {}; - - for (int i = 0; i < num_outputs_max; i++) { - if (is_channel_set(i, generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()))) { - pwm_values.values[i] = pwm_value; - - } else { - pwm_values.values[i] = _min_mc_pwm_values.values[i]; - } - - pwm_values.channel_count++; - } - - return apply_pwm_limits(pwm_values, pwm_limit_type::TYPE_MINIMUM); -} - -bool VtolType::set_idle_fw() -{ - if (_param_sys_ctrl_alloc.get()) { - return true; - } - - struct pwm_output_values pwm_values {}; - - for (int i = 0; i < num_outputs_max; i++) { - if (is_channel_set(i, generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()))) { - pwm_values.values[i] = PWM_DEFAULT_MIN; - - } else { - pwm_values.values[i] = _min_mc_pwm_values.values[i]; - } - - pwm_values.channel_count++; - } - - return apply_pwm_limits(pwm_values, pwm_limit_type::TYPE_MINIMUM); -} - -bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type) -{ - const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH; - - int fd = px4_open(dev, 0); - - if (fd < 0) { - PX4_WARN("can't open %s", dev); - return false; - } - - int ret; - - if (type == pwm_limit_type::TYPE_MINIMUM) { - ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - } else { - ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); - } - - px4_close(fd); - - - if (ret != OK) { - PX4_DEBUG("failed setting max values"); - return false; - } - - return true; -} - -void VtolType::set_all_motor_state(const motor_state target_state, const int value) -{ - if (_param_sys_ctrl_alloc.get()) { - return; - } - - set_main_motor_state(target_state, value); - set_alternate_motor_state(target_state, value); -} - -void VtolType::set_main_motor_state(const motor_state target_state, const int value) -{ - if (_param_sys_ctrl_alloc.get()) { - return; - } - - if (_main_motor_state != target_state || target_state == motor_state::VALUE) { - - if (set_motor_state(target_state, _main_motor_channel_bitmap, value)) { - _main_motor_state = target_state; - } - } -} - -void VtolType::set_alternate_motor_state(const motor_state target_state, const int value) -{ - if (_param_sys_ctrl_alloc.get()) { - return; - } - - if (_alternate_motor_state != target_state || target_state == motor_state::VALUE) { - - if (set_motor_state(target_state, _alternate_motor_channel_bitmap, value)) { - _alternate_motor_state = target_state; - } - } -} - -bool VtolType::set_motor_state(const motor_state target_state, const int32_t channel_bitmap, const int value) -{ - switch (target_state) { - case motor_state::ENABLED: - for (int i = 0; i < num_outputs_max; i++) { - if (is_channel_set(i, channel_bitmap)) { - _current_max_pwm_values.values[i] = _max_mc_pwm_values.values[i]; - } - } - - break; - - case motor_state::DISABLED: - for (int i = 0; i < num_outputs_max; i++) { - if (is_channel_set(i, channel_bitmap)) { - _current_max_pwm_values.values[i] = _disarmed_pwm_values.values[i]; - } - } - - break; - - case motor_state::IDLE: - - for (int i = 0; i < num_outputs_max; i++) { - if (is_channel_set(i, channel_bitmap)) { - _current_max_pwm_values.values[i] = _param_vt_idle_pwm_mc.get(); - } - } - - break; - - case motor_state::VALUE: - for (int i = 0; i < num_outputs_max; i++) { - if (is_channel_set(i, channel_bitmap)) { - _current_max_pwm_values.values[i] = value; - } - } - - break; - } - - _current_max_pwm_values.channel_count = num_outputs_max; - - return apply_pwm_limits(_current_max_pwm_values, pwm_limit_type::TYPE_MAXIMUM); -} - -int VtolType::generate_bitmap_from_channel_numbers(const int channels) -{ - int channel_bitmap = 0; - int channel_numbers = channels; - - int tmp; - - for (int i = 0; i < num_outputs_max; ++i) { - tmp = channel_numbers % 10; - - if (tmp == 0) { - break; - } - - channel_bitmap |= 1 << (tmp - 1); - channel_numbers = channel_numbers / 10; - } - - return channel_bitmap; -} - -bool VtolType::is_channel_set(const int channel, const int bitmap) -{ - return bitmap & (1 << channel); -} - - float VtolType::pusher_assist() { // Altitude above ground is distance sensor altitude if available, otherwise local z-position diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index b8dbd554d7..ae05080e93 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -44,7 +44,6 @@ #define VTOL_TYPE_H #include -#include #include #include #include @@ -78,17 +77,6 @@ enum VtolForwardActuationMode { ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND }; -// these are states that can be applied to a selection of multirotor motors. -// e.g. if we need to shut off some motors after transitioning to fixed wing mode -// we can individually disable them while others might still need to be enabled to produce thrust. -// we can select the target motors via VT_FW_MOT_OFFID -enum class motor_state { - ENABLED = 0, // motor max pwm will be set to the standard max pwm value - DISABLED, // motor max pwm will be set to a value that shuts the motor off - IDLE, // motor max pwm will be set to VT_IDLE_PWM_MC - VALUE // motor max pwm will be set to a specific value provided, see set_motor_state() -}; - /** * @brief Used to specify if min or max pwm values should be altered */ @@ -99,7 +87,7 @@ enum class pwm_limit_type { class VtolAttitudeControl; -class VtolType: public ModuleParams +class VtolType : public ModuleParams { public: @@ -233,9 +221,6 @@ protected: bool _tecs_running = false; hrt_abstime _tecs_running_ts = 0; - motor_state _main_motor_state = motor_state::DISABLED; - motor_state _alternate_motor_state = motor_state::DISABLED; - hrt_abstime _last_loop_ts = 0; float _transition_dt = 0; @@ -243,29 +228,6 @@ protected: bool _quadchute_command_treated{false}; - - /** - * @brief Sets mc motor minimum pwm to VT_IDLE_PWM_MC which ensures - * that they are spinning in mc mode. - * - * @return true on success - */ - bool set_idle_mc(); - - /** - * @brief Sets mc motor minimum pwm to PWM_MIN which ensures that the - * motors stop spinning on zero throttle in fw mode. - * - * @return true on success - */ - bool set_idle_fw(); - - void set_all_motor_state(motor_state target_state, int value = 0); - - void set_main_motor_state(motor_state target_state, int value = 0); - - void set_alternate_motor_state(motor_state target_state, int value = 0); - float update_and_get_backtransition_pitch_sp(); SlewRate _spoiler_setpoint_with_slewrate; @@ -304,57 +266,13 @@ protected: (ParamFloat) _param_mpc_land_alt2, (ParamFloat) _param_vt_lnd_pitch_min, - (ParamBool) _param_sys_ctrl_alloc, - (ParamInt) _param_vt_idle_pwm_mc, - (ParamInt) _param_vt_mot_id, - (ParamBool) _param_vt_mc_on_fmu, - (ParamInt) _param_vt_fw_mot_offid, (ParamFloat) _param_vt_spoiler_mc_ld ) private: - - hrt_abstime _throttle_blend_start_ts{0}; // time at which we start blending between transition throttle and fixed wing throttle - /** - * @brief Stores the max pwm values given by the system. - */ - struct pwm_output_values _min_mc_pwm_values {}; - struct pwm_output_values _max_mc_pwm_values {}; - struct pwm_output_values _disarmed_pwm_values {}; - - struct pwm_output_values _current_max_pwm_values {}; - - int32_t _main_motor_channel_bitmap = 0; - int32_t _alternate_motor_channel_bitmap = 0; - - /** - * @brief Adjust minimum/maximum pwm values for the output channels. - * - * @param pwm_output_values Struct containing the limit values for each channel - * @param[in] type Specifies if min or max limits are adjusted. - * - * @return True on success. - */ - bool apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type); - - /** - * @brief Determines if channel is set in a bitmap. - * - * @param[in] channel The channel - * @param[in] bitmap The bitmap to check on. - * - * @return True if set, false otherwise. - */ - bool is_channel_set(const int channel, const int bitmap); - - // generates a bitmap from a number format, e.g. 1235 -> first, second, third and fifth bits should be set. - int generate_bitmap_from_channel_numbers(const int channels); - - bool set_motor_state(const motor_state target_state, const int32_t channel_bitmap, const int value); - void resetAccelToPitchPitchIntegrator() { _accel_to_pitch_integ = 0.f; } bool shouldBlendThrottleAfterFrontTransition() { return _throttle_blend_start_ts != 0; };