diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index bce77bce8a..7d3784c151 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 76d9ff99e8..2faae54c77 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -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 ] diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c11307d616..4dcd2dbaf4 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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 diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index cd9de33e97..e51bacf7ba 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -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 diff --git a/src/drivers/drv_input_capture.h b/src/drivers/drv_input_capture.h index 78f069f716..b90711ff89 100644 --- a/src/drivers/drv_input_capture.h +++ b/src/drivers/drv_input_capture.h @@ -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); diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index ea2a60e157..e1090ac1bb 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -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) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index ebcba34f7a..56a75a7054 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -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(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("", "UART device", false); diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index cc9b45c0f4..a1fc6768db 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -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( diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index a0cb7aba1d..c355ce79bc 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -88,8 +88,6 @@ int PWMOut::init() return ret; } - // XXX best would be to register / de-register the device depending on modes - /* 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); @@ -104,6 +102,10 @@ int PWMOut::init() /* force a reset of the update rate */ _current_update_rate = 0; + _num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE); + _pwm_mask = ((1u << _num_outputs) - 1) << _output_base; + _mixing_output.setMaxNumOutputs(_num_outputs); + // Getting initial parameter values update_params(); @@ -112,249 +114,6 @@ int PWMOut::init() return 0; } -int PWMOut::set_mode(Mode mode) -{ - unsigned old_mask = _pwm_mask; - bool old_pwm_initialized = _pwm_initialized; - - /* - * Configure for PWM 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 */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0000'0001 << _output_base; - _pwm_initialized = false; - _num_outputs = 1; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - 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 */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0000'0011 << _output_base; - _pwm_initialized = false; - _num_outputs = 2; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - 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 */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0000'0111 << _output_base; - _pwm_initialized = false; - _num_outputs = 3; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - 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 */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0000'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 4; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - 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 */ - _pwm_default_rate = 400; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0000'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 4; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - 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 */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0001'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 5; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - case MODE_6PWM: - PX4_DEBUG("MODE_6PWM"); - - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0011'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 6; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - case MODE_8PWM: - PX4_DEBUG("MODE_8PWM"); - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'1111'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 8; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - - case MODE_12PWM: - PX4_DEBUG("MODE_12PWM"); - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'1111'1111'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 12; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - - case MODE_14PWM: - PX4_DEBUG("MODE_14PWM"); - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0011'1111'1111'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 14; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; -#endif - - case MODE_NONE: - PX4_DEBUG("MODE_NONE"); - - _pwm_default_rate = 10; /* artificially reduced output rate */ - _pwm_alt_rate = 10; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0x0; - _pwm_initialized = false; - _num_outputs = 0; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - break; - - default: - return -EINVAL; - } - - if (old_mask != _pwm_mask) { - /* disable servo outputs - no need to set rates */ - if (old_mask != 0) { - up_pwm_servo_deinit(old_mask); - _pwm_on = false; - } - - if (old_pwm_initialized != _pwm_initialized) { - _all_instances_ready.fetch_sub(1); - } - } - - _mode = mode; - return OK; -} - /* When set_pwm_rate is called from either of the 2 IOCTLs: * * PWM_SERVO_SET_UPDATE_RATE - Sets the "alternate" channel's rate to the callers's rate specified @@ -530,21 +289,6 @@ int PWMOut::task_spawn(int argc, char *argv[]) return PX4_OK; } -void PWMOut::capture_trampoline(void *context, uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) -{ - PWMOut *dev = static_cast(context); - dev->capture_callback(chan_index, edge_time, edge_state, overflow); -} - -void PWMOut::capture_callback(uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) -{ - fprintf(stdout, "FMU: Capture chan:%" PRId32 " time:%" PRId64 " state:%" PRId32 " overflow:%" PRId32 "\n", chan_index, - edge_time, edge_state, - overflow); -} - bool PWMOut::update_pwm_out_state(bool on) { if (on && !_pwm_initialized && _pwm_mask != 0) { @@ -629,11 +373,6 @@ void PWMOut::Run() // push backup schedule ScheduleDelayed(_backup_schedule_interval_us); - if (_new_mode_request.load() != MODE_NO_REQUEST) { - set_mode(_new_mode_request.load()); - _new_mode_request.store(MODE_NO_REQUEST); - } - _mixing_output.update(); /* update PWM status if armed or if disarmed PWM values are set */ @@ -843,46 +582,7 @@ void PWMOut::update_params() int PWMOut::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("pwm_out%u, not in a PWM mode", _instance); - break; - } + int ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ if (ret == -ENOTTY) { @@ -1145,64 +845,26 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET(10): case PWM_SERVO_SET(9): case PWM_SERVO_SET(8): - if (_mode < MODE_14PWM) { - ret = -EINVAL; - break; - } - #endif #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - /* FALLTHROUGH */ case PWM_SERVO_SET(7): - - /* FALLTHROUGH */ case PWM_SERVO_SET(6): - if (_mode < MODE_8PWM) { - ret = -EINVAL; - break; - } - #endif - #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - /* FALLTHROUGH */ case PWM_SERVO_SET(5): - if (_mode < MODE_6PWM) { - ret = -EINVAL; - break; - } - #endif #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - - /* FALLTHROUGH */ case PWM_SERVO_SET(4): - if (_mode < MODE_5PWM) { - ret = -EINVAL; - break; - } - #endif - - /* FALLTHROUGH */ case PWM_SERVO_SET(3): - if (_mode < MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ case PWM_SERVO_SET(2): - if (_mode < MODE_3PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ case PWM_SERVO_SET(1): case PWM_SERVO_SET(0): + if (cmd - PWM_SERVO_SET(0) >= (int)_num_outputs) { + ret = -EINVAL; + break; + } + if (arg <= 2100) { up_pwm_servo_set(cmd - PWM_SERVO_SET(0) + _output_base, arg); @@ -1220,62 +882,26 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET(10): case PWM_SERVO_GET(9): case PWM_SERVO_GET(8): - if (_mode < MODE_14PWM) { - ret = -EINVAL; - break; - } - #endif #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - /* FALLTHROUGH */ case PWM_SERVO_GET(7): case PWM_SERVO_GET(6): - if (_mode < MODE_8PWM) { - ret = -EINVAL; - break; - } - #endif - #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - /* FALLTHROUGH */ case PWM_SERVO_GET(5): - if (_mode < MODE_6PWM) { - ret = -EINVAL; - break; - } - #endif #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - - /* FALLTHROUGH */ case PWM_SERVO_GET(4): - if (_mode < MODE_5PWM) { - ret = -EINVAL; - break; - } - #endif - - /* FALLTHROUGH */ case PWM_SERVO_GET(3): - if (_mode < MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ case PWM_SERVO_GET(2): - if (_mode < MODE_3PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ 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; @@ -1305,142 +931,11 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; 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; - } - + *(unsigned *)arg = _num_outputs; 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_12PWM: - ret = set_mode(MODE_12PWM); - break; - - case PWM_SERVO_MODE_14PWM: - ret = set_mode(MODE_14PWM); - 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; - case PWM_SERVO_ENTER_TEST_MODE: _test_mode = true; break; @@ -1498,344 +993,6 @@ void PWMOut::peripheral_reset(int ms) board_peripheral_reset(ms); } -int PWMOut::capture_ioctl(struct file *filp, int cmd, 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 PWMOut::fmu_new_mode(PortMode new_mode) -{ - if (!is_running()) { - return -1; - } - - PWMOut::Mode pwm_mode0 = PWMOut::MODE_NONE; - PWMOut::Mode pwm_mode1 = PWMOut::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 */ - pwm_mode0 = PWMOut::MODE_4PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 5 - pwm_mode0 = PWMOut::MODE_5PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 6 - pwm_mode0 = PWMOut::MODE_6PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 8 - pwm_mode0 = PWMOut::MODE_8PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 12 - //pwm_mode0 = PWMOut::MODE_12PWM; - pwm_mode0 = PWMOut::MODE_8PWM; - pwm_mode1 = PWMOut::MODE_4PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 14 - //pwm_mode0 = PWMOut::MODE_14PWM; - pwm_mode0 = PWMOut::MODE_8PWM; - pwm_mode1 = PWMOut::MODE_6PWM; -#endif - break; - - case PORT_PWM1: - /* select 2-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_1PWM; - break; - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - - case PORT_PWM14: - /* select 14-pin PWM mode */ - //pwm_mode0 = PWMOut::MODE_14PWM; - pwm_mode0 = PWMOut::MODE_8PWM; - pwm_mode1 = PWMOut::MODE_6PWM; - break; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - - case PORT_PWM12: - /* select 12-pin PWM mode */ - //pwm_mode0 = PWMOut::MODE_12PWM; - pwm_mode0 = PWMOut::MODE_8PWM; - pwm_mode1 = PWMOut::MODE_4PWM; - break; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - case PORT_PWM8: - /* select 8-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_8PWM; - break; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - case PORT_PWM6: - /* select 6-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_6PWM; - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - - case PORT_PWM5: - /* select 5-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_5PWM; - break; - - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM5CAP1: - /* select 5-pin PWM mode 1 capture */ - pwm_mode0 = PWMOut::MODE_5PWM1CAP; - break; - -# endif -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 4 - - case PORT_PWM4: - /* select 4-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_4PWM; - break; - - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM4CAP1: - /* select 4-pin PWM mode 1 capture */ - pwm_mode0 = PWMOut::MODE_4PWM1CAP; - break; - - case PORT_PWM4CAP2: - /* select 4-pin PWM mode 2 capture */ - pwm_mode0 = PWMOut::MODE_4PWM2CAP; - break; - -# endif - - case PORT_PWM3: - /* select 3-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_3PWM; - break; - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM3CAP1: - /* select 3-pin PWM mode 1 capture */ - pwm_mode0 = PWMOut::MODE_3PWM1CAP; - break; -# endif - - case PORT_PWM2: - /* select 2-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_2PWM; - break; - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM2CAP2: - /* select 2-pin PWM mode 2 capture */ - pwm_mode0 = PWMOut::MODE_2PWM2CAP; - break; - -# endif -#endif - - default: - return -1; - } - - if (PWM_OUT_MAX_INSTANCES > 0) { - PWMOut *pwm0 = _objects[0].load(); // TODO: get_instance(); - - if (pwm0 && pwm_mode0 != pwm0->get_mode()) { - pwm0->request_mode(pwm_mode0); - } - } - - if (PWM_OUT_MAX_INSTANCES > 1) { - PWMOut *pwm1 = _objects[1].load(); // TODO: get_instance(); - - if (pwm1 && pwm_mode1 != pwm1->get_mode()) { - pwm1->request_mode(pwm_mode1); - } - } - - return OK; -} - -void PWMOut::request_mode(Mode new_mode) -{ - if (_new_mode_request.load() != MODE_NO_REQUEST) { - PX4_ERR("already being set"); // not expected to happen - return; - } - - _new_mode_request.store(new_mode); - ScheduleNow(); - // wait until processed - int max_time = 1000; - - while (_new_mode_request.load() != MODE_NO_REQUEST && max_time-- > 0) { - px4_usleep(1000); - } -} - namespace { @@ -1989,7 +1146,6 @@ int PWMOut::custom_command(int argc, char *argv[]) return 1; } - PortMode new_mode = PORT_MODE_UNSET; const char *verb = argv[myoptind]; /* does not operate on a FMU instance */ @@ -2046,93 +1202,6 @@ int PWMOut::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 PWMOut::fmu_new_mode(new_mode); - } - if (!strcmp(verb, "test")) { return test(dev); } @@ -2154,53 +1223,6 @@ int PWMOut::print_status() PX4_INFO("%d - Max update rate: %i Hz", _instance, _current_update_rate); - const char *mode_str = nullptr; - - switch (_mode) { - case MODE_NONE: mode_str = "no pwm"; break; - - case MODE_1PWM: mode_str = "pwm1"; break; - - case MODE_2PWM: mode_str = "pwm2"; break; - - case MODE_2PWM2CAP: mode_str = "pwm2cap2"; break; - - case MODE_3PWM: mode_str = "pwm3"; break; - - case MODE_3PWM1CAP: mode_str = "pwm3cap1"; break; - - case MODE_4PWM: mode_str = "pwm4"; break; - - case MODE_4PWM1CAP: mode_str = "pwm4cap1"; break; - - case MODE_4PWM2CAP: mode_str = "pwm4cap2"; break; - - case MODE_5PWM: mode_str = "pwm5"; break; - - case MODE_5PWM1CAP: mode_str = "pwm5cap1"; break; - - case MODE_6PWM: mode_str = "pwm6"; break; - - case MODE_8PWM: mode_str = "pwm8"; break; - - case MODE_12PWM: mode_str = "pwm12"; break; - - case MODE_14PWM: mode_str = "pwm14"; 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("%d - PWM Mode: %s", _instance, mode_str); - } - perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); _mixing_output.printStatus(); @@ -2217,65 +1239,21 @@ int PWMOut::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This module is responsible for driving the output and reading the input pins. For boards without a separate IO chip +This module is responsible for driving the output pins. For boards without a separate IO chip (eg. Pixracer), it uses the main channels. On boards with an IO chip (eg. Pixhawk), it uses the AUX channels, and the px4io driver is used for main ones. It listens on the actuator_controls topics, does the mixing and writes the PWM outputs. -The module is configured via mode_* commands. This defines which of the first N pins the driver should occupy. -By using mode_pwm4 for example, pins 5 and 6 can be used by the camera trigger driver or by a PWM rangefinder -driver. Alternatively, pwm_out can be started in one of the capture modes, and then drivers can register a capture -callback with ioctl calls. +On startup, the module tries to occupy all available pins for PWM/Oneshot output. +It skips all pins already in use (e.g. by a camera trigger module). ### Implementation By default the module runs on a work queue with a callback on the uORB actuator_controls topic. - -### Examples -It is typically started with: -$ pwm_out mode_pwm -To drive all available pins. - -Capture input (rising and falling edges) and print on the console: start pwm_out in one of the capture modes: -$ pwm_out mode_pwm3cap1 -This will enable capturing on the 4th pin. Then do: -$ pwm_out test - -Use the `pwm` command for further configurations (PWM rate, levels, ...), and the `mixer` command to load -mixer files. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("pwm_out", "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 pwm_out if not running already"); - - PRINT_MODULE_USAGE_COMMAND("mode_gpio"); -#if defined(BOARD_HAS_PWM) - PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "Select all available pins as PWM"); -# if BOARD_HAS_PWM >= 14 - PRINT_MODULE_USAGE_COMMAND("mode_pwm14"); -# endif -# if BOARD_HAS_PWM >= 12 - PRINT_MODULE_USAGE_COMMAND("mode_pwm12"); -# endif -# if BOARD_HAS_PWM >= 8 - PRINT_MODULE_USAGE_COMMAND("mode_pwm8"); -# endif -# if 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 - PRINT_MODULE_USAGE_COMMAND("mode_pwm1"); -#endif + PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND_DESCR("sensor_reset", "Do a sensor reset (SPI bus)"); PRINT_MODULE_USAGE_ARG("", "Delay time in ms between reset and re-enabling", true); @@ -2285,7 +1263,7 @@ mixer files. PRINT_MODULE_USAGE_COMMAND_DESCR("i2c", "Configure I2C clock rate"); PRINT_MODULE_USAGE_ARG(" ", "Specify the bus id (>=0) and rate in Hz", false); - PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test inputs and outputs"); + PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test outputs"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index c58655bf45..a1a7b61ad9 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -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 _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;