fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 21:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete PWM_SERVO_SET_MODE

Browse Source
This commit is contained in:
Daniel Agar
2022-01-04 12:10:43 -05:00
parent ae9bf0afd3
commit 212525a78e
6 changed files with 5 additions and 298 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_pwm_output.h
-5
View File
@@ -203,11 +203,6 @@ typedef uint16_t servo_position_t;
/** make failsafe non-recoverable (termination) if it occurs */
#define PWM_SERVO_SET_TERMINATION_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 27)
/** 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)
/*
*
*
src/drivers/pwm_out/PWMOut.cpp
-17
View File
@@ -1079,23 +1079,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();
src/drivers/pwm_out/PWMOut.hpp
-1
View File
@@ -145,7 +145,6 @@ private:
bool _pwm_on{false};
uint32_t _pwm_mask{0};
bool _pwm_initialized{false};
bool _test_mode{false};
bool _esc_calibration_mode{false};
hrt_abstime _esc_calibration_last{0};
src/drivers/px4io/px4io.cpp
+3 -20
View File
@@ -238,7 +238,6 @@ private:
float _analog_rc_rssi_volt{-1.f}; ///< analog RSSI voltage
bool _test_fmu_fail{false}; ///< To test what happens if IO loses FMU
bool _in_test_mode{false}; ///< true if PWM_SERVO_ENTER_TEST_MODE is active
MixingOutput _mixing_output{"PWM_MAIN", PX4IO_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
@@ -433,7 +432,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);
}
@@ -585,7 +584,7 @@ void PX4IO::Run()
SmartLock lock_guard(_lock);
// ESC calibration
if (!_mixing_output.armed().armed && !_test_fmu_fail && !_in_test_mode) {
if (!_mixing_output.armed().armed && !_test_fmu_fail) {
if (_mixing_output.armed().in_esc_calibration_mode) {
_esc_calibration_mode = true;
_esc_calibration_last = _mixing_output.armed().timestamp;
@@ -1892,7 +1891,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
ret = -EINVAL;
} else {
if (!_test_fmu_fail && _in_test_mode) {
if (!_test_fmu_fail) {
/* send a direct PWM value */
ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
@@ -1941,22 +1940,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();
src/systemcmds/motor_ramp/motor_ramp.cpp
-14
View File
@@ -392,13 +392,6 @@ int motor_ramp_thread_main(int argc, char *argv[])
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;
}
@@ -505,13 +498,6 @@ int motor_ramp_thread_main(int argc, char *argv[])
}
}
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);
}
src/systemcmds/pwm/pwm.cpp
+2 -241
View File
@@ -103,10 +103,6 @@ Note that in OneShot mode, the PWM range [1000, 2000] is automatically mapped to
Set the PWM rate for all channels to 400 Hz:
$ pwm rate -a -r 400
Test the outputs of eg. channels 1 and 3, and set the PWM value to 1200 us:
$ pwm arm
$ pwm test -c 13 -p 1200
)DESCR_STR");
@@ -130,15 +126,11 @@ $ pwm test -c 13 -p 1200
PRINT_MODULE_USAGE_COMMAND_DESCR("disarmed", "Set Disarmed PWM value");
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 'failsafe', 'disarmed', 'min', 'max' and 'test' require a PWM value:");
PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'failsafe', 'disarmed', 'min', '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', 'failsafe', 'disarmed', 'min', 'max', 'test' and 'steps' "
PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'rate', 'oneshot', 'failsafe', 'disarmed', '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);
@@ -576,237 +568,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);