fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete PWM_SERVO_SET, PWM_SERVO_SET_MODE, systemcmds/motor_ramp, and pwm_out test

Browse Source
This commit is contained in:
Daniel Agar 2022-01-03 18:56:34 -05:00
parent df44df2df6
commit d077ca15fb
39 changed files with 4 additions and 1119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/kconfig/cmake_kconfig_lut.txt
View File

@ -164,7 +164,6 @@ led_control,CONFIG_SYSTEMCMDS_LED_CONTROL=y
mft,CONFIG_SYSTEMCMDS_MFT=y
microbench,CONFIG_SYSTEMCMDS_MICROBENCH=y
mixer,CONFIG_SYSTEMCMDS_MIXER=y
motor_ramp,CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
motor_test,CONFIG_SYSTEMCMDS_MOTOR_TEST=y
mtd,CONFIG_SYSTEMCMDS_MTD=y
netman,CONFIG_SYSTEMCMDS_NETMAN=y

1
boards/airmind/mindpx-v2/default.px4board
View File

@ -85,7 +85,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/atl/mantis-edu/default.px4board
View File

@ -37,7 +37,6 @@ CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y

1
boards/av/x-v1/default.px4board
View File

@ -82,7 +82,6 @@ CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_NETMAN=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/beaglebone/blue/default.px4board
View File

@ -61,7 +61,6 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_DYN=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y

1
boards/cubepilot/cubeorange/default.px4board
View File

@ -84,7 +84,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/cubepilot/cubeyellow/default.px4board
View File

@ -86,7 +86,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/emlid/navio2/default.px4board
View File

@ -63,7 +63,6 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_DYN=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y

1
boards/holybro/durandal-v1/default.px4board
View File

@ -81,7 +81,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/holybro/pix32v5/default.px4board
View File

@ -91,7 +91,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/modalai/fc-v1/default.px4board
View File

@ -86,7 +86,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/modalai/fc-v2/default.px4board
View File

@ -81,7 +81,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/mro/ctrl-zero-f7-oem/default.px4board
View File

@ -84,7 +84,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/mro/ctrl-zero-f7/default.px4board
View File

@ -84,7 +84,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/mro/ctrl-zero-h7-oem/default.px4board
View File

@ -83,7 +83,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/mro/ctrl-zero-h7/default.px4board
View File

@ -84,7 +84,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/mro/pixracerpro/default.px4board
View File

@ -82,7 +82,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/mro/x21-777/default.px4board
View File

@ -85,7 +85,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/mro/x21/default.px4board
View File

@ -85,7 +85,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/nxp/fmuk66-e/default.px4board
View File

@ -79,7 +79,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/nxp/fmuk66-v3/default.px4board
View File

@ -83,7 +83,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/px4/fmu-v3/default.px4board
View File

@ -88,7 +88,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/px4/fmu-v4/default.px4board
View File

@ -89,7 +89,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/px4/fmu-v4pro/default.px4board
View File

@ -86,7 +86,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/px4/fmu-v5/default.px4board
View File

@ -91,7 +91,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/px4/fmu-v5x/default.px4board
View File

@ -92,7 +92,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NETMAN=y

1
boards/px4/fmu-v6u/default.px4board
View File

@ -84,7 +84,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

1
boards/px4/raspberrypi/default.px4board
View File

@ -60,7 +60,6 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_DYN=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y

1
boards/px4/sitl/default.px4board
View File

@ -50,7 +50,6 @@ CONFIG_SYSTEMCMDS_DYN=y
CONFIG_SYSTEMCMDS_FAILURE=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y

1
boards/scumaker/pilotpi/default.px4board
View File

@ -58,7 +58,6 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_DYN=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_PWM=y

1
boards/uvify/core/default.px4board
View File

@ -70,7 +70,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
CONFIG_SYSTEMCMDS_LED_CONTROL=y
CONFIG_SYSTEMCMDS_MFT=y
CONFIG_SYSTEMCMDS_MIXER=y
CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
CONFIG_SYSTEMCMDS_MOTOR_TEST=y
CONFIG_SYSTEMCMDS_MTD=y
CONFIG_SYSTEMCMDS_NSHTERM=y

8
src/drivers/drv_pwm_output.h
View File

@ -194,11 +194,6 @@ typedef uint16_t servo_position_t;
/** force safety switch on (to enable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 28)
/** 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)
/*
*
*
@ -207,9 +202,6 @@ typedef uint16_t servo_position_t;
*
*/
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _PX4_IOC(_PWM_SERVO_BASE, 0x30 + _servo)
/** get a single specific servo value */
#define PWM_SERVO_GET(_servo) _PX4_IOC(_PWM_SERVO_BASE, 0x50 + _servo)

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

@ -411,10 +411,6 @@ bool PWMOut::update_pwm_out_state(bool on)
bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
if (_test_mode) {
return false;
}
/* output to the servos */
if (_pwm_initialized) {
for (size_t i = 0; i < num_outputs; i++) {
@ -855,47 +851,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
}
break;
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 14
case PWM_SERVO_SET(13):
case PWM_SERVO_SET(12):
case PWM_SERVO_SET(11):
case PWM_SERVO_SET(10):
case PWM_SERVO_SET(9):
case PWM_SERVO_SET(8):
#endif
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 8
case PWM_SERVO_SET(7):
case PWM_SERVO_SET(6):
#endif
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 6
case PWM_SERVO_SET(5):
#endif
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 5
case PWM_SERVO_SET(4):
#endif
case PWM_SERVO_SET(3):
case PWM_SERVO_SET(2):
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) {
unsigned channel = cmd - PWM_SERVO_SET(0) + _output_base;
if (_pwm_mask & (1 << channel)) {
up_pwm_servo_set(channel, arg);
}
} else {
ret = -EINVAL;
}
break;
#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 14
case PWM_SERVO_GET(13):
@ -956,23 +911,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
*(unsigned *)arg = _num_outputs;
break;
case PWM_SERVO_SET_MODE: {
switch (arg) {
case PWM_SERVO_ENTER_TEST_MODE:
_test_mode = true;
break;
case PWM_SERVO_EXIT_TEST_MODE:
_test_mode = false;
break;
default:
ret = -EINVAL;
}
break;
}
case MIXERIOCRESET:
_mixing_output.resetMixerThreadSafe();
@ -997,165 +935,8 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
return ret;
}
int PWMOut::test(const char *dev)
{
int fd;
unsigned servo_count = 0;
unsigned pwm_value = 1000;
int direction = 1;
int ret;
int rv = -1;
fd = ::open(dev, O_RDWR);
if (fd < 0) {
PX4_ERR("open fail");
return -1;
}
if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) {
PX4_ERR("Failed to Enter pwm test mode");
goto err_out_no_test;
}
if (::ioctl(fd, PWM_SERVO_ARM, 0) < 0) {
PX4_ERR("servo arm failed");
goto err_out;
}
if (::ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
PX4_ERR("Unable to get servo count");
goto err_out;
}
PX4_INFO("Testing %u servos", servo_count);
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
PX4_INFO("Press CTRL-C or 'c' to abort.");
for (;;) {
/* sweep all servos between 1000..2000 */
servo_position_t servos[servo_count];
for (unsigned i = 0; i < servo_count; i++) {
servos[i] = pwm_value;
}
for (unsigned i = 0; i < servo_count; i++) {
if (::ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
PX4_ERR("servo %u set failed", i);
goto err_out;
}
}
if (direction > 0) {
if (pwm_value < 2000) {
pwm_value++;
} else {
direction = -1;
}
} else {
if (pwm_value > 1000) {
pwm_value--;
} else {
direction = 1;
}
}
/* readback servo values */
for (unsigned i = 0; i < servo_count; i++) {
servo_position_t value;
if (::ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) {
PX4_ERR("error reading PWM servo %u", i);
goto err_out;
}
if (value != servos[i]) {
PX4_ERR("servo %u readback error, got %" PRIu16 " expected %" PRIu16, i, value, servos[i]);
goto err_out;
}
}
/* Check if user wants to quit */
char c;
ret = ::poll(&fds, 1, 0);
if (ret > 0) {
::read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
PX4_INFO("User abort");
break;
}
}
}
rv = 0;
err_out:
if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) {
PX4_ERR("Failed to Exit pwm test mode");
}
err_out_no_test:
::close(fd);
return rv;
}
int PWMOut::custom_command(int argc, char *argv[])
{
int ch = 0;
int myoptind = 0;
const char *myoptarg = nullptr;
const char *dev = PX4FMU_DEVICE_PATH;
while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'd':
if (nullptr == strstr(myoptarg, "/dev/")) {
PX4_WARN("device %s not valid", myoptarg);
print_usage(nullptr);
return 1;
}
dev = myoptarg;
break;
}
}
if (myoptind >= argc) {
print_usage(nullptr);
return 1;
}
const char *verb = argv[myoptind];
/* start pwm_out if not running */
if (!is_running()) {
int ret = PWMOut::task_spawn(argc, argv);
if (ret) {
return ret;
}
}
if (!strcmp(verb, "test")) {
return test(dev);
}
return print_usage("unknown command");
}
@ -1225,7 +1006,6 @@ By default the module runs on a work queue with a callback on the uORB actuator_
PRINT_MODULE_USAGE_NAME("pwm_out", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test outputs");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;

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

@ -147,7 +147,6 @@ private:
bool _pwm_on{false};
uint32_t _pwm_mask{0};
bool _pwm_initialized{false};
bool _test_mode{false};
unsigned _num_disarmed_set{0};

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

@ -429,7 +429,7 @@ bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
{
SmartLock lock_guard(_lock);
if (!_test_fmu_fail && !_in_test_mode) {
if (!_test_fmu_fail) {
/* output to the servos */
io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);
}
@ -1755,32 +1755,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
ret = dsm_bind_ioctl(arg);
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
/* TODO: we could go lower for e.g. TurboPWM */
unsigned channel = cmd - PWM_SERVO_SET(0);
/* PWM needs to be either 0 or in the valid range. */
if ((arg != 0) && ((channel >= _max_actuators) ||
(arg < PWM_LOWEST_MIN) ||
(arg > PWM_HIGHEST_MAX))) {
ret = -EINVAL;
} else {
if (!_test_fmu_fail && _in_test_mode) {
/* send a direct PWM value */
ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
} else {
/* Just silently accept the ioctl without doing anything
* in test mode. */
ret = OK;
}
}
break;
}
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): {
unsigned channel = cmd - PWM_SERVO_GET(0);
@ -1816,22 +1790,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_SET_MODE: {
// reset all channels to disarmed when entering/leaving test mode, so that we don't
// accidentially use values from previous tests
pwm_output_values pwm_disarmed;
if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) {
for (unsigned i = 0; i < _max_actuators; ++i) {
io_reg_set(PX4IO_PAGE_DIRECT_PWM, i, pwm_disarmed.values[i]);
}
}
_in_test_mode = (arg == PWM_SERVO_ENTER_TEST_MODE);
ret = (arg == PWM_SERVO_ENTER_TEST_MODE || PWM_SERVO_EXIT_TEST_MODE) ? 0 : -EINVAL;
}
break;
case MIXERIOCRESET:
PX4_DEBUG("MIXERIOCRESET");
_mixing_output.resetMixerThreadSafe();

40
src/systemcmds/motor_ramp/CMakeLists.txt
View File

@ -1,40 +0,0 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE systemcmds__motor_ramp
MAIN motor_ramp
COMPILE_FLAGS
-Wno-write-strings
SRCS
motor_ramp.cpp
)

5
src/systemcmds/motor_ramp/Kconfig
View File

@ -1,5 +0,0 @@
menuconfig SYSTEMCMDS_MOTOR_RAMP
bool "motor_ramp"
default n
---help---
Enable support for motor_ramp

533
src/systemcmds/motor_ramp/motor_ramp.cpp
View File

@ -1,533 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 motor_ramp.cpp
*
* @author Andreas Antener <andreas@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/getopt.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include "systemlib/err.h"
#include "uORB/topics/actuator_controls.h"
enum RampState {
RAMP_INIT,
RAMP_MIN,
RAMP_RAMP,
RAMP_WAIT
};
enum Mode {
RAMP,
SINE,
SQUARE
};
static bool _thread_should_exit = false; /**< motor_ramp exit flag */
static bool _thread_running = false; /**< motor_ramp status flag */
static int _motor_ramp_task; /**< Handle of motor_ramp task / thread */
static float _ramp_time;
static int _min_pwm;
static int _max_pwm;
static Mode _mode;
static const char *_mode_c;
static const char *_pwm_output_dev = "/dev/pwm_output0";
/**
* motor_ramp management function.
*/
extern "C" __EXPORT int motor_ramp_main(int argc, char *argv[]);
/**
* Mainloop of motor_ramp.
*/
int motor_ramp_thread_main(int argc, char *argv[]);
bool min_pwm_valid(int pwm_value);
bool max_pwm_valid(int pwm_value);
int set_min_pwm(int fd, unsigned long max_channels, int pwm_value);
int set_out(int fd, unsigned long max_channels, float output);
int prepare(int fd, unsigned long *max_channels);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason) {
PX4_ERR("%s", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Application to test motor ramp up.
Before starting, make sure to stop any running attitude controller:
$ mc_rate_control stop
$ fw_att_control stop
When starting, a background task is started, runs for several seconds (as specified), then exits.
### Example
$ motor_ramp sine -a 1100 -r 0.5
)DESCR_STR");
PRINT_MODULE_USAGE_NAME_SIMPLE("motor_ramp", "command");
PRINT_MODULE_USAGE_ARG("ramp|sine|square", "mode", false);
PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/pwm_output0", nullptr, "Pwm output device", true);
PRINT_MODULE_USAGE_PARAM_INT('a', 0, 900, 1500, "Select minimum pwm duty cycle in usec", false);
PRINT_MODULE_USAGE_PARAM_INT('b', 2000, 901, 2100, "Select maximum pwm duty cycle in usec", true);
PRINT_MODULE_USAGE_PARAM_FLOAT('r', 1.0f, 0.0f, 65536.0f, "Select motor ramp duration in sec", true);
PRINT_MODULE_USAGE_PARAM_COMMENT("WARNING: motors will ramp up to full speed!");
}
/**
* The motor_ramp app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int motor_ramp_main(int argc, char *argv[])
{
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
bool error_flag = false;
bool set_pwm_min = false;
_max_pwm = 2000;
_ramp_time = 1.0f;
if (_thread_running) {
PX4_WARN("motor_ramp already running\n");
/* this is not an error */
return 0;
}
if (argc < 4) {
usage("missing parameters");
return 1;
}
while ((ch = px4_getopt(argc, argv, "d:a:b:r:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'd':
if(!strcmp(myoptarg, "/dev/pwm_output0") || !strcmp(myoptarg, "/dev/pwm_output1")){
_pwm_output_dev = myoptarg;
} else {
usage("pwm output device not found");
error_flag = true;
}
break;
case 'a':
_min_pwm = atoi(myoptarg);
if (!min_pwm_valid(_min_pwm)) {
usage("min PWM not in range");
error_flag = true;
} else {
set_pwm_min = true;
}
break;
case 'b':
_max_pwm = atoi(myoptarg);
if (!max_pwm_valid(_max_pwm)) {
usage("max PWM not in range");
error_flag = true;
}
break;
case 'r':
_ramp_time = atof(myoptarg);
if (_ramp_time <= 0) {
usage("ramp time must be greater than 0");
error_flag = true;
}
break;
default:
PX4_WARN("unrecognized flag");
error_flag = true;
break;
}
}
_thread_should_exit = false;
if(!set_pwm_min){
PX4_WARN("pwm_min not set. use -a flag");
error_flag = true;
}
if (!strcmp(argv[myoptind], "ramp")) {
_mode = RAMP;
} else if (!strcmp(argv[myoptind], "sine")) {
_mode = SINE;
} else if (!strcmp(argv[myoptind], "square")) {
_mode = SQUARE;
} else {
usage("selected mode not valid");
error_flag = true;
}
_mode_c = myoptarg;
if(error_flag){
return 1;
}
_motor_ramp_task = px4_task_spawn_cmd("motor_ramp",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40,
2000,
motor_ramp_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)nullptr);
return 0;
}
bool min_pwm_valid(int pwm_value)
{
return pwm_value >= 900 && pwm_value <= 1500;
}
bool max_pwm_valid(int pwm_value)
{
return pwm_value <= 2100 && pwm_value > _min_pwm;
}
int set_min_pwm(int fd, unsigned long max_channels, int pwm_value)
{
int ret;
struct pwm_output_values pwm_values {};
pwm_values.channel_count = max_channels;
for (unsigned i = 0; i < max_channels; i++) {
pwm_values.values[i] = pwm_value;
}
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {
PX4_ERR("failed setting min values");
return 1;
}
return 0;
}
int set_out(int fd, unsigned long max_channels, float output)
{
int ret;
int pwm = (_max_pwm - _min_pwm) * output + _min_pwm;
for (unsigned i = 0; i < max_channels; i++) {
ret = ioctl(fd, PWM_SERVO_SET(i), pwm);
if (ret != OK) {
PX4_ERR("PWM_SERVO_SET(%d), value: %d", i, pwm);
return 1;
}
}
return 0;
}
int prepare(int fd, unsigned long *max_channels)
{
/* make sure no other source is publishing control values now */
struct actuator_controls_s actuators;
int act_sub = orb_subscribe(ORB_ID(actuator_controls_0));
/* clear changed flag */
orb_copy(ORB_ID(actuator_controls_0), act_sub, &actuators);
/* wait 50 ms */
px4_usleep(50000);
/* now expect nothing changed on that topic */
bool orb_updated;
orb_check(act_sub, &orb_updated);
if (orb_updated) {
PX4_ERR("ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
"\tmc_rate_control stop\n"
"\tfw_att_control stop\n");
return 1;
}
/* get number of channels available on the device */
if (px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)max_channels) != OK) {
PX4_ERR("PWM_SERVO_GET_COUNT");
return 1;
}
/* tell IO/FMU that its ok to disable its safety with the switch */
if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
PX4_ERR("PWM_SERVO_SET_ARM_OK");
return 1;
}
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
PX4_ERR("PWM_SERVO_ARM");
return 1;
}
/* tell IO to switch off safety without using the safety switch */
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
PX4_ERR("PWM_SERVO_SET_FORCE_SAFETY_OFF");
return 1;
}
return 0;
}
int motor_ramp_thread_main(int argc, char *argv[])
{
_thread_running = true;
unsigned long max_channels = 0;
struct pwm_output_values last_spos;
struct pwm_output_values last_min;
unsigned servo_count;
int fd = px4_open(_pwm_output_dev, 0);
if (fd < 0) {
PX4_ERR("can't open %s", _pwm_output_dev);
_thread_running = false;
return 1;
}
/* get the number of servo channels */
if (px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) < 0) {
PX4_ERR("PWM_SERVO_GET_COUNT");
px4_close(fd);
_thread_running = false;
return 1;
}
/* get current servo values */
for (unsigned i = 0; i < servo_count; i++) {
if (px4_ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]) < 0) {
PX4_ERR("PWM_SERVO_GET(%d)", i);
px4_close(fd);
_thread_running = false;
return 1;
}
}
/* get current pwm min */
if (px4_ioctl(fd, PWM_SERVO_GET_MIN_PWM, (long unsigned int)&last_min) < 0) {
PX4_ERR("failed getting pwm min values");
px4_close(fd);
_thread_running = false;
return 1;
}
if (px4_ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) {
PX4_ERR("Failed to Enter pwm test mode");
px4_close(fd);
_thread_running = false;
return 1;
}
if (prepare(fd, &max_channels) != OK) {
_thread_should_exit = true;
}
set_out(fd, max_channels, 0.0f);
float dt = 0.001f; // prevent division with 0
float timer = 0.0f;
hrt_abstime start = 0;
hrt_abstime last_run = 0;
enum RampState ramp_state = RAMP_INIT;
float output = 0.0f;
while (!_thread_should_exit) {
if (last_run > 0) {
dt = hrt_elapsed_time(&last_run) * 1e-6;
} else {
start = hrt_absolute_time();
}
last_run = hrt_absolute_time();
timer = hrt_elapsed_time(&start) * 1e-6;
switch (ramp_state) {
case RAMP_INIT: {
PX4_INFO("setting pwm min: %d", _min_pwm);
set_min_pwm(fd, max_channels, _min_pwm);
ramp_state = RAMP_MIN;
break;
}
case RAMP_MIN: {
if (timer > 3.0f) {
PX4_INFO("starting %s: %.2f sec", _mode_c, (double)_ramp_time);
start = hrt_absolute_time();
ramp_state = RAMP_RAMP;
}
set_out(fd, max_channels, output);
break;
}
case RAMP_RAMP: {
if (_mode == RAMP) {
output += 1000.0f * dt / (_max_pwm - _min_pwm) / _ramp_time;
} else if (_mode == SINE) {
// sine outpout with period T = _ramp_time and magnitude between [0,1]
// phase shift makes sure that it starts at zero when timer is zero
output = 0.5f * (1.0f + sinf(M_TWOPI_F * timer / _ramp_time - M_PI_2_F));
} else if (_mode == SQUARE) {
output = fmodf(timer, _ramp_time) > (_ramp_time * 0.5f) ? 1.0f : 0.0f;
}
if ((output > 1.0f && _mode == RAMP) || (timer > 3.0f * _ramp_time)) {
// for ramp mode we set output to 1, for others we just leave it as is
output = _mode != RAMP ? output : 1.0f;
start = hrt_absolute_time();
ramp_state = RAMP_WAIT;
PX4_INFO("%s finished, waiting", _mode_c);
}
set_out(fd, max_channels, output);
break;
}
case RAMP_WAIT: {
if (timer > 0.5f) {
_thread_should_exit = true;
PX4_INFO("stopping");
break;
}
set_out(fd, max_channels, output);
break;
}
}
// rate limit
px4_usleep(2000);
}
if (fd >= 0) {
/* set current pwm min */
if (px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&last_min) < 0) {
PX4_ERR("failed setting pwm min values");
px4_close(fd);
_thread_running = false;
return 1;
}
/* set previous servo values */
for (unsigned i = 0; i < servo_count; i++) {
if (px4_ioctl(fd, PWM_SERVO_SET(i), (unsigned long)last_spos.values[i]) < 0) {
PX4_ERR("PWM_SERVO_SET(%d)", i);
px4_close(fd);
_thread_running = false;
return 1;
}
}
if (px4_ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) {
PX4_ERR("Failed to Exit pwm test mode");
px4_close(fd);
_thread_running = false;
return 1;
}
px4_close(fd);
}
_thread_running = false;
return 0;
}

241
src/systemcmds/pwm/pwm.cpp
View File

@ -123,15 +123,11 @@ $ pwm test -c 13 -p 1200
PRINT_MODULE_USAGE_COMMAND_DESCR("min", "Set Minimum PWM value");
PRINT_MODULE_USAGE_COMMAND_DESCR("max", "Set Maximum PWM value");
PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Set Output to a specific value until 'q' or 'c' or 'ctrl-c' pressed");
PRINT_MODULE_USAGE_COMMAND_DESCR("steps", "Run 5 steps from 0 to 100%");
PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'min', 'max' and 'test' require a PWM value:");
PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'min' and 'max' require a PWM value:");
PRINT_MODULE_USAGE_PARAM_INT('p', -1, 0, 4000, "PWM value (eg. 1100)", false);
PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'rate', 'oneshot', 'min', 'max', 'test' and 'steps' "
PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'rate', 'oneshot', 'min', 'max' "
"additionally require to specify the channels with one of the following commands:");
PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, nullptr, "select channels in the form: 1234 (1 digit per channel, 1=first)",
true);
@ -159,7 +155,7 @@ pwm_main(int argc, char *argv[])
bool oneshot = false;
int ch;
int ret;
int rv = 1;
char *ep;
uint32_t set_mask = 0;
unsigned group;
@ -497,237 +493,6 @@ pwm_main(int argc, char *argv[])
return 0;
} else if (!strcmp(command, "test")) {
if (set_mask == 0) {
usage("no channels set");
return 1;
}
if (pwm_value == 0) {
usage("no PWM provided");
return 1;
}
/* get current servo values */
struct pwm_output_values last_spos;
for (unsigned i = 0; i < servo_count; i++) {
ret = px4_ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
if (ret != OK) {
PX4_ERR("PWM_SERVO_GET(%d)", i);
return 1;
}
}
/* perform PWM output */
/* Open console directly to grab CTRL-C signal */
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) {
PX4_ERR("Failed to Enter pwm test mode");
goto err_out_no_test;
}
PX4_INFO("Press CTRL-C or 'c' to abort.");
while (1) {
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1 << i) {
ret = px4_ioctl(fd, PWM_SERVO_SET(i), pwm_value);
if (ret != OK) {
PX4_ERR("PWM_SERVO_SET(%d)", i);
goto err_out;
}
}
}
/* abort on user request */
char c;
ret = poll(&fds, 1, 0);
if (ret > 0) {
ret = read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
/* reset output to the last value */
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1 << i) {
ret = px4_ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
if (ret != OK) {
PX4_ERR("PWM_SERVO_SET(%d)", i);
goto err_out;
}
}
}
PX4_INFO("User abort\n");
rv = 0;
goto err_out;
}
}
/* Delay longer than the max Oneshot duration */
px4_usleep(2542);
#ifdef __PX4_NUTTX
/* Trigger all timer's channels in Oneshot mode to fire
* the oneshots with updated values.
*/
up_pwm_update(0xff);
#endif
}
rv = 0;
err_out:
if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) {
rv = 1;
PX4_ERR("Failed to Exit pwm test mode");
}
err_out_no_test:
return rv;
} else if (!strcmp(command, "steps")) {
if (set_mask == 0) {
usage("no channels set");
return 1;
}
/* get current servo values */
struct pwm_output_values last_spos;
for (unsigned i = 0; i < servo_count; i++) {
ret = px4_ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]);
if (ret != OK) {
PX4_ERR("PWM_SERVO_GET(%d)", i);
return 1;
}
}
/* perform PWM output */
/* Open console directly to grab CTRL-C signal */
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
PX4_WARN("Running 5 steps. WARNING! Motors will be live in 5 seconds\nPress any key to abort now.");
px4_sleep(5);
if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) {
PX4_ERR("Failed to Enter pwm test mode");
goto err_out_no_test;
}
unsigned off = 900;
unsigned idle = 1300;
unsigned full = 2000;
unsigned steps_timings_us[] = {2000, 5000, 20000, 50000};
unsigned phase = 0;
unsigned phase_counter = 0;
unsigned const phase_maxcount = 20;
for (unsigned steps_timing_index = 0;
steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
steps_timing_index++) {
PX4_INFO("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]);
while (1) {
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1 << i) {
unsigned val;
if (phase == 0) {
val = idle;
} else if (phase == 1) {
/* ramp - depending how steep it is this ramp will look instantaneous on the output */
val = idle + (full - idle) * ((float)phase_counter / phase_maxcount);
} else {
val = off;
}
ret = px4_ioctl(fd, PWM_SERVO_SET(i), val);
if (ret != OK) {
PX4_ERR("PWM_SERVO_SET(%d)", i);
goto err_out;
}
}
}
/* abort on user request */
char c;
ret = poll(&fds, 1, 0);
if (ret > 0) {
ret = read(0, &c, 1);
if (ret > 0) {
/* reset output to the last value */
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1 << i) {
ret = px4_ioctl(fd, PWM_SERVO_SET(i), last_spos.values[i]);
if (ret != OK) {
PX4_ERR("PWM_SERVO_SET(%d)", i);
goto err_out;
}
}
}
PX4_INFO("User abort\n");
rv = 0;
goto err_out;
}
}
if (phase == 1) {
px4_usleep(steps_timings_us[steps_timing_index] / phase_maxcount);
} else if (phase == 0) {
px4_usleep(50000);
} else if (phase == 2) {
px4_usleep(50000);
} else {
break;
}
phase_counter++;
if (phase_counter > phase_maxcount) {
phase++;
phase_counter = 0;
}
}
}
rv = 0;
goto err_out;
} else if (!strcmp(command, "status") || !strcmp(command, "info")) {
printf("device: %s\n", dev);