fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 03:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete unused PWM IOCTLs

Browse Source
This commit is contained in:
Daniel Agar
2022-09-09 09:14:09 -04:00
parent bcdd2203d3
commit ce337a3d80
35 changed files with 198 additions and 1331 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_dshot.h
+140
View File
@@ -0,0 +1,140 @@
/****************************************************************************
*
* Copyright (c) 2017-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DShot servo output interface.
*
*/
#pragma once
#include <px4_platform_common/defines.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include <board_config.h>
#include "drv_orb_dev.h"
__BEGIN_DECLS
typedef enum {
DShot_cmd_motor_stop = 0,
DShot_cmd_beacon1,
DShot_cmd_beacon2,
DShot_cmd_beacon3,
DShot_cmd_beacon4,
DShot_cmd_beacon5,
DShot_cmd_esc_info, // V2 includes settings
DShot_cmd_spin_direction_1,
DShot_cmd_spin_direction_2,
DShot_cmd_3d_mode_off,
DShot_cmd_3d_mode_on,
DShot_cmd_settings_request, // Currently not implemented
DShot_cmd_save_settings,
DShot_cmd_spin_direction_normal = 20,
DShot_cmd_spin_direction_reversed = 21,
DShot_cmd_led0_on, // BLHeli32 only
DShot_cmd_led1_on, // BLHeli32 only
DShot_cmd_led2_on, // BLHeli32 only
DShot_cmd_led3_on, // BLHeli32 only
DShot_cmd_led0_off, // BLHeli32 only
DShot_cmd_led1_off, // BLHeli32 only
DShot_cmd_led2_off, // BLHeli32 only
DShot_cmd_led4_off, // BLHeli32 only
DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off
DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off
DShot_cmd_signal_line_telemetry_disable = 32,
DShot_cmd_signal_line_continuous_erpm_telemetry = 33,
DShot_cmd_MAX = 47, // >47 are throttle values
DShot_cmd_MIN_throttle = 48,
DShot_cmd_MAX_throttle = 2047
} dshot_command_t;
/**
* Intialise the Dshot outputs using the specified configuration.
*
* @param channel_mask Bitmask of channels (LSB = channel 0) to enable.
* This allows some of the channels to remain configured
* as GPIOs or as another function. Already used channels/timers will not be configured as DShot
* @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
* @return <0 on error, the initialized channels mask.
*/
__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq);
/**
* Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method)
*/
__EXPORT extern void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry);
/**
* Set the current dshot throttle value for a channel (motor).
*
* @param channel The channel to set.
* @param throttle The output dshot throttle value in [0 = DSHOT_DISARM_VALUE, 1 = DSHOT_MIN_THROTTLE, 1999 = DSHOT_MAX_THROTTLE].
* @param telemetry If true, request telemetry from that motor
*/
static inline void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
{
dshot_motor_data_set(channel, throttle + DShot_cmd_MIN_throttle, telemetry);
}
/**
* Send DShot command to a channel (motor).
*
* @param channel The channel to set.
* @param command dshot_command_t
* @param telemetry If true, request telemetry from that motor
*/
static inline void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry)
{
dshot_motor_data_set(channel, command, telemetry);
}
/**
* Trigger dshot data transfer.
*/
__EXPORT extern void up_dshot_trigger(void);
/**
* Arm or disarm dshot outputs (This will enable/disable complete timer for safety purpose.).
*
* When disarmed, dshot output no pulse.
*
* @param armed If true, outputs are armed; if false they
* are disarmed.
*/
__EXPORT extern int up_dshot_arm(bool armed);
__END_DECLS
src/drivers/drv_pwm_output.h
+3 -182
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2015, 2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,10 +34,6 @@
/**
* @file PWM servo output interface.
*
* Servo values can be set with the PWM_SERVO_SET ioctl, by writing a
* pwm_output_values structure to the device
* Writing a value of 0 to a channel suppresses any output for that
* channel.
*/
#pragma once
@@ -52,24 +48,8 @@
__BEGIN_DECLS
/**
* Path for the default PWM output device.
*
* Note that on systems with more than one PWM output path (e.g.
* PX4FMU with PX4IO connected) there may be other devices that
* respond to this protocol.
*/
#define PWM_OUTPUT_BASE_DEVICE_PATH "/dev/pwm_output"
#define PWM_OUTPUT0_DEVICE_PATH "/dev/pwm_output0"
#define PWM_OUTPUT1_DEVICE_PATH "/dev/pwm_output1"
#define PWM_OUTPUT_MAX_CHANNELS 16
struct pwm_output_values {
uint32_t channel_count;
uint16_t values[PWM_OUTPUT_MAX_CHANNELS];
};
/* Use defaults unless the board override the defaults by providing
* PX4_PWM_ALTERNATE_RANGES and a replacement set of
* constants
@@ -123,12 +103,6 @@ struct pwm_output_values {
*/
#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
/**
* Servo output signal type, value is actual servo output pulse
* width in microseconds.
*/
typedef uint16_t servo_position_t;
/*
* ioctl() definitions
*
@@ -137,112 +111,14 @@ typedef uint16_t servo_position_t;
*/
#define _PWM_SERVO_BASE 0x2a00
/** arm all servo outputs handle by this driver */
#define PWM_SERVO_ARM _PX4_IOC(_PWM_SERVO_BASE, 0)
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _PX4_IOC(_PWM_SERVO_BASE, 1)
/** get default servo update rate */
#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 2)
/** set alternate servo update rate */
#define PWM_SERVO_SET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 3)
/** get alternate servo update rate */
#define PWM_SERVO_GET_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 4)
/** get the number of servos in *(unsigned *)arg */
#define PWM_SERVO_GET_COUNT _PX4_IOC(_PWM_SERVO_BASE, 5)
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
#define PWM_SERVO_SET_SELECT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 6)
/** check the selected update rates */
#define PWM_SERVO_GET_SELECT_UPDATE_RATE _PX4_IOC(_PWM_SERVO_BASE, 7)
/** set the 'ARM ok' bit, which activates the safety switch */
#define PWM_SERVO_SET_ARM_OK _PX4_IOC(_PWM_SERVO_BASE, 8)
/** clear the 'ARM ok' bit, which deactivates the safety switch */
#define PWM_SERVO_CLEAR_ARM_OK _PX4_IOC(_PWM_SERVO_BASE, 9)
/** start DSM bind */
#define DSM_BIND_START _PX4_IOC(_PWM_SERVO_BASE, 10)
/** get the PWM value for failsafe */
#define PWM_SERVO_GET_FAILSAFE_PWM _PX4_IOC(_PWM_SERVO_BASE, 13)
/** get the PWM value when disarmed */
#define PWM_SERVO_GET_DISARMED_PWM _PX4_IOC(_PWM_SERVO_BASE, 15)
/** set the minimum PWM value the output will send */
#define PWM_SERVO_SET_MIN_PWM _PX4_IOC(_PWM_SERVO_BASE, 16)
/** get the minimum PWM value the output will send */
#define PWM_SERVO_GET_MIN_PWM _PX4_IOC(_PWM_SERVO_BASE, 17)
/** set the maximum PWM value the output will send */
#define PWM_SERVO_SET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 18)
/** get the maximum PWM value the output will send */
#define PWM_SERVO_GET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 19)
/*
*
*
* WARNING WARNING WARNING! DO NOT EXCEED 47 IN IOC INDICES HERE!
*
*
*/
/** get a single specific servo value */
#define PWM_SERVO_GET(_servo) _PX4_IOC(_PWM_SERVO_BASE, 0x50 + _servo)
/** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels
* whose update rates must be the same.
*/
#define PWM_SERVO_GET_RATEGROUP(_n) _PX4_IOC(_PWM_SERVO_BASE, 0x70 + _n)
/** specific rates for configuring the timer for OneShot or PWM */
#define PWM_RATE_ONESHOT 0u
#define PWM_RATE_LOWER_LIMIT 1u
#define PWM_RATE_UPPER_LIMIT 10000u
typedef enum {
DShot_cmd_motor_stop = 0,
DShot_cmd_beacon1,
DShot_cmd_beacon2,
DShot_cmd_beacon3,
DShot_cmd_beacon4,
DShot_cmd_beacon5,
DShot_cmd_esc_info, // V2 includes settings
DShot_cmd_spin_direction_1,
DShot_cmd_spin_direction_2,
DShot_cmd_3d_mode_off,
DShot_cmd_3d_mode_on,
DShot_cmd_settings_request, // Currently not implemented
DShot_cmd_save_settings,
DShot_cmd_spin_direction_normal = 20,
DShot_cmd_spin_direction_reversed = 21,
DShot_cmd_led0_on, // BLHeli32 only
DShot_cmd_led1_on, // BLHeli32 only
DShot_cmd_led2_on, // BLHeli32 only
DShot_cmd_led3_on, // BLHeli32 only
DShot_cmd_led0_off, // BLHeli32 only
DShot_cmd_led1_off, // BLHeli32 only
DShot_cmd_led2_off, // BLHeli32 only
DShot_cmd_led4_off, // BLHeli32 only
DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off
DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off
DShot_cmd_signal_line_telemetry_disable = 32,
DShot_cmd_signal_line_continuous_erpm_telemetry = 33,
DShot_cmd_MAX = 47, // >47 are throttle values
DShot_cmd_MIN_throttle = 48,
DShot_cmd_MAX_throttle = 2047
} dshot_command_t;
/*
* Low-level PWM output interface.
*
@@ -331,7 +207,7 @@ __EXPORT extern void up_pwm_update(unsigned channel_mask);
* @param channel The channel to set.
* @param value The output pulse width in microseconds.
*/
__EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value);
__EXPORT extern int up_pwm_servo_set(unsigned channel, uint16_t value);
/**
* Get the current output value for a channel.
@@ -340,62 +216,7 @@ __EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value);
* @return The output pulse width in microseconds, or zero if
* outputs are not armed or not configured.
*/
__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
__EXPORT extern uint16_t up_pwm_servo_get(unsigned channel);
/**
* Intialise the Dshot outputs using the specified configuration.
*
* @param channel_mask Bitmask of channels (LSB = channel 0) to enable.
* This allows some of the channels to remain configured
* as GPIOs or as another function. Already used channels/timers will not be configured as DShot
* @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
* @return <0 on error, the initialized channels mask.
*/
__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq);
/**
* Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method)
*/
__EXPORT extern void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry);
/**
* Set the current dshot throttle value for a channel (motor).
*
* @param channel The channel to set.
* @param throttle The output dshot throttle value in [0 = DSHOT_DISARM_VALUE, 1 = DSHOT_MIN_THROTTLE, 1999 = DSHOT_MAX_THROTTLE].
* @param telemetry If true, request telemetry from that motor
*/
static inline void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
{
dshot_motor_data_set(channel, throttle + DShot_cmd_MIN_throttle, telemetry);
}
/**
* Send DShot command to a channel (motor).
*
* @param channel The channel to set.
* @param command dshot_command_t
* @param telemetry If true, request telemetry from that motor
*/
static inline void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry)
{
dshot_motor_data_set(channel, command, telemetry);
}
/**
* Trigger dshot data transfer.
*/
__EXPORT extern void up_dshot_trigger(void);
/**
* Arm or disarm dshot outputs (This will enable/disable complete timer for safety purpose.).
*
* When disarmed, dshot output no pulse.
*
* @param armed If true, outputs are armed; if false they
* are disarmed.
*/
__EXPORT extern int up_dshot_arm(bool armed);
__END_DECLS
src/drivers/dshot/DShot.h
+5 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,8 +32,7 @@
****************************************************************************/
#pragma once
#include <drivers/device/device.h>
#include <drivers/drv_input_capture.h>
#include <drivers/drv_dshot.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
@@ -59,16 +58,16 @@ static constexpr int DSHOT_DISARM_VALUE = 0;
static constexpr int DSHOT_MIN_THROTTLE = 1;
static constexpr int DSHOT_MAX_THROTTLE = 1999;
class DShot : public ModuleBase<DShot>, public OutputModuleInterface
class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
{
public:
DShot();
virtual ~DShot();
~DShot() override;
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
virtual int init();
int init();
void mixerChanged() override;
src/drivers/pwm_out/PWMOut.cpp
-243
View File
@@ -51,7 +51,6 @@ static bool is_running()
}
PWMOut::PWMOut(int instance, uint8_t output_base) :
CDev((instance == 0) ? PX4FMU_DEVICE_PATH : PX4FMU_DEVICE_PATH"1"),
OutputModuleInterface((instance == 0) ? MODULE_NAME"0" : MODULE_NAME"1", px4::wq_configurations::hp_default),
_instance(instance),
_output_base(output_base),
@@ -65,31 +64,12 @@ PWMOut::~PWMOut()
/* make sure servos are off */
up_pwm_servo_deinit(_pwm_mask);
/* clean up the alternate device node */
unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
perf_free(_cycle_perf);
perf_free(_interval_perf);
}
int PWMOut::init()
{
/* do regular cdev init */
int ret = CDev::init();
if (ret != OK) {
return ret;
}
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* lets not be too verbose */
} else if (_class_instance < 0) {
PX4_ERR("FAILED registering class device");
}
_num_outputs = FMU_MAX_ACTUATORS;
_pwm_mask = ((1u << _num_outputs) - 1) << _output_base;
@@ -103,37 +83,6 @@ int PWMOut::init()
return 0;
}
void PWMOut::update_current_rate()
{
/*
* Adjust actuator topic update rate to keep up with
* the highest servo update rate configured.
*
* We always mix at max rate; some channels may update slower.
*/
int max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
// oneshot
if ((_pwm_default_rate == 0) || (_pwm_alt_rate == 0)) {
max_rate = 2000;
} else {
// run up to twice PWM rate to reduce end-to-end latency
// actual pulse width only updated for next period regardless of output module
max_rate *= 2;
}
// max interval 0.5 - 100 ms (10 - 2000Hz)
const int update_interval_in_us = math::constrain(1000000 / max_rate, 500, 100000);
if (_current_update_rate != max_rate) {
PX4_INFO("instance: %d, max rate: %d, default: %d, alt: %d", _instance, max_rate, _pwm_default_rate, _pwm_alt_rate);
}
_current_update_rate = max_rate;
_mixing_output.setMaxTopicUpdateRate(update_interval_in_us);
}
int PWMOut::task_spawn(int argc, char *argv[])
{
for (unsigned instance = 0; instance < (sizeof(_objects) / sizeof(_objects[0])); instance++) {
@@ -282,8 +231,6 @@ void PWMOut::Run()
return;
}
SmartLock lock_guard(_lock);
perf_begin(_cycle_perf);
perf_count(_interval_perf);
@@ -369,186 +316,6 @@ void PWMOut::update_params()
_first_param_update = false;
}
int PWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
SmartLock lock_guard(_lock);
int ret = pwm_ioctl(filp, cmd, arg);
/* if nobody wants it, let CDev have it */
if (ret == -ENOTTY) {
ret = CDev::ioctl(filp, cmd, arg);
}
return ret;
}
int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
int ret = OK;
PX4_DEBUG("pwm_out%u: ioctl cmd: %d, arg: %ld", _instance, cmd, arg);
switch (cmd) {
case PWM_SERVO_ARM:
update_pwm_out_state(true);
break;
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
break;
case PWM_SERVO_DISARM:
/* Ignore disarm if disarmed PWM is set already. */
if (_num_disarmed_set == 0) {
update_pwm_out_state(false);
}
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
*(uint32_t *)arg = _pwm_default_rate;
break;
case PWM_SERVO_SET_UPDATE_RATE:
ret = -EINVAL;
break;
case PWM_SERVO_GET_UPDATE_RATE:
*(uint32_t *)arg = _pwm_alt_rate;
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = -EINVAL;
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
*(uint32_t *)arg = _pwm_alt_rate_channels;
break;
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.failsafeValue(i);
}
pwm->channel_count = FMU_MAX_ACTUATORS;
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.disarmedValue(i);
}
pwm->channel_count = FMU_MAX_ACTUATORS;
break;
}
case PWM_SERVO_SET_MIN_PWM:
ret = -EINVAL;
break;
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.minValue(i);
}
pwm->channel_count = FMU_MAX_ACTUATORS;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET_MAX_PWM:
ret = -EINVAL;
break;
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) {
pwm->values[i] = _mixing_output.maxValue(i);
}
pwm->channel_count = FMU_MAX_ACTUATORS;
arg = (unsigned long)&pwm;
}
break;
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 14
case PWM_SERVO_GET(13):
case PWM_SERVO_GET(12):
case PWM_SERVO_GET(11):
case PWM_SERVO_GET(10):
case PWM_SERVO_GET(9):
case PWM_SERVO_GET(8):
#endif
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 8
case PWM_SERVO_GET(7):
case PWM_SERVO_GET(6):
#endif
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 6
case PWM_SERVO_GET(5):
#endif
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 5
case PWM_SERVO_GET(4):
#endif
case PWM_SERVO_GET(3):
case PWM_SERVO_GET(2):
case PWM_SERVO_GET(1):
case PWM_SERVO_GET(0):
if (cmd - PWM_SERVO_GET(0) >= (int)_num_outputs) {
ret = -EINVAL;
break;
}
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0) + _output_base);
break;
case PWM_SERVO_GET_RATEGROUP(0):
case PWM_SERVO_GET_RATEGROUP(1):
case PWM_SERVO_GET_RATEGROUP(2):
case PWM_SERVO_GET_RATEGROUP(3):
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 5
case PWM_SERVO_GET_RATEGROUP(4):
#endif
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 6
case PWM_SERVO_GET_RATEGROUP(5):
#endif
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 8
case PWM_SERVO_GET_RATEGROUP(6):
case PWM_SERVO_GET_RATEGROUP(7):
#endif
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 14
case PWM_SERVO_GET_RATEGROUP(8):
case PWM_SERVO_GET_RATEGROUP(9):
case PWM_SERVO_GET_RATEGROUP(10):
case PWM_SERVO_GET_RATEGROUP(11):
case PWM_SERVO_GET_RATEGROUP(12):
case PWM_SERVO_GET_RATEGROUP(13):
#endif
*(uint32_t *)arg = _pwm_mask & up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
*(unsigned *)arg = _num_outputs;
break;
default:
ret = -ENOTTY;
break;
}
return ret;
}
int PWMOut::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
@@ -556,16 +323,6 @@ int PWMOut::custom_command(int argc, char *argv[])
int PWMOut::print_status()
{
if (_class_instance == CLASS_DEVICE_PRIMARY) {
PX4_INFO("%d - PWM_MAIN 0x%04" PRIx32, _instance, _pwm_mask);
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
PX4_INFO("%d - PWM_AUX 0x%04" PRIx32, _instance, _pwm_mask);
} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
PX4_INFO("%d - PWM_EXTRA 0x%04" PRIx32, _instance, _pwm_mask);
}
PX4_INFO("%d - Max update rate: %i Hz", _instance, _current_update_rate);
perf_print_counter(_cycle_perf);
src/drivers/pwm_out/PWMOut.hpp
+3 -19
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -41,10 +41,8 @@
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/cdev/CDev.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <px4_arch/io_timer.h>
#include <px4_platform_common/px4_config.h>
@@ -56,22 +54,15 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/parameter_update.h>
using namespace time_literals;
#if !defined(DIRECT_PWM_OUTPUT_CHANNELS)
# error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS"
#endif
#define PX4FMU_DEVICE_PATH "/dev/px4fmu"
static constexpr int PWM_OUT_MAX_INSTANCES{(DIRECT_PWM_OUTPUT_CHANNELS > 8) ? 2 : 1};
extern pthread_mutex_t pwm_out_module_mutex;
class PWMOut : public cdev::CDev, public OutputModuleInterface
class PWMOut : public OutputModuleInterface
{
public:
PWMOut() = delete;
@@ -102,9 +93,7 @@ public:
static int test(const char *dev);
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
int init() override;
int init();
uint32_t get_pwm_mask() const { return _pwm_mask; }
void set_pwm_mask(uint32_t mask) { _pwm_mask = mask; }
@@ -139,7 +128,6 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
unsigned _num_outputs{0};
int _class_instance{-1};
bool _pwm_on{false};
uint32_t _pwm_mask{0};
@@ -151,10 +139,6 @@ private:
perf_counter_t _cycle_perf;
perf_counter_t _interval_perf;
void update_current_rate();
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg);
bool update_pwm_out_state(bool on);
void update_params();
src/drivers/px4io/px4io.cpp
+6 -190
View File
@@ -177,7 +177,6 @@ private:
unsigned _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO
unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO
int _class_instance{-1};
bool _first_param_update{true};
uint32_t _group_channels[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {};
@@ -357,11 +356,6 @@ PX4IO::~PX4IO()
{
delete _interface;
/* clean up the alternate device node */
if (_class_instance >= 0) {
unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
}
/* deallocate perfs */
perf_free(_cycle_perf);
perf_free(_interval_perf);
@@ -476,8 +470,6 @@ int PX4IO::init()
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
_mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL);
}
@@ -492,24 +484,24 @@ int PX4IO::init()
void PX4IO::updateDisarmed()
{
pwm_output_values pwm{};
uint16_t values[PX4IO_MAX_ACTUATORS] {};
for (unsigned i = 0; i < _max_actuators; i++) {
pwm.values[i] = _mixing_output.disarmedValue(i);
values[i] = _mixing_output.disarmedValue(i);
}
io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm.values, _max_actuators);
io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, values, _max_actuators);
}
void PX4IO::updateFailsafe()
{
pwm_output_values pwm{};
uint16_t values[PX4IO_MAX_ACTUATORS] {};
for (unsigned i = 0; i < _max_actuators; i++) {
pwm.values[i] = _mixing_output.actualFailsafeValue(i);
values[i] = _mixing_output.actualFailsafeValue(i);
}
io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, _max_actuators);
io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, values, _max_actuators);
}
void PX4IO::Run()
@@ -1312,187 +1304,11 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
/* regular ioctl? */
switch (cmd) {
case PWM_SERVO_ARM:
PX4_DEBUG("PWM_SERVO_ARM");
/* set the 'armed' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED);
break;
case PWM_SERVO_SET_ARM_OK:
PX4_DEBUG("PWM_SERVO_SET_ARM_OK");
/* set the 'OK to arm' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK);
break;
case PWM_SERVO_CLEAR_ARM_OK:
PX4_DEBUG("PWM_SERVO_CLEAR_ARM_OK");
/* clear the 'OK to arm' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0);
break;
case PWM_SERVO_DISARM:
PX4_DEBUG("PWM_SERVO_DISARM");
/* clear the 'armed' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
PX4_DEBUG("PWM_SERVO_GET_DEFAULT_UPDATE_RATE");
/* get the default update rate */
*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
break;
case PWM_SERVO_SET_UPDATE_RATE:
PX4_DEBUG("PWM_SERVO_SET_UPDATE_RATE");
ret = -EINVAL;
break;
case PWM_SERVO_GET_UPDATE_RATE:
PX4_DEBUG("PWM_SERVO_GET_UPDATE_RATE");
/* get the alternative update rate */
*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE: {
PX4_DEBUG("PWM_SERVO_SET_SELECT_UPDATE_RATE");
ret = -EINVAL;
break;
}
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
PX4_DEBUG("PWM_SERVO_GET_SELECT_UPDATE_RATE");
*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
break;
case PWM_SERVO_GET_FAILSAFE_PWM: {
PX4_DEBUG("PWM_SERVO_GET_FAILSAFE_PWM");
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
pwm->channel_count = _max_actuators;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _mixing_output.failsafeValue(i);
}
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
PX4_DEBUG("PWM_SERVO_GET_DISARMED_PWM");
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
pwm->channel_count = _max_actuators;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _mixing_output.disarmedValue(i);
}
break;
}
case PWM_SERVO_SET_MIN_PWM: {
PX4_DEBUG("PWM_SERVO_SET_MIN_PWM");
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
if (pwm->channel_count > _max_actuators) {
/* fail with error */
return -E2BIG;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] != 0 && false) {
_mixing_output.minValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MIN);
}
}
break;
}
case PWM_SERVO_GET_MIN_PWM: {
PX4_DEBUG("PWM_SERVO_GET_MIN_PWM");
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
pwm->channel_count = _max_actuators;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _mixing_output.minValue(i);
}
break;
}
case PWM_SERVO_SET_MAX_PWM: {
PX4_DEBUG("PWM_SERVO_SET_MAX_PWM");
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
if (pwm->channel_count > _max_actuators) {
/* fail with error */
return -E2BIG;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] != 0 && false) {
_mixing_output.maxValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MAX, (uint16_t)PWM_HIGHEST_MAX);
}
}
}
break;
case PWM_SERVO_GET_MAX_PWM: {
PX4_DEBUG("PWM_SERVO_GET_MAX_PWM");
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
pwm->channel_count = _max_actuators;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _mixing_output.maxValue(i);
}
}
break;
case PWM_SERVO_GET_COUNT:
PX4_DEBUG("PWM_SERVO_GET_COUNT");
*(unsigned *)arg = _max_actuators;
break;
case DSM_BIND_START:
/* bind a DSM receiver */
ret = dsm_bind_ioctl(arg);
break;
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): {
unsigned channel = cmd - PWM_SERVO_GET(0);
if (channel >= _max_actuators) {
ret = -EINVAL;
} else {
/* fetch a current PWM value */
uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel);
if (value == _io_reg_get_error) {
ret = -EIO;
} else {
*(servo_position_t *)arg = value;
}
}
break;
}
case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel);
if (*(uint32_t *)arg == _io_reg_get_error) {
ret = -EIO;
}
break;
}
case PX4IO_SET_DEBUG:
PX4_DEBUG("PX4IO_SET_DEBUG");