Daniel Agar
3 years ago
39 changed files with 4 additions and 1119 deletions
@ -1,40 +0,0 @@
@@ -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 |
||||
) |
||||
@ -1,5 +0,0 @@
@@ -1,5 +0,0 @@
|
||||
menuconfig SYSTEMCMDS_MOTOR_RAMP |
||||
bool "motor_ramp" |
||||
default n |
||||
---help--- |
||||
Enable support for motor_ramp |
||||
@ -1,533 +0,0 @@
@@ -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; |
||||
} |
||||
Loading…
Reference in new issue