Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 17:35:22 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

added option to output sine wave

Browse Source
This commit is contained in:
Roman
2016-04-25 22:39:19 +02:00
committed by Andreas Antener
parent b3c3d6f88f
commit 87dc996a41
1 changed files with 25 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/motor_ramp/motor_ramp.cpp
+25 -6
View File
@@ -36,6 +36,7 @@
* Application to test motor ramp up.
*
* @author Andreas Antener <andreas@uaventure.com>
* @author Roman Bapst <bapstroman@gmail.com>
*/
#include <px4_config.h>
@@ -63,6 +64,8 @@ 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 bool _sine_output = false;
enum RampState {
RAMP_INIT,
@@ -102,7 +105,8 @@ usage(const char *reason)
}
PX4_WARN("[motor_ramp]\n\nWARNING: motors will ramp up to full speed!\n");
PX4_WARN("[motor_ramp] usage: motor_ramp <min_pwm> <ramp_time>");
PX4_WARN("[motor_ramp] usage: motor_ramp <min_pwm> <ramp_time> <-s>");
PX4_WARN("[motor_ramp] setting option <-s> will enable sinus output with period <ramp_time>");
PX4_WARN("[motor_ramp] example:");
PX4_WARN("[motor_ramp] nsh> sdlog2 on");
PX4_WARN("[motor_ramp] nsh> mc_att_control stop");
@@ -139,6 +143,13 @@ int motor_ramp_main(int argc, char *argv[])
_ramp_time = atof(argv[2]);
// check if we should apply sine ouput instead of ramp
if (argc > 3) {
if (strcmp(argv[3], "-s") == 0) {
_sine_output = true;
}
}
if (!(_ramp_time > 0)) {
usage("ramp time must be greater than 0");
return 1;
@@ -284,7 +295,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
case RAMP_MIN: {
if (timer > 3.0f) {
PX4_WARN("[motor_ramp] starting ramp: %.2f sec", (double)_ramp_time);
PX4_WARN("[motor_ramp] starting %s: %.2f sec", _sine_output ? "sine" : "ramp", (double)_ramp_time);
start = hrt_absolute_time();
ramp_state = RAMP_RAMP;
}
@@ -294,13 +305,21 @@ int motor_ramp_thread_main(int argc, char *argv[])
}
case RAMP_RAMP: {
output += dt / _ramp_time;
if (!_sine_output) {
output += dt / _ramp_time;
if (output > 1.0f) {
output = 1.0f;
} else {
// 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 * 1e6f) - M_PI_2_F));
}
if ((output > 1.0f && !_sine_output) || (timer > 3.0f * _ramp_time * 1e6f && _sine_output)) {
// for ramp mode we set output to 1, for sine mode we just leave it as is
output = _sine_output ? output : 1.0f;
start = hrt_absolute_time();
ramp_state = RAMP_WAIT;
PX4_WARN("[motor_ramp] ramp finished, waiting");
PX4_WARN("[motor_ramp] %s finished, waiting", _sine_output ? "sine" : "ramp");
}
set_out(fd, max_channels, output);