diff --git a/Tools/kconfig/cmake_kconfig_lut.txt b/Tools/kconfig/cmake_kconfig_lut.txt index 6d6734cb9b..5fb433f340 100644 --- a/Tools/kconfig/cmake_kconfig_lut.txt +++ b/Tools/kconfig/cmake_kconfig_lut.txt @@ -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 diff --git a/boards/airmind/mindpx-v2/default.px4board b/boards/airmind/mindpx-v2/default.px4board index 27a4eb09d5..7fa0fd5407 100644 --- a/boards/airmind/mindpx-v2/default.px4board +++ b/boards/airmind/mindpx-v2/default.px4board @@ -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 diff --git a/boards/atl/mantis-edu/default.px4board b/boards/atl/mantis-edu/default.px4board index 5c0cbf44d0..b2ae8b9599 100644 --- a/boards/atl/mantis-edu/default.px4board +++ b/boards/atl/mantis-edu/default.px4board @@ -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 diff --git a/boards/av/x-v1/default.px4board b/boards/av/x-v1/default.px4board index 31203faa23..64098d9a33 100644 --- a/boards/av/x-v1/default.px4board +++ b/boards/av/x-v1/default.px4board @@ -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 diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index 7502d7158b..3a3a6b1b96 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -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 diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index 0519b56bcf..c3d9fc11b5 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -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 diff --git a/boards/cubepilot/cubeyellow/default.px4board b/boards/cubepilot/cubeyellow/default.px4board index 2ff0a8b9f2..de21b0335d 100644 --- a/boards/cubepilot/cubeyellow/default.px4board +++ b/boards/cubepilot/cubeyellow/default.px4board @@ -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 diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index 047948c2dc..0a2b6e8984 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -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 diff --git a/boards/holybro/durandal-v1/default.px4board b/boards/holybro/durandal-v1/default.px4board index 7dd68d2abb..ad95a4d34b 100644 --- a/boards/holybro/durandal-v1/default.px4board +++ b/boards/holybro/durandal-v1/default.px4board @@ -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 diff --git a/boards/holybro/pix32v5/default.px4board b/boards/holybro/pix32v5/default.px4board index 3c2201fd2b..2f2261f7dc 100644 --- a/boards/holybro/pix32v5/default.px4board +++ b/boards/holybro/pix32v5/default.px4board @@ -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 diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index a6bf763d38..781c2c622d 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -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 diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index 2a5274e9cf..1919acaf63 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -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 diff --git a/boards/mro/ctrl-zero-f7-oem/default.px4board b/boards/mro/ctrl-zero-f7-oem/default.px4board index 51d07dafd7..63082ffddd 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.px4board +++ b/boards/mro/ctrl-zero-f7-oem/default.px4board @@ -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 diff --git a/boards/mro/ctrl-zero-f7/default.px4board b/boards/mro/ctrl-zero-f7/default.px4board index da1355154d..ccc15ac84d 100644 --- a/boards/mro/ctrl-zero-f7/default.px4board +++ b/boards/mro/ctrl-zero-f7/default.px4board @@ -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 diff --git a/boards/mro/ctrl-zero-h7-oem/default.px4board b/boards/mro/ctrl-zero-h7-oem/default.px4board index 9d2672be06..ba0f234141 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -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 diff --git a/boards/mro/ctrl-zero-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board index b82a38f14c..f0b48bac7f 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -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 diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index 5a217f25c7..0c042a5efa 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -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 diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index f3aa964a0e..938cae18f0 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -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 diff --git a/boards/mro/x21/default.px4board b/boards/mro/x21/default.px4board index c313f6bfde..3c4aa1c306 100644 --- a/boards/mro/x21/default.px4board +++ b/boards/mro/x21/default.px4board @@ -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 diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index 68ee627a6d..9cf0984223 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -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 diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index 975be6aeee..9109ae1fe5 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -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 diff --git a/boards/px4/fmu-v3/default.px4board b/boards/px4/fmu-v3/default.px4board index 39dd15c9f5..625bc18605 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/default.px4board @@ -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 diff --git a/boards/px4/fmu-v4/default.px4board b/boards/px4/fmu-v4/default.px4board index ad770f7d30..c6af5f672f 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/default.px4board @@ -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 diff --git a/boards/px4/fmu-v4pro/default.px4board b/boards/px4/fmu-v4pro/default.px4board index 114af8df2f..fcae6f86b1 100644 --- a/boards/px4/fmu-v4pro/default.px4board +++ b/boards/px4/fmu-v4pro/default.px4board @@ -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 diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index 8f02bceaba..f420af13af 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -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 diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 2ca0baee59..7be267da2d 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -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 diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index fb7d938542..dde51d912b 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -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 diff --git a/boards/px4/raspberrypi/default.px4board b/boards/px4/raspberrypi/default.px4board index 9d8ec5f242..23b26e7e4d 100644 --- a/boards/px4/raspberrypi/default.px4board +++ b/boards/px4/raspberrypi/default.px4board @@ -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 diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 9418787f99..8f50e4f2bb 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -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 diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index 3a9e96484b..14648a8b00 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -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 diff --git a/boards/uvify/core/default.px4board b/boards/uvify/core/default.px4board index ce934bbabf..ec610096d1 100644 --- a/boards/uvify/core/default.px4board +++ b/boards/uvify/core/default.px4board @@ -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 diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 832962dc5f..be9b4098f3 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -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) diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 61cafe60bb..6b6da82b74 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -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; diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index fd6491f54d..87c4f91ebe 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -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}; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ea2c05af5d..6b4b460893 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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(); diff --git a/src/systemcmds/motor_ramp/CMakeLists.txt b/src/systemcmds/motor_ramp/CMakeLists.txt deleted file mode 100644 index b5684aeb6d..0000000000 --- a/src/systemcmds/motor_ramp/CMakeLists.txt +++ /dev/null @@ -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 - ) diff --git a/src/systemcmds/motor_ramp/Kconfig b/src/systemcmds/motor_ramp/Kconfig deleted file mode 100644 index 8445ecb29b..0000000000 --- a/src/systemcmds/motor_ramp/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig SYSTEMCMDS_MOTOR_RAMP - bool "motor_ramp" - default n - ---help--- - Enable support for motor_ramp \ No newline at end of file diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp deleted file mode 100644 index 077d1fcff8..0000000000 --- a/src/systemcmds/motor_ramp/motor_ramp.cpp +++ /dev/null @@ -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 - * @author Roman Bapst - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#include -#include - -#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; -} diff --git a/src/systemcmds/pwm/pwm.cpp b/src/systemcmds/pwm/pwm.cpp index e9f1b4895b..bf6f802f2e 100644 --- a/src/systemcmds/pwm/pwm.cpp +++ b/src/systemcmds/pwm/pwm.cpp @@ -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);