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

pwm_out, dshot: remove mode_* handling and capture ioctl's

Browse Source 
They now use all unused pins, whereas camera_capture, camera_trigger and
pwm_input modules start before to reserve their pins.
This commit is contained in:
Beat Küng 2021-09-08 12:14:38 +02:00 committed by Daniel Agar
parent 062dd28f4d
commit 1ee423e3c8
10 changed files with 71 additions and 2263 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -21,8 +21,6 @@ then
fi
# initialize script variables
set AUX_MODE none
set AUX_BANK2 none
set IO_PRESENT no
set MAV_TYPE none
set MIXER none

94
ROMFS/px4fmu_common/init.d/rc.interface
View File

@ -29,12 +29,6 @@ then
fi
fi
# USE_IO is set to 'no' for all boards w/o px4io driver or SYS_USE_IO disabled
if [ $USE_IO = no -a $AUX_BANK2 = none ]
then
set AUX_MODE none
fi
#
# Set the default output mode if none was set.
#
@ -76,15 +70,6 @@ then
fi
fi
if [ $OUTPUT_MODE = $OUTPUT_CMD ]
then
if ! $OUTPUT_CMD mode_$FMU_MODE
then
echo "$OUTPUT_CMD start failed"
tune_control play error
fi
fi
if [ $OUTPUT_MODE = uavcan_esc ]
then
if param compare UAVCAN_ENABLE 0
@ -98,6 +83,15 @@ then
. ${R}etc/init.d/rc.io
fi
if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ]
then
if ! $OUTPUT_CMD start
then
echo "$OUTPUT_CMD start failed"
tune_control play error
fi
fi
#
# Start IO for RC input if needed.
#
@ -161,7 +155,7 @@ else
fi
fi
if [ $MIXER_AUX != none -a $AUX_MODE != none ]
if [ $MIXER_AUX != none ]
then
#
# Load aux mixer.
@ -179,34 +173,26 @@ then
if [ $MIXER_AUX_FILE != none ]
then
# Start the output module
if $OUTPUT_CMD mode_${AUX_MODE}
# Append aux mixer to main device.
if param greater SYS_HITL 0
then
# Append aux mixer to main device.
if param greater SYS_HITL 0
if mixer append ${OUTPUT_DEV} ${MIXER_AUX_FILE}
then
if mixer append ${OUTPUT_DEV} ${MIXER_AUX_FILE}
then
echo "INFO [init] Mixer: ${MIXER_AUX_FILE} appended to ${OUTPUT_DEV}"
else
echo "ERROR [init] Failed appending mixer: ${MIXER_AUX_FILE}"
fi
fi
if [ -e $OUTPUT_AUX_DEV -a $OUTPUT_MODE != hil ]
then
if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE}
then
echo "INFO [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}"
else
echo "ERROR [init] Failed loading mixer: ${MIXER_AUX_FILE}"
fi
echo "INFO [init] Mixer: ${MIXER_AUX_FILE} appended to ${OUTPUT_DEV}"
else
echo "INFO [init] setting PWM_AUX_OUT none"
set PWM_AUX_OUT none
echo "ERROR [init] Failed appending mixer: ${MIXER_AUX_FILE}"
fi
fi
if [ -e $OUTPUT_AUX_DEV -a $OUTPUT_MODE != hil ]
then
if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE}
then
echo "INFO [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}"
else
echo "ERROR [init] Failed loading mixer: ${MIXER_AUX_FILE}"
fi
else
echo "ERROR: Could not start: pwm_out mode_pwm"
tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
echo "INFO [init] setting PWM_AUX_OUT none"
set PWM_AUX_OUT none
fi
@ -228,36 +214,6 @@ fi
param set PWM_AUX_OUT ${PWM_AUX_OUT}
if [ $MIXER_AUX != none -a $AUX_MODE = none -a -e $OUTPUT_AUX_DEV ]
then
#
# Load aux mixer.
#
if [ -f ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.aux.mix ]
then
set MIXER_AUX_FILE ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.aux.mix
else
if [ -f /etc/mixers/${MIXER_AUX}.aux.mix ]
then
set MIXER_AUX_FILE /etc/mixers/${MIXER_AUX}.aux.mix
fi
fi
if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE}
then
echo "INFO [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}"
# Set PWM_AUX output frequency.
if [ $PWM_AUX_RATE != none ]
then
pwm rate -c ${PWM_AUX_OUT} -r ${PWM_AUX_RATE} -d ${OUTPUT_AUX_DEV}
fi
else
echo "ERROR [init] Failed loading mixer: ${MIXER_AUX_FILE}"
fi
fi
if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ]
then
if [ $PWM_OUT != none ]

63
ROMFS/px4fmu_common/init.d/rcS
View File

@ -22,10 +22,8 @@ set +e
#
set R /
set AUTOCNF no
set AUX_MODE pwm
set FCONFIG /fs/microsd/etc/config.txt
set FEXTRAS /fs/microsd/etc/extras.txt
set FMU_MODE pwm
set FRC /fs/microsd/etc/rc.txt
set IOFW "/etc/extras/px4_io-v2_default.bin"
set IO_PRESENT no
@ -376,52 +374,9 @@ else
commander start
fi
# Sensors on the PWM interface bank.
if param compare -s SENS_EN_LL40LS 1
then
# Clear pins 5 and 6.
set FMU_MODE pwm4
set AUX_MODE pwm4
fi
# Check if ATS is enabled
if param compare FD_EXT_ATS_EN 1
then
# Clear pins 5 and 6.
set FMU_MODE pwm4
set AUX_MODE pwm4
fi
if param greater -s TRIG_MODE 0
then
if param compare TRIG_PINS_EX 0
then
# We ONLY support trigger on pins 5+6 or 7+8 when simultanously using AUX for actuator output.
if param compare TRIG_PINS 56
then
# clear pins 5 and 6
set FMU_MODE pwm4
set AUX_MODE pwm4
else
if param compare TRIG_PINS 78
then
# clear pins 7 and 8
set FMU_MODE pwm6
set AUX_MODE pwm6
else
set FMU_MODE none
set AUX_MODE none
fi
fi
else
if param compare TRIG_PINS_EX 12288
then
set FMU_MODE pwm12
set AUX_MODE pwm12
fi
fi
camera_trigger start
camera_feedback start
fi
@ -449,14 +404,7 @@ else
rc_input start $RC_INPUT_ARGS
fi
#
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface,
# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
#
. ${R}etc/init.d/rc.vehicle_setup
# Camera capture driver
# Camera capture driver (before pwm_out)
if param greater -s CAM_CAP_FBACK 0
then
if camera_capture start
@ -465,6 +413,13 @@ else
fi
fi
#
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface,
# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
#
. ${R}etc/init.d/rc.vehicle_setup
#
# Start the navigator.
#
@ -564,10 +519,8 @@ fi
#
unset R
unset AUTOCNF
unset AUX_MODE
unset FCONFIG
unset FEXTRAS
unset FMU_MODE
unset FRC
unset IO_PRESENT
unset IOFW

7
src/drivers/camera_trigger/camera_trigger_params.c
View File

@ -131,15 +131,14 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0);
* Camera trigger pin
*
* Selects which FMU pin is used (range: AUX1-AUX8 on Pixhawk controllers with an I/O board,
* MAIN1-MAIN8 on controllers without an I/O board. The PWM interface takes two pins per camera, while relay
* MAIN1-MAIN8 on controllers without an I/O board).
*
* The PWM interface takes two pins per camera, while relay
* triggers on every pin individually. Example: Value 56 would trigger on pins 5 and 6.
* For GPIO mode Pin 6 will be triggered followed by 5. With a value of 65 pin 5 will
* be triggered followed by 6. Pins may be non contiguous. I.E. 16 or 61.
* In GPIO mode the delay pin to pin is < .2 uS.
*
* Note: only with a value of 56 or 78 it is possible to use the lower pins for
* actuator outputs (e.g. ESC's).
*
* @min 1
* @max 12345678
* @decimal 0

106
src/drivers/drv_input_capture.h
View File

@ -48,26 +48,10 @@
__BEGIN_DECLS
/**
* Path for the default capture input device.
*
*
*/
#define INPUT_CAPTURE_BASE_DEVICE_PATH "/dev/capture"
#define INPUT_CAPTURE0_DEVICE_PATH "/dev/capture0"
typedef void (*capture_callback_t)(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
/**
* Maximum number of PWM input channels supported by the device.
*/
#ifndef INPUT_CAPTURE_MAX_CHANNELS
#define INPUT_CAPTURE_MAX_CHANNELS 6
#endif
typedef uint16_t capture_filter_t;
typedef uint16_t capture_t;
typedef enum input_capture_edge {
Disabled = 0,
@ -76,12 +60,6 @@ typedef enum input_capture_edge {
Both = 3
} input_capture_edge;
typedef struct input_capture_element_t {
hrt_abstime time_stamp;
input_capture_edge edge;
bool overrun;
} input_capture_element_t;
typedef struct input_capture_stats_t {
uint32_t edges;
uint32_t overflows;
@ -90,90 +68,6 @@ typedef struct input_capture_stats_t {
uint16_t latency;
} input_capture_stats_t;
/**
* input capture values for a channel
*
* This allows for Capture input driver values to be set without a
* param_get() dependency
*/
typedef struct input_capture_config_t {
uint8_t channel;
capture_filter_t filter;
input_capture_edge edge;
capture_callback_t callback;
void *context;
} input_capture_config_t;
/*
* ioctl() definitions
*
* Note that ioctls and ORB updates should not be mixed, as the
* behaviour of the system in this case is not defined.
*/
#define _INPUT_CAP_BASE 0x2d00
/** Set Enable a channel arg is pointer to input_capture_config
* with all parameters set.
* edge controls the mode: Disable will free the capture channel.
* (When edge is Disabled call back and context are ignored)
* context may be null. If callback and context are null the
* callback will be disabled.
* */
#define INPUT_CAP_SET _PX4_IOC(_INPUT_CAP_BASE, 0)
/** Set the call back on a capture channel - arg is pointer to
* input_capture_config with channel call back and context set
* context may be null. If both ate null the call back will be
* disabled */
#define INPUT_CAP_SET_CALLBACK _PX4_IOC(_INPUT_CAP_BASE, 1)
/** Get the call back on a capture channel - arg is pointer to
* input_capture_config with channel set.
*/
#define INPUT_CAP_GET_CALLBACK _PX4_IOC(_INPUT_CAP_BASE, 2)
/** Set Edge a channel arg is pointer to input_capture_config
* with channel and edge set */
#define INPUT_CAP_SET_EDGE _PX4_IOC(_INPUT_CAP_BASE, 3)
/** Get Edge for a channel arg is pointer to input_capture_config
* with channel set */
#define INPUT_CAP_GET_EDGE _PX4_IOC(_INPUT_CAP_BASE, 4)
/** Set Filter input filter channel arg is pointer to input_capture_config
* with channel and filter set */
#define INPUT_CAP_SET_FILTER _PX4_IOC(_INPUT_CAP_BASE, 5)
/** Set Filter input filter channel arg is pointer to input_capture_config
* with channel set */
#define INPUT_CAP_GET_FILTER _PX4_IOC(_INPUT_CAP_BASE, 6)
/** Get the number of capture in *(unsigned *)arg */
#define INPUT_CAP_GET_COUNT _PX4_IOC(_INPUT_CAP_BASE, 7)
/** Set the number of capture in (unsigned)arg - allows change of
* split between servos and capture */
#define INPUT_CAP_SET_COUNT _PX4_IOC(_INPUT_CAP_BASE, 8)
/** Get channel stats - arg is pointer to input_capture_config
* with channel set.
*/
#define INPUT_CAP_GET_STATS _PX4_IOC(_INPUT_CAP_BASE, 9)
/** Get channel stats - arg is pointer to input_capture_config
* with channel set.
*/
#define INPUT_CAP_GET_CLR_STATS _PX4_IOC(_INPUT_CAP_BASE, 10)
/*
*
*
* WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE!
*
*
*/
__EXPORT int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter,
capture_callback_t callback, void *context);

20
src/drivers/drv_pwm_output.h
View File

@ -218,25 +218,7 @@ typedef uint16_t servo_position_t;
/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _PX4_IOC(_PWM_SERVO_BASE, 32)
/** set auxillary output mode. These correspond to enum Mode in px4fmu/fmu.cpp */
#define PWM_SERVO_MODE_NONE 0
#define PWM_SERVO_MODE_1PWM 1
#define PWM_SERVO_MODE_2PWM 2
#define PWM_SERVO_MODE_2PWM2CAP 3
#define PWM_SERVO_MODE_3PWM 4
#define PWM_SERVO_MODE_3PWM1CAP 5
#define PWM_SERVO_MODE_4PWM 6
#define PWM_SERVO_MODE_4PWM1CAP 7
#define PWM_SERVO_MODE_4PWM2CAP 8
#define PWM_SERVO_MODE_5PWM 9
#define PWM_SERVO_MODE_5PWM1CAP 10
#define PWM_SERVO_MODE_6PWM 11
#define PWM_SERVO_MODE_8PWM 12
#define PWM_SERVO_MODE_12PWM 13
#define PWM_SERVO_MODE_14PWM 14
#define PWM_SERVO_MODE_4CAP 15
#define PWM_SERVO_MODE_5CAP 16
#define PWM_SERVO_MODE_6CAP 17
/** set auxillary output mode */
#define PWM_SERVO_ENTER_TEST_MODE 18
#define PWM_SERVO_EXIT_TEST_MODE 19
#define PWM_SERVO_SET_MODE _PX4_IOC(_PWM_SERVO_BASE, 34)

849
src/drivers/dshot/DShot.cpp
View File

@ -78,6 +78,8 @@ int DShot::init()
_mixing_output.setDriverInstance(_class_instance);
_output_mask = (1u << _num_outputs) - 1;
// Getting initial parameter values
update_params();
@ -86,186 +88,6 @@ int DShot::init()
return OK;
}
int DShot::set_mode(const Mode mode)
{
unsigned old_mask = _output_mask;
/*
* Configure for output.
*
* Note that regardless of the configured mode, the task is always
* listening and mixing; the mode just selects which of the channels
* are presented on the output pins.
*/
switch (mode) {
case MODE_1PWM:
// default output rates
_output_mask = 0x1;
_outputs_initialized = false;
_num_outputs = 1;
break;
#if defined(BOARD_HAS_CAPTURE)
case MODE_2PWM2CAP: // v1 multi-port with flow control lines as PWM
up_input_capture_set(2, Rising, 0, NULL, NULL);
up_input_capture_set(3, Rising, 0, NULL, NULL);
PX4_DEBUG("MODE_2PWM2CAP");
#endif
// FALLTHROUGH
case MODE_2PWM: // v1 multi-port with flow control lines as PWM
PX4_DEBUG("MODE_2PWM");
// default output rates
_output_mask = 0x3;
_outputs_initialized = false;
_num_outputs = 2;
break;
#if defined(BOARD_HAS_CAPTURE)
case MODE_3PWM1CAP: // v1 multi-port with flow control lines as PWM
PX4_DEBUG("MODE_3PWM1CAP");
up_input_capture_set(3, Rising, 0, NULL, NULL);
#endif
// FALLTHROUGH
case MODE_3PWM: // v1 multi-port with flow control lines as PWM
PX4_DEBUG("MODE_3PWM");
// default output rates
_output_mask = 0x7;
_outputs_initialized = false;
_num_outputs = 3;
break;
#if defined(BOARD_HAS_CAPTURE)
case MODE_4PWM1CAP:
PX4_DEBUG("MODE_4PWM1CAP");
up_input_capture_set(4, Rising, 0, NULL, NULL);
#endif
// FALLTHROUGH
case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs
PX4_DEBUG("MODE_4PWM");
// default output rates
_output_mask = 0xf;
_outputs_initialized = false;
_num_outputs = 4;
break;
#if defined(BOARD_HAS_CAPTURE)
case MODE_4PWM2CAP:
PX4_DEBUG("MODE_4PWM2CAP");
up_input_capture_set(5, Rising, 0, NULL, NULL);
// default output rates
_output_mask = 0x0f;
_outputs_initialized = false;
_num_outputs = 4;
break;
#endif
#if defined(BOARD_HAS_CAPTURE)
case MODE_5PWM1CAP:
PX4_DEBUG("MODE_5PWM1CAP");
up_input_capture_set(5, Rising, 0, NULL, NULL);
#endif
// FALLTHROUGH
case MODE_5PWM: // v1 or v2 multi-port as 5 PWM outs
PX4_DEBUG("MODE_5PWM");
// default output rates
_output_mask = 0x1f;
_outputs_initialized = false;
_num_outputs = 5;
break;
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
case MODE_6PWM:
PX4_DEBUG("MODE_6PWM");
// default output rates
_output_mask = 0x3f;
_outputs_initialized = false;
_num_outputs = 6;
break;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
PX4_DEBUG("MODE_8PWM");
// default output rates
_output_mask = 0xff;
_outputs_initialized = false;
_num_outputs = 8;
break;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12
case MODE_12PWM:
PX4_DEBUG("MODE_12PWM");
// default output rates
_output_mask = 0xfff;
_outputs_initialized = false;
_num_outputs = 12;
break;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
case MODE_14PWM:
PX4_DEBUG("MODE_14PWM");
// default output rates
_output_mask = 0x3fff;
_outputs_initialized = false;
_num_outputs = 14;
break;
#endif
case MODE_NONE:
PX4_DEBUG("MODE_NONE");
_output_mask = 0x0;
_outputs_initialized = false;
_num_outputs = 0;
if (old_mask != _output_mask) {
// disable motor outputs
enable_dshot_outputs(false);
}
break;
default:
return -EINVAL;
}
_mode = mode;
return OK;
}
int DShot::task_spawn(int argc, char *argv[])
{
DShot *instance = new DShot();
@ -289,24 +111,9 @@ int DShot::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
void DShot::capture_trampoline(void *context, const uint32_t channel_index, const hrt_abstime edge_time,
const uint32_t edge_state, const uint32_t overflow)
{
DShot *dev = static_cast<DShot *>(context);
dev->capture_callback(channel_index, edge_time, edge_state, overflow);
}
void DShot::capture_callback(const uint32_t channel_index, const hrt_abstime edge_time,
const uint32_t edge_state, const uint32_t overflow)
{
fprintf(stdout, "DShot: Capture chan:%" PRId32 " time:%" PRId64 " state:%" PRId32 " overflow:%" PRId32 "\n",
channel_index, edge_time, edge_state,
overflow);
}
void DShot::enable_dshot_outputs(const bool enabled)
{
if (enabled && !_outputs_initialized && _output_mask != 0) {
if (enabled && !_outputs_initialized) {
DShotConfig config = (DShotConfig)_param_dshot_config.get();
unsigned int dshot_frequency = DSHOT600;
@ -644,57 +451,6 @@ void DShot::update_params()
}
int DShot::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret;
// try it as a Capture ioctl next
ret = capture_ioctl(filp, cmd, arg);
if (ret != -ENOTTY) {
return ret;
}
// if we are in valid PWM mode, try it as a PWM ioctl as well
switch (_mode) {
case MODE_1PWM:
case MODE_2PWM:
case MODE_3PWM:
case MODE_4PWM:
case MODE_5PWM:
case MODE_2PWM2CAP:
case MODE_3PWM1CAP:
case MODE_4PWM1CAP:
case MODE_4PWM2CAP:
case MODE_5PWM1CAP:
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
case MODE_6PWM:
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
case MODE_8PWM:
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12
case MODE_12PWM:
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
case MODE_14PWM:
#endif
ret = pwm_ioctl(filp, cmd, arg);
break;
default:
PX4_DEBUG("not in a PWM mode");
break;
}
// if nobody wants it, let CDev have it
if (ret == -ENOTTY) {
ret = CDev::ioctl(filp, cmd, arg);
}
return ret;
}
int DShot::pwm_ioctl(file *filp, const int cmd, const unsigned long arg)
{
int ret = OK;
@ -703,145 +459,8 @@ int DShot::pwm_ioctl(file *filp, const int cmd, const unsigned long arg)
lock();
switch (cmd) {
case PWM_SERVO_GET_COUNT:
switch (_mode) {
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
case MODE_14PWM:
*(unsigned *)arg = 14;
break;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12
case MODE_12PWM:
*(unsigned *)arg = 12;
break;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
case MODE_8PWM:
*(unsigned *)arg = 8;
break;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
case MODE_6PWM:
*(unsigned *)arg = 6;
break;
#endif
case MODE_5PWM:
case MODE_5PWM1CAP:
*(unsigned *)arg = 5;
break;
case MODE_4PWM:
case MODE_4PWM1CAP:
case MODE_4PWM2CAP:
*(unsigned *)arg = 4;
break;
case MODE_3PWM:
case MODE_3PWM1CAP:
*(unsigned *)arg = 3;
break;
case MODE_2PWM:
case MODE_2PWM2CAP:
*(unsigned *)arg = 2;
break;
case MODE_1PWM:
*(unsigned *)arg = 1;
break;
default:
ret = -EINVAL;
break;
}
break;
case PWM_SERVO_SET_MODE: {
switch (arg) {
case PWM_SERVO_MODE_NONE:
ret = set_mode(MODE_NONE);
break;
case PWM_SERVO_MODE_1PWM:
ret = set_mode(MODE_1PWM);
break;
case PWM_SERVO_MODE_2PWM:
ret = set_mode(MODE_2PWM);
break;
case PWM_SERVO_MODE_2PWM2CAP:
ret = set_mode(MODE_2PWM2CAP);
break;
case PWM_SERVO_MODE_3PWM:
ret = set_mode(MODE_3PWM);
break;
case PWM_SERVO_MODE_3PWM1CAP:
ret = set_mode(MODE_3PWM1CAP);
break;
case PWM_SERVO_MODE_4PWM:
ret = set_mode(MODE_4PWM);
break;
case PWM_SERVO_MODE_4PWM1CAP:
ret = set_mode(MODE_4PWM1CAP);
break;
case PWM_SERVO_MODE_4PWM2CAP:
ret = set_mode(MODE_4PWM2CAP);
break;
case PWM_SERVO_MODE_5PWM:
ret = set_mode(MODE_5PWM);
break;
case PWM_SERVO_MODE_5PWM1CAP:
ret = set_mode(MODE_5PWM1CAP);
break;
case PWM_SERVO_MODE_6PWM:
ret = set_mode(MODE_6PWM);
break;
case PWM_SERVO_MODE_8PWM:
ret = set_mode(MODE_8PWM);
break;
case PWM_SERVO_MODE_4CAP:
ret = set_mode(MODE_4CAP);
break;
case PWM_SERVO_MODE_5CAP:
ret = set_mode(MODE_5CAP);
break;
case PWM_SERVO_MODE_6CAP:
ret = set_mode(MODE_6CAP);
break;
default:
ret = -EINVAL;
}
break;
}
case MIXERIOCRESET:
_mixing_output.resetMixerThreadSafe();
break;
case MIXERIOCLOADBUF: {
@ -859,303 +478,16 @@ int DShot::pwm_ioctl(file *filp, const int cmd, const unsigned long arg)
unlock();
// if nobody wants it, let CDev have it
if (ret == -ENOTTY) {
ret = CDev::ioctl(filp, cmd, arg);
}
return ret;
}
int DShot::capture_ioctl(file *filp, const int cmd, const unsigned long arg)
{
int ret = -EINVAL;
#if defined(BOARD_HAS_CAPTURE)
lock();
input_capture_config_t *pconfig = 0;
input_capture_stats_t *stats = (input_capture_stats_t *)arg;
if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP ||
_mode == MODE_4PWM1CAP || _mode == MODE_5PWM1CAP ||
_mode == MODE_4PWM2CAP) {
pconfig = (input_capture_config_t *)arg;
}
switch (cmd) {
case INPUT_CAP_SET:
if (pconfig) {
ret = up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter,
pconfig->callback, pconfig->context);
}
break;
case INPUT_CAP_SET_CALLBACK:
if (pconfig) {
ret = up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context);
}
break;
case INPUT_CAP_GET_CALLBACK:
if (pconfig) {
ret = up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context);
}
break;
case INPUT_CAP_GET_STATS:
if (arg) {
ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, false);
}
break;
case INPUT_CAP_GET_CLR_STATS:
if (arg) {
ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, true);
}
break;
case INPUT_CAP_SET_EDGE:
if (pconfig) {
ret = up_input_capture_set_trigger(pconfig->channel, pconfig->edge);
}
break;
case INPUT_CAP_GET_EDGE:
if (pconfig) {
ret = up_input_capture_get_trigger(pconfig->channel, &pconfig->edge);
}
break;
case INPUT_CAP_SET_FILTER:
if (pconfig) {
ret = up_input_capture_set_filter(pconfig->channel, pconfig->filter);
}
break;
case INPUT_CAP_GET_FILTER:
if (pconfig) {
ret = up_input_capture_get_filter(pconfig->channel, &pconfig->filter);
}
break;
case INPUT_CAP_GET_COUNT:
ret = OK;
switch (_mode) {
case MODE_5PWM1CAP:
case MODE_4PWM1CAP:
case MODE_3PWM1CAP:
*(unsigned *)arg = 1;
break;
case MODE_2PWM2CAP:
case MODE_4PWM2CAP:
*(unsigned *)arg = 2;
break;
default:
ret = -EINVAL;
break;
}
break;
case INPUT_CAP_SET_COUNT:
ret = OK;
switch (_mode) {
case MODE_3PWM1CAP:
set_mode(MODE_3PWM1CAP);
break;
case MODE_2PWM2CAP:
set_mode(MODE_2PWM2CAP);
break;
case MODE_4PWM1CAP:
set_mode(MODE_4PWM1CAP);
break;
case MODE_4PWM2CAP:
set_mode(MODE_4PWM2CAP);
break;
case MODE_5PWM1CAP:
set_mode(MODE_5PWM1CAP);
break;
default:
ret = -EINVAL;
break;
}
break;
default:
ret = -ENOTTY;
break;
}
unlock();
#else
ret = -ENOTTY;
#endif
return ret;
}
int DShot::module_new_mode(const PortMode new_mode)
{
if (!is_running()) {
return -1;
}
DShot::Mode mode;
mode = DShot::MODE_NONE;
switch (new_mode) {
case PORT_FULL_GPIO:
case PORT_MODE_UNSET:
break;
case PORT_FULL_PWM:
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 4
// select 4-pin PWM mode
mode = DShot::MODE_4PWM;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 5
mode = DShot::MODE_5PWM;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 6
mode = DShot::MODE_6PWM;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 8
mode = DShot::MODE_8PWM;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 12
mode = DShot::MODE_12PWM;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 14
mode = DShot::MODE_14PWM;
#endif
break;
case PORT_PWM1:
// select 2-pin PWM mode
mode = DShot::MODE_1PWM;
break;
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
case PORT_PWM8:
// select 8-pin PWM mode
mode = DShot::MODE_8PWM;
break;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
case PORT_PWM6:
// select 6-pin PWM mode
mode = DShot::MODE_6PWM;
break;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5
case PORT_PWM5:
// select 5-pin PWM mode
mode = DShot::MODE_5PWM;
break;
# if defined(BOARD_HAS_CAPTURE)
case PORT_PWM5CAP1:
// select 5-pin PWM mode 1 capture
mode = DShot::MODE_5PWM1CAP;
break;
# endif
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 4
case PORT_PWM4:
// select 4-pin PWM mode
mode = DShot::MODE_4PWM;
break;
# if defined(BOARD_HAS_CAPTURE)
case PORT_PWM4CAP1:
// select 4-pin PWM mode 1 capture
mode = DShot::MODE_4PWM1CAP;
break;
case PORT_PWM4CAP2:
// select 4-pin PWM mode 2 capture
mode = DShot::MODE_4PWM2CAP;
break;
# endif
case PORT_PWM3:
// select 3-pin PWM mode
mode = DShot::MODE_3PWM;
break;
# if defined(BOARD_HAS_CAPTURE)
case PORT_PWM3CAP1:
// select 3-pin PWM mode 1 capture
mode = DShot::MODE_3PWM1CAP;
break;
# endif
case PORT_PWM2:
// select 2-pin PWM mode
mode = DShot::MODE_2PWM;
break;
# if defined(BOARD_HAS_CAPTURE)
case PORT_PWM2CAP2:
// select 2-pin PWM mode 2 capture
mode = DShot::MODE_2PWM2CAP;
break;
# endif
#endif
default:
return -1;
}
DShot *object = get_instance();
if (mode != object->get_mode()) {
// (re)set the output mode
object->set_mode(mode);
}
return OK;
}
int DShot::custom_command(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNSET;
const char *verb = argv[0];
if (!strcmp(verb, "telemetry")) {
@ -1244,142 +576,11 @@ int DShot::custom_command(int argc, char *argv[])
}
}
/*
* Mode switches.
*/
if (!strcmp(verb, "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
// mode: defines which outputs to drive (others may be used by other tasks such as camera capture)
#if defined(BOARD_HAS_PWM)
} else if (!strcmp(verb, "mode_pwm1")) {
new_mode = PORT_PWM1;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
} else if (!strcmp(verb, "mode_pwm6")) {
new_mode = PORT_PWM6;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5
} else if (!strcmp(verb, "mode_pwm5")) {
new_mode = PORT_PWM5;
# if defined(BOARD_HAS_CAPTURE)
} else if (!strcmp(verb, "mode_pwm5cap1")) {
new_mode = PORT_PWM5CAP1;
# endif
} else if (!strcmp(verb, "mode_pwm4")) {
new_mode = PORT_PWM4;
# if defined(BOARD_HAS_CAPTURE)
} else if (!strcmp(verb, "mode_pwm4cap1")) {
new_mode = PORT_PWM4CAP1;
} else if (!strcmp(verb, "mode_pwm4cap2")) {
new_mode = PORT_PWM4CAP2;
# endif
} else if (!strcmp(verb, "mode_pwm3")) {
new_mode = PORT_PWM3;
# if defined(BOARD_HAS_CAPTURE)
} else if (!strcmp(verb, "mode_pwm3cap1")) {
new_mode = PORT_PWM3CAP1;
# endif
} else if (!strcmp(verb, "mode_pwm2")) {
new_mode = PORT_PWM2;
# if defined(BOARD_HAS_CAPTURE)
} else if (!strcmp(verb, "mode_pwm2cap2")) {
new_mode = PORT_PWM2CAP2;
# endif
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
} else if (!strcmp(verb, "mode_pwm8")) {
new_mode = PORT_PWM8;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12
} else if (!strcmp(verb, "mode_pwm12")) {
new_mode = PORT_PWM12;
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
} else if (!strcmp(verb, "mode_pwm14")) {
new_mode = PORT_PWM14;
#endif
}
// was a new mode set?
if (new_mode != PORT_MODE_UNSET) {
// switch modes
return DShot::module_new_mode(new_mode);
}
return print_usage("unknown command");
}
int DShot::print_status()
{
const char *mode_str = nullptr;
switch (_mode) {
case MODE_NONE: mode_str = "no outputs"; break;
case MODE_1PWM: mode_str = "outputs1"; break;
case MODE_2PWM: mode_str = "outputs2"; break;
case MODE_2PWM2CAP: mode_str = "outputs2cap2"; break;
case MODE_3PWM: mode_str = "outputs3"; break;
case MODE_3PWM1CAP: mode_str = "outputs3cap1"; break;
case MODE_4PWM: mode_str = "outputs4"; break;
case MODE_4PWM1CAP: mode_str = "outputs4cap1"; break;
case MODE_4PWM2CAP: mode_str = "outputs4cap2"; break;
case MODE_5PWM: mode_str = "outputs5"; break;
case MODE_5PWM1CAP: mode_str = "outputs5cap1"; break;
case MODE_6PWM: mode_str = "outputs6"; break;
case MODE_8PWM: mode_str = "outputs8"; break;
case MODE_4CAP: mode_str = "cap4"; break;
case MODE_5CAP: mode_str = "cap5"; break;
case MODE_6CAP: mode_str = "cap6"; break;
default:
break;
}
if (mode_str) {
PX4_INFO("Mode: %s", mode_str);
}
PX4_INFO("Outputs initialized: %s", _outputs_initialized ? "yes" : "no");
PX4_INFO("Outputs used: 0x%" PRIx32, _output_mask);
PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
@ -1406,6 +607,9 @@ int DShot::print_usage(const char *reason)
This is the DShot output driver. It is similar to the fmu driver, and can be used as drop-in replacement
to use DShot as ESC communication protocol instead of PWM.
On startup, the module tries to occupy all available pins for DShot output.
It skips all pins already in use (e.g. by a camera trigger module).
It supports:
- DShot150, DShot300, DShot600, DShot1200
- telemetry via separate UART and publishing as esc_status message
@ -1419,36 +623,7 @@ After saving, the reversed direction will be regarded as the normal one. So to r
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("dshot", "driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)");
PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start the module if not running already");
PRINT_MODULE_USAGE_COMMAND("mode_gpio");
PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "Select all available pins as PWM");
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
PRINT_MODULE_USAGE_COMMAND("mode_pwm14");
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12
PRINT_MODULE_USAGE_COMMAND("mode_pwm12");
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
PRINT_MODULE_USAGE_COMMAND("mode_pwm8");
#endif
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
PRINT_MODULE_USAGE_COMMAND("mode_pwm6");
PRINT_MODULE_USAGE_COMMAND("mode_pwm5");
PRINT_MODULE_USAGE_COMMAND("mode_pwm5cap1");
PRINT_MODULE_USAGE_COMMAND("mode_pwm4");
PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap1");
PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap2");
PRINT_MODULE_USAGE_COMMAND("mode_pwm3");
PRINT_MODULE_USAGE_COMMAND("mode_pwm3cap1");
PRINT_MODULE_USAGE_COMMAND("mode_pwm2");
PRINT_MODULE_USAGE_COMMAND("mode_pwm2cap2");
#endif
#if defined(BOARD_HAS_PWM)
PRINT_MODULE_USAGE_COMMAND("mode_pwm1");
#endif
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND_DESCR("telemetry", "Enable Telemetry on a UART");
PRINT_MODULE_USAGE_ARG("<device>", "UART device", false);

64
src/drivers/dshot/DShot.h
View File

@ -64,64 +64,13 @@ public:
DShot();
virtual ~DShot();
enum Mode {
MODE_NONE = 0,
MODE_1PWM,
MODE_2PWM,
MODE_2PWM2CAP,
MODE_3PWM,
MODE_3PWM1CAP,
MODE_4PWM,
MODE_4PWM1CAP,
MODE_4PWM2CAP,
MODE_5PWM,
MODE_5PWM1CAP,
MODE_6PWM,
MODE_8PWM,
MODE_12PWM,
MODE_14PWM,
MODE_4CAP,
MODE_5CAP,
MODE_6CAP,
};
/** Mode given via CLI */
enum PortMode {
PORT_MODE_UNSET = 0,
PORT_FULL_GPIO,
PORT_FULL_PWM,
PORT_PWM14,
PORT_PWM12,
PORT_PWM8,
PORT_PWM6,
PORT_PWM5,
PORT_PWM4,
PORT_PWM3,
PORT_PWM2,
PORT_PWM1,
PORT_PWM3CAP1,
PORT_PWM4CAP1,
PORT_PWM4CAP2,
PORT_PWM5CAP1,
PORT_PWM2CAP2,
PORT_CAPTURE,
};
static void capture_trampoline(void *context, const uint32_t channel_index, const hrt_abstime edge_time,
const uint32_t edge_state, const uint32_t overflow);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
Mode get_mode() { return _mode; }
virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
/** change the mode of the running module */
static int module_new_mode(const PortMode new_mode);
void mixerChanged() override;
/** @see ModuleBase::print_status() */
@ -141,8 +90,6 @@ public:
*/
int send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index);
int set_mode(const Mode new_mode);
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
@ -179,19 +126,12 @@ private:
int last_motor_index{-1};
};
void capture_callback(const uint32_t channel_index, const hrt_abstime edge_time,
const uint32_t edge_state, const uint32_t overflow);
int capture_ioctl(file *filp, const int cmd, const unsigned long arg);
void enable_dshot_outputs(const bool enabled);
void init_telemetry(const char *device);
void handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data);
int pwm_ioctl(file *filp, const int cmd, const unsigned long arg);
int request_esc_info();
void Run() override;
@ -215,7 +155,7 @@ private:
bool _outputs_on{false};
bool _waiting_for_esc_info{false};
unsigned _num_outputs{0};
static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
uint32_t _output_mask{0};
int _class_instance{-1};
@ -224,8 +164,6 @@ private:
Command _current_command{};
Mode _mode{MODE_NONE};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
DEFINE_PARAMETERS(

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

File diff suppressed because it is too large Load Diff

65
src/drivers/pwm_out/PWMOut.hpp
View File

@ -40,7 +40,6 @@
#include <drivers/device/device.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_input_capture.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_pwm_output.h>
#include <lib/cdev/CDev.hpp>
@ -64,33 +63,10 @@
using namespace time_literals;
/** Mode given via CLI */
enum PortMode {
PORT_MODE_UNSET = 0,
PORT_FULL_GPIO,
PORT_FULL_PWM,
PORT_PWM14,
PORT_PWM12,
PORT_PWM8,
PORT_PWM6,
PORT_PWM5,
PORT_PWM4,
PORT_PWM3,
PORT_PWM2,
PORT_PWM1,
PORT_PWM3CAP1,
PORT_PWM4CAP1,
PORT_PWM4CAP2,
PORT_PWM5CAP1,
PORT_PWM2CAP2,
PORT_CAPTURE,
};
#if !defined(BOARD_HAS_PWM)
# error "board_config.h needs to define BOARD_HAS_PWM"
#endif
// TODO: keep in sync with drivers/camera_capture
#define PX4FMU_DEVICE_PATH "/dev/px4fmu"
static constexpr int PWM_OUT_MAX_INSTANCES{(DIRECT_PWM_OUTPUT_CHANNELS > 8) ? 2 : 1};
@ -99,29 +75,6 @@ extern pthread_mutex_t pwm_out_module_mutex;
class PWMOut : public cdev::CDev, public OutputModuleInterface
{
public:
enum Mode {
MODE_NONE,
MODE_1PWM,
MODE_2PWM,
MODE_2PWM2CAP,
MODE_3PWM,
MODE_3PWM1CAP,
MODE_4PWM,
MODE_4PWM1CAP,
MODE_4PWM2CAP,
MODE_5PWM,
MODE_5PWM1CAP,
MODE_6PWM,
MODE_8PWM,
MODE_12PWM,
MODE_14PWM,
MODE_4CAP,
MODE_5CAP,
MODE_6CAP,
MODE_NO_REQUEST
};
PWMOut() = delete;
explicit PWMOut(int instance = 0, uint8_t output_base = 0);
@ -148,30 +101,20 @@ public:
static bool trylock_module() { return (pthread_mutex_trylock(&pwm_out_module_mutex) == 0); }
static void unlock_module() { pthread_mutex_unlock(&pwm_out_module_mutex); }
/** change the FMU mode of the running module */
static int fmu_new_mode(PortMode new_mode);
static int test(const char *dev);
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual int init();
int set_mode(Mode mode);
Mode get_mode() const { return _mode; }
uint32_t get_pwm_mask() const { return _pwm_mask; }
void set_pwm_mask(uint32_t mask) { _pwm_mask = mask; }
uint32_t get_alt_rate_channels() const { return _pwm_alt_rate_channels; }
unsigned get_alt_rate() const { return _pwm_alt_rate; }
unsigned get_default_rate() const { return _pwm_default_rate; }
void request_mode(Mode new_mode);
static int set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
static void capture_trampoline(void *context, uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state,
uint32_t overflow);
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;
@ -189,10 +132,6 @@ private:
MixingOutput _mixing_output{FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
Mode _mode{MODE_NONE};
px4::atomic<Mode> _new_mode_request{MODE_NO_REQUEST};
uint32_t _backup_schedule_interval_us{1_s};
unsigned _pwm_default_rate{50};
@ -216,8 +155,6 @@ private:
perf_counter_t _cycle_perf;
perf_counter_t _interval_perf;
void capture_callback(uint32_t chan_index,
hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
void update_current_rate();
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
@ -229,8 +166,6 @@ private:
static void sensor_reset(int ms);
static void peripheral_reset(int ms);
int capture_ioctl(file *filp, int cmd, unsigned long arg);
PWMOut(const PWMOut &) = delete;
PWMOut operator=(const PWMOut &) = delete;