Skip to content

Commit

Permalink
motor_ramp: set fmu in test mode and main cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
DanielePettenuzzo committed Jan 20, 2019
1 parent 652c9df commit f5aaefd
Showing 1 changed file with 118 additions and 39 deletions.
157 changes: 118 additions & 39 deletions src/systemcmds/motor_ramp/motor_ramp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
#include <px4_module.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <px4_getopt.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
Expand Down Expand Up @@ -75,11 +76,14 @@ enum Mode {
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 float _ramp_time = 1.0f;
static int _min_pwm;
static int _max_pwm;
static int _max_pwm = 2000;
static Mode _mode;
static char *_mode_c;
static char *_pwm_output_dev = "/dev/pwm_output0";
static struct pwm_output_values _last_spos;
static unsigned _servo_count;

/**
* motor_ramp management function.
Expand Down Expand Up @@ -124,16 +128,17 @@ Before starting, make sure to stop any running attitude controller:
When starting, a background task is started, runs for several seconds (as specified), then exits.
Note: this command currently only supports the `/dev/pwm_output0` output.
### Example
$ motor_ramp sine 1100 0.5
$ 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_ARG("<min_pwm> <time> [<max_pwm>]", "pwm value in us, time in sec", 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!");

Expand All @@ -149,66 +154,109 @@ Note: this command currently only supports the `/dev/pwm_output0` output.
*/
int motor_ramp_main(int argc, char *argv[])
{
if (argc < 4) {
usage("missing parameters");
return 1;
}
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
bool error_flag = false;
bool set_pwm_min = false;

if (_thread_running) {
PX4_WARN("motor_ramp already running\n");
/* this is not an error */
return 0;
}

if (!strcmp(argv[1], "ramp")) {
_mode = RAMP;
if (argc < 4) {
usage("missing parameters");
return 1;
}

} else if (!strcmp(argv[1], "sine")) {
_mode = SINE;
while ((ch = px4_getopt(argc, argv, "d:a:b:r:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {

} else if (!strcmp(argv[1], "square")) {
_mode = SQUARE;
case 'd':
if(!strcmp(myoptarg, "/dev/pwm_output0") || !strcmp(myoptarg, "/dev/pwm_output1")){
_pwm_output_dev = const_cast<char*>(myoptarg);
} else {
usage("pwm output device not found");
error_flag = true;
}
break;

} else {
usage("selected mode not valid");
return 1;
}
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;
}

_mode_c = argv[1];
break;

_min_pwm = atoi(argv[2]);
case 'b':
_max_pwm = atoi(myoptarg);

if (!min_pwm_valid(_min_pwm)) {
usage("min PWM not in range");
return 1;
}
if (!max_pwm_valid(_max_pwm)) {
usage("max PWM not in range");
error_flag = true;
}

_ramp_time = atof(argv[3]);
break;

if (argc > 4) {
_max_pwm = atoi(argv[4]);
case 'r':
_ramp_time = atof(myoptarg);

if (!max_pwm_valid(_max_pwm)) {
usage("max PWM not in range");
return 1;
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 = 1;
}


if (!strcmp(argv[myoptind], "ramp")) {
_mode = RAMP;

} else if (!strcmp(argv[myoptind], "sine")) {
_mode = SINE;

} else if (!strcmp(argv[myoptind], "square")) {
_mode = SQUARE;

} else {
_max_pwm = 2000;
usage("selected mode not valid");
error_flag = true;
}

if (!(_ramp_time > 0)) {
usage("ramp time must be greater than 0");
_mode_c = const_cast<char*>(myoptarg);

if(error_flag){
return 1;
}

_thread_should_exit = false;
_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;
}

Expand Down Expand Up @@ -317,7 +365,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
{
_thread_running = true;

char *dev = PWM_OUTPUT0_DEVICE_PATH;
char *dev = _pwm_output_dev;
unsigned long max_channels = 0;

int fd = px4_open(dev, 0);
Expand All @@ -326,6 +374,26 @@ int motor_ramp_thread_main(int argc, char *argv[])
PX4_ERR("can't open %s", dev);
}

/* 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");
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);
return 1;
}
}

if (px4_ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) {
PX4_ERR("Failed to Enter pwm test mode");
return 1;
}

if (prepare(fd, &max_channels) != OK) {
_thread_should_exit = true;
}
Expand Down Expand Up @@ -413,8 +481,19 @@ int motor_ramp_thread_main(int argc, char *argv[])
}

if (fd >= 0) {
/* disarm */
ioctl(fd, PWM_SERVO_DISARM, 0);
/* 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);
return 1;
}
}

if (px4_ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) {
PX4_ERR("Failed to Exit pwm test mode");
return 1;
}

px4_close(fd);
}
Expand Down

0 comments on commit f5aaefd

Please sign in to comment.