Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 10:17:45 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

delete unused PWM IOCTLs

Browse Source
This commit is contained in:
Daniel Agar
2022-08-24 17:24:25 -04:00
parent bcdd2203d3
commit ce337a3d80
35 changed files with 198 additions and 1331 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol
-2
View File
@@ -76,8 +76,6 @@ param set-default NAV_ACC_RAD 5
param set-default VT_FWD_THRUST_EN 4 param set-default VT_FWD_THRUST_EN 4
param set-default VT_F_TRANS_THR 0.75 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_B_TRANS_DUR 8
param set-default VT_TYPE 2 param set-default VT_TYPE 2
ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
-2
View File
@@ -55,8 +55,6 @@ param set-default RTL_RETURN_ALT 30
param set-default SDLOG_DIRS_MAX 7 param set-default SDLOG_DIRS_MAX 7
param set-default VT_F_TRANS_THR 0.75 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 VT_TYPE 2
param set-default CA_AIRFRAME 2 param set-default CA_AIRFRAME 2
ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad
-3
View File
@@ -116,8 +116,6 @@ param set-default MAV_1_FORWARD 1
param set-default SER_TEL2_BAUD 57600 param set-default SER_TEL2_BAUD 57600
param set-default VT_TYPE 2 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_F_TRANS_THR 1
param set-default VT_PITCH_MIN 8 param set-default VT_PITCH_MIN 8
param set-default VT_FW_QC_P 55 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_B_TRANS_DUR 8
param set-default VT_FWD_THRUST_SC 4 param set-default VT_FWD_THRUST_SC 4
param set-default VT_F_TRANS_DUR 1 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_REV_OUT 0.5
param set-default VT_B_TRANS_THR 0.7 param set-default VT_B_TRANS_THR 0.7
param set-default VT_TRANS_TIMEOUT 22 param set-default VT_TRANS_TIMEOUT 22
ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark
-3
View File
@@ -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_B_TRANS_DUR 12
param set-default VT_ELEV_MC_LOCK 0 param set-default VT_ELEV_MC_LOCK 0
param set-default VT_FWD_THRUST_SC 1.2 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_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_PSHER_RMP_DT 2
param set-default VT_TRANS_MIN_TM 4 param set-default VT_TRANS_MIN_TM 4
param set-default VT_TYPE 2 param set-default VT_TYPE 2
ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor
-3
View File
@@ -16,10 +16,7 @@
param set-default MAV_TYPE 21 param set-default MAV_TYPE 21
param set-default VT_IDLE_PWM_MC 1100
param set-default VT_TYPE 1 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_AIRFRAME 3
param set-default CA_ROTOR_COUNT 4 param set-default CA_ROTOR_COUNT 4
boards/airmind/mindpx-v2/src/init.c
-1
View File
@@ -74,7 +74,6 @@
#include <px4_platform_common/init.h> #include <px4_platform_common/init.h>
#include <px4_platform/board_dma_alloc.h> #include <px4_platform/board_dma_alloc.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h> #include <px4_arch/io_timer.h>
/**************************************************************************** /****************************************************************************
boards/ark/cannode/src/init.c
-1
View File
@@ -69,7 +69,6 @@
#include <px4_platform_common/init.h> #include <px4_platform_common/init.h>
#include <px4_platform/gpio.h> #include <px4_platform/gpio.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h> #include <px4_arch/io_timer.h>
# if defined(FLASH_BASED_PARAMS) # if defined(FLASH_BASED_PARAMS)
boards/px4/fmu-v4/src/init.c
-1
View File
@@ -77,7 +77,6 @@
#include <px4_platform_common/init.h> #include <px4_platform_common/init.h>
#include <px4_platform/board_dma_alloc.h> #include <px4_platform/board_dma_alloc.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h> #include <px4_arch/io_timer.h>
/**************************************************************************** /****************************************************************************
boards/uvify/core/src/init.c
-1
View File
@@ -77,7 +77,6 @@
#include <px4_platform_common/init.h> #include <px4_platform_common/init.h>
#include <px4_platform/board_dma_alloc.h> #include <px4_platform/board_dma_alloc.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h> #include <px4_arch/io_timer.h>
/**************************************************************************** /****************************************************************************
platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c
+2 -2
View File
@@ -61,12 +61,12 @@
//#include <chip.h> //#include <chip.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); 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); return io_channel_get_ccr(channel);
} }
platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp
+5 -5
View File
@@ -287,15 +287,15 @@ led_pwm_servo_set(unsigned channel, uint8_t cvalue)
return 0; return 0;
} }
unsigned
led_pwm_servo_get(unsigned channel) unsigned led_pwm_servo_get(unsigned channel)
{ {
if (channel >= 3) { if (channel >= 3) {
return 0; return 0;
} }
unsigned timer = led_pwm_channels[channel].timer_index; unsigned timer = led_pwm_channels[channel].timer_index;
servo_position_t value = 0; uint16_t value = 0;
/* test timer for validity */ /* test timer for validity */
if ((led_pwm_timers[timer].base == 0) || 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); unsigned period = led_pwm_timer_get_period(timer);
return ((value + 1) * 255 / period); return ((value + 1) * 255 / period);
} }
int
led_pwm_servo_init(void) int led_pwm_servo_init()
{ {
/* do basic timer initialisation first */ /* do basic timer initialisation first */
for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) {
platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c
+2 -2
View File
@@ -62,12 +62,12 @@
#include <kinetis.h> #include <kinetis.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); 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); return io_channel_get_ccr(channel);
} }
platforms/nuttx/src/px4/nxp/kinetis/led_pwm/led_pwm.cpp
+5 -5
View File
@@ -278,15 +278,15 @@ led_pwm_servo_set(unsigned channel, uint8_t cvalue)
return 0; return 0;
} }
unsigned
led_pwm_servo_get(unsigned channel) unsigned led_pwm_servo_get(unsigned channel)
{ {
if (channel >= 3) { if (channel >= 3) {
return 0; return 0;
} }
unsigned timer = led_pwm_channels[channel].timer_index; unsigned timer = led_pwm_channels[channel].timer_index;
servo_position_t value = 0; uint16_t value = 0;
/* test timer for validity */ /* test timer for validity */
if ((led_pwm_timers[timer].base == 0) || 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); unsigned period = led_pwm_timer_get_period(timer);
return ((value + 1) * 255 / period); return ((value + 1) * 255 / period);
} }
int
led_pwm_servo_init(void) int led_pwm_servo_init()
{ {
/* do basic timer initialisation first */ /* do basic timer initialisation first */
for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) {
platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c
+2 -2
View File
@@ -59,12 +59,12 @@
#include "s32k1xx_pin.h" #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); 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); return io_channel_get_ccr(channel);
} }
platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp
+5 -5
View File
@@ -279,15 +279,15 @@ led_pwm_servo_set(unsigned channel, uint8_t cvalue)
return 0; return 0;
} }
unsigned
led_pwm_servo_get(unsigned channel) unsigned led_pwm_servo_get(unsigned channel)
{ {
if (channel >= 3) { if (channel >= 3) {
return 0; return 0;
} }
unsigned timer = led_pwm_channels[channel].timer_index; unsigned timer = led_pwm_channels[channel].timer_index;
servo_position_t value = 0; uint16_t value = 0;
/* test timer for validity */ /* test timer for validity */
if ((led_pwm_timers[timer].base == 0) || 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); unsigned period = led_pwm_timer_get_period(timer);
return ((value + 1) * 255 / period); return ((value + 1) * 255 / period);
} }
int
led_pwm_servo_init(void) int led_pwm_servo_init()
{ {
/* do basic timer initialisation first */ /* do basic timer initialisation first */
for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) {
platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c
+2 -2
View File
@@ -58,12 +58,12 @@
#include <px4_arch/io_timer.h> #include <px4_arch/io_timer.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); 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); return io_channel_get_ccr(channel);
} }
platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
+1 -1
View File
@@ -43,7 +43,7 @@
#include <stm32_tim.h> #include <stm32_tim.h>
#include <px4_arch/dshot.h> #include <px4_arch/dshot.h>
#include <px4_arch/io_timer.h> #include <px4_arch/io_timer.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_dshot.h>
#define MOTOR_PWM_BIT_1 14u #define MOTOR_PWM_BIT_1 14u
platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c
+2 -2
View File
@@ -62,12 +62,12 @@
#include <stm32_tim.h> #include <stm32_tim.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); 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); return io_channel_get_ccr(channel);
} }
platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp
+5 -8
View File
@@ -56,11 +56,8 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/px4_macros.h> #include <systemlib/px4_macros.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h> #include <px4_arch/io_timer.h>
#if defined(BOARD_HAS_LED_PWM) #if defined(BOARD_HAS_LED_PWM)
/* Board can override rate */ /* Board can override rate */
@@ -251,15 +248,15 @@ led_pwm_servo_set(unsigned channel, uint8_t cvalue)
return 0; return 0;
} }
unsigned
led_pwm_servo_get(unsigned channel) unsigned led_pwm_servo_get(unsigned channel)
{ {
if (channel >= 3) { if (channel >= 3) {
return 0; return 0;
} }
unsigned timer = led_pwm_channels[channel].timer_index; unsigned timer = led_pwm_channels[channel].timer_index;
servo_position_t value = 0; uint16_t value = 0;
/* test timer for validity */ /* test timer for validity */
if ((led_pwm_timers[timer].base == 0) || 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); unsigned period = led_pwm_timer_get_period(timer);
return ((value + 1) * 255 / period); return ((value + 1) * 255 / period);
} }
int
led_pwm_servo_init(void) int led_pwm_servo_init()
{ {
/* do basic timer initialisation first */ /* do basic timer initialisation first */
for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) {
src/drivers/drv_dshot.h
+140
View File
@@ -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 <px4_platform_common/defines.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include <board_config.h>
#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
src/drivers/drv_pwm_output.h
+3 -182
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -34,10 +34,6 @@
/** /**
* @file PWM servo output interface. * @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 #pragma once
@@ -52,24 +48,8 @@
__BEGIN_DECLS __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 #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 /* Use defaults unless the board override the defaults by providing
* PX4_PWM_ALTERNATE_RANGES and a replacement set of * PX4_PWM_ALTERNATE_RANGES and a replacement set of
* constants * constants
@@ -123,12 +103,6 @@ struct pwm_output_values {
*/ */
#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX #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 * ioctl() definitions
* *
@@ -137,112 +111,14 @@ typedef uint16_t servo_position_t;
*/ */
#define _PWM_SERVO_BASE 0x2a00 #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 */ /** start DSM bind */
#define DSM_BIND_START _PX4_IOC(_PWM_SERVO_BASE, 10) #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 */ /** specific rates for configuring the timer for OneShot or PWM */
#define PWM_RATE_ONESHOT 0u #define PWM_RATE_ONESHOT 0u
#define PWM_RATE_LOWER_LIMIT 1u #define PWM_RATE_LOWER_LIMIT 1u
#define PWM_RATE_UPPER_LIMIT 10000u #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. * 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 channel The channel to set.
* @param value The output pulse width in microseconds. * @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. * 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 * @return The output pulse width in microseconds, or zero if
* outputs are not armed or not configured. * 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 __END_DECLS
src/drivers/dshot/DShot.h
+5 -6
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -32,8 +32,7 @@
****************************************************************************/ ****************************************************************************/
#pragma once #pragma once
#include <drivers/device/device.h> #include <drivers/drv_dshot.h>
#include <drivers/drv_input_capture.h>
#include <lib/mixer_module/mixer_module.hpp> #include <lib/mixer_module/mixer_module.hpp>
#include <px4_platform_common/getopt.h> #include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h> #include <px4_platform_common/module.h>
@@ -59,16 +58,16 @@ static constexpr int DSHOT_DISARM_VALUE = 0;
static constexpr int DSHOT_MIN_THROTTLE = 1; static constexpr int DSHOT_MIN_THROTTLE = 1;
static constexpr int DSHOT_MAX_THROTTLE = 1999; static constexpr int DSHOT_MAX_THROTTLE = 1999;
class DShot : public ModuleBase<DShot>, public OutputModuleInterface class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
{ {
public: public:
DShot(); DShot();
virtual ~DShot(); ~DShot() override;
/** @see ModuleBase */ /** @see ModuleBase */
static int custom_command(int argc, char *argv[]); static int custom_command(int argc, char *argv[]);
virtual int init(); int init();
void mixerChanged() override; void mixerChanged() override;
src/drivers/pwm_out/PWMOut.cpp
-243
View File
@@ -51,7 +51,6 @@ static bool is_running()
} }
PWMOut::PWMOut(int instance, uint8_t output_base) : 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), OutputModuleInterface((instance == 0) ? MODULE_NAME"0" : MODULE_NAME"1", px4::wq_configurations::hp_default),
_instance(instance), _instance(instance),
_output_base(output_base), _output_base(output_base),
@@ -65,31 +64,12 @@ PWMOut::~PWMOut()
/* make sure servos are off */ /* make sure servos are off */
up_pwm_servo_deinit(_pwm_mask); 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(_cycle_perf);
perf_free(_interval_perf); perf_free(_interval_perf);
} }
int PWMOut::init() 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; _num_outputs = FMU_MAX_ACTUATORS;
_pwm_mask = ((1u << _num_outputs) - 1) << _output_base; _pwm_mask = ((1u << _num_outputs) - 1) << _output_base;
@@ -103,37 +83,6 @@ int PWMOut::init()
return 0; 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[]) int PWMOut::task_spawn(int argc, char *argv[])
{ {
for (unsigned instance = 0; instance < (sizeof(_objects) / sizeof(_objects[0])); instance++) { for (unsigned instance = 0; instance < (sizeof(_objects) / sizeof(_objects[0])); instance++) {
@@ -282,8 +231,6 @@ void PWMOut::Run()
return; return;
} }
SmartLock lock_guard(_lock);
perf_begin(_cycle_perf); perf_begin(_cycle_perf);
perf_count(_interval_perf); perf_count(_interval_perf);
@@ -369,186 +316,6 @@ void PWMOut::update_params()
_first_param_update = false; _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[]) int PWMOut::custom_command(int argc, char *argv[])
{ {
return print_usage("unknown command"); return print_usage("unknown command");
@@ -556,16 +323,6 @@ int PWMOut::custom_command(int argc, char *argv[])
int PWMOut::print_status() 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); PX4_INFO("%d - Max update rate: %i Hz", _instance, _current_update_rate);
perf_print_counter(_cycle_perf); perf_print_counter(_cycle_perf);
src/drivers/pwm_out/PWMOut.hpp
+3 -19
View File
@@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -41,10 +41,8 @@
#include <drivers/device/device.h> #include <drivers/device/device.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <lib/cdev/CDev.hpp>
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/mixer_module/mixer_module.hpp> #include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <px4_arch/io_timer.h> #include <px4_arch/io_timer.h>
#include <px4_platform_common/px4_config.h> #include <px4_platform_common/px4_config.h>
@@ -56,22 +54,15 @@
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp> #include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
using namespace time_literals; 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}; static constexpr int PWM_OUT_MAX_INSTANCES{(DIRECT_PWM_OUTPUT_CHANNELS > 8) ? 2 : 1};
extern pthread_mutex_t pwm_out_module_mutex; extern pthread_mutex_t pwm_out_module_mutex;
class PWMOut : public cdev::CDev, public OutputModuleInterface class PWMOut : public OutputModuleInterface
{ {
public: public:
PWMOut() = delete; PWMOut() = delete;
@@ -102,9 +93,7 @@ public:
static int test(const char *dev); static int test(const char *dev);
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; int init();
int init() override;
uint32_t get_pwm_mask() const { return _pwm_mask; } uint32_t get_pwm_mask() const { return _pwm_mask; }
void set_pwm_mask(uint32_t mask) { _pwm_mask = 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}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
unsigned _num_outputs{0}; unsigned _num_outputs{0};
int _class_instance{-1};
bool _pwm_on{false}; bool _pwm_on{false};
uint32_t _pwm_mask{0}; uint32_t _pwm_mask{0};
@@ -151,10 +139,6 @@ private:
perf_counter_t _cycle_perf; perf_counter_t _cycle_perf;
perf_counter_t _interval_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); bool update_pwm_out_state(bool on);
void update_params(); void update_params();
src/drivers/px4io/px4io.cpp
+6 -190
View File
@@ -177,7 +177,6 @@ private:
unsigned _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO unsigned _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO
unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO
int _class_instance{-1};
bool _first_param_update{true}; bool _first_param_update{true};
uint32_t _group_channels[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {}; 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; delete _interface;
/* clean up the alternate device node */
if (_class_instance >= 0) {
unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
}
/* deallocate perfs */ /* deallocate perfs */
perf_free(_cycle_perf); perf_free(_cycle_perf);
perf_free(_interval_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 */ /* 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) { 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); _mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL);
} }
@@ -492,24 +484,24 @@ int PX4IO::init()
void PX4IO::updateDisarmed() void PX4IO::updateDisarmed()
{ {
pwm_output_values pwm{}; uint16_t values[PX4IO_MAX_ACTUATORS] {};
for (unsigned i = 0; i < _max_actuators; i++) { 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() void PX4IO::updateFailsafe()
{ {
pwm_output_values pwm{}; uint16_t values[PX4IO_MAX_ACTUATORS] {};
for (unsigned i = 0; i < _max_actuators; i++) { 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() void PX4IO::Run()
@@ -1312,187 +1304,11 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
/* regular ioctl? */ /* regular ioctl? */
switch (cmd) { 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: case DSM_BIND_START:
/* bind a DSM receiver */ /* bind a DSM receiver */
ret = dsm_bind_ioctl(arg); ret = dsm_bind_ioctl(arg);
break; 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: case PX4IO_SET_DEBUG:
PX4_DEBUG("PX4IO_SET_DEBUG"); PX4_DEBUG("PX4IO_SET_DEBUG");
src/modules/commander/esc_calibration.cpp
+3 -110
View File
@@ -91,8 +91,10 @@ static void set_motor_actuators(uORB::Publication<actuator_test_s> &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; int return_code = PX4_OK;
uORB::Publication<actuator_test_s> actuator_test_pub{ORB_ID(actuator_test)}; uORB::Publication<actuator_test_s> actuator_test_pub{ORB_ID(actuator_test)};
// since we publish multiple at once, make sure the output driver subscribes before we publish // 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; 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<battery_status_s> 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);
}
}
src/modules/simulation/pwm_out_sim/PWMSim.cpp
-126
View File
@@ -42,7 +42,6 @@
#include <px4_platform_common/sem.hpp> #include <px4_platform_common/sem.hpp>
PWMSim::PWMSim(bool hil_mode_enabled) : PWMSim::PWMSim(bool hil_mode_enabled) :
CDev(PWM_OUTPUT0_DEVICE_PATH),
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
{ {
_mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC); _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.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC);
_mixing_output.setIgnoreLockdown(hil_mode_enabled); _mixing_output.setIgnoreLockdown(hil_mode_enabled);
CDev::init();
} }
void void
@@ -66,8 +63,6 @@ PWMSim::Run()
return; return;
} }
SmartLock lock_guard(_lock);
_mixing_output.update(); _mixing_output.update();
// check for parameter updates // check for parameter updates
@@ -121,127 +116,6 @@ PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne
return false; 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 int
PWMSim::task_spawn(int argc, char *argv[]) PWMSim::task_spawn(int argc, char *argv[])
{ {
src/modules/simulation/pwm_out_sim/PWMSim.hpp
+1 -4
View File
@@ -52,10 +52,9 @@
#define PARAM_PREFIX "HIL_ACT" #define PARAM_PREFIX "HIL_ACT"
#endif #endif
using namespace time_literals; using namespace time_literals;
class PWMSim : public cdev::CDev, public ModuleBase<PWMSim>, public OutputModuleInterface class PWMSim : public ModuleBase<PWMSim>, public OutputModuleInterface
{ {
public: public:
PWMSim(bool hil_mode_enabled); PWMSim(bool hil_mode_enabled);
@@ -72,8 +71,6 @@ public:
/** @see ModuleBase::print_status() */ /** @see ModuleBase::print_status() */
int print_status() override; 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], bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override; unsigned num_outputs, unsigned num_control_groups_updated) override;
src/modules/vtol_att_control/standard.cpp
+1 -3
View File
@@ -290,11 +290,9 @@ void Standard::update_transition_state()
mc_weight = time_since_trans_start / _param_vt_b_trans_ramp.get(); 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 // set idle speed for MC actuators
if (!_flag_idle_mc) { if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc(); _flag_idle_mc = true;
} }
} }
src/modules/vtol_att_control/tailsitter.cpp
+1 -1
View File
@@ -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); const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_b_trans_dur.get(), 0.1f);
if (!_flag_idle_mc) { if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc(); _flag_idle_mc = true;
} }
if (tilt > 0.01f) { if (tilt > 0.01f) {
src/modules/vtol_att_control/tiltrotor.cpp
+1 -15
View File
@@ -263,13 +263,7 @@ void Tiltrotor::update_mc_state()
// normal operation // normal operation
_tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get(); _tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get();
_mc_yaw_weight = 1.0f; _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() void Tiltrotor::update_fw_state()
@@ -309,7 +303,6 @@ void Tiltrotor::update_transition_state()
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) { if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
// for the first part of the transition all rotors are enabled // 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 // tilt rotors forward up to certain angle
if (_tilt_control <= _param_vt_tilt_trans.get()) { if (_tilt_control <= _param_vt_tilt_trans.get()) {
@@ -357,12 +350,6 @@ void Tiltrotor::update_transition_state()
_mc_roll_weight = 0.0f; _mc_roll_weight = 0.0f;
_mc_yaw_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 // add minimum throttle for front transition
_thrust_transition = math::max(_thrust_transition, FRONTTRANS_THR_MIN); _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 // set idle speed for rotary wing mode
if (!_flag_idle_mc) { if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc(); _flag_idle_mc = true;
} }
// tilt rotors back once motors are idle // 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 // while we quickly rotate back the motors keep throttle at idle
// turn on all MC motors // turn on all MC motors
set_all_motor_state(motor_state::ENABLED);
_mc_throttle_weight = 0.0f; _mc_throttle_weight = 0.0f;
_mc_roll_weight = 0.0f; _mc_roll_weight = 0.0f;
_mc_pitch_weight = 0.0f; _mc_pitch_weight = 0.0f;
src/modules/vtol_att_control/vtol_att_control_main.h
-1
View File
@@ -50,7 +50,6 @@
#pragma once #pragma once
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/geo/geo.h> #include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
src/modules/vtol_att_control/vtol_att_control_params.c
-46
View File
@@ -39,18 +39,6 @@
* @author Sander Smeets <sander@droneslab.com> * @author Sander Smeets <sander@droneslab.com>
*/ */
/**
* 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) * 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); 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. * 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); 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. * Minimum pitch angle during hover.
* *
src/modules/vtol_att_control/vtol_type.cpp
+2 -251
View File
@@ -75,72 +75,14 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
_airspeed_validated = _attc->get_airspeed(); _airspeed_validated = _attc->get_airspeed();
_tecs_status = _attc->get_tecs_status(); _tecs_status = _attc->get_tecs_status();
_land_detected = _attc->get_land_detected(); _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() 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); _flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRateVtol);
_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRateVtol); _spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRateVtol);
return true; return true;
} }
void VtolType::parameters_update() void VtolType::parameters_update()
@@ -156,13 +98,11 @@ void VtolType::parameters_update()
void VtolType::update_mc_state() void VtolType::update_mc_state()
{ {
if (!_flag_idle_mc) { if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc(); _flag_idle_mc = true;
} }
resetAccelToPitchPitchIntegrator(); resetAccelToPitchPitchIntegrator();
VtolType::set_all_motor_state(motor_state::ENABLED);
// copy virtual attitude setpoint to real attitude setpoint // copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); 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() void VtolType::update_fw_state()
{ {
if (_flag_idle_mc) { if (_flag_idle_mc) {
_flag_idle_mc = !set_idle_fw(); _flag_idle_mc = false;
} }
resetAccelToPitchPitchIntegrator(); resetAccelToPitchPitchIntegrator();
_last_thr_in_fw_mode = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; _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 // copy virtual attitude setpoint to real attitude setpoint
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_mc_roll_weight = 0.0f; _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() float VtolType::pusher_assist()
{ {
// Altitude above ground is distance sensor altitude if available, otherwise local z-position // Altitude above ground is distance sensor altitude if available, otherwise local z-position
src/modules/vtol_att_control/vtol_type.h
+1 -83
View File
@@ -44,7 +44,6 @@
#define VTOL_TYPE_H #define VTOL_TYPE_H
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <lib/slew_rate/SlewRate.hpp> #include <lib/slew_rate/SlewRate.hpp>
#include <px4_platform_common/module_params.h> #include <px4_platform_common/module_params.h>
@@ -78,17 +77,6 @@ enum VtolForwardActuationMode {
ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND 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 * @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 VtolAttitudeControl;
class VtolType: public ModuleParams class VtolType : public ModuleParams
{ {
public: public:
@@ -233,9 +221,6 @@ protected:
bool _tecs_running = false; bool _tecs_running = false;
hrt_abstime _tecs_running_ts = 0; 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; hrt_abstime _last_loop_ts = 0;
float _transition_dt = 0; float _transition_dt = 0;
@@ -243,29 +228,6 @@ protected:
bool _quadchute_command_treated{false}; 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(); float update_and_get_backtransition_pitch_sp();
SlewRate<float> _spoiler_setpoint_with_slewrate; SlewRate<float> _spoiler_setpoint_with_slewrate;
@@ -304,57 +266,13 @@ protected:
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, (ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
(ParamFloat<px4::params::VT_LND_PITCH_MIN>) _param_vt_lnd_pitch_min, (ParamFloat<px4::params::VT_LND_PITCH_MIN>) _param_vt_lnd_pitch_min,
(ParamBool<px4::params::SYS_CTRL_ALLOC>) _param_sys_ctrl_alloc,
(ParamInt<px4::params::VT_IDLE_PWM_MC>) _param_vt_idle_pwm_mc,
(ParamInt<px4::params::VT_MOT_ID>) _param_vt_mot_id,
(ParamBool<px4::params::VT_MC_ON_FMU>) _param_vt_mc_on_fmu,
(ParamInt<px4::params::VT_FW_MOT_OFFID>) _param_vt_fw_mot_offid,
(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld (ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld
) )
private: private:
hrt_abstime _throttle_blend_start_ts{0}; // time at which we start blending between transition throttle and fixed wing throttle 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; } void resetAccelToPitchPitchIntegrator() { _accel_to_pitch_integ = 0.f; }
bool shouldBlendThrottleAfterFrontTransition() { return _throttle_blend_start_ts != 0; }; bool shouldBlendThrottleAfterFrontTransition() { return _throttle_blend_start_ts != 0; };