fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol
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_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

2
ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
View File

@ -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

3
ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad
View File

@ -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

3
ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark
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_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

3
ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor
View File

@ -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

1
boards/airmind/mindpx-v2/src/init.c
View File

@ -74,7 +74,6 @@
#include <px4_platform_common/init.h>
#include <px4_platform/board_dma_alloc.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h>
/****************************************************************************

1
boards/ark/cannode/src/init.c
View File

@ -69,7 +69,6 @@
#include <px4_platform_common/init.h>
#include <px4_platform/gpio.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h>
# if defined(FLASH_BASED_PARAMS)

1
boards/px4/fmu-v4/src/init.c
View File

@ -77,7 +77,6 @@
#include <px4_platform_common/init.h>
#include <px4_platform/board_dma_alloc.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h>
/****************************************************************************

1
boards/uvify/core/src/init.c
View File

@ -77,7 +77,6 @@
#include <px4_platform_common/init.h>
#include <px4_platform/board_dma_alloc.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h>
/****************************************************************************

4
platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c
View File

@ -61,12 +61,12 @@
//#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);
}
servo_position_t up_pwm_servo_get(unsigned channel)
uint16_t up_pwm_servo_get(unsigned channel)
{
return io_channel_get_ccr(channel);
}

10
platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp
View File

@ -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++) {

4
platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c
View File

@ -62,12 +62,12 @@
#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);
}
servo_position_t up_pwm_servo_get(unsigned channel)
uint16_t up_pwm_servo_get(unsigned channel)
{
return io_channel_get_ccr(channel);
}

10
platforms/nuttx/src/px4/nxp/kinetis/led_pwm/led_pwm.cpp
View File

@ -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++) {

4
platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c
View File

@ -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);
}

10
platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp
View File

@ -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++) {

4
platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c
View File

@ -58,12 +58,12 @@
#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);
}
servo_position_t up_pwm_servo_get(unsigned channel)
uint16_t up_pwm_servo_get(unsigned channel)
{
return io_channel_get_ccr(channel);
}

2
platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
View File

@ -43,7 +43,7 @@
#include <stm32_tim.h>
#include <px4_arch/dshot.h>
#include <px4_arch/io_timer.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_dshot.h>
#define MOTOR_PWM_BIT_1 14u

4
platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c
View File

@ -62,12 +62,12 @@
#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);
}
servo_position_t up_pwm_servo_get(unsigned channel)
uint16_t up_pwm_servo_get(unsigned channel)
{
return io_channel_get_ccr(channel);
}

13
platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp
View File

@ -56,11 +56,8 @@
#include <systemlib/err.h>
#include <systemlib/px4_macros.h>
#include <drivers/drv_pwm_output.h>
#include <px4_arch/io_timer.h>
#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++) {

140
src/drivers/drv_dshot.h Normal file
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

185
src/drivers/drv_pwm_output.h
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
* 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

11
src/drivers/dshot/DShot.h
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
* modification, are permitted provided that the following conditions
@ -32,8 +32,7 @@
****************************************************************************/
#pragma once
#include <drivers/device/device.h>
#include <drivers/drv_input_capture.h>
#include <drivers/drv_dshot.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <px4_platform_common/getopt.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_MAX_THROTTLE = 1999;
class DShot : public ModuleBase<DShot>, public OutputModuleInterface
class DShot final : public ModuleBase<DShot>, 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;

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

@ -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);

22
src/drivers/pwm_out/PWMOut.hpp
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
* modification, are permitted provided that the following conditions
@ -41,10 +41,8 @@
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/cdev/CDev.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <px4_arch/io_timer.h>
#include <px4_platform_common/px4_config.h>
@ -56,22 +54,15 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/parameter_update.h>
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();

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

@ -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");

113
src/modules/commander/esc_calibration.cpp
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;
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
@ -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<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);
}
}

126
src/modules/simulation/pwm_out_sim/PWMSim.cpp
View File

@ -42,7 +42,6 @@
#include <px4_platform_common/sem.hpp>
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[])
{

5
src/modules/simulation/pwm_out_sim/PWMSim.hpp
View File

@ -52,10 +52,9 @@
#define PARAM_PREFIX "HIL_ACT"
#endif
using namespace time_literals;
class PWMSim : public cdev::CDev, public ModuleBase<PWMSim>, public OutputModuleInterface
class PWMSim : public ModuleBase<PWMSim>, 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;

4
src/modules/vtol_att_control/standard.cpp
View File

@ -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;
}
}

2
src/modules/vtol_att_control/tailsitter.cpp
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);
if (!_flag_idle_mc) {
_flag_idle_mc = set_idle_mc();
_flag_idle_mc = true;
}
if (tilt > 0.01f) {

16
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -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;

1
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -50,7 +50,6 @@
#pragma once
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>

46
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -39,18 +39,6 @@
* @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)
*
@ -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.
*

253
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -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

84
src/modules/vtol_att_control/vtol_type.h
View File

@ -44,7 +44,6 @@
#define VTOL_TYPE_H
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/mathlib/mathlib.h>
#include <lib/slew_rate/SlewRate.hpp>
#include <px4_platform_common/module_params.h>
@ -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<float> _spoiler_setpoint_with_slewrate;
@ -304,57 +266,13 @@ protected:
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
(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
)
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; };