Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

added parameters to motor_ramp

Browse Source
This commit is contained in:
Andreas Antener
2016-04-12 21:07:09 +02:00
parent 1aeb139157
commit 0b930a36b9
1 changed files with 66 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/motor_ramp/motor_ramp.cpp
+65 -54
View File
@@ -61,9 +61,12 @@
#include "systemlib/systemlib.h" #include "systemlib/systemlib.h"
#include "systemlib/err.h" #include "systemlib/err.h"
static bool thread_should_exit = false; /**< motor_ramp exit flag */ static bool _thread_should_exit = false; /**< motor_ramp exit flag */
static bool thread_running = false; /**< motor_ramp status flag */ static bool _thread_running = false; /**< motor_ramp status flag */
static int motor_ramp_task; /**< Handle of motor_ramp task / thread */ static int _motor_ramp_task; /**< Handle of motor_ramp task / thread */
static float _ramp_time;
static int _min_pwm;
static orb_advert_t _actuator_outputs_pub = nullptr;
enum RampState { enum RampState {
RAMP_INIT, RAMP_INIT,
@@ -82,6 +85,12 @@ extern "C" __EXPORT int motor_ramp_main(int argc, char *argv[]);
*/ */
int motor_ramp_thread_main(int argc, char *argv[]); int motor_ramp_thread_main(int argc, char *argv[]);
bool min_pwm_valid(unsigned pwm_value);
void set_min_pwm(unsigned pwm_value);
void publish(struct actuator_outputs_s &actuator_outputs, float output);
/** /**
* Print the correct usage. * Print the correct usage.
*/ */
@@ -91,10 +100,11 @@ static void
usage(const char *reason) usage(const char *reason)
{ {
if (reason) { if (reason) {
PX4_WARN("%s\n", reason); PX4_WARN("[motor_ramp] %s", reason);
} }
PX4_WARN("usage: motor_ramp {start|stop|status} [-p <additional params>]\n\n"); PX4_WARN("[motor_ramp] usage: motor_ramp <min_pwm> <ramp_time>");
PX4_WARN("[motor_ramp] example: motor_ramp 1100 0.5\n");
} }
/** /**
@@ -107,49 +117,48 @@ usage(const char *reason)
*/ */
int motor_ramp_main(int argc, char *argv[]) int motor_ramp_main(int argc, char *argv[])
{ {
if (argc < 2) { if (argc < 3) {
usage("missing command"); usage("missing parameters");
return 1; return 1;
} }
if (!strcmp(argv[1], "start")) { if (_thread_running) {
PX4_WARN("motor_ramp already running\n");
if (thread_running) { /* this is not an error */
PX4_WARN("motor_ramp already running\n");
/* this is not an error */
return 0;
}
thread_should_exit = false;
motor_ramp_task = px4_task_spawn_cmd("motor_ramp",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
motor_ramp_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
return 0; return 0;
} }
if (!strcmp(argv[1], "stop")) { _min_pwm = atoi(argv[1]);
thread_should_exit = true;
return 0; if (!min_pwm_valid(_min_pwm)) {
usage("min pwm not in range");
return 1;
} }
if (!strcmp(argv[1], "status")) { _ramp_time = strtof(argv[2], NULL);
if (thread_running) {
PX4_WARN("\trunning\n");
} else { if (!(_ramp_time > 0)) {
PX4_WARN("\tnot started\n"); usage("ramp time must be greater than 0");
} return 1;
return 0;
} }
_thread_should_exit = false;
_motor_ramp_task = px4_task_spawn_cmd("motor_ramp",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
motor_ramp_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
return 0;
usage("unrecognized command"); usage("unrecognized command");
return 1; return 1;
} }
bool min_pwm_valid(unsigned pwm_value) {
return pwm_value > 900 && pwm_value < 1300;
}
void set_min_pwm(unsigned pwm_value) void set_min_pwm(unsigned pwm_value)
{ {
int ret; int ret;
@@ -157,7 +166,7 @@ void set_min_pwm(unsigned pwm_value)
char *dev = PWM_OUTPUT0_DEVICE_PATH; char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = px4_open(dev, 0); int fd = px4_open(dev, 0);
if (fd < 0) {PX4_WARN("can't open %s", dev);} if (fd < 0) {PX4_WARN("[motor_ramp] can't open %s", dev);}
ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values; struct pwm_output_values pwm_values;
@@ -171,29 +180,35 @@ void set_min_pwm(unsigned pwm_value)
ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {PX4_WARN("failed setting min values");} if (ret != OK) {PX4_WARN("[motor_ramp] failed setting min values");}
px4_close(fd); px4_close(fd);
} }
void publish(orb_advert_t actuator_outputs_pub, struct actuator_outputs_s &actuator_outputs, float output) void publish(struct actuator_outputs_s &actuator_outputs, float output)
{ {
//PX4_WARN("[motor_ramp] publish %f", (double)output); //PX4_WARN("[motor_ramp] publish %f", (double)output);
for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) { for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
actuator_outputs.output[i] = output; actuator_outputs.output[i] = output;
} }
orb_publish(ORB_ID(actuator_outputs), actuator_outputs_pub, &actuator_outputs); if (_actuator_outputs_pub == nullptr) {
_actuator_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &actuator_outputs);
} else {
orb_publish(ORB_ID(actuator_outputs), _actuator_outputs_pub, &actuator_outputs);
}
} }
int motor_ramp_thread_main(int argc, char *argv[]) int motor_ramp_thread_main(int argc, char *argv[])
{ {
PX4_WARN("[motor_ramp] starting"); PX4_WARN("[motor_ramp] starting");
thread_running = true; _thread_running = true;
struct actuator_outputs_s actuator_outputs = {}; struct actuator_outputs_s actuator_outputs = {};
orb_advert_t actuator_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &actuator_outputs); publish(actuator_outputs, 0.0f);
int ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); int ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
/* wakeup source */ /* wakeup source */
@@ -209,13 +224,9 @@ int motor_ramp_thread_main(int argc, char *argv[])
hrt_abstime last_run = 0; hrt_abstime last_run = 0;
enum RampState ramp_state = RAMP_INIT; enum RampState ramp_state = RAMP_INIT;
unsigned min_pwm = 1100;
float ramp_time = 0.5f;
float output = 0.0f; float output = 0.0f;
publish(actuator_outputs_pub, actuator_outputs, 0.0f); while (!_thread_should_exit) {
while (!thread_should_exit) {
if (last_run > 0) { if (last_run > 0) {
dt = hrt_elapsed_time(&last_run) * 1e-6; dt = hrt_elapsed_time(&last_run) * 1e-6;
@@ -242,14 +253,14 @@ int motor_ramp_thread_main(int argc, char *argv[])
switch (ramp_state) { switch (ramp_state) {
case RAMP_INIT: { case RAMP_INIT: {
if (min_pwm > 1350) { if (!min_pwm_valid(_min_pwm)) {
thread_should_exit = true; _thread_should_exit = true;
PX4_WARN("[motor_ramp] pwm min too high %d", min_pwm); PX4_WARN("[motor_ramp] min pwm not in range %d", _min_pwm);
break; break;
} }
PX4_WARN("[motor_ramp] setting pwm min %d", min_pwm); PX4_WARN("[motor_ramp] setting pwm min %d", _min_pwm);
set_min_pwm(min_pwm); set_min_pwm(_min_pwm);
ramp_state = RAMP_MIN; ramp_state = RAMP_MIN;
break; break;
} }
@@ -265,7 +276,7 @@ int motor_ramp_thread_main(int argc, char *argv[])
} }
case RAMP_RAMP: { case RAMP_RAMP: {
output += dt / ramp_time; output += dt / _ramp_time;
if (output > 1.0f) { if (output > 1.0f) {
output = 1.0f; output = 1.0f;
@@ -274,13 +285,13 @@ int motor_ramp_thread_main(int argc, char *argv[])
PX4_WARN("[motor_ramp] ramp finished, waiting"); PX4_WARN("[motor_ramp] ramp finished, waiting");
} }
publish(actuator_outputs_pub, actuator_outputs, output); publish(actuator_outputs, output);
break; break;
} }
case RAMP_WAIT: { case RAMP_WAIT: {
if (timer > 3.0f) { if (timer > 3.0f) {
thread_should_exit = true; _thread_should_exit = true;
break; break;
} }
@@ -289,11 +300,11 @@ int motor_ramp_thread_main(int argc, char *argv[])
} }
} }
publish(actuator_outputs_pub, actuator_outputs, 0.0f); publish(actuator_outputs, 0.0f);
PX4_WARN("[motor_ramp] exiting"); PX4_WARN("[motor_ramp] exiting");
thread_running = false; _thread_running = false;
return 0; return 0;
} }